2005 IEEE/RSJ International Conference on Intelligent Robots and Systems
Emergency Stop Algorithm for Walking Humanoid Robots
Mitsuharu Morisawa, Shuuji Kajita, Kensuke Harada, Kiyoshi Fujiwara
Fumio Kanehiro, Kenji Kaneko, Hirohisa Hirukawa
National Institute of Advanced Industrial Science and Technology
Tsukuba Central 2,1-1-1 Umezono, Tsukuba, Ibaraki, 305-8568, Japan
Email: m.morisawa, s.kajita, kensuke.harada, k-fujiwara,
f-kanehiro, k.kaneko, hiro.hirukawa@aist.go.jp
Abstract— This paper presents an emergency stop algorithm
of a walking humanoid robot. There are many cases which
force a walking robot to stop quickly without falling. Since an
emergency occurs at unpredictable timing and at any state of
robot, the stopping motion must be generated in real-time.
To overcome these problems, our emergency stop motion is
divided into four phases according to the role of the ZeroMoment Point (ZMP). In each phase, approximate analytical
solutions of the Center of Gravity (COG) dynamics is used to
generate the motion. During the single support phase, a landing
time and position are determined by evaluating the average
velocity of the swing leg and the horizontal position of the COG.
During the double support phase, the travel distance of the COG
and the ZMP are evaluated. The validity of the proposed method
is confirmed by simulation and experiment using a humanoid
robot HRP-2.
Fig. 1.
Index Terms— Emergency stop, Biped walking, Real-time gait
planning, Walking pattern generation.
a real-time connectivity of the COG trajectory and that of the
ZMP one. However, these methods require more than two
steps to stop the robot. Sugihara proposed the fast online gait
planning method with boundary condition relaxation[9]. In
this method, because the walking cycle is constant, humanoid
robots will be possible to stop by one step only in some cases
especially slow walking.
In this paper, we propose a method to generate emergency stop motion in real-time which can lead the robot to
stop within one step. The features of proposed method are
admitting the ZMP overshoot and optimizing motion time
according to the initial states of the humanoid robot.
The rest of this paper is organized as follows. The stop
motion parameters are derived from the aspect of real-time
gait planning in Section II. In section III, the stop motion parameters are determined by fast iterative computation. Then,
the motion of the upper body joints are generated in Section
IV. Furthermore, crouching down motion is introduced for
avoidinf a singular configulation. Simulation results of the
emergency stop motion are shown in V. Actual walking in
experiment is tested using humanoid robot HRP-2[10] in
Section VI. Finally, we conclude in Section VII.
I. I NTRODUCTION
Recently so-called human-friendly machines have begun
to spread in society, for example, pet robot, human-like
robot, high performance home robot and so on. Industrial
countries will need such machines due to the fast aging
society. Because humanoid robots have the advantage of
working in environments which is designed for human being,
they are expected to assist our society in future.
From the practical point of view, it is necessary for humanoid robot to have the capability of walking where humans
can. In such environment, not only unknown disturbance may
hit humanoid robots, but also humanoid robots may hurt to
humans or objects (Fig.1). To avoid physical damage, Fujiwara achieved to fall over without fatal damage by humansized humanoid robot[1], [2]. However, the falling down can
not necessarily avoid damages. One approach to solve this
problem is that the robot stop immediately. Up to now, some
real-time control methods have been proposed. Shimoyama
proposed the method for improvement of stability of biped
robot by changing the landing position[3]. Kajita[4] proposed
the method of a real-time walking pattern generation using
analytical solution. Nishiwaki developed the online method
of a walking pattern generation of humanoid robot by dynamically stable mixture and connection of pre-designed motion
trajectories every one step[7]. Harada[6] also realized realtime gait planning using analytical solution and the ZMP[8]
trajectory which is expressed as spline function and discussed
0-7803-8912-3/05/$20.00 ©2005 IEEE.
Emergency stop.
II. R EAL - TIME GAIT PLANNING FOR EMERGENCY STOP
A. Strategy of emergency stop
In an original operation, stop motion generator just monitors internal states Fig.2 (a). If humanoid robots are put in
abnormal circumstances, it will be preferable for robots to
take a statically stable posture as soon as possible. Moreover,
31
Fig. 2.
Fig. 3.
stop.
the new motion must be connected with the original motion smoothly. The difference between the original landing
position of the next step and the newly planned one will
prevent humanoid robots from keeping balance. An analytical
solution is used [5], [6] to calculate the motion trajectory
within one sampling time, In theses generation methods, the
COG and the ZMP trajectories can be generated by satisfying
the boundary condition. In our approach, the COG and the
ZMP trajectories are generated by setting the initial condition
and the boundary conditions in every one phase, respectively
to change a landing position and a landing time.
Let us consider a humanoid robot walking on a flat floor.
While we focus on the motion of the humanoid robot within
the sagittal (x-z) plane, the motion in the lateral (y-z) plane
is accompanied by the stop motion parameter in the sagittal
plane. Let us assume that the ZMP trajectory of the humanoid
robot is given by a polynomial function of degree . Let
℄ be the trajectory of the ZMP of the
j-th phase corresponding to four phases i iv in the stop
motion. The trajectory of the ZMP in the sagittal plane can
be expressed as
Signal flow of proposed control system.
Strategy of emergency
Fig. 4. The COG and the ZMP
trajectories in emergency stop.
stop motion must be generated in real-time from any state
of robots. The proposed control system is shown in Fig.2.
If the joint angular velocity and the differentiation of the
ZMP can not be obtained, the stop motion generator always
estimates it. When the stop motion generator receives an
emergency stop signal, the COG and the ZMP trajectories
will be generated according to the internal states at the time.
Thus, during the playback of off-line walking pattern, the
stop motion generator can be also used to make the walking
humanoid robot to stop instantly.
(1)
i iv
where i iv are scalar coefficients.
℄ be the trajectory of the COG
Let
corresponding to the ZMP trajectory belonging the j-th phase
in the stop motion, and let
℄ be
the angular momentum of the robot about the COG. The
relationship between the ZMP and the COG within the sagittal
plane is expressed by the following equation,
According to the role of the ZMP for the generation of the
COG trajectory, the emergency stop motion is divided into
four phases shown in Fig.3 and Fig.4, that is i.e.
(i)
Landing Phase,
(ii)
ZMP Transition Phase,
(iii) Slowdown Phase,
(iv) Halt Phase,
where Landing Phase is the single support phase in which
swing leg moves to the target landing position. In ZMP
trasition Phase, the ZMP moves from the rear leg to the fore
leg. In Slowdown Phase, the velocity of the COG is adjusted
to 0. Finally, the COG comes to a halt in Halt Phase. If the
emergency signal is given in double support phase, the stop
motion can also deal with the emergency by starting from
ZMP Transition Phase.
(2)
The height of the COG is divided into constant part and
variable part
. By setting
, the ZMP
equation (2) can be rewritten by
(3)
(4)
(5)
where we suppose that the influence of the COG motion in the
vertical direction and effect of angular momentum are small
enough for emergency stop , and focus on the
horizontal motion. Substituting (1) into (4) and solving with
respect to , an analytical solution of the position of the
B. Real-time gait planning[6]
A halt motion must be generated by changing the original
walking pattern to stop humanoid robot suddenly. Moreover,
32
where and denote the beginning and the
end of the phase respectively. Then, the velocity of the COG
in (9) can be also expressed by
COG is obtained as
(6)
i iv
(7)
(9)
i iv
(iii)
In the generation of emergency stop motion, the aim is to
make the COG halt without falling over as soon as possible.
parameters in each phase including two
There are
coefficients of the hyperbolic functions and coefficients
of the ZMP trajectory in (6). The trajectory continuity of the
COG and the ZMP are taken into account such as
ii iv
ii iv
(iii)
(11)
(i)
(i)
(iii)
ii
(iii)
ii
(iii)
ii
iii
(17)
(i)
i
(ii)
(i)
ii
(iii)
iii
(i)
denotes the k-th order differentiation
¼ .
The stop condition (18) contains six unknowns about the
ZMP position and time period in each phase. In (18), we
assume that the ZMP is not changed in Landing Phase(i)
and Slowdown Phase(iii). The boundary conditions between
ZMP Transition Phase(ii) and Slowdown Phase(iii) can be
represented by the initial ZMP position and the step length
,
(i)
(14)
(18)
where is the time of emergency stop motion and (i)
(13)
(16)
(i)
(12)
i
ii
(i)
(iii)
iii
(19)
(i)
(20)
Thus, the unknowns are finally reduced to four i.e. , i , ii
and iii and the condition of the stop motion can be given as
(iii) i ii iii
(21)
where the step length in (21) can be explicitly represented by
i iv
From the position and the velocity of the COG (15) and (16)
and the conditions of the trajectory continuity (10)-(13), the
stop condition (17) can be rewritten by
(ii)
(iii)
Therefore, the unknown coefficients of the COG trajectory
are determined from the initial condition of the COG and the
boundary condition of the ZMP. Then, the COG trajectory is
expressed as
(10)
iii
(iii)
Note that the continuity of the COG trajectory in more than
the second order differentiation is guaranteed by satisfying
the continuity of the ZMP trajectory. Additionally, to reduce
the number of unknown parameters, the boundary conditions
of the higher degree derivatives of the ZMP in (13) are set to
zero, that is
A. Single support phase
In ZMP Transition Phase(ii), the ZMP is moved from the
rear foot to the fore foot to decelerate the COG at the next
Slowdown Phase(iii). At this moment, the displacement of the
ZMP can be moved according to the step length at landing.
When the emergency motion begins on the landing phase, the
velocity of the COG at the end of Slowdown Phase should
become zero, i.e.
C. Derivation of gait parameter
III. D ETERMINATION OF STOP MOTION PARAMETER
(8)
where the terminal time and the ZMP value in
each phase are two unknowns as the gait parameters, if the
initial values of the COG and the ZMP are given.
i iv
, and and i iv
where
donate scalar coefficients. Then, by differentiating (6) with
respect to , the velocity of the COG can also be obtained as
an analytical solution,
i ii iii
(15)
(22)
Landing Phase(i) should be long enough to let swing leg reach
to the landing position with the limited joint angle and angular
33
¾
and gradient vector can be obtained analytically.
Thus, the solution can be derived quickly, because the iteration usually converge in several steps. Additionally, in this
method, it is necessary to check if the generated step length
exceeds the limit. Here, the step length can be obtained by
substituting the generated time periods into (22).
Lastly, in Halt Phase, the terminal states of the COG and
the ZMP are set to the center of the distance between the feet
as
(iv)
iv
Fig. 5.
(iv)
iv
The effect of stop motion parameters in the single support phase.
velocity. The stride should become exponentially long as the
phase needs more time. On the other hand, shorter landing
time needs to move the foot more quickly and the joint
angular velocity of swing leg will exceed the limit. Moreover,
a long period of the double support phase makes the displacement of the COG in the emergency stop larger. Because it is
difficult to check the limitations of the joint angular velocity
in real-time for all periods of the emergency stop motion,
the unknown parameters are determined by evaluating the
average velocity of the foot in the generalized coordinates
and the period of the double support phase until the end of
Slowdown Phase(iii). The determination of unknowns can be
formulated as the optimization problem:
i ii iii
subject to
i ii iii
where
i iiiii
¾
iii
i 0
i ¾
iv
(28)
ii iii
(29)
In the double support phase, the value of displacement of the
ZMP is considered by the objective function to prevent from
overshoot. Thus, the objective function is given as
(24)
i ii iii
i ii iii
(25)
´ µ ´ µ
´µ
℄
¾
½
ii iii
¾ iii
i ¾
(30)
Substituting (29) into (30), the unconstrained optimization
problem is derived as,
The optimization problem in (25) can be solved by Newton’s
method. The unknowns can be obtained by iterative computation:
If the weighting coefficient of the ZMP displacement is
enough smaller than that of the COG displacement , the
maximum ZMP becomes large and the displacement of the
COG becomes short as shown in Fig. 6. In contrary, !! the
larger weight coefficient will be the smaller maximum
ZMP variation and the shorter COG displacement.
Substituting (22) into (24), the optimization problem in (23)
is reduced to the unconstrained optimization problem:
¾µ
´
(27)
The emergency stop motion in the double support phase
can be generated as well as in the single support phase
using ZMP Transition Phase(ii) as an initial phase. Then, the
condition (17) to the stop can also be utilized as well as the
single support phase. When the humanoid robots have to stop
urgently during the double support phase, it is impossible to
change the landing position. However, if the generated ZMP
trajectory stays inside the supporting polygon, the trajectory
can be used. At this time, the maximum displacement of the
ZMP can be obtained by the COG position in (15) and the
stop condition (17),
and and denote scalar weighting coefficients. The
effect of motion parameters is shown in Fig.5. The smaller
proportion of the weighting coefficient of the average velocity
to that of the period of the double support phase makes the
step length longer and the displacement of the COG shorter.
·½
(iv)
B. Double support phase
is the objective function which is defined as
(iv)
iv
Therefore, the COG and the ZMP trajectories are generated simultaneously by solving two boundary condition problem[6].
(23)
(iv)
(i) ¼
iv
(26)
ii iii
i ii iii is the time vector of each phase
where
in -th step at the iterative computation. In (26), Hessian
ii iii
(31)
The optimization problem in (31) can be also solved by
Newton’s method as well as (26).
34
Fig. 6.
The effect of stop motion parameters in the double support phase.
C. Marginal Case
When an emergency signal was triggered at close to
touchdown or lefe-off, the stop motion generator calculates
unrealizable trajectories of the COG and the ZMP. In other
words, the landing position and the variation value of the
ZMP go over the limits at the late of the single and the double
support phase. In these cases, the humanoid robots will stop
at next supporting phase.
Fig. 7.
Flowchart of the emergency stop.
IV. G ENERAT ION OF UPPE R BODY MOTION
In Halt Phase, the trajectories of the upper body joints are
generated as the fifth order polynomial function. However,
these upper body joints rotate independently.
Thus, overall algorithm is shown by a flowchart in Fig.7.
A. Crouch down motion
The crouching motion is effective for a stable stop by the
following two reasons. Firstly, over extension of knee joints
can be avoided. Secondly, the stability can be increased by
getting down the COG. The crouching down motion can be
realized by lowering the COG. The trajectory of the vertical
COG is given as the fifth order polynomial function. The
initial and final continuity of the acceleration are considered.
V. S IMULATION
A. Emergency stop at the single support phase
B. Halt of upper body
Fig.8 shows simulation result of the emergency stop motion
with crouching down in the single support phase. Before the
emergency stop, the robot is walking with step length of
0.3[m] and step cycle of 0.8[s]. The weighting factor and
are set to 5.0 and 1000.0 respectively. In this simulation,
the command of emergency stop is given at 3.4[s] at the late
single support phase. Thus, in the original walking pattern, the
single support phase has a little time to terminate. The stop
motion generator postponed the touchdown time and extended
the step length.
In Fig.9, we can see that the humanoid robot can stop
within one step. Then, the knee joints in the crouching down
motion are compared with the constant height of the COG
in Fig.10. The left and the right legs are the rear and the
fore respectively. Although the maximum knee joint angle
in the rear leg increases, the rotate range of the knee joints
in the fore leg can be reduced as shown in Fig.10. Thus, the
crouching motion can prevent from the expanding of the knee
joint.
In emergency stop motion, it is necessary to stop the upper
body joints as well as leg joints. The upper body joints are
stopped through two phases:
(i)
Slowdown Phase,
(ii)
Halt Phase.
In Slowdown Phase, the angular velocity and acceleration of
the upper body joints are set to zero. The trajectories of these
joints are given as the third order polynomial function by
using initial joint angle (i) ¼ and angular velocity (i) ¼
as
(i)
¼ (i) ¼ ¼
(i)
(i)
¼
¼
¼¾
i ¼
i
¼ ¾
¼
i
(i)
¼ ¿
(32)
Then, the maximum joint angles to check the range of joint
limits are obtained,
(i) (i)
¼
i
(i)
¼ i
¼
(33)
35
Fig. 8.
Fig. 9.
Fig. 11.
Snapshot of the emergency stop with crouch down.
Fig. 12.
The COG and the ZMP trajectories of the emergency stop.
Snapshot of the emergency stop in the double support phase.
The COG and the ZMP trajectories of the emergency stop.
VI. E XPERIMENT
Since it is guaranteed that the emergency stop motion
can be realized, we tested it on the actual humanoid robot
HRP-2[10]. The same weighting coefficients are used as
the simulation. The step length and step cycle are set to
0.15[m] and 0.8[s] respectively. The timing to command the
emergency stop was set to the half of the single support phase.
The snapshots of the emergency stop motion are shown in
Fig.13.
Fig. 10.
VII. C ONCLUSION
Comparison of the knee joints.
This paper presented the emergency stop algorithm of a
walking humanoid robot. The emergency stop motion was
divided into four phases according to the role of the ZeroMoment Point (ZMP). In each phase, approximate analytical
solutions of the Center of Gravity (COG) dynamics was used
to generate the motion. During the single support phase, a
landing time and position were determined by evaluating
the average velocity of the swing leg and the horizontal
position of the COG. During the double support phase, the
travel distance of the COG and the ZMP were evaluated. The
emergency stop to more complicate motion such as dancing
and so on is an issue in our future.
B. Emergency stop at the double support phase
The simulation result of the emergency stop motion in the
double support phase is shown in Fig.11. The walking pattern
is used as well as the single support phase. The command of
emergency stop is set as 3.5[s]. The weighting coefficients
and are set to 50.0 and 1000.0 respectively. The generated
trajectories of the COG and the ZMP are shown in Fig.12. We
can see that the overshoot of the generated ZMP trajectory
at about 4 seconds. The overshoot wa caused by quantization
error of phase transition time due to the fixed servo cycle.
36
Fig. 13.
Snapshot of the emergency stop in experiment.
R EFERE NCES
[1] K.Fujiwara, et al.,”UKEMI:Falling Motion Control to Minimize Damage to Biped Humanoid Robot,” Proc. of IEEE/RSJ Int. Conf. on IROS,
pp.2521-pp.2526, 2002
[2] K.Fujiwara, et al.,”The First Human-size Humanoid that can Fall Over
Safely and Stand-up Again,” Proc. of IEEE/RSJ Int. Conf. on IROS,
pp.1920-pp.1926, 2003
[3] H.Miura, et al., ”Dynamic walk of a biped,” Int. Jour. of Robotics
Research, Vol.3, No.2, pp.60-72, 1984
[4] S.Kajita, et al., ”The 3D Linear Inverted Pendulum Mode: A simple
modeling for a biped walking pattern generation,” Proc. of IEEE/RSJ
Int. Conf. on IROS, pp.239-pp.246, 2001.
[5] K.Harada, et al., ”Real-Time Planning of Humanoid Robot’s Gait for
Force Controlled Manipulation,” Proc. of IEEE Int. Conf. Robotics and
Automation, 2004.
[6] K.Harada, et al., ”An Analytical Method on Real-time Gait Planning for
a Humanoid Robot,” Proc. of the 2000 IEEE-RAS Int. Conf. Humanoid
Robots, 2004.
[7] K.Nishiwaki, et al., ”Online Mixture and Connection of Basic Motions
for Humanoid Walking Control by Footprint Specification,” Proc. of
IEEE Int. Conf. on Robotics and Automation, pp.4110-4115, 2001.
[8] M.Vukobratovic, and D.Juricic, ”Contribution to the Synthiesis of Biped
Gait,”, IEEE Trans. on Bio-Med. Eng., vol.BME-16, no.1, pp.1-6,1969.
[9] T.Sugihara, et al., ”A Fast Online Gait Planning with Boundary
Condition Relaxation for Humanoid Robot,” Proc. of IEEE Int. Conf.
on Robotics and Automation, 2005.
[10] K.Kaneko, et al., ”The Humanoid Robot HRP2,” Proc. of IEEE Int.
Conf. on Robotics and Automation, 2004.
37