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

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 5th-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.


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.

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 i s as a state of the motion and the set of all states forms a state-space which is denoted as: (1) State i s 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 i s : 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. Let us describe the establishment of a state-space in more details. Fig. 1 shows the state transition process of rising motion, where " " represents contact points between the robot and the floor, "×" represents the relative position of the COG, and the curve of COG is the polynomial interpolation of the relative positions of COG. According to the contact states, we allocate 5 states to the state-space of a rising motion. Sometimes, a single state is far from enough to represent a motion phase. In this case, we need refine the state by dividing the corresponding contact state into several sub-states. In Fig.1

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: 1. generate a series of trajectories between the neighboring states; 2. select the trajectories with a large stability margin.

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:  (3) and (4) and not valid if ZMP is out side of i Ω .

State-Transition Method
where: . According to the constraints (7)-(9), we can easily obtain a set of joint trajectories by 5 thorder polynomial interpolation. But the question becomes: although stable at the boundary points (at states i s and 1 + i s ), 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.
In (10) is the variable term, and where: Considering the boundary constraints (8)-(9), the set of  (2), note that the joint variable j q and joint torque j τ must satisfy the following constraints: 1. Joints limit constraint: − j q , + j q are angular position limits of joint j.

Joints velocity limit constraint:
− j q ( + j q ) is angular velocity minimum(maximum)of joint j.

Joints acceleration limit constraint:
is angular acceleration minimum (maximum) of joint j. 4. Actuator constraint: is torque minimum (maximum) of joint actuator j. j q 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 = we can keep joint i out the searching space conveniently.

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: where: davg is the average stability margin; n is the number of the sampling points for the trajectories; di 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 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.

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.
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 S1-S5 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.  The whole state transition process consists of S1 S2, S2 S3…S5-2 S5-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 S4 S5-1. Here, only the whist and ankle joints are kept in the searching space, that is, 0 is specified for the searching.
The simulation results are shown in Fig.8. Undergoing the state transitions S1 S2, S2 S3…S5-2 S5-3, 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.

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.