Fall Prediction of Legged Robots Based on Energy State and Its Implication of Balance Augmentation: A Study on the Humanoid Zhibin Li, Chengxu Zhou, Juan Castano, Xin Wang, Francesca Negrello, Nikos G. Tsagarakis, and Darwin G. Caldwell Abstract—In this paper, we propose an Energy based Fall Prediction (EFP) which observes the real-time balance status of a humanoid robot during standing. The EFP provides an analytic and quantitative measure of the level of balance. Both simulation and experimental studies were conducted and compared with the previously proposed indicators, such as Capture Point (CP) and Foot Rotation Indicator (FRI). The EFP also suggests the balance augmentation by active foot tilting to create larger potential barriers. As a proof of concept, a hybrid balance controller was designed to stabilize the robot including under-actuation phases so the robot can also balance with shoes. Our study reveals that both EFP and CP successfully predict falling about 0.2s in advance for the tested robot, while the FRI fails due to the light weight of the foot and limited resolution of the force/torque measurement.
I. I NTRODUCTION Legged systems gain tremendous versatility over the wheeled ones but trade off the stability and risk tipping themselves. Therefore, the physical quantities for the realtime measurement of the balance status of legged robots, particularly humanoids, are critical. As analyzed in the work of Hofmann [1], the zero moment point (ZMP) [2] is a powerful feasibility criterion for validating the dynamic gaits, however, it does not indicate a balance status while the robot is in action. A paradox was proposed by Hofmann that a robot could have its center of mass (COM) falling while the ZMP is still inside the support polygon. To address this issue, some physical quantities were proposed for describing the balance state of the legged systems. Pratt et al [3] proposed the Capture Point (CP), where if the point support would be instantaneously placed, the robot would eventually come to a complete stop. The Capture Point resolves the issue by examining the zero orbital energy at an instantaneous point foot based on the linear inverted pendulum model (LIPM). Another quantity, the foot rotation indicator (FRI), examines the stability from the wrench applied on the foot [4]. Regarding the nearest edge of the foot as the pivot, if the ankle torque is larger than the net torque created by the ankle force and the gravitational torque, then the foot starts rolling since it is not glued on the ground. The control issue arises since the rolling foot creates one underactuated degree of freedom (DOF) around the pivot, where the torque is zero, and the center of pressure (COP) diminishes into an uncontrollable point. From the perspective of control theory, the boundary of the control effort reduces to zero, thus the system becomes uncontrollable. Therefore, to ensure the preconditions of classical control, both CP and FRI shall be constrained within the size of the foot.
It should be noted that both the Capture Point (CP) and FRI point are the intersection points on the terrain surface. Though both are the two dimensional quantities which can be compared to the physical size of the foot, their nature determines that a range of variation is needed for these points to vary if they are employed to be the controlled variables. In general, this implies that a certain size of foot is required and the controlled variable must be inside the polygon of support which suffices the classical control methodology. The only exception is that the CP can potentially move out of the support foot for a very limited time if hip torque is applied. During our previous study on the standing balancing of COMAN [5], it was found that the robot did not necessarily fall while feet were tilting, and the proposed strategy in the following work [6] successfully handled the under-actuation coming from the foot tilting without losing balance. Moreover, the study of bio-mechanics also shows that the stance foot rolls around the toe during walking [7] [8]. This leads to our hypothesis that there might exit another physical index that reflects a more generic balance indication. Most importantly, a better understanding of the stability of a legged system could place a correct design and control requirements. We are motivated to evaluate the energy state to determine the global stability to allow the legged system to perform a diversity of movement without balancing all forces at every instant as long as the system’s energy is globally bounded. This is also an underlying principle for many passive dynamic walkers [9] [10] [11]. The common groundwork of their control captures the physics of controlling the kinetic energy to avoid falling. Previous work based on the energy state was studied in [12]. A similar work to ours was also reported in [13] where a rimless wheel model was used for the instantaneous falling prediction. In this paper, the principle of controlling kinetic energy for the fall prediction is elaborated and demonstrated with the focus on the standing case that the robot can be stable as long as the total mechanical energy is bounded. Hence, there is no necessity that the COP must be controlled inside the support foot, thus no restriction of keeping feet flat. Rather, the COP is allowed to move to the narrow strip at the edge of the contact polygon. The paper is organized as follows. Section II presents the principles of the balance state prediction and its implication of balance augmentation, and also explains the sensor fusion and the hybrid controller for proving our concept. Section III and Section IV shows the results of successful push recovery, falling prediction, and the comparison of the energy
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore. 0.5 0.4 Segmental COM 0.3 COM Overall
Center of Mass
𝐻𝑟 𝑐𝑜𝑚
z
Joint 0.2
H 0 I
Apex of potential energy
y Fo
0.1
𝐻
𝑟𝑐𝑜𝑚
𝐹𝑟 𝑝𝑐𝑜𝑚
x
0
𝐹𝑟 𝑐𝑜𝑚
-0.1
𝑔
-0.2
𝐻𝑟 𝑓𝑟
𝐻
y
z 𝐹
-0.4
x
df
E Ek E p
y
-0.3
x
E0
E 0
E 0
𝐹𝑟 𝑝𝑖𝑣𝑜𝑡
-0.5 -0.2
Fig. 1.
𝑟𝑓𝑙
r
r0
z
𝐻𝑟 𝐹𝑜
0.4
Fig. 2.
0.2
0
Potential apex and its relation with mechanical energy.
Modeling 0.2 of the robot, blue dots are the segmental masses. 0
0.4
-0.2
-0.4
kinetic energy of the robot can still be measured through the kinematics of the robot combined with the IMU sensor. Denote r0 the virtual leg, d the foot segment, ω the angular velocity, and rf the virtual pendulum pointing from the frontal foot to the COM, thus
based falling prediction (EFP) with other balance indicators in simulations and experiments respectively. We summarize and conclude our study in Section V. II. P RINCIPLES OF FALL P REDICTION AND I TS C ONTROL I MPLICATION
rf = r0 − df .
When under strong pushes during standing, the robot tips around the edge of the foot in an inverted pendulum manner. Therefore we use inverted pendulum model (IPM) to represent better the system’s behavior rather than the linear inverted pendulum model (LIPM). The dynamics of the system is hence formulated in the polar coordinate since the physics law is the same regardless of the coordinate system. The use of IPM has several advantages as follows: 1) avoids the constant COM height constraint and resembles a ballistic COM motion during push recovery; 2) all the external forces/torques appear in an unified manner as torque inputs around the pivot. We focus on the case of balancing without the assistance of upper limbs, and define balance is a set of states where the robot’s COM could maintain above the support polygon within a future time horizon. The current kinetic energy of the robot is measured based on the IPM dynamics for determining weather or not the robot is losing balance. Fig. 1 shows the model where r˙com denotes the COM velocity. The vector r˙com together with the gravity defines a plane of motion, which intersects the boundary of support polygon. This intersection is the instantaneous pivot. Therefore, given a perturbation, there always exits a pivot and an equivalent pendulum pointing from the pivot to the COM, that allows applying the simplified IPM to analyze the balance state. If the total mechanical energy is lower than the maximum potential energy apex created by the current support polygon, then the robot is allowed to have an under-actuation phase, in other words, tipping around the edge of the foot.
(1)
Similarly, the virtual pendulum rb pointing from the rear foot to the COM is rb = r0 − db . The inertia tensor I¯ around an instantaneous pivot is 2 0 0 ry + rz2 0 , rx2 + rz2 I¯ = Ic + m 0 0 0 rx2 + ry2
(2)
(3)
where r is the virtual pendulum rf or rb . Hence, the kinetic energy of the system Ek around an instantaneous pivot can be approximated by Ek =
1 T ¯ T ω RIR ω, 2
(4)
where R is the rotational matrix of the robot by approximating the whole system as one rigid body. The gravitational work from the swept angle θ0 to θf is Wg = mgr (cos(θ0 ) − cos(θf )) , 1 ≈ mgr θf2 − θ02 , 2
(5) (6)
where r is the norm of krk. Since the angles are limited inside the friction cone, the approximation in (6) is valid. We are interested whether or not the COM could cross the apex (θf = 0) of the potential energy, as shown in Fig. 2. Regarding the potential apex as the zero baseline, the potential energy is
A. Mechanical Energy Based Balance Prediction
1 Ep ≈ − mgrθ02 . 2
Fig. 2 shows the simplified model consists of a rigid body with mass m, inertia tensor Ic , a massless inverted pendulum and a foot. When an impact applies at the robot, the delivered momentum is difficult to be measured due to the lack of tactile sensors on the surface of the robot. However, the
(7)
Define the total mechanical energy as E = Ek + Ep , 2
(8)
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore.
the foot tilting angle is about 5 − 15◦ , thus the resulted ankle torque reduction is insignificant (0.38% − 3.4%). Fig. 3 (a) shows a horizontal impulse J that produces an initial linear velocity of v = J/m. The velocity component perpendicular to the virtual pendulum is v cos β, where β is the initial angle of rf with respect to the vertical line
Ek 0 Ek* cos 2 ( ) v cos( )
F
v
r0 r
z y
x
df (a)
Fig. 3.
r0
r df (b)
r0
r df
p
β = atan2 (rf (x), rf (z)) .
(c)
The velocity component v sin β will damp out eventually. So the effective kinetic energy received by the robot is
The implication of balance argumentation by active foot tilting.
Ek = J 2 cos2 β/2m. then by examining E, it can be quantified whether or not the energy injected by the external disturbance could topple the robot over the edge of the support foot: E < 0, return before the potential apex; E = 0, rest exactly at the potential apex; E > 0, cross over the potential apex.
(12)
The initial potential energy is Ep = mg(r0 (z) − rf ).
(13)
Hence, the critical impulse to tip over the robot can be analytically predicted by setting Ek + Ep = 0, yields q m J= 2g(rf − r0 (z)). (14) cos β
When E < 0, the robot will return before the potential apex. Substitute (4) and the current angle θ0 into (6) and let (8) equal to zero, we obtain the angular position θf where the COM has zero angular velocity before the potential apex. s 2Ek . (9) θf = θ02 − mgr
A reasonable case study for the COMAN humanoid is to set r0 = [0.04, 0, 0.55]T m, df = [0.13, 0, 0]T m and m = 34 kg. Without active foot tilting, the critical impulse is 13.05 Ns. By setting the foot pitch from 0◦ to 15◦ incrementally by 5◦ into (10) and (14), the critical impulses are 20.7 Ns, 26.1 Ns, and 30.5 Ns respectively at the ankle joint rotation θp of 5◦ , 10◦ , and 15◦ . Hence, the energy based balance criterion suggests that active foot tilting can enlarge the potential apex of the system so that the impulse rejection can be augmented by 58% upto 133% by sacrificing only a few percent of the maximum ankle torque. Fig. 4 demosntrates one successful balance recovery by very large foot tilting similar to humans. For the LIPM, define zc the constant height of the COM, and x the initial COM position with respect to the instantaneous pivot, the normalized mechanical energy E [14] is
The prediction using (9) can be used when E < 0 for determining the stability margin δ in terms of angular position, for example, keeping kθf k < δ. B. Balance Augmentation by Enlarging Potential Apex The principle of bounded mechanical energy as a balance criterion implies the augmentation of disturbance rejection by increasing the potential apex. Fig. 3 (a) illustrates a forward push narrative for the analysis and other cases can be ruled out by the same principle. In Fig. 3 (b), the robot locks all joints as one rigid body, then the virtual pendulum is rf = r0 − df . So the kinetic energy to overcome the potential apex is mg(rf − r0 (z)) in this planar case. Then, assume another scenario as in Fig. 3 (c) where the ankle joint actively pitches downward by θp , then the virtual pendulum becomes rf = r0 − Rpitch (θp )df .
(11)
1 2 g 2 x˙ − x . (15) 2 2zc q The critical COM velocity is x˙ = zgc x by setting (15) = 0. So the LIPM can reject the maximum impulse of r g J = mx˙ = m x. (16) zc E=
(10)
Substitute the aforementioned parameters, the critical impulse is 12.9 Ns, which is slightly smaller than that of the IPM without foot tilting. This is logical because the constant COM height constrained by the LIPM does not redirect the velocity produced by the impact.
Undoubtedly, by rotating two vectors r0 and df away from each other, the norm of the vector rf increases. So the elongated virtual pendulum rf creates a larger potential barrier, as the two red horizontal lines indicate in Fig. 3, for the disturbance to overcome. As a consequence, actively tilting the foot trades off the maximum ankle torque when the vertical acceleration is zero. In the flat footed case, the maximum ankle torque is τmax = mgdf . However, if the foot tilts by angle θ with respect to the horizon, the maximum ankle torque becomes τmax = mgdf cos θ. In our simulation and experimental study,
C. Analysis of Convergent and Divergent Phases In this work, the convergent phase is defined when the COM is approaching the pivot, and the divergent phase is defined when the COM is departing from the pivot. Taking the frontal virtual pendulum for example, there are two convergent phases and two divergent phases. For each convergent phase, there are 3
Position [m]
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore.
Fig. 4. Balance augmentation by large foot tilting (spaced at 0.2s interval) with no restriction of a flat footed behavior.
Fig. 6.
0.15 0.13 0.11 0.09 0.07 0.05 0.03 0.01 0.01 0.03 0.05 0
COM Estimation COP
10 20 30 40 50 60 70 80 90 100 110 120 130 140 Time [s]
The COM estimation compared to COP during static motion.
E0
(1)
E0
encoder position combined with the orientation of the pelvis measured by the inertial measurement unit (IMU) in order to compute the current COM state of the robot. The local COM of each limb is obtained from the estimation in the mechanical design software Pro/E with reasonable precision, only the torso’s COM vector needs to be identified due to the imprecise COM data of the battery, on-board PC and other accessories. The COM vector of torso was identified via least square fitting by correlating the COP and overall COM during very static movements. The WBS feedback is used to obtain the virtual inverted pendulum vector when the robot is pushed to a direction during standing. Fig. 1 illustrates geometric relation for calculating the virtual inverted pendulum vector F rpcom . The local pelvis frame ΣH is attached at the center of the pelvis, and its orientation is measured by the IMU. A base frame ΣF is located at the geometric center of the support polygon with its vertical axis aligned with gravity and the heading direction the same as ΣH . Then the origin of ΣF in ΣH is calculated as follows, H rf l + H rf r H rFo = , (17) 2
E0 (3)
(1)
(3)
E0
(2)
(4)
(a)
Fig. 5.
(2)
(4)
(b)
Deterministic cases in convergent and divergent phases.
two possibilities depending on the energy state. Therefore, for one tilting virtual pendulum, there are totally six cases. Fig. 5 (a) shows the convergent and divergent phases while the COM’s horizontal projection is inside the support polygon. In the convergent phase (Fig. 5 (a)-1), the COM either passes over the pivot (E > 0) and becomes unstable, or returns back before the apex (E < 0). In the divergent phase (Fig. 5 (a)-2), the COM goes away from the pivot and falls into the polygon, which is stable. To sum up, when the ground projection of COM is above the polygon, only E > 0 in the convergent phase (marked by red) is unstable. Fig. 5 (b) shows the convergent and divergent phases while the COM’s horizontal projection lies outside the support polygon. During the divergent phase (Fig. 5 (b)-1 marked by red), the system is unstable. During the convergent phase (Fig. 5 (b)-2), only the case E > 0 (Fig. 5 (b)-3) allows the COM to cross over the apex and returns to the support polygon. Otherwise, the COM cannot cross the apex and falls eventually (Fig. 5 (b)-4). To sum up, when horizontal COM projection lies in the support polygon, only E > 0 in the convergent phase is unstable; when horizontal COM projection lies outside the support polygon, only E > 0 in the convergent phase is stable. Similar analysis can be done for the virtual pendulum formed by the COM and the rear edge of the support polygon. The above analysis provides the definite classification of balance status in different cases.
where the left superscript denotes the reference frame. The COM in ΣF is obtained using (18), F
rcom = F rHo + F RH H rcom ,
(18)
and
F H
rHo rcom
= −F RH H rFo , = Σmi H ri /Σmi ,
(19)
where mi is the mass of ith link and H ri is the position vector of ith link’s COM in ΣH . Hence, the virtual inverted pendulum vector is given by F
rpcom = F rcom − F rpivot ,
(20)
where the vector F rpivot is the pivot of rotation when the robot tilts in one direction. Fig. 6 shows the validation of the calibrated COM estimation where the hip position x moved almost statically. Hence the COP and COM position shall be nearly identical during this static movement. The error of the COM estimation is small enough to be used in the following feedback control.
D. Whole Body COM Estimation The Whole Body Sensing (WBS) module is developed to compute the forward kinematics of the robot using the link 4
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore.
(a) Recovery from forward push (spaced at 0.3s interval)
Front Back Push Forward Push Backward 0.5
1.0
1.5
2.0
2.5 3.0 3.5 Time [s]
4.0
4.5
5.0
0.13 0.11 0.09 0.07 0.05 0.03 0.01 0.01 0.03 0.05 0.070.0
Position [m]
Normalized Energy [J/kg]
0.00 0.02 0.04 0.06 0.08 0.10 0.12 0.14 0.160.0
5.5
Capture Point Push Forward Push Backward
0.5
1.0
1.5
2.0
(a)
Simulation of balance state prediction under different pushes.
FRI Left FRI Right Push Forward Push Backward
0.5
1.0
1.5
2.0
2.5 3.0 3.5 Time [s]
4.0
4.5
5.0
0.20
0.10
0.14 0.12 0.10 0.08 0.06 0.04 0.02 0.00 −0.02 −0.04 −0.06 −0.08 −0.10 0.0
0.050.0
5.5
0.5
1.0
1.5
2.0
0.09
2.5 3.0 3.5 Time [s]
4.0
4.5
5.0
5.5
(d)
Simulation of balance prediction during successful push recovery.
Front Push Forward Fall timestamp Fall detection
0.1
0.2
0.3
0.4
0.5 0.6 Time [s]
0.7
0.8
0.9
1.0
0.30 0.28 0.26 0.24 0.22 0.20 0.18 0.16 0.14 0.12 0.10 0.08 0.06 0.04 0.02 0.0
Capture Point COM Push Forward Fall timestamp Entry Divergence Fall detection
0.1
0.2
0.3
0.4
0.5 0.6 Time [s]
0.7
0.8
0.9
1.0
0.7
0.8
0.9
1.0
(b) 0.38
FRI Left FRI Right Push Forward Front Edge of Foot
Foot pitch angle [rad]
Position [m]
0.11
5.5
0.05
(a)
0.13
5.0
0.00
Position [m]
Normalized Energy [J/kg]
Fig. 8.
4.5
Left Foot Pitch Right Foot Pitch Push Forward Push Backward
0.15
(c)
E. A Hybrid Balance Controller Our proposed balance state prediction aims to prove the concept that a legged robot can be considered stable by evaluating the mechanical energy, and further, it implies the allowance of under-actuation that is beyond the conventional restriction of keeping feet flat on the terrain. Apart from the balance augmentation, an additional advantage of the underactuation (foot tilting) is the good foot-ground clearance, so the GRF always resides in the frontal foot, keeping the maximum ankle torque for balance recovery. Certainly, in theory, one can also control the COP to be exactly at the frontal edge of the foot without tilting up the foot. However, this is fairly difficult to control in practice since any external force disturbance or internal dynamical coupling can press the foot down thus move the COP away from the edge when the foot-ground clearance is zero. This means that the ankle torque is no longer the maximum. As a proof of concept, a hybrid balance controller is designed for balancing the robot involving under-actuation phases. It consists of a Cartesian impedance controller at the COM, the virtual spring damper placed at the foot using virtual model control concept [15], and a velocity controller for regulating the upper body in an upright posture. An impedance control loop is placed at the COM level using the COM estimation feedback presented in Section II-D, and the ankle of the robot is torque controlled for generating the control effort. To produce the under-actuation, the torque limit is set higher than the counteracting torque created by the weight of the robot. The torque limit is increased until the foot tilting appears. To prevent over tilting of the foot and instability, virtual spring-dampers placed at the feet generate unilateral torque based on the feedback of the pitch angle of the feet to reduce the desired ankle torque. The virtual stiffness and damping are tuned to ensure that an equilibrium can be reached when the feet have reasonable clearance off the ground, for instance, 10◦ or 20◦ . The torso attitude is regulated by the velocity controller that commands the velocity reference in proportional to the orientation error.
4.0
(b)
Foot pitch angle [rad]
Position [m]
(b) Falling from large forward push (spaced at 0.1s interval) Fig. 7.
0.13 0.11 0.09 0.07 0.05 0.03 0.01 0.01 0.03 0.05 0.070.0
2.5 3.0 3.5 Time [s]
0.07 0.05
0.28
Left Foot Pitch Right Foot Pitch Push Forward
0.18
0.08
0.03 0.01 0.0
0.1
0.2
0.3
0.4
0.5 0.6 Time [s]
(c) Fig. 9.
0.7
0.8
0.9
1.0
−0.02 0.0
0.1
0.2
0.3
0.4
0.5 0.6 Time [s]
(d)
Simulation of balance prediction during forward falling.
Two types of studies were carried out: balance recovery without falling, and falling prediction under large forward pushes. The force impulse was designed with half sinusoidal shape and applied at the center of the pelvis, which was close to the COM. So the momentum transferred to the robot was approximately 2A∆t π Ns, where A was the magnitude of the impulse, and ∆t was the pulse width. In the first case, the impulse had A = 250N and ∆t = 0.1s, resulting in delivered momentum of 50 π Ns. In the second case, the impulse had A = 400N and ∆t = 0.1s, resulting in delivered momentum of 80 π Ns. The accompanied video will show that the balance state prediction is being displayed in real-time when the robot is under different pushes. Fig. 7 (a) shows the snapshots of balance recovery under the disturbance of 50 π Ns momentum. Fig. 8 (a) shows the estimation of mechanical energy of the robot that was always smaller than zero which predicted a stable state. For the comparison with other indicators, the Capture Point was also computed and it never exited the area of support polygon, as shown in Fig. 8 (b). However, the FRI data in Fig. 8 (c) shows that the FRI approached very close to frontal and rear edges of the foot during the under-actuation phase, but never
III. S IMULATION The balance state prediction was studied on the simulated COMAN humanoid [16] in the Open Dynamic Engine (ODE). 5
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore.
went beyond the limit. The occurrence of under-actuation was clearly indicated by the absolute orientation of both feet as shown in Fig. 8 (d). Fig. 7 (b) shows the snapshots of falling under a large impulse of 80 π Ns, which started at 0.5s and ended at 0.6s. Fig. 9 (a) shows that the mechanical energy was already larger than zero at 0.555s, so the EFP detected falling at 0.555s before the impulse finished. The Capture Point detected falling at 0.565s. The COM reached at the edge of support polygon at 0.754s, so the time margins after the fall detection were 0.199s and 0.189s for the EFP and the CP respectively. The FRI had two overshoots beyond the size of the foot in Fig. 9 (c): the first was due to the push, and the second was due to the rapid forward falling acceleration after crossing the potential apex. The first FRI detected a value greater than the size limit of foot from 0.6s to 0.644s for a very short time. The magnitude of the first FRI overshoot was 0.1324 m, which was only 2.4 mm beyond the edge of the foot. So the best measurable FRI deviation from the foot edge was only 1.2% of the foot size (0.2m). Our revealed limitation of FRI was in agreement with previous study in [4]. Note that in simulation, the sensor feedback can be obtained more accurately than in practice, so this indicates the difficulty to use FRI measurement as an indicator. In fact, even in the first simulation without falling (Fig. 8 (c)), the measured FRI was no greater than the maximum size of the foot despite the foot indeed rotated. It was because of the small mass of the foot plate (0.2kg) that a very small net torque applied on foot could already generate sufficient angular acceleration to rotate the foot.
(a) Recovery from forward push
(b) Recovery from backward push
(c) Falling from large forward push
0.02 0.00 0.02 0.04 0.06 0.08 0.10 0.12 0.14 0.16 0.18 0.200
0.15 0.13 0.11 0.09 0.07 0.05 0.03 0.01 0.01 0.03 0.05 0.070
Position [m]
Experiments of balance state prediction (spaced at 0.25s interval).
Normalized Energy [J/kg]
Fig. 10.
Front Back Push Forward Push Backward 1
2
3
4
IV. E XPERIMENTS
5 6 Time [s]
7
8
9
10
Capture Point Push Forward Push Backward
1
2
3
4
(a) 0.13 0.11 0.09 0.07 0.05 0.03 0.01 0.01 0.03 0.05 0.070
FRI Left FRI Right Push Forward Push Backward
1
2
3
4
5 6 Time [s]
(c) Fig. 11.
7
8
9
10
11
(b)
7
8
9
10
Foot pitch angle [rad]
Position [m]
To further validate our EFP concept and its comparison with CP and FRI, we carried out experiments on the COMAN humanoids: moderate forward pushes without falling, large forward pushes with falling. During the experiments, the EFP was computed in real-time and streamed into an independent thread for displaying on the screen. The performance could be better understood by the accompanied video. Fig. 10 (a) and (b) shows the successful balance recovery from the forward/backward pushes, and Fig. 10 (c) shows a forward falling after a strong push. Note that during the falling experiment, the human intervention occurred before the robot fell on the ground. The time of this intervention was marked in the following data plots. Fig. 11 presents the data of EFP, CP, FRI and the orientation of the feet during the balance recovery (Fig. 10 (a) and (b)). The mechanical energy was always below zero, predicting a stable balance state which was true. During the forward push, the EFP was marginally below zero, and the feet had been significantly lifted off the ground for more than 1.0s as Fig. 11 (d) shows. In this stable case, EFP and FRI were all within their regions of stability, but CP had an erroneous prediction of falling for a short moment. Fig. 12 presents the same type of data during forward falling. The push was applied at 0.88s. The EFP and CP
5 6 Time [s]
11
0.25 0.20 0.15 0.10 0.05 0.00 0.05 0.10 0.150
Left Foot Pitch Right Foot Pitch Push Forward Push Backward
1
2
3
4
5 6 Time [s]
7
8
9
10
11
(d) Experimental data of successful push recovery.
predicted fall at 1.11s and 1.1s respectively, and the COM went out of the support polygon at 1.39s in reality. Thus, the time margin of predicting ahead was 0.28s for EFP and 0.29s for CP. Unfortunately, the FRI was not able to predict fall with the equipped F/T senors in our robot. Fig. 13 shows an additional experiment of standing balancing of COMAN robot that wears shoes. As previously discussed and proved that the system is stable as long as the mechanical energy is bounded by the potential apexes (one in the front, the other at the back), a variety of control actions that attenuate the mechanical energy can be applied to stabilize the standing posture. In this experiment, the change of balance controller was only the equilibrium offset of the virtual springdamper for the feet, because the real feet started with an initial 6
0.02 0.00 0.02 0.04 0.06 0.08 0.10 0.12 0.140.0
0.34
Front Push Forward Fall timestamp Human Intervention Fall detection
0.29 Position [m]
Normalized Energy [J/kg]
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore.
0.24 0.19 0.14 0.09
0.2
0.4
0.6
0.8
1.0 1.2 Time [s]
1.4
1.6
0.040.0
1.8
0.2
0.4
0.6
(a) 0.13
Position [m]
0.07 0.05
FRI Left FRI Right Push Forward Human Intervention
0.01 0.2
0.4
0.6
0.8
1.0 1.2 Time [s]
1.4
1.6
1.8
Foot pitch angle [rad]
0.11
0.03
0.60 0.55 0.50 0.45 0.40 0.35 0.30 0.25 0.20 0.15 0.10 0.05 0.000.0
1.0 1.2 Time [s]
1.4
1.6
1.8
1.4
1.6
1.8
Left Foot Pitch Right Foot Pitch Push Forward Human Intervention
0.2
(c) Fig. 12.
0.8
(b)
0.09
0.010.0
have shown the consistent results. The time difference between EFP and CP are 0.01s (simulation), 0.01s and 0.02s, which are only at 10−20ms time scale. The FRI only predicts falling in simulation with very marginal values. To conclude, the physical quantity of fall prediction should be measurable via feedback. Both CP and EFP satisfy this requirement and provide a time margin about 0.2s before the robot enters the divergent phase of falling. The CP does not always predict correctly at the marginal case, as shown in Fig. 11 (b). Hence, for increasing the successful rate, it is sensible to fuse of the CP and the EFP together for the robust and reliable balance state estimation and fall prediction. Thus, a more complex control framework can be made based on this balance indication, for instance, to apply ankle or hip strategy [17] to keep stance balance. In our future work, we will explore the time margin permitted by the EFP to activate the stepping strategy developed in [18] with smooth transitions of activating different controllers.
Capture Point COM Push Forward Fall timestamp Human Intervention Entry Divergence Fall detection
0.4
0.6
0.8
1.0 1.2 Time [s]
(d)
Experimental data of forward falling under large pushes.
ACKNOWLEDGMENT This work is supported by the FP7 European project WALKMAN (ICT 2013-10). The research of balance augmentation by foot tilting was conducted under the Open Project (ICT1426) in the State Key Laboratory of Industrial Control Technology (SKLICT) at the Zhejiang University.
(a) Balance recovery from forward push
R EFERENCES [1] A. G. Hofmann, “Robust execution of bipedal walking tasks from biomechanical principles,” Ph.D. dissertation, 2006. [2] M. Vukobratovic and B. Borovac, “Zero-moment point - thirty five years of its life,” International Journal of Humanoid Robotics, vol. 1, pp. 157– 173, 2004. [3] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in IEEE-RAS International Conference on Humanoid Robots, December 2006, pp. 200–207. [4] M. Popovic, A. Goswami, and H. Herr, “Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control Implications,” The International Journal of Robotics Research, vol. 24, no. 12, pp. 1013–1032, Dec. 2005. [5] Z. Li, N. Tsagarakis, and D. Caldwell, “A passivity based admittance control for stabilizing the compliant humanoid COMAN,” in IEEE-RAS International Conference on Humanoid Robots, Osaka, Japan, Nov. 29th - Dec. 1st 2012, pp. 44–49. [6] Z. Li, N. Tsagarakis, and D. Caldwell, “Stabilizing humanoids on slopes using terrain inclination estimation,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2013, pp. 4124– 4129. [7] A. D. Kuo, “The six determinants of gait and the inverted pendulum analogy: A dynamic walking perspective,” Human movement science, vol. 26, no. 4, pp. 617–656, 2007. [8] D. A. Winter, Biomechanics and motor control of human movement. John Wiley & Sons, Inc., 2009. [9] T. McGeer, “Passive dynamic walking,” International Journal of Robotics Research (Special Issue on Legged Locomotion), vol. 9, no. 2, pp. 62–82, 1990. [10] A. D. Kuo, “Stabilization of lateral motion in passive dynamic walking,” The International Journal of Robotics Research, vol. 18, no. 9, pp. 917– 930, 1999. [11] F. Asano, M. Yamakita, N. Kamamichi, and Z.-W. Luo, “A novel gait generation for biped walking robots based on mechanical energy constraint,” IEEE Transactions on Robotics and Automation, vol. 20, no. 3, pp. 565–573, 2004. [12] E. Garcia and P. Gonzalez de Santos, “An improved energy stability margin for walking machines subject to dynamic effects,” Robotica, vol. 23, no. 1, pp. 13–20, 2005.
(b) Balance recovery from backward push Fig. 13.
The COMAN robot that balances with human shoes.
pitch angle of 4◦ inside the shoes. All the rest of the control parameters were the same. Our preliminary balance controller was made to create potential apexes by tilting feet actively, which was a preliminary attempt to prove the concept that mechanical energy is the balance indicator. Therefore, in the future, more sophisticated controllers that comply with this principle could be further designed for an optimal performance. V. C ONCLUSION Our study proves that both the proposed Energy based Fall Prediction (EFP) and Capture Point (CP) successfully predict the falling in advance, while the FRI does not in the experiments because of the light feet and limited resolution of the force/torque measurement in feet. Particularly, the EFP and the CP have similar prediction timing in both simulations and experiments. The time margins counting from the prediction to the entry of the divergent phase are 0.199s (simulation), 0.28s and 0.25 for the EFP, and 0.189s (simulation), 0.29s and 0.23s for the CP. Both the simulations and experiments 7
Author’s preprint version for academic circulation only. Please find official paper from IEEE Xplore.
[13] S. Yun and A. Goswami, “Momentum-based reactive stepping controller on level and non-level ground for humanoid robot push recovery,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2011, pp. 3943–3950. [14] S. Kajita, H. Hirukawa, K. Yokoi, and K. Harada, Humanoid Robots. Ohm-sha, Ltd., 2005. [15] J. Pratt, C. M. Chew, A. Torres, P. Dilworth, and G. Pratt, “Virtual Model Control: An Intuitive Approach for Bipedal Locomotion,” The International Journal of Robotics Research, vol. 20, no. 2, pp. 129– 143, Feb. 2001. [16] N. Tsagarakis, S. Morfey, G. Medrano-Cerda, Z. Li, and D. Caldwell, “Compliant humanoid coman: Optimal joint stiffness tuning for modal frequency control,” in IEEE International Conference on Robotics and Automation (ICRA), 2013, pp. 665–670. [17] D. N. Nenchev and A. Nishio, “Ankle and hip strategies for balance recovery of a biped subjected to an impact,” Robotica, vol. 26, no. 5, pp. 643–653, 2008. [18] Z. Li, C. Zhou, H. Dallali, N. Tsagarakis, and D. Caldwell, “Comparison Study of Two Inverted Pendulum Models for Balance Recovery,” in IEEE-RAS International Conference on Humanoid Robots, Madrid, Spain, November 18-20 2014, pp. 67–72.
8
