International Journal of Advanced Robotic Systems

OPEN V!/ ACCESS ARTICLE

A Novel Biped Pattern Generator Based on Extended ZMP and Extended Cart-table Model

Regular Paper

Guangbin Sun1, Hong Wang1* and Zhiguo Lu1

1 Northeastern University, Shenyang, China Corresponding author(s) E-mail: roboneu537@gmail.com

Received 28 August 2014; Accepted 23 February 2015

DOI: 10.5772/60783

© 2015 Author(s). Licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract

This paper focuses on planning patterns for biped walking on complex terrains. Two problems are solved: ZMP (zero moment point) cannot be used on uneven terrain, and the conventional cart-table model does not allow vertical CM (centre of mass) motion. For the ZMP definition problem, we propose the extended ZMP (EZMP) concept as an extension of ZMP to uneven terrains. It can be used to judge dynamic balance on universal terrains. We achieve a deeper insight into the connection and difference between ZMP and EZMP by adding different constraints. For the model problem, we extend the cart-table model by using a dynamic constraint instead of constant height constraint, which results in a mathematically symmetric set of three equations. In this way, the vertical motion is enabled and the resultant equations are still linear. Based on the extended ZMP concept and extended cart-table model, a biped pattern generator using triple preview controllers is constructed and implemented simultaneously to three dimensions. Using the proposed pattern generator, the Atlas robot is simulated. The simulation results show the robot can walk stably on rather complex terrains by accurately tracking extended ZMP.

Keywords Biped walking, Pattern generation, Rough terrain, Extended ZMP, Extended cart-table model

1. Introduction

It is important to plan walking motion on rough ground for a humanoid or biped robot to perform useful tasks [1]. To plan robot motion, a criterion is required to quantify the walking stability or dynamic balance. As a stability criterion, ZMP has been widely used in motion planning or control of legged robots. ZMP, zero moment point, is a point where the horizontal moments generated by ground reaction forces are zero [2]. Dynamic balance or stability can be judged by checking whether ZMP is within convex hull encompassing the support foot/feet. Although other stability criteria have also been proposed [3-6], ZMP is still the most widely used for its simplicity of form and convenience to measure. It is also problematic in the sense that ZMP is not defined if two feet contact with different planes [7].

To achieve rough ground walking and not make the problem too complex, one issue is of how to define the ZMP on rough ground. The Honda US patent defines the virtual ZMP and virtual surface as linear combination of two local ZMPs and contacting surfaces of both feet, respectively, changing linearly in time from one foot to the other [8]. However, the definition does not conform to the ZMP concept because the resultant moment of gravity and inertial force is not perpendicular to the virtual surface [7].

The virtual slope method was proposed by Sato et al. with virtual ZMP defined on the virtual slope [9]. This method can extend ZMP to uneven terrain. However, the virtual support polygon, which is on the virtual slope, depends on the CM position and it is difficult to determine the exact support polygon in advance.

Under the ZMP criterion, one of the most prevalent ZMP models is the cart-table model proposed by Kajita et al. [10]. The cart-table model assumes the CM motion is constrained on a horizontal plane. This model dramatically reduced the complexity and was widely used in walk planning and control [11-14]. However, the linearity of the cart-table model may not be preserved when vertical motion of CM is allowed, for example, to climb stairs [9]. It can be tackled by assuming a negligible effect of vertical motion on horizontal dynamics [15]. Another way is to compensate the horizontal ZMP error from vertical CM motion by manipulating angular momentum [16]. This paper proposes a model with a dynamics constraint rather than the constraint of constant CM height, and the cart-table model is extended to 3D space. This model does not need angular momentum to compensate for horizontal ZMP error as in [16]. In [17], a new frame was attached to the slope that is parallel to the slope surface to decouple horizontal and vertical motion in planning slope walking. Robot motion is represented in the new frame in a simple form. However, the resulting model depends on the slope gradient. This may need parameter reconfiguration whenever the slope gradient changes. Our model avoids this problem by enabling vertical motion while preserving model linearity.

It is important to change walking pattern in real time for more flexibility. This requires an online motion planning. Ching-Long Shih et al. use the virtual constraint method to generate reference walking trajectories assuming the robot to have pointed feet, and the walking stability is enhanced by a supplemental stabilizing controller [18]. There is also work based on genetic algorithm methods, which make the design of motions more intuitive and versatile [19]. Kajita et al. proposed a preview control-based method taking the pattern planning as a servo tracking problem with ZMP as plant output [10]. The preview control allows the time and location of foot landing to be changed online. This is useful for taking agile motion like emergent stop to avoid collision with obstacles [20]. This feature is therefore important for rough ground walking, as step length and cycle possibly need be modified. Feng et al. used a dynamic programming technique, which also uses future information like preview control, to generate CM motions, and achieved walking on rough terrain [21]. Nishiwaki et al. proposed balancing the robot by changing the reference ZMP of preview control in the permissible region or foot position in the next step for walking on rough terrains [1]. However, it was not related to how to define ZMP on rough ground or how to design vertical motion whilst keeping the robot dynamically balanced. The extended cart-table model and the extended ZMP concept, we propose, will tackle these problems in an elegant style for walking on complex terrains.

This paper mainly focuses on establishing a framework of planning walking on complex terrains. A new criterion -extended ZMP - is proposed as an extension of ZMP to general terrains. Based on some assumptions compatible with ZMP, the extended ZMP concept is explicitly defined and a dynamic equation is derived. We use a novel method based on constraint to analyse EZMP dynamics. It is found that extended ZMP dynamics have the same form as ZMP only under appropriate constraints. Furthermore, the additional constraint can be removed if the angular momentum around CM is neglected. To make the extended ZMP-CM equation linear and enable vertical motion, a dynamic constraint is added to the conventional cart-table model of (E)ZMP equations to form an extended cart-table model. The resulting equation set is composed of three equations in a similar form. To compute CM motion from extended ZMP trajectory, preview control is used in all three directions.

This paper is organized as follows. Section 2 generalizes the classical ZMP concept to universal terrains. Section 3 introduces our proposed model - the extended cart-table model. Section 4 implements triple preview controllers on the extended cart-table model. Section 5 presents the general framework of planning biped walking on rough terrains using preview control, combining the above-mentioned extended ZMP concept and extended cart-table model. Section 6 gives some simulation examples of Atlas robot walking on complex terrains, and the paper is concluded in Section 7 with some discussions on limitations and our future work.

2. Extending ZMP Concept to Rough Terrains

2.1 Extended ZMP Concept

ZMP, or zero moment point, is a point on the horizontal plane where the tipping moment around the horizontal axes is zero [2]. ZMP is on the contact surface and coincides with the CoP (centre of pressure). It is unable to define the ZMP on uneven ground, and a virtual ZMP should be defined instead. To extend the ZMP concept, the following definitions are given:

Definition 1 (VCS): VCS (virtual contact surface) is an arbitrary virtual surface with a finite slope.

Definition 2 (EZMP):EZMP (extended zero moment point) is a point on VCS where the ground reaction moment tangent to the VCS is zero.

EZMP is used for rough terrain walking. The next section presents the systematic theoretical framework of EZMP.

2.2 Derivation and Analysis of EZMP Based on Constraining

Based on Definition 2, we can derive the EZMP equations in the following.

Let n=(nx,ny,nz)T and Tv=(zpx,Tpy,Tpz)T be normal vector of VCS at EZMP p=(px, py, pz)T and the ground reaction torque

around p. From Definition 2, it is easy to derive that n and Tp should be in line. Let Tnp=(znpxyTnpy,Tnpz)T be the cross-product of n and Tp, the following equation can be obtained:

Cx = gLcy + aLz +aPy ( x - Px ) - aPx (y - Py ) (a)

Cy = yLcx + ßLcz +ßPy (x - Px) - ßPx (y - Py) (b)

Cz =a(LCx + (pp + Mg)(y - py) - Py (z - pz)) (c)

-ß (Ly - (P + Mg ) ( X - Px ) + Px ( Z - Pz ))

This is unfolded as:

V nPZ 0

-n t + n t z Py y Pz (01

n t - n t z px x pz = 0

-n t + n t y Px x py 0 ,0 0

a = -ny ,ß = -nx g = nz

To simplify the dynamics, we add the constraints:

Considering Tp can also be expressed in multi-body dynamics:

L - pc x Mg + (P - Mg)x p

Cx = 0 Cy = 0

Cz = 0

(b) (c)

The following can be derived:

\ p= /

(Lx + Mgy + Pp - (Pz + Mg) Py ^

Ly - Mgx - PxPz+(Pz + Mg) Px Lz + PxPy - PyPx

where L = (Lx, Ly, Lz)T is the angular momentum around the

coordinate origin O, pc = (x, y, z)T is centre of mass, P = (Px, Py, Pz)T is translational momentum, M is total mass and g = (0, 0, -g)T is gravitational acceleration vector.

Let Lc = (Lcx, Lcy, Lcz)T be the angular momentum around pc. The derivatives of angular momentums around O and pc can be related as follows:

(yPz - zPy ^

zPx - xPz

xPy - yPx

Substitute (5) into (4), and then substitute (4) into (2), we obtain:

Cx +gPx (z - Pz) - 7(Pz + Mg) (x - px ) Cy -gPy (z - Pz) + g(Pz + Mg) (y - Py)

g(Px (z - Pz)-(Pz + Mg) (x - Px )) = 0 (a) r(Py (z - Pz)- (Pz + Mg)(y - Py )) = 0 (b)

By solving (9a) and (9b), we get:

arLcx = ßrLcy

If y * 0 means VCP is not vertical, divide (10) and (11) by y and substitute them into (7c); the simple result is obtained as Cz=0. This makes (9c) a redundant constraint.

From equation (10), the following is derived:

z - Pz Px = x---- x (a)

z +g z - Pz

z + g '

which is simply the same as classical ZMP dynamics without angular momentum around CM [22]. This means the same EZMP equation (12) as ZMP can be derived by setting constraints (9a) and (9b).

Equation (12) can be arranged as:

Py (x-Px )- Px (y - Py )= 0 (13)

L = Lc +

Py = y

Px (z-pz)-(Pz + Mg ) ( x-px) = 0 (a) P ( z - Pz )- (Pz + Mg ) ( y - py ) = 0 (b)

If we assume a negligible centroidal angular momentum and its derivative, i.e., L c=0, and substitute (13) and (14) into (7), it is easy to prove the constraint (9a) and (9b) are satisfied by (12).

If y =0 means VCP is vertical, (9c) cannot be derived from constraints (9a) and (9b). Therefore, constraint (9c) is indispensible. However, by neglecting the centroidal angular momentum, it can also be shown that constraint (9a) (9b) and (9c) are all satisfied by (12) and can be removed (no matter y * 0 or y =0).

We conclude the above as theorems:

Theorem 1:f y * 0 and constraint (9a) and (9b) are satisfied, then the exact EZMP equation is in the same form as ZMP without centroidal angular momentum, expressed as (12).

Theorem 2:If we neglect centroidal angular momentum, then the approximate EZMP equation is in the same form as ZMP without centroidal angular momentum, expressed as (12). The constraint (9) is satisfied implicitly.

In the above two theorems, it is very interesting to notice that the EZMP equations are in the same form while bearing different meanings such as exact and approximate, respectively. This indicates that matching of EZMP dynamics (12) to reality relies on the degree to which the constraint (9) is satisfied. Moreover, if a=6=0, (6) is exactly the ZMP equation based on multibody dynamics. In this sense, EZMP is compatible to ZMP definition.

Until now, we have derived the EZMP dynamics on an undetermined virtual surface (VCS). In constraint (9), we find the translational and rotational dynamics in vertical and horizontal directions are entangled. It is not trivial to solve the equations in such a case. Luckily, if the angular momentum around CM is neglected, as in Theorem 2, (9) is a redundant constraint satisfied implicitly by (12) and is eliminated.

Assumption *(optional): centroidal angular momentum is neglected and VCS is assumed a plane for simplicity, named virtual contact plane (VCP). As shown in Figure 1, in single support phase this plane (pi-p2-pi') passes the three centres of the contacting areas for stance foot (p2) and swing foot at lifting (p1) and landing (p1'). The double support phase shares the same VCP as the future single support phase. In flying case (if it exists), the EZMP does not apply like ZMP does not either and VCP is not defined.

To compare ZMP and EZMP, consider a special case when p1 and p2 are on the horizontal plane while p1' is off the horizontal plane. ZMP and EZMP can both be defined at p1 and p2. EZMP requires a resultant torque of ground reaction forces normal to the VCP, and ZMP requires a normal to horizontal plane. In this sense, ZMP is a special

case of EZMP when VCP is horizontal. When left and right foot are at p/ and p2 in double support phase, ZMP can no longer be defined and EZMP is still available by the definition that resultant torque of ground reaction forces is normal to the VCP.

Figure 1. VCP definition

The EZMP trajectory is easy to prescribe by considering Assumption *. This solution uses a simpler VCP not changing with time during the double support phase, yet compatible to ZMP definition with a resultant of ground reaction torque perpendicular to the VCP plane under the condition of neglecting centroidal angular momentum. Moreover, the support polygon does not change with CM compared with virtual slope method in [9]. The design of EZMP is also easy and straightforward without parameters of vague meaning compared with other criteria like FSW [23] and CWS [5]. Another bonus feature is that the VCS surface can also be used to restrict the CM motion within a workspace by modifying its shape, and a stabilizer using translational and angular momentums can also be designed considering constraint (9).

Although constraint (9) is removed in this paper for simplicity by neglecting centroidal angular momentum, the two equations in (12) are still nonlinear. In the next section, we will consider adding another constraint to the EZMP equation set (12) to allow the vertical motion of CM while decoupling motions in all three directions. In this way, a linear model in 3D space is obtained.

3. Extended Cart-Table Model with Dynamic Constraint

Kajita et al. introduced ZMP to the 3D-LIPM model and constraining the CM motion on a plane with a given slope [10]. This method can be used to plan walking on uneven ground. It adapts constraint planes to complex terrains. When a horizontal plane is used, the cart-table model is derived [10]. This model is used prevalently in biped robot motion planning and control, but it does not specify the vertical motion. This section will find a linear model in 3D space in terms of EZMP, which incorporates vertical motion with a single constraint.

Biped robot dynamics can be expressed exactly as CM dynamics plus rotational dynamics around CM. It is assumed that the centroidal angular momentum around CM for walking on rough ground can also be neglected in EZMP dynamics, as is done with ZMP on flat ground. Under this condition (or under the more general constraint (9)), as we have shown, the EZMP equation is the same as ZMP (12).

If the CM height z is set constant, the equation (12) becomes a cart-table model in term of EZMP [10]. However, to allow the vertical motion we choose a more general constraint as follows

(15) r

Pz + cg = z - cz (16)

where c is a constant.

Substituting (15) into (12) and combining (16) gives us

Px = x - cx

Py = y - cy

aux _ —

Pz = Z - Cz

(b) (c)

where EZMP.

pr=Pz + ^^

paux=(px, p pzaux)T is named auxiliary

The first two rows of (17) are the same as the constant height cart-table model (in terms of EZMP). The third row serves as a constraint. In this sense, equation set (17) is a natural generalization of the cart-table model to EZMP and to the 3D space of CM; here it is named the extended cart-table model. It is very interesting to notice that the third row has the same linear form as the first two rows. This means all the motion planning or control methods available to the cart-table model are also applicable to our extended cart-table model. In this paper, we are especially interested in applying the preview control. Once the trajectory paux = ( px, py, pzaux )T is given, the preview control can be applied to the three equations of (17) simultaneously. As long as the p!*ux in the third equation of (17) is tracked accurately enough, the constraint (15) is met and the first two rows of (17) remains linear as if no CM height change happens. In this sense, the z-dimension motion is generated using a similar mechanism to what is used in horizontal motion generation. Figure 2a shows the auxiliary EZMP and CM trajectory cluster with c varying from 0.02 s2 to 0.12 s2 (step = 0.01 s2). Figures 2b and 2c show the sagittal and frontal views. The trajectories are generated by solving the discretized equation of (17) using the Thomas algorithm

[24]. Each paux trajectory consists of a horizontal line and sloped line. With c increasing paux and CM gets farther away off the paux curve at the turning point. Away from the turning point, CM approaches the paux curve. This means the change of c affects the dynamics of CM at turning point of EZMP, and it is favourable to set c as constant.

Y [m] 0 -0.05 (b)

-0.1 0 1.5

0.2 x [m]

0.2 x [m]

Figure 2. The auxiliary EZMP and CM trajectories for different c values

Remark 1: Mathematically, pc - paux=cp c can be derived from Equation (17). paux=(px, py, pzaux)T can be interpreted by taking paux as a point from which the vector points at CM pc is parallel to CM acceleration. Considering pzaux = pz+cg, pzaux is governed by pz and constraint parameter c. pz is constrained by VCP and can be interpreted to be the vertical component of EZMP which affects pzaux. In turn, pzaux determines where the CM evolves according pc - paux = cp c.

EZMP can be chosen at a general surface not limited to VCP. Physically, EZMP is a point on a plane where the moment generated by gravity, inertia force and external force except ground reaction force is along the VCP normal vector. VCP (or VCS) is a reference surface which provides consistency between the EZMP prescription and measurement. More complex surfaces are also applicable. Whatever surface is used results in the same equation, (12) or (17). Mathematically, z-direction motion in (17) will affect how the constraint parameter c evolves with given pz. Only the z-direction motion evolves as given in (17); c is a constant and the EZMP equation remains linear. In particular, setting pZaux=cg or pz=0 results in a constant CM height motion and (17) becomes a classical cart-table model.

4. Triple Preview Control

Most of the inverted pendulum model-based motion planning methods cannot change the future ZMP in real time. The preview control of ZMP was used to tackle this problem [10]. In our case of planning a walk on rough ground, the extended cart-table model is used. In the sagittal plane (for example), the state space model in continuous time is formed as:

x "0 1 0" x "0"

x = 0 0 1 x + 0

x 0 0 0 x 1

Px =[1 0 -c]

where uc = x is a derivative of the forward acceleration of CM as system input.

The equation (18) is discretized as:

q(k +1) = Aq(k) + Bu(k) r (k) = Cq(k)

q(k) = [ x(kT) x(kT) x(kT )]T u(k) = uc (kT) r (k) = Px (kT)

1 T t%' rt y" /6

A= 0 1 T , B = t y /2

0 0 1 T

C = [1 0 -c]

T is the sample time.

For preview control, the performance index is given as:

J = E Qe (rref (i) - r(i))2 + AqT(i)QxAq(i) + RAu(i)

where Qe, Qx, and R are nonnegative definite matrices. Aq(k ) = q(k) - q(k -1) is the incremental state and Au(k ) =u(k) - u(k -1) is the incremental input.

By preview control theory, the optimal controller with preview time of np is given as [10, 25, 26]:

u(k) = -keE(r(i) - rrf (i)) - kxq(k)

-E k f(i)rref (k + i)

where np is the previewed length in dummy time points. ke, kx, and kf are gains calculated from Qe, Qx and R by preview control theory [25, 26].

In the same way, the preview controllers for y and z dimensions can be obtained. Three preview controllers are implemented in x, y and z axes simultaneously. We call the controller group triple preview controller. It should be noted that in the z-axis controller, paux = pz + cg rather than pz is used.

The z-dimensional controller uses a mechanism that is similar to what is used in horizontal dimensions. The z-dimension controller guarantees the CM trajectory satisfies a specific constraint (Equation (15). Under the constraint, the x and y of CM goes as if there is no height change. Although the constant c somewhat limits the versatility of CM motion compared with the nonlinear optimisation method [21], the usage of the linear model decreases the computing burden and vertical CM motion is still considered.

5. Pattern Planning using Extended Cart-Table Model Based on EZMP under Triple Preview Control

In this section we will show the general framework of generating motion patterns on universal rough terrains using an extended cart table model and EZMP concept under preview control.

5.1 General Procedures for Pattern Planning

At a minimum, we need to assign the spatial gait parameters (step/stride length, width and clearance) as well as temporal gait parameters (gait cycle and occupancies of double/single support phases) to begin the walk motion planning. The foot landing position and time can be calculated accordingly. To judge stability, we give the concept of the virtual support polygon (VSP) defined as the convex hull encompassing the supporting foot/feet projections onto the VCP plane. For single support phase, the EZMP trajectory is designed assuming the largest stability margin, which means EZMP is located at centre of VSP polygon. For the double support phase, the EZMP trajectory is generated by linear interpolating in three directions simultaneously, given initial and terminal CoPs as boundary points. EZMP is constrained within the VSP polygon. Foot trajectories are calculated using spline interpolation considering the boundary locations and time of feet. Additionally, avoiding obstacles is also considered in calculating the foot trajectories. When all of the above is performed, the CM trajectory is generated using a triple

preview controller (21) (similar in y and z dimensions) in Section 4 applied to extended cart-table model (17) in Section 3. The joint angles are calculated by inverse kinematics and fed to the robot.

tp■ (p - Pi) x F + (P - P2) * f2 ■ P * F - (Pi * F + P2 * F2)

5.1.1 EZMP Trajectory Prescription

We have proved in Section 2 that the EZMP follows the same dynamics equation as the ZMP equation (12) under constraint (9). Moreover, VCS (virtual contact surface) is chosen to be the VCP plane and the angular momentum around CM is neglected to eliminate constraint (9). Therefore, for the motion planning part, no change is made to the ZMP equation to adapt to uneven ground. However, if angular momentum is considered, EZMP must also satisfy constraint (9) exactly. In Assumption *, we have assumed the VCP is a plane passing three centres of contacting areas for stance foot and swing foot at lifting and landing. The desired EZMP trajectory is also required to lie in VCP to be feasible. During single support, we choose the desired EZMP to be at the contact centre of stance foot. During the double support phase, the EZMP are defined by linear interpolating of the contacting centres of both supporting feet. It should also be noted that the VCP definition should be the same for EZMP planning and measuring/calculating.

5.1.2 Generating CM Trajectory by Preview Control

Upon the prescription of the EZMP trajectory, preview controller (21) can be applied to model (17). Note the auxiliary counterpart pzaux is used rather than pz in the vertical direction. Because the EZMP trajectory should be previewed, a buffer fifo is used to store the future EZMP within the preview period Tp. The system state x= (x, x, x)T (and counterparts in y and z directions) is obtained at every time point as the most important data of pattern planning.

5.1.3 EZMP Measurement

Although EZMP is defined on a virtual plane (VCP), it can only be measured from the pressure sensors mounted under the foot bottom as well as the ankle torque feedbacks. Like the planned EZMP mentioned above, the measured EZMP should also conform to the Definition 2 and Assumption *.

Let f|oc = ( f£c, fl°c, 0)T and N|oc = (0, 0, N/oc)T be the friction and normal pressure forces acting on ith foot in local frame (i=1, 2, meaning left and right foot). Let F( = (Fix, Fiy, Fiz )T=Rf + Nioc) be the ground reaction force acting at individual CoP pi of each foot in global frame, where Ri is foot rotation matrix. Hereafter, if not assigned the superscript loc, the quantity is defined in global frame by default. Let F=(Fx, Fy, Fz)T =F1+F2 and Tp=(Tpx, Tpy, Tpz)T be the resultants of ground reaction force and torque at EZMP p=(px, py, pz)T. Immediately we obtain the following equation:

Fp = F1P1 + F2P

2F2 ^p

where F is a matrix equivalent to F x operation. Ft (i=1, 2) are defined similarly.

' 0 -Fz Fy ^ ' 0 -Fz Fy

F = Fz 0 - Fx , F = Fz 0 -Fix

l-Fy Fx 0 0 [-F'y Fix 0

Multiply both sides of (23) by a matrix Tproj which projects the (23) to the VCP plane:

Tpro/Fp Tpro/Fpi + TprojF2P2 Tproj^p

^ F 'p = Fy + F/P2 -rnp

where T.=E - nnT (E is 3x3 identity matrix). I3, I31', I32' and Tp are the resulting matrices and vector by operator Tproj.

Let the first two elements of Tp in (24) equal zero and the following is obtained:

It = F1P1 +F2P2

where r and r (i=1, 2) are sub-matrices consisting of the first two rows of I3 and I31' respectively.

Additionally, the EZMP p is on the VCP plane, which is formulated as follows:

n.(p - ri) = 0

nTp = n1^

where n is the normal vector of VCP, which passes the footprint centre rx for the first landing foot (the other foot can also be used).

Concatenate Eq. (25) and (27), we obtain:

^p = Z (28)

where ^ =

It is easy to prove rank(^)=3 if and only if F • n = I F I I n I cos(0)^0 (Fnot in VCP, violated rarely).

If F is not in VCP, we can get the unique solution as follows:

p = ^-1Z

If F is in VCP, ^ is not invertible. In this case, p can take any value.

Remark 2: Friction fioc are easy to be calculated from measured ankle torques t^ i and normal pressure Nl°c expressed in local frame of each ankle, which are available in most biped robots. Let ploc (i=1, 2) be the CoP position of foot i in local frame. Neglecting the foot mass, the following equation exists:

Ck, _-pO x Nfc - pf x fo

^ pO x fO =-pO x Nfc ^ pf _ b'r (with b'° = -p'° x NO )

where pl°c is a matrix equivalent to pioc x operation (similar to F definition). Let Dioc be the sub-matrix consisting of the first two rows and first two columns of pl°c and dioc be the vector consisting of the first two elements of bloc. Then the friction fioc can be solved as follows:

( DO )-1 dO 0

It is easy to prove, with the z-component of ploc not zero (distance between ankle and foot bottom not zero, always satisfied), Dl°c is invertible.

6. Implementation and Results

Our proposed motion planning method is tested using a simulated Atlas robot shown in Figure 3, configured with real robot specification (https://bitbucket.org/osrf/ drcsim/src/c69ecab26a55/ros/atlas_description/urdf/). The Webots software is used for simulation (http://www.cyber-botics.com/).

Four FSR sensors are attached beneath each foot to measure pressure. The ankle joint torque feedback is also used to calculate the frictions. The extended ZMP are calculated from pressure and friction measurements. The simulation is run on a computer with Intel Core i7 CPU of 2.67GHz and 8G memory.

Figure 3. Atlas robot

The EZMP concept and extended cart-table model are used combined with preview control. The general procedure of pattern planning is as follows.

Initialization

Calculating preview controller gains Setting initial values of CM, EZMP... Evaluating buffer fifo Step i=0; Time fc=0; for Step i=:+1 Planning foot and EZMP trajectories; for Time fc=:+1

Measuring FSR, joint angles and ankle torques Calculating EZMP and tracking error Applying preview control Applying walking stabilizer Calculating joint angles by inverse kinematics Executing joint angles

The main goal of this paper is to construct a general framework to plan walking on complex terrains. Thus, we

will consider planning patterns for climbing stairs at first and then rough terrains. Climbing stairs can be taken as the basis for general rough terrain walking. Currently, the landscape is assumed to be exactly known.

The constraint constant in the extended cart-table model is c=0.0861 s2. The walk stride length and step wide are 0.36 m and 0.2 m. Foot clearance is determined by the height of obstacles and desired landing position.

6.1 Stabilizer

Considering that the centroidal angular momentum is neglected in walk planning, the modelling error can deteriorate the stability. Therefore, a walking stabilizer is indispensable. The sudden change of ZMP reference by the stabilizer will cause a large tracking error of preview control in the same control loop. We find that the tracking is not as sensitive to system state (CM motion) change as to (E-) ZMP change in our case. Therefore, we use a modified version of the typical stabilizer using a linear inverted pendulum [27]. The resultant controller is formed as follows (x-dimension):

X = x' - /q (ps - pc') - k2 (.V - xd ) - A-, (.V - xd ) (32)

where xd is the desired acceleration of CM generated from preview control. kv k2 and k3 are controller gains which are tuned by the experiment. The 1/ and z dimensions use the same form of equations, except that paux is used instead of p for z dimension. In this paper the gains are determined as k1 = 1.95, k2 = -0.975 and k3 = -1.95 for x and y dimensions and k1 = 1.95, k2 = -0.000975 and k3 = -0.00975 for the z dimension. This controller uses CM acceleration instead of ZMP modification to stabilize the robot.

6.2 Walking on Stairs

Figure 4. Landscape of stairs

Figure 5. EZMP prescription for walking on stairs

The walking planning method in this paper (with dynamic constraint of CM) is compared with a comparison planning method of which the vertical motion affects the horizontal motion. To generate the vertical CM motion in the comparison method, we use a logistic function-based interpolator from [28] with boundary points. The logistic function interpolator is defined as:

The whole landscape is shown in Figure 4. The upstairs are constructed with 0.08 m increment. The top surface of every stair is flat with an area of 0.32x0.2 m2 encompassing the robot foot bottom. The robot walks from level floor up stairs. Walking cycle is 1.2 s including 2/3 single support phases and 1/3 double support phases. The sampling time is 10 ms and preview period is 2 s which is generally long enough to achieve good preview control. Considering Equation (21), weighting matrix Qe = 100000, Qx = diag(10,10,10) and R = 1x10-6 and gains ke, kx, and k are gains calculated from Qe, Qx and R by preview control theory [25, 26]. The planned extended ZMP is shown as the thick line in Figure 5. The green T-shapes show the position and posture of the supporting foot. The dark grey surface composed of triangles with thick lines as boundary edges is the VCP plane where the EZMP passes. The light grey hexagons are the VSP polygons.

1 + exp í -2Dt

t0 + tn

(to < t < tn )

(t = to) (t = ts)

where t is time and z is vertical CM position. At = tn -10. Subscript 0 and n denotes initial and terminal condition. This interpolator maps the typical logistic function from the infinite domain of definition to a finite interval [t0, tn].

The EZMP tracking in 3D view without and with dynamic constraint are shown in Figures 6 and 7. x=0~0.18 m and x=0.18 ~0.72 m correspond to the walking on level floor and

to - ttn - t

upstairs. The measured EZMP in blue lines are constrained in the hexagons and the tracking error of EZMP ezmpd = ( px, py, pzd)T with vertical motion dynamically constrained (Figure 7) is smaller than the error with vertical motion dynamically unconstrained (Figure 6). The planned trajectories by preview control and logistic interpolator (33) are shown in (a,c,e) and (b,d,f) of Figure 8. The corresponding measured trajectories are shown in (a,c,e) and (b,d,f) of Figure 9. t=0~1.46 s and t = 1.46~2.9 s correspond to the walking on level floor and up stairs. The superscript 'prevw' and 'meas' means EZMP outputed from preview controller and measured from sensors. As shown in Figure 8 and 9, the horizontal components of auxiliary EZMP paux,d=(Px, Py, Pzaux'd)T=(Px, Py, Pzd)T + (0, 0, cg)T are tracked more accurately when vertical motion is dynamically constrained (b, d and f in Figures 8 and 9) compared with the case with vertical motion being dynamically unconstrained (a, c and e in Figure 8 and 9). In the case of constraining the vertical CM motion (b, d and f of Figures 8 and 9), the auxiliary EZMP in z dimension pzaux,d is tracked accurately enough, which guarantees the horizontal dynamics to be linear. Therefore the x and y dimensions also show fairly good paux,d tracking. This results in a good tracking of EZMP ezmpd (Figure 7). The remaining discrepancy is mainly from the part of multi-body dynamics not compensated by the stabilizer. Nevertheless, with the vertical CM motion dynamically constrained, the EZMP error is acceptable and the robot can walk up stairs with stability, as shown in b and d of Figure 11, while the robot stumbles during climbing the stairs in the case with vertical CM dynamically unconstrained as shown in a and c of Figure 11.

Figure 7. EZMP tracking in 3D view dynamically constraining vertical CM motion

0.8 0.6 0.4 0.2 0

-0.2 0.2

-0.2 1.1

£ 0.9

Figure 6. EZMP tracking in 3D view not constraining vertical CM motion

1 2 30 1 2 3 time [s]

Figure 8. Motion planning ignoring (a, c and e) and considering vertical motion effect (b, d and f)

0.8 0.6

-0.2 0.2

-0.2 1.1

j= 0.9

0 1 2 30 1 2 3 time[s]

Figure 9. Motion tracking ignoring (a, c and e) and considering vertical motion effect (b, d and f)

6.3 Walking on Rough Terrains

As shown in Figure 11, the stones on which the robot walks have a top surface of 14° inclination at most. The height of stones varies greatly from 0.12 m to 0.32 m. The stone slopes have a rotation angle around the vertical direction from -15° to 15°. To allow a large contact area, the foot posture is planned to be parallel with the stone top surface pointing at the long side direction of stone. The walking cycle is 2.4 s including 2/3 single support phases and 1/3 double support phases. The sampling time is 20 ms and the preview period is 2 s. The desired and measured EZMPs are shown in Figure 12. As can be seen, the EZMP in all three dimensions is tracked accurately, except around 10.8 s and 15.1 s.

Figure 10. Snapshots taken for walking upstairs with vertical motion effect ignored (a and c) and considered (b and d)

Figure 11. Rough terrain

The outlier points in Figure 12 can be explained by the large condition number of ^ around 10.8 s and 15.1 s, shown in Figure 13. Although the outlier points fall outside of support polygons, the instability cannot be indicated using the erroneous EZMP value. The stabilizer is switched off at those points to rule out the inaccurate measurement. The robot can successfully walk through the rough terrain as shown in Figure 14.

(The video can be accessed at: https://www.youtube.com/ watch?v=6QypO7TP3gs).

Remark 3: It should be noted that the robot will fall down immediately if effect of vertical dynamics are not considered. The extended cart-table model we propose deals with the vertical motion in a simple and elegant style. While ZMP cannot be defined in this case, the EZMP can be defined on a 3D surface and more simply chosen to be on the VCP plane in our case. The proposed concept of VCP allows the calculation of measured EZMP using normal vector of VCP and ankle torques as well as foot pressures. Besides, the VSP polygon is determined as the convex hull encompassing footprint projections onto the VCP. The measurability of EZMP and use of VSP polygon enables the judgment of stability.

0.15 0.1 0.05

-0.05 -0.1

1.3' 1.2 I 1.1

time [s]

Figure 12. EZMP tracking for walking on rough terrain

Rank and Condition Number of D^oc

- -rank

Rank and Condition Number of Dw

— -rank

Rank and Condition Number of *

— -rank

- cond. num.

- cond. num.

xlO2 4

- cond. num.

3 | 1 S

time [s]

Figure 13. Rank and condition number of matrices for walking on rough terrain

Figure 14. Snapshots taken for walking on rough terrain

7. Conclusion

In this paper, we established a general framework of planning walks on complex terrains with vertical dynamics incorporated. Two problems were solved: ZMP is undefined on uneven ground and cart-table model does not consider vertical dynamics.

An EZMP concept was proposed to extend ZMP to universal uneven terrains. The EZMP is defined on a general surface (VCS). For simplicity in this paper, VCS was chosen as an inclined plane (VCP) passing contacting centres of the single supporting foot and swing foot at liftoff and touch-down. By adding constraint (9), the EZMP dynamics equation was simplified dramatically. The EZMP can be used to judge stability on rough terrain by checking if the EZMP is within the convex hull encompassing footprint projections of supporting feet onto the VCP plane. The extended cart-table model was proposed with vertical dynamics rather than constant height as a constraint in the cart-table model. In this way, the new model preserves the linear form and allows vertical motion. A pattern generator was established by applying triple preview controllers to the extended cart-table model based on EZMP. We also presented the method of measuring EZMP calculated from pressure and friction measurements. Friction is indirectly measured from pressures and ankle joint torques. Simulation examples of walking on stairs and rough terrain were presented. The walking planning method on complex terrains was proved effective by impressive walking with good tracking of EZMP.

Our future work will include how to generalize the VCP plane to a general surface (VCS), which can be changed to manoeuvre the CM motion to keep the robot within the configuration space (determined by link lengths and joint rotation limits). On the other hand, more versatile motions can be achieved.

8. Acknowledgements

This work was supported by the National Natural Science Foundation of China (grant number 61071057) and the

University Innovation Team of Liaoning Province (LT2014006). We sincerely thank Christopher G. Atkeson and Siyuan Feng for the detailed explanations of the ideas in their paper. We sincerely thank Shuuji Kajita for answering the questions to his paper.

9. References

[1] Nishiwaki K., and Kagami S. Trajectory design and control of edge-landing walking of a humanoid for higher adaptability to rough terrain. IEEE/RSJ International Conference on Intelligent Robots and Systems. 2012, 3432--3439.

[2] Vukobratovic M., and Stepanenko J. On the stability of anthropomorphic systems. Mathematical Biosciences. 1972, 15(1): 1--37.

[3] Goswami A. Foot rotation indicator (FRI) point: A new gait planning tool to evaluate postural stability of biped robots. Proceedings IEEE International Conference on Robotics and Automation. 1999, 1: 47—52.

[4] Takao S., Yokokohji Y., and Yoshikawa T. FSW (feasible solution of wrench) for multi-legged robots. IEEE International Conference on Robotics and Automation. 2003, 3815--3820.

[5] Hirukawa H., Hattori S., Kajita S., Harada K., Kaneko K., Kanehiro F., Morisawa M., and Nakaoka S. A pattern generator of humanoid robots walking on a rough terrain. 2007 IEEE International Conference on Robotics and Automation. 2007, 2181--2187.

[6] Harada K., Kajita S., Kaneko K., and Hirukawa H. Dynamics and balance of a humanoid robot during manipulation tasks. IEEE Transactions on Robotics. 2006, 22(3): 568--575.

[7] Sardain P., and Bessonnet G. Forces acting on a biped robot. Center of pressure - zero moment point. IEEE Transactions on Systems, Man, and Cybernetics - Part A: Systems and Humans. 2004, 34(5): 630-637.

[8] Hasegawa T., and Takenaka T. Gait generation system for a legged mobile robot. Google Patents. 1994.

[9] Sato T., Sakaino S., Ohashi E., and Ohnishi K. Walking trajectory planning on stairs using virtual slope for biped robots. IEEE Transactions on Industrial Electronics. 2011, 58(4): 1385--1396.

[10] Kajita S., Kanehiro F., Kaneko K., Fujiwara K., Harada K., Yokoi K., and Hirukawa H. Biped walking pattern generation by using preview control of zero-moment point. Proceedings of IEEE International Conference on Robotics and Automation. 2003, 2: 1620--1626.

[11] Perrin N., Stasse O., Lamiraux F., and Yoshida E. A biped walking pattern generator based on "halfsteps" for dimensionality reduction. IEEE Interna-

tional Conference on Robotics and Automation.

2011, 1270--1275.

[12] Erbatur K., and Kurt O. Natural ZMP trajectories for biped robot reference generation. IEEE Transactions on Industrial Electronics. 2009, 56(3): 835--845, Mar.

[13] Seokmin H., Yonghwan O., Doik K., and Bum-Jae Y. Real-time walking pattern generation method for humanoid robots by combining feedback and feedforward controller. IEEE Transactions on Industrial Electronics. 2014, 61(1): 355-364, 2014-01-01.

[14] Santacruz C., and Nakamura Y. Analytical real-time pattern generation for trajectory modification and footstep replanning of humanoid robots. IEEE/RSJ International Conference on Intelligent Robots and Systems. 2012, 2095--2100.

[15] Sugihara T., and Nakamura Y. A fast online gait planning with boundary condition relaxation for humanoid robots. Proceedings of IEEE International Conference on Robotics and Automation. 2005, 305--310.

[16] Harada K., Kajita S., Kaneko K., and Hirukawa H. An analytical method for real-time gait planning for humanoid robots. International Journal of Humanoid Robotics. 2006, 3(01): 1-19.

[17] Huang W., Chew C.-M., Zheng Y., and Hong G.-S. Pattern generation for bipedal walking on slopes and stairs. 8th IEEE-RAS International Conference on Humanoid Robots. 2008, 205--210.

[18] Shih C.-L., Lee W.-Y., and Wu C.-P. Planning and control of stable walking for a 3D bipedal robot. International Journal of Advanced Robotic Systems.

2012, 9(47).

[19] Hettiarachchi D. S., and Iba H. An evolutionary computational approach to humanoid motion planning. International Journal of Advanced Robotic Systems. 2012, 9(167).

[20] Tanaka T., Takubo T., Inoue K., and Arai T. Emergent stop for humanoid robots. IEEE/RSJ International Conference on Intelligent Robots and Systems. 2006, 3970--3975.

[21] Feng S., Xinjilefu X., Huang W., and Atkeson C. G. 3D walking based on online optimization. IEEE-RAS International Conference on Humanoid Robots. Atlanta, GA, 2013, 1-7.

[22] Morisawa M., Kajita S., Kaneko K., Harada K., Kanehiro F., Fujiwara K., and Hirukawa H. Pattern generation of biped walking constrained on parametric surface. Proceedings of the 2005 IEEE International Conference on Robotics and Automation. 2005, 2405--2410.

[23] Hirukawa H., Hattori S., Harada K., Kajita S., Kaneko K., Kanehiro F., Fujiwara K., and Morisawa M. A universal stability criterion of the foot contact of legged robots-adios ZMP. Proceedings 2006 IEEE

International Conference on Robotics and Automation. 2006, 1976--1983.

[24] Ugurlu B., Hirabayashi T., and Kawamura A. A unified control frame for stable bipedal walking. Industrial Electronics, 2009. IECON'09. 35th Annual Conference of IEEE. 2009, 4167-4172.

[25] Katayama T., Ohki T., Inoue T., and Kato T. Design of an optimal controller for a discrete-time system subject to previewable demand. International Journal of Control. 1985, 41(3): 677--699.

[26] Park J., and Youm Y. General ZMP preview control for bipedal walking. IEEE International Conference on Robotics and Automation. 2007, 2682--2687.

[27] Kajita S., Miura K., Morisawa M., Kaneko K., Kanehiro F., and Yokoi K. Evaluation of a stabilizer for biped walk with toe support phase. 12th IEEE-RAS International Conference on Humanoid Robots. 2012, 586--592.

[28] Qing X. B. An interpolatory method that fits the need of adjustable and monotony property based on S type functions (Chinese). Journal of Applied Physiology. 2001, (2): 1-3.