Study on State Transition Method Applied to Motion Planning for a Humanoid Robot

Abstract

This paper presents an approach of motion planning for a humanoid robot using a state transition method. In this method, motion planning is simplified by introducing a state-space to describe the whole motion series. And each state in the state-space corresponds to a contact state specified during the motion. The continuous motion is represented by a sequence of discrete states. The concept of the transition between two neighboring states, that is the state transition, can be realized by using some traditional path planning methods. Considering the dynamical stability of the robot, a state transition method based on search strategy is proposed. Different sets of trajectories are generated by using a variable 5^th-order polynomial interpolation method. After quantifying the stabilities of these trajectories, the trajectories with the largest stability margin are selected as the final state transition trajectories. Rising motion process is exemplified to validate the method and the simulation results show the proposed method to be feasible and effective.

Keywords

state-space state transition motion planning humanoid robots

1. Introduction

There are many applications for humanoid robots, including using them in areas deemed to be too hazardous for humans. The area of the humanoid robotic system has been extensively studied over the last decade. As humanoid robots obtain human-like versatility in not only the mechanical but also the control sense, it seems to be a hard problem to design their behaviors. In this sense, human beings have never met such a problem in their motion planning. The humanoid robot is a complex system typically with great degrees of freedom, so to develop practical motion planning algorithms for humanoid robots is a daunting task. The problem is further complicated by the fact that humanoid robots must be controlled very carefully in order to maintain overall static and dynamic stability. As an important research topic, motion planning is crucial for a stable motion of humanoid robots. While the studies in the past years focused mostly on the trajectory generation of biped motion (Hirai 1998, Yamaguchi 1996, Park 1998, Hasegawa 2000, Huang 2001), the methods lack applicability for the design of other complex motions. But in the real-life environment, humanoid robots must deal with all kinds of difficult tasks, such as stepping over obstacles, climbing up stairs, standing up/lying down, or even jogging, jumping, rolling and so on. These pose a particular challenge on motion planning of humanoid robots.

In order to improve the human-like versatility in motion planning, some researchers have investigated a lot of novel methods and these researches have attracted more and more attentions. The HRP-II humanoid robot (Fumio 2003) is the first human-sized humanoid robot that can fall over safely and stand-up again. According to the contact conditions between the robot and the floor, researchers divide the whole motion into several contact states. The set can be represented by a graph, whose nodes are the discrete contact states and arcs the motion that makes the transitions between the neighboring states. In this way, the getting up motion can be realized by the transition between the contact states. Yamamoto, T. et al. (Yamamoto 2002) proposed a concept of global dynamics to generalize humanoid body motion as a transition of “envelopes”, where body dynamics is exploited and control input is adopted only at the “nodes”, where the body is unstable and control input is necessary. With the “node-to-node” manner, which means the control input is treated as discreet time, they expect to describe control in a state machine-like manner. Ogura, T. et al. (Ogura 2003)introduced a State-Nets system to describe the behavior of a humanoid robot. In this network, arcs represent whole-body motions of a robot, and nodes represent robot states, or multi-sensor body-images. To expand the network, they proposed a method to integrate nodes with a clustering method, and to create arcs by generating the robot's motions using a GA-based learning method.

Enlightened by these ideas, the concept of state is extended and applied to our motion planning algorithm. By this means, a continuous motion of a humanoid robot is represented by a state-space, in which the states are defined according to the different contact states between the robot and the ground, and the formulated states contain the necessary information for the motion planning. Considering the dynamical stability, a state transition method based on searching strategy is proposed by using variable 5^th-order polynomial interpolation. This paper is organized as follows. In Section 2, the state-space formulation and establishment method is presented. The state transition method based on a searching method is presented in Section 3. The simulation results of a rising motion are given in Section 4. Finally, the conclusion is given in Section 5.

2. A state-space Formulation and Establishment

In the control sense, the continuous motion of a humanoid robot can be represented by a set of nodes, and each node corresponds to a specific state during the motion. In this way, the motion planning will become feasible for its transforming from an infinite model to a finite one. If we define S_i as a state of the motion and the set of all states forms a state-space which is denoted as:

S = {s_{1}, s_{2} \dots s_{N}}

(1)

State S_i contains all kinds of information to express a state of the robot, and provides the necessary parameter for the control at the same time. Here, we adopt the following form to describe a state s_i:

s_{i} {θ_{i}; {\dot{θ}}_{i}; {\ddot{θ}}_{i}; C O G_{i}; Ω_{i} ∣ t_{i}}

(2)

where, θ_i = [q₁, q₂,…,q_n]^T is the variable vector of the joint angles; ${\dot{θ}}_{i}$ and ${\ddot{θ}}_{i}$ are the variable vectors of the joint velocities and the joint accelerations, respectively; COG_i = [cg_x, cg_y, cg_z]^T is the Cartesian coordinate of the COG (center of gravity); Ω_i is the support polygon; t_i is the initial time of the state.

With the experiment of humans, we can get some useful data about motion. But these data cannot be applied directly to the humanoid robot because of kinematics and dynamics inconsistence between the human subject (whose motion is recorded) and the humanoid. However, this experiment reveals how a human plans a motion and utilizes his body to accomplish all kinds of behaviors. Furthermore, the recorded data provide the evidence for the planner to extract states from a continuous motion and establish a state-space for it.

During the study, we find that the contact state between the robot and the floor is very important for the establishment of a state-space. In order to keep the balance of the robot, the COG in the case of static stability or the ZMP in the case of dynamic stability, must be controlled according to its support polygon. But, the support polygon changes drastically in accordance with the contact state while executing a motion. Therefore, it is preferable to allocate an appropriate state corresponding to each contact state. At the same time, the motion between two neighboring states will hold the uniform support polygon (constraint for the motion), which is preferable to reduce the computation effort and assign a proper controller to each state.

3. State Transition Method Based on Search Strategy

Different from other robotic manipulators, a humanoid robot has no fixed root and tends to tip over easily during motion. From the viewpoint of stability, the dynamic constraints should be introduced to guarantee the dynamic stability of a motion. Our research is focused on how to generate a trajectory between two neighboring states subjected to some dynamic stability constraints. We generalize this problem as the state-transition method. To solve this problem, we propose a method consisting of the following steps:

generate a series of trajectories between the neighboring states;

select the trajectories with a large stability margin.

3.1 ZMP Stability Criterion

ZMP criterion (Vukobratovic 1969, Vukobratovic 2004) is an important criterion, which has been broadly used to generate biped control algorithm. The main idea of this criterion is to maintain the ZMP (Zero Momentum Point) inside of the support polygon to achieve dynamical stability. The ZMP is a point on the ground where the sum of all momentums is zero. Using this principle, the ZMP can be computed as follows:

x_{Z M P} = \frac{\sum_{i} m_{i} ({\ddot{z}}_{i} + g) x_{i} - \sum_{i} m_{i} {\ddot{x}}_{i} z_{i} - \sum_{i} I_{i y} {\ddot{θ}}_{i y}}{\sum_{i} m_{i} ({\ddot{z}}_{i} + g)}

(3)

y_{Z M P} = \frac{\sum_{i} m_{i} ({\ddot{z}}_{i} + g) y_{i} - \sum_{i} m_{i} {\ddot{y}}_{i} z_{i} - \sum_{i} I_{i x} {\ddot{θ}}_{i x}}{\sum_{i} m_{i} ({\ddot{z}}_{i} + g)}

(4)

where (x_ZMP, y_ZMP,0) is ZMP coordinate, (x_i, y_i, z_i) is the mass center of the link i in the coordinate system, m_i is the mass of the link i, g is the gravitational acceleration. I_ix and I_iy are the inertia moment components, θ_ix and θ_iy are the angular velocity around axes x and y.

For a state $s_{i} {θ_{i}; {\dot{θ}}_{i}; {\ddot{θ}}_{i}; C O G_{i}; Ω_{i} ∣ t_{i}}$ , we can get the coordinate of the ZMP by (3) and (4) as

Z M P = {[x_{Z M P}, y_{Z M P}]}^{T}

(5)

Assuming Ω_i is the support polygon corresponding to the state s_i, the state $s_{i} {θ_{i}; {\dot{θ}}_{i}; {\ddot{θ}}_{i}; C O G_{i}; Ω_{i} ∣ t_{i}}$ is valid if

Z M P \in Ω_{i}

(6)

and not valid if ZMP is out side of Ω_i.

3.2 State-Transition Method

Suppose that we have a state-space S = {s₁, s₂ … s_N} to represent the whole motion process, and each state in this state-space satisfies the stability criterion. To generate a set of smooth joint trajectories lies between two neighboring states s_i and s_i+1, it is necessary that the first derivative (velocity) terms $\dot{θ}$ be differential, the second derivative (acceleration) terms $\ddot{θ}$ be continuous in time interval T_i = t_i+1 – t_i. For a state transition s_i → s_i+1, the boundary constraints are specified as:

θ (t_{0}) = θ_{i}, θ (t_{f}) = θ_{i + 1}

(7)

\dot{θ} (t_{0}) = {\dot{θ}}_{i}, \dot{θ} (t_{f}) = {\dot{θ}}_{i + 1}

(8)

\ddot{θ} (t_{0}) = {\ddot{θ}}_{i}, \ddot{θ} (t_{f}) = {\ddot{θ}}_{i + 1}

(9)

where: t₀ = t_i, t_f = t_i+1. According to the constraints (7)–(9), we can easily obtain a set of joint trajectories by 5^th-order polynomial interpolation. But the question becomes: although stable at the boundary points (at states s_i and s_i+1), the state transition may still be failed for the trajectories are not subject to any dynamical constraints. To plan a set of joint trajectories that accounts for system dynamics, we propose a method synthesizes the decoupling and the searching thoughts by: 1) decoupling the problem by first obtaining a set of kinematics trajectories, and subsequently sampling and checking the dynamic stability of the kinematics trajectories, and 2) searching the possible trajectories directly and determine the last trajectories with a large stability margin.

By adding variable terms to the polynomial without breaking the boundary constraints (7)–(9), we can produce various state transition trajectories as follows.

\begin{aligned} θ^{k} (t) & = \sum_{j = 0}^{5} a_{i j} (t - t_{i})^{j} + V (w_{k}, t) \\ k = 1, 2, \dots, t \in [t_{i}, t_{i + 1}] \end{aligned}

(10)

In (10), θ^k (t) is the k^th set of trajectories; i the index of i^th state transition; a_i0,a_i1,…,a_i5 are coefficients of the polynomial, which can be computed as:

{\begin{cases} a_{i 0} = θ_{i} \\ a_{i 1} = {\dot{θ}}_{i} \\ a_{i 2} = \frac{{\ddot{θ}}_{i}}{2} \\ a_{i 3} = \frac{20 θ_{i + 1} - 20 θ_{i} - (8 {\dot{θ}}_{i + 1} + 12 {\dot{θ}}_{i}) T_{i} - (3 {\ddot{θ}}_{i} - {\ddot{θ}}_{i + 1}) T_{i}^{2}}{2 T_{i}^{3}} \\ a_{i 4} = \frac{30 θ_{i} - 30 θ_{i + 1} - (14 {\dot{θ}}_{i + 1} + 16 {\dot{θ}}_{i}) T_{i} - (3 {\ddot{θ}}_{i} - 2 {\ddot{θ}}_{i + 1}) T_{i}^{2}}{2 T_{i}^{4}} \\ a_{i 5} = \frac{12 θ_{i + 1} - 12 θ_{i} - (6 {\dot{θ}}_{i + 1} + 6 {\dot{θ}}_{i}) T_{i} - (3 {\ddot{θ}}_{i} - {\ddot{θ}}_{i + 1}) T_{i}^{2}}{2 T_{i}^{5}} \end{cases}

(11)

V(w_k,t) is the variable term, and $w_{k} = [w_{1}^{k}, w_{2}^{k}, \dots, w_{n}^{k}]^{T}$ is the variable. Here, we define V(w_k, t) as:

V (w_{k}, t) = w_{k} (t - t_{0}) σ_{1} (t - t_{f})^{σ_{2}}

(12)

where: σ_1,2 are the power of t – t₀ and t – t_f respectively. Considering the boundary constraints (8)–(9), the set of σ_1,2 ≥ 3 is defined to satisfy $\dot{V} ∣ t_{0}, t_{f} = 0$ and $\ddot{V} ∣ t_{0}, t_{f} = 0$ . With V(w_k, t), different sets of trajectories can be produced by defining different values for w_k and σ_1,2. In order to reduce the computation effort, we treat σ_1,2 as constants, and the minimum values are specified as σ₁ = σ₂ = 3 in the searching. For θ = [q₁,q₂,…,q_n]^T defined in (2), note that the joint variable q_j and joint torque τ_j, must satisfy the following constraints:

3. Joints limit constraint:

q_{j}^{^{-}} \leq q_{j} (t) \leq q_{j}^{+}

(13)

$q_{j}^{^{-}}, q_{j}^{+}$ , are angular position limits of joint j.

4. Joints velocity limit constraint:

{\dot{q}}_{j}^{-} \leq {\dot{q}}_{j} (t) \leq {\dot{q}}_{j}^{+}

(14)

${\dot{q}}_{j}^{-} ({\dot{q}}_{j}^{+})$ is angular velocity minimum(maximum)of joint j.

5. Joints acceleration limit constraint:

{\ddot{q}}_{j}^{-} \leq {\ddot{q}}_{j} (t) \leq {\ddot{q}}_{j}^{+}

(15)

${\ddot{q}}_{j}^{-} ({\ddot{q}}_{j}^{+})$ is angular acceleration minimum (maximum) of joint j.

6. Actuator constraint:

τ_{j}^{^{-}} \leq τ_{j} (t) \leq τ_{j}^{+}

(16)

$τ_{j}^{^{-}} (τ_{j}^{+})$ is torque minimum (maximum) of joint actuator j.

q_j can be obtained by using above algorithm and τ_j can be computed using L-E or Newton-Euler formulations. Based on (10)–(16), and ZMP (3) and (4), a set of trajectories with the largest stability margin can be acquired by exhaustive searching computation. However, owning to the large searching space resulted from the large dimension of the joint space, it is quit time consuming to produce a set of joint trajectories with the largest stability margin. In fact, the stability of the robot can be compensated by the partial body motions, e.g. the trunk or swing leg motions (Huang 2001, Lim 2000, Lim 2002). Similarly, the searching efficiency can be greatly improved if we ignore some joints in the searching that change unobviously or contribute little to the dynamic stability in the state transition process. By specifying $w_{i}^{k} = 0$ in $w_{k} = [w_{1}^{k}, w_{2}^{k}, \dots, w_{n}^{k}]^{T}$ we can keep joint i out the searching space conveniently.

3.3 Sampling-based Dynamical Stability Quantification

To improve the efficiency of the searching, we propose a sampling-based method to quantify the dynamic stability of a set of joint trajectories. Firstly, we introduce the concept of the average stability margin as the quantitative measure of a stable extent according to the relationship between the ZMP position and the stable region. The minimal distance d from ZMP to the boundary of the support polygon is called the stability margin (Sugano 1993, Huang 2001). Especially, d is positive if ZMP is within the support polygon, and negative if ZMP is outside of the support polygon. For a set of trajectories, the average stability margin is defined as:

d_{a v g} = \frac{1}{n} \sum_{i = 1}^{n} d_{i}

(17)

where: d_avg is the average stability margin; n is the number of the sampling points for the trajectories; d_i is the stability margin of the set of trajectories corresponding to the i^th sampling point. According to the algorithm of the state transition method, the set of trajectories searching for the largest stability margin can be formulated as

max {d_{a v g} (w_{k}, σ_{1, 2} = 3)}

(18)

We generalize the method as the algorithm shown in Fig.3 and Fig.4, where Fig.3 shows the flow of the whole motion planning, and Fig.4 shows the detailed algorithm of the state transition method that is the dotted block in Fig.3.

Fig. 1.

State Transition process of rising motion

Fig. 2.

State Transition process of biped walking motion

Fig. 3.

Flow of the motion planning

Fig. 4.

Algorithm of the state transition method

4. Simulation Examples

The humanoid robot consists of a body, legs, and arms (Fig.5). Each leg has 5 degrees of freedom: 2 degrees of freedom for the hip joint, 1 degree of freedom for the knee joint, and 2 degrees of freedom for the ankle joint. Each arm has 3 degrees of freedom: 2 degrees of freedom for the shoulder joint, and 1 degree of freedom for the elbow joint. Fig.6 shows the external view of the prototype we have developed.

Fig. 5.

Configuration of the humanoid robot

Fig. 6.

External view of the prototype

We choose a rising motion that the robot gets up from the floor with the face upward as our simulation example. For our research, rising motion is more representative for its diversiform contact states. As shown in Fig.1, five typical states S₁-S₅ are defined corresponding to five different contact states from lying down to supporting the whole body with two feet, and the information of each state is shown in Table 1. In Table 1, q1 ˜ q4 are the pitch angles of the joints: ankle, knee, wrist, and shoulder, respectively. For simplicity, the support polygon Ω_i is denoted by the contact points of the body.

Table 1.

Definition of the states for rising motion

S _i		S ₁	S ₂	S ₃	S ₄	S _5-1	S _5-2	S _5-3
Var.		S ₁	S ₂	S ₃	S ₄	S _5-1	S _5-2	S _5-3
t _i		0.0	4.0	9.0	14.0	18.0	20.0	25.0
θ_i (deg)	q ₁	0.0	3.0	30.0	−85.0	−80.0	−55.0	0.0
	q ₂	0.0	0.0	76.0	150.0	145.0	106.0	0.0
	q ₃	0.0	67.8	90.0	30.0	65.0	52.0	0.0
	q ₄	0.0	42.0	60.0	60.0	50.0	33.0	0.0
${\dot{θ}}_{i}$ (deg/s)	${\dot{q}}_{1}$	0.0	1.0	−1.6	6.0	7.4	14.2	0.0
	${\dot{q}}_{2}$	0.0	0.0	36.0	0.0	−8.7	−30.0	0.0
	${\dot{q}}_{3}$	0.0	21.4	−2.6	13.0	0.0	−14.5	0.0
	${\dot{q}}_{4}$	0.0	13.5	0.0	0.0	−6.2	−9.2	0.0
${\ddot{θ}}_{i}$ (deg/s²)	${\ddot{q}}_{1}$	0.0	0.0	0.0	0.0	0.0	0.0	0.0
	${\ddot{q}}_{2}$	0.0	0.0	0.0	0.0	0.0	0.0	0.0
	${\ddot{q}}_{3}$	10.5	−3.0	−8.3	10.0	0.0	17.0	0.0
	${\ddot{q}}_{4}$	0.0	0.0	0.0	0.0	0.0	0.0	0.0
Ω_i		body	feet, hip, hands	feet, hip, hands	feet, knees	feet	feet	feet

The whole state transition process consists of S₁→S₂, S₂→S₃…S_5-2→S_5-3. Using the state transition method, we produce a series sets of trajectories and the set of trajectories with the largest stability margin was selected as the final. Fig.7 shows the trajectories generated in the state transition S₄→S_5-1. Here, only the whist and ankle joints are kept in the searching space, that is, $w_{2}^{k} = w_{4}^{k} = 0$ is specified for the searching.

The simulation results are shown in Fig.8. Undergoing the state transitions , the humanoid robot can get up from the floor from the lying down posture to the standing up posture. From the ZMP trajectory (shown in Fig.9 and Fig.10) we can see that the ZMP trajectory is always near the center of the stable region, that is, the robot has a large stability margin.

Fig. 8.

Simulation results of rising motion

Fig. 9.

ZMP trajectory in y-axis

Fig. 10.

ZMP trajectory in x-axis

5. Conclusion

In this paper, we propose a state transition method for the motion planning for a humanoid robot. Firstly, we simplify the description of a motion by introducing a state-space to represent the whole motion series. Then, considering the dynamical stability of the robot, a state transition method based on search strategy is proposed to generate the trajectories for the motion. The state transition method consists of two parts: one is to generate various state transition trajectories by using 5^th-order polynomial interpolation, and the other is to quantify the dynamical stability of the trajectories and select the trajectories with the largest stability margin as the final. A simulation example of rising motion is shown, and the results show the effectiveness of the proposed method.

To deal with the uncertainties and realize real-time motion generation, our future study will focus on mainly two aspects: 1) improve our state transition algorithm to be a faster algorithm; 2) combine the planning method we proposed with the real-time control.

References

Fumio

Kenji

& Kiyoshi

(2003). The first humanoid robot that has the same size as a human and that can lie down and get up, Proceedings of the IEEE International Conference on Robotics & Automation, Taipei, Taiwan, pp.1633–1639

Hasegawa

& Arakawa

(2000). Trajectory generation for biped locomotion robot, Mechatronics, Vol. 10, pp. 67–89

Hirai

Hirose

; & Takenada

(1998). The development of Honda humanoid robot, IEEE International Conference on Robotics and Automation, Leuven, Belgium, pp.1321–1326

Huang

Yokoi

& Kajita

(2001). Planning Walking Patterns for a Biped Robot, IEEE Transition on Robotics and Automation, Vol.17, No.3, pp. 280–289

Lim

H.-O.

& Takanishi

(2000). Waseda biped humanoid robots realizing human-like motion, Proceedings of International Workshop on Advanced Motion Control, Nagoya, Japan, pp: 525–530

Lim

H.-O.

Kaneshima

& Takanishi

(2002). Online walking pattern generation for biped humanoid robot with trunk, Proceedings of IEEE International Conference on Robotics and Automation, Washington, DC, USA, pp. 3111–3116

Ogura

Okada

& Inaba

(2003). Behavior network acquisition in multi-sensor space for whole-body humanoid, Proceedings of IEEE International Conference on Multi-sensor Fusion and Integration for Intelligent Systems, Taipei, Taiwan, pp. 317–322

Park

J. H.

& Rhee

Y. K.

(1998). ZMP Trajectory Generation for Reduced Trunk Motion of Biped Robot, Proceedings of IEEE/RST International. Conference on Intelligent Robots and Systems, Victoria, B.C., Canada, pp. 90–95

Sugano

Huang

& Kato

(1993). Stability Criteria in Controlling Mobile Robotic Systems, Proceedings of the 1993 IEEE/RSJ International Conference on Intelligent Robots and Systems, Yokohama, Japan, pp. 832–838

10.

Vukobratovic

& Juricic

(1969). Contribution to the synthesis of biped gait, IEEE Transition on Bio-Med, Vol.16, No.1, pp. 1–6

11.

Vukobratovic

(2004). Zero Moment Point-thirty five years of its life, International Journal of Humanoid Robotics, Vol.1, No.1, pp. 157–173

12.

Yamaguchi

Kinoshita

Takanishi

& Kato

(1996). Development of dynamic walking system for humans living floor, Proceedings of the IEEE International Conference on Robotics & Automation, Minneapolis, MN, USA, pp. 232–239

13.

Yamamoto

& Kuniyoshi

(2002). Stability and controllability in a rising motion: a global dynamics approach, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, EPFL, Switzerland, pp. 2467–2472