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

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=(n_x,n_y,n_z)^T and τ_p=(τ_px,τ_py,τ_pz)^T be normal vector of VCS at EZMP p=(p_x, p_y, p_z)^T and the ground reaction torque around p. From Definition 2, it is easy to derive that n and τ_p should be in line. Let τ_np=(τ_npx,τ_npy,τ_npz)^T be the cross-product of n and τ_p, the following equation can be obtained:

τ_{n p} = n \times τ_{p} = 0

(1)

This is unfolded as:

(\begin{array}{l} τ_{n p x} \\ τ_{n p y} \\ τ_{n p z} \end{array}) = (\begin{array}{l} - n_{z} τ_{p y} + n_{y} τ_{p z} \\ n_{z} τ_{p x} - n_{x} τ_{p z} \\ - n_{y} τ_{p x} + n_{x} τ_{p y} \end{array}) = (\begin{array}{l} 0 \\ 0 \\ 0 \end{array})

(2)

Considering τ_p can also be expressed in multi-body dynamics:

τ_{p} = \dot{L} - p_{c} \times M g + (\dot{P} - M g) \times p

(3)

(\begin{array}{l} τ_{p x} \\ τ_{p y} \\ τ_{p z} \end{array}) = (\begin{array}{l} {\dot{L}}_{x} + M g y + {\dot{P}}_{y} p_{z} - ({\dot{P}}_{z} + M g) p_{y} \\ {\dot{L}}_{y} - M g x - {\dot{P}}_{x} p_{z} + ({\dot{P}}_{z} + M g) p_{x} \\ {\dot{L}}_{z} + {\dot{P}}_{x} p_{y} - {\dot{P}}_{y} p_{x} \end{array})

(4)

where L = (L_x, L_y, L_z)^T is the angular momentum around the coordinate origin O , p_c = (x, y, z)^T is centre of mass, P = (P_x, P_y, P_z)^T is translational momentum, M is total mass and g = (0, 0, −g)^T is gravitational acceleration vector.

Let L_c = (L_cx, L_cy, L_cz)^T be the angular momentum around p_c. The derivatives of angular momentums around O and p_c can be related as follows:

\dot{L} = {\dot{L}}_{c} + (\begin{array}{l} y {\dot{P}}_{z} - z {\dot{P}}_{y} \\ z {\dot{P}}_{x} - x {\dot{P}}_{z} \\ x {\dot{P}}_{y} - y {\dot{P}}_{x} \end{array})

(5)

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

(\begin{array}{l} - τ_{n p x} \\ τ_{n p y} \\ τ_{n p z} \end{array}) = (\begin{array}{l} C_{x} + γ {\dot{P}}_{x} (z - p_{z}) - γ ({\dot{P}}_{z} + M g) (x - p_{x}) \\ C_{y} - γ {\dot{P}}_{y} (z - p_{z}) + γ ({\dot{P}}_{z} + M g) (y - p_{y}) \\ C_{z} \end{array}) = (\begin{array}{l} 0 \\ 0 \\ 0 \end{array})

(6)

where

\begin{matrix} C_{x} = γ {\dot{L}}_{c y} + α {\dot{L}}_{c z} + α {\dot{P}}_{y} (x - p_{x}) - α {\dot{P}}_{x} (y - p_{y}) (a) \\ C_{y} = γ {\dot{L}}_{c x} + β {\dot{L}}_{c z} + β {\dot{P}}_{y} (x - p_{x}) - β {\dot{P}}_{x} (y - p_{y}) (b) \\ C_{z} = α ({\dot{L}}_{c x} + ({\dot{P}}_{z} + M g) (y - p_{y}) - {\dot{P}}_{y} (z - p_{z})) (c) \\ - β ({\dot{L}}_{c y} - ({\dot{P}}_{z} + M g) (x - p_{x}) + {\dot{P}}_{x} (z - p_{z})) \end{matrix}

(7)

and

α = - n_{y}, β = - n_{x}, γ = n_{z}

(8)

To simplify the dynamics, we add the constraints:

\begin{array}{l} C_{x} = 0 (a) \\ C_{y} = 0 (b) \\ C_{z} = 0 (c) \end{array}

(9)

The following can be derived:

\begin{array}{l} γ ({\dot{P}}_{x} (z - p_{z}) - ({\dot{P}}_{z} + M g) (x - p_{x})) = 0 (a) \\ γ ({\dot{P}}_{y} (z - p_{z}) - ({\dot{P}}_{z} + M g) (y - p_{y})) = 0 (b) \end{array}

(10)

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

α γ {\dot{L}}_{c x} = β γ {\dot{L}}_{c y}

(11)

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

From equation (10), the following is derived:

\begin{array}{l} p_{x} = x - \frac{z - p_{z}}{\ddot{z} + g} \ddot{x} (a) \\ p_{y} = y - \frac{z - p_{z}}{\ddot{z} + g} \ddot{y} (b) \end{array}

(12)

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:

{\dot{P}}_{y} (x - p_{x}) - {\dot{P}}_{x} (y - p_{y}) = 0

(13)

and

\begin{array}{l} {\dot{P}}_{x} (z - p_{z}) - ({\dot{P}}_{z} + M g) (x - p_{x}) = 0 (a) \\ {\dot{P}}_{y} (z - p_{z}) - ({\dot{P}}_{z} + M g) (y - p_{y}) = 0 (b) \end{array}

(14)

If we assume a negligible centroidal angular momentum and its derivative, i.e., ${\dot{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 γ = 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 γ≠0 or γ = 0).

We conclude the above as theorems:

Theorem 1:If γ≠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 α=β=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 (p₁−p₂−p₁') passes the three centres of the contacting areas for stance foot (p₂) and swing foot at lifting (p₁) and landing (p₁'). 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.

Figure 1.

VCP definition

To compare ZMP and EZMP, consider a special case when p₁ and p₂ are on the horizontal plane while p₁' is off the horizontal plane. ZMP and EZMP can both be defined at p₁ and p₂. 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 p₂ 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.

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

\frac{z - p_{z}}{\ddot{z} + g} = c

(15)

p_{z} + c g = z - c \ddot{z}

(16)

where c is a constant.

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

\begin{array}{l} p_{x} = x - c \ddot{x} (a) \\ p_{y} = y - c \ddot{y} (b) \\ p_{z}^{a u x} = z - c \ddot{z} (c) \end{array}

(17)

where $p_{z}^{a u x} = p_{z} + c g$ . p ^aux=(p _x, p _y, p _z ^aux)^T is named auxiliary EZMP.

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 p^aux = (p_x, p_y, p_z^aux)^T is given, the preview control can be applied to the three equations of (17) simultaneously. As long as the $p_{z}^{a u x}$ 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 s² to 0.12 s² (step = 0.01 s²). 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 p^aux trajectory consists of a horizontal line and sloped line. With c increasing p^aux and CM gets farther away off the p^aux curve at the turning point. Away from the turning point, CM approaches the p^aux 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.

Figure 2.

The auxiliary EZMP and CM trajectories for different c values

Remark 1: Mathematically, p _c – p ^aux = c ${\ddot{p}}_{c}$ can be derived from Equation (17). p^aux = (p_x, p_y, p_z^aux)^T can be interpreted by taking p^aux as a point from which the vector points at CM p_c is parallel to CM acceleration. Considering p_z^aux = p_z+cg, p_z^aux is governed by p_z and constraint parameter c. p_z is constrained by VCP and can be interpreted to be the vertical component of EZMP which affects p_z^aux. In turn, p_z^aux determines where the CM evolves according p _c – p ^aux = c ${\ddot{p}}_{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 p_z. Only the z-direction motion evolves as given in (17); c is a constant and the EZMP equation remains linear. In particular, setting $p_{z}^{a u x} \equiv c g$ or p_z≡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:

\begin{array}{l} \frac{d}{d t} [\begin{matrix} x \\ \dot{x} \\ \ddot{x} \end{matrix}] = [\begin{matrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{matrix}] [\begin{matrix} x \\ \dot{x} \\ \ddot{x} \end{matrix}] + [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] u_{c} \\ p_{x} = [\begin{matrix} 1 & 0 & - c \end{matrix}] [\begin{matrix} x \\ \dot{x} \\ \ddot{x} \end{matrix}] \end{array}

(18)

where

u_{c} = \overset{…}{x}

is a derivative of the forward acceleration of CM as system input.

The equation (18) is discretized as:

\begin{array}{l} q (k + 1) = A q (k) + B u (k) \\ r (k) = C q (k) \end{array}

(19)

where

\begin{array}{l} q (k) = {[\begin{matrix} x (k T) & \dot{x} (k T) & \ddot{x} (k T) \end{matrix}]}^{T} \\ u (k) = u_{c} (k T) \\ r (k) = p_{x} (k T) \end{array}

A = [\begin{matrix} 1 & T & \frac{T^{2}}{2} \\ 0 & 1 & T \\ 0 & 0 & 1 \end{matrix}], B = [\begin{matrix} \frac{T^{3}}{6} \\ \frac{T^{2}}{2} \\ T \end{matrix}], C = [\begin{matrix} 1 & 0 & - c \end{matrix}]

T is the sample time.

For preview control, the performance index is given as:

J = \sum_{i = k}^{\infty} Q_{e} {(r^{r e f} (i) - r (i))}^{2} + Δ q^{T} (i) Q_{x} Δ q (i) + R Δ u (i)

(20)

where Q_e, Q_x, and R are nonnegative definite matrices. Δq(k) = q(k)−q(k −1) is the incremental state and Δu(k) = u(k)-u(k − 1) is the incremental input.

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

\begin{matrix} u (k) = - k_{e} \sum_{i = 0}^{k} (r (i) - r^{r e f} (i)) - k_{x} q (k) \\ - \sum_{i = 1}^{n_{p}} k_{f} (i) r^{r e f} (k + i) \end{matrix}

(21)

where n_p is the previewed length in dummy time points. k_e, k_x, and k_f are gains calculated from Q_e, Q_x 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, $p_{z}^{a u x} = p_{z} + c g$ rather than p_z 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.

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 $p_{z}^{aux}$ is used rather than p_z 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 T_p. The system state $x = {(x, \dot{x}, \ddot{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_{i}^{l o c} = {(f_{i x}^{l o c}, f_{i y}^{l o c}, 0)}^{T}$ and $N_{i}^{l o c} = {(0, 0, N_{i}^{l o c})}^{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_{i} = {(F_{i x}, F_{i y}, F_{i z})}^{T} = R_{i} (f_{i}^{l o c} + N_{i}^{l o c})$ be the ground reaction force acting at individual CoP p_i of each foot in global frame, where R_i is foot rotation matrix. Hereafter, if not assigned the superscript loc, the quantity is defined in global frame by default. Let F=(F_x, F_y, F_z)^T=F₁+F₂ and τ_p=(τ_px, τ_py, τ_pz)^T be the resultants of ground reaction force and torque at EZMP p=(p_x, p_y, p_z)^T. Immediately we obtain the following equation:

\begin{matrix} τ_{p} = (p - p_{1}) \times F_{1} + (p - p_{2}) \times F_{2} \\ = p \times F - (p_{1} \times F_{1} + p_{2} \times F_{2}) \end{matrix}

(22)

\overset{⌢}{F} p = {\overset{⌢}{F}}_{1} p_{1} + {\overset{⌢}{F}}_{2} p_{2} - τ_{p}

(23)

where $\overset{⌢}{F}$ is a matrix equivalent to F× operation. ${\overset{⌢}{F}}_{i}$ (i =1, 2) are defined similarly.

\overset{⌢}{F} = (\begin{matrix} 0 & - F_{z} & F_{y} \\ F_{z} & 0 & - F_{x} \\ - F_{y} & F_{x} & 0 \end{matrix}), {\overset{⌢}{F}}_{i} = (\begin{matrix} 0 & - F_{i z} & F_{i y} \\ F_{i z} & 0 & - F_{i x} \\ - F_{i y} & F_{i x} & 0 \end{matrix})

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

\begin{array}{l} T_{p r o j} \overset{⌢}{F} p = T_{p r o j} {\overset{⌢}{F}}_{1} p_{1} + T_{p r o j} {\overset{⌢}{F}}_{2} p_{2} - T_{p r o j} τ_{p} \\ \Rightarrow {\overset{⌢}{F}}^{'} p = {\overset{⌢}{F}}_{1}^{'} p_{1} + {\overset{⌢}{F}}_{2}^{'} p_{2} - τ_{p}^{'} \end{array}

(24)

where T_proj = E – nn^T (E is 3×3 identity matrix). ${\overset{⌢}{F}}^{'}$ , ${\overset{⌢}{F}}_{1}^{'}$ , ${\overset{⌢}{F}}_{2}^{'}$ and $τ_{p}^{'}$ are the resulting matrices and vector by operator T_proj.

Let the first two elements of $τ_{p}^{'}$ in (24) equal zero and the following is obtained:

Γ p = Γ_{1} p_{1} + Γ_{2} p_{2}

(25)

where Γ and Γi (i=1, 2) are sub-matrices consisting of the first two rows of ${\overset{⌢}{F}}^{'}$ and ${\overset{⌢}{F}}_{i}^{'}$ respectively.

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

n \cdot (p - r_{1}) = 0

(26)

n^{T} p = n^{T} r_{1}

(27)

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

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

Ψ p = Ζ

(28)

where $Ψ = (\begin{array}{l} Γ \\ n^{T} \end{array}), Ζ = (\begin{array}{l} Γ_{1} p_{1} + Γ_{2} p_{2} \\ n^{T} r_{1} \end{array})$

It is easy to prove rank(Ψ) = 3 if and only if F · n = |F| |n| cos(θ)≠0 (Fnot in VCP, violated rarely).

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

\tilde{p} = Ψ^{- 1} Ζ

(29)

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

Remark 2: Friction f _i ^loc are easy to be calculated from measured ankle torques $τ_{a n k, i}^{l o c}$ and normal pressure $N_{i}^{l o c}$ expressed in local frame of each ankle, which are available in most biped robots. Let $p_{i}^{l o c}$ (i=1, 2) be the CoP position of foot i in local frame. Neglecting the foot mass, the following equation exists:

\begin{array}{l} τ_{a n k, i}^{l o c} = - p_{i}^{l o c} \times N_{i}^{l o c} - p_{i}^{l o c} \times f_{i}^{l o c} \\ \Rightarrow p_{i}^{l o c} \times f_{i}^{l o c} = - p_{i}^{l o c} \times N_{i}^{l o c} - τ_{a n k, i}^{l o c} \\ \Rightarrow {\overset{⌢}{p}}_{i}^{l o c} f_{i}^{l o c} = b_{i}^{l o c} (w i t h b_{i}^{l o c} = - p_{i}^{l o c} \times N_{i}^{l o c} - τ_{a n k, i}^{l o c}) \end{array}

(30)

where ${\overset{⌢}{p}}_{i}^{l o c}$ is a matrix equivalent to $p_{i}^{l o c} \times$ operation (similar to $\overset{⌢}{F}$ definition). Let $D_{i}^{l o c}$ be the sub-matrix consisting of the first two rows and first two columns of ${\overset{⌢}{p}}_{i}^{l o c}$ and $d_{i}^{l o c}$ be the vector consisting of the first two elements of $b_{i}^{l o c}$ . Then the friction $f_{i}^{l o c}$ can be solved as follows:

f_{i}^{l o c} = (\begin{matrix} {(D_{i}^{l o c})}^{- 1} d_{i}^{l o c} \\ 0 \end{matrix})

(31)

It is easy to prove, with the z-component of $p_{i}^{l o c}$ not zero (distance between ankle and foot bottom not zero, always satisfied), $D_{i}^{l o 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/).

Figure 3.

Atlas robot

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.

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 k=0; for Step i=:+1 Planning foot and EZMP trajectories; for Time k=:+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 end end

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 s². 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):

\ddot{x} = {\ddot{x}}^{d} - k_{1} (p_{x} - p_{x}^{d}) - k_{2} (x - x^{d}) - k_{3} (\dot{x} - {\dot{x}}^{d})

(32)

where ${\ddot{x}}^{d}$ is the desired acceleration of CM generated from preview control. k₁, k₂ and k₃ are controller gains which are tuned by the experiment. The y and z dimensions use the same form of equations, except that p^aux is used instead of p for z dimension. In this paper the gains are determined as k₁ = 1.95, k₂ = −0.975 and k₃ = −1.95 for x and y dimensions and k₁ = 1.95, k₂ = −0.000975 and k₃ = −0.00975 for the z dimension. This controller uses CM acceleration instead of ZMP modification to stabilize the robot.

6.2 Walking on Stairs

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.32×0.2 m² 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 Q_e = 100000, Q_x = diag(10,10,10) and R = 1×10⁻⁶ and gains k_e, k_x, and k_f are gains calculated from Q_e, Q_x 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.

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:

\begin{array}{l} z = \\ {\begin{matrix} z_{0} + \frac{z_{n} - z_{0}}{1 + exp {- 2 Δ t (\frac{t_{0} + t_{n}}{2} - t) \frac{1}{t_{0} - t} \frac{1}{t_{n} - t}}} (t_{0} < t < t_{n}) \\ z_{0} (t = t_{0}) \\ z_{n} (t = t_{n}) \end{matrix} \end{array}

(33)

where t is time and z is vertical CM position. Δt = t_n −t₀. 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 [t₀, t_n].

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 upstairs. The measured EZMP in blue lines are constrained in the hexagons and the tracking error of EZMP $e z m p^{d} = {(p_{x}, p_{y}, p_{z}^{d})}^{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 $p^{aux,d} = {(p_{x}, p_{y}, p_{z}^{aux,d})}^{T} = {(p_{x}, p_{y}, p_{z}^{d})}^{T} + {(0, 0, c g)}^{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 $p_{z}^{aux,d}$ is tracked accurately enough, which guarantees the horizontal dynamics to be linear. Therefore the x and y dimensions also show fairly good p^aux,d tracking. This results in a good tracking of EZMP ezmp^d (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 6.

EZMP tracking in 3D view not constraining vertical CM motion

Figure 7.

EZMP tracking in 3D view dynamically constraining vertical CM motion

Figure 8.

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

Figure 9.

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

Figure 10.

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

Figure 11.

Rough terrain

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 12.

EZMP tracking for walking on 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.

Figure 13.

Rank and condition number of matrices for walking on rough terrain

Figure 14.

Snapshots taken for walking on rough terrain

(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.

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.

Footnotes

8.

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.

References

Nishiwaki

, and Kagami

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.

Vukobratović

, and Stepanenko

On the stability of anthropomorphic systems. Mathematical Biosciences. 1972, 15(1): 1–37.

Goswami

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.

Takao

Yokokohji

, and Yoshikawa

FSW (feasible solution of wrench) for multi-legged robots. IEEE International Conference on Robotics and Automation. 2003, 3815–3820.

Hirukawa

Hattori

Kajita

Harada

Kaneko

Kanehiro

Morisawa

, and Nakaoka

A pattern generator of humanoid robots walking on a rough terrain. 2007 IEEE International Conference on Robotics and Automation. 2007, 2181–2187.

Harada

Kajita

Kaneko

, and Hirukawa

Dynamics and balance of a humanoid robot during manipulation tasks. IEEE Transactions on Robotics. 2006, 22(3): 568–575.

Sardain

, and Bessonnet

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.

Hasegawa

, and Takenaka

Gait generation system for a legged mobile robot. Google Patents. 1994.

Sato

Sakaino

Ohashi

, and Ohnishi

Walking trajectory planning on stairs using virtual slope for biped robots. IEEE Transactions on Industrial Electronics. 2011, 58(4): 1385–1396.

10.

Kajita

Kanehiro

Kaneko

Fujiwara

Harada

Yokoi

, and Hirukawa

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

Stasse

Lamiraux

, and Yoshida

A biped walking pattern generator based on “halfsteps” for dimensionality reduction. IEEE International Conference on Robotics and Automation. 2011, 1270–1275.

12.

Erbatur

, and Kurt

Natural ZMP trajectories for biped robot reference generation. IEEE Transactions on Industrial Electronics. 2009, 56(3): 835–845, Mar.

13.

Seokmin

Yonghwan

Doik

, and Bum-Jae

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

, and Nakamura

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

, and Nakamura

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

Kajita

Kaneko

, and Hirukawa

An analytical method for real-time gait planning for humanoid robots. International Journal of Humanoid Robotics. 2006, 3(01): 1–19.

17.

Huang

Chew

C.-M.

Zheng

, 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

An evolutionary computational approach to humanoid motion planning. International Journal of Advanced Robotic Systems. 2012, 9(167).

20.

Tanaka

Takubo

Inoue

, and Arai

Emergent stop for humanoid robots. IEEE/RSJ International Conference on Intelligent Robots and Systems. 2006, 3970–3975.

21.

Feng

Xinjilefu

Huang

, and Atkeson

C. G.

3D walking based on online optimization. IEEE-RAS International Conference on Humanoid Robots. Atlanta, GA, 2013, 1–7.

22.

Morisawa

Kajita

Kaneko

Harada

Kanehiro

Fujiwara

, and Hirukawa

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

Hattori

Harada

Kajita

Kaneko

Kanehiro

Fujiwara

, and Morisawa

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

Hirabayashi

, and Kawamura

A unified control frame for stable bipedal walking. Industrial Electronics, 2009. IECON'09. 35th Annual Conference of IEEE. 2009, 4167–4172.

25.

Katayama

Ohki

Inoue

, and Kato

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

, and Youm

General ZMP preview control for bipedal walking. IEEE International Conference on Robotics and Automation. 2007, 2682–2687.

27.

Kajita

Miura

Morisawa

Kaneko

Kanehiro

, and Yokoi

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.