balance recovery of a planar biped using the foot ... · balance recovery of a planar biped using...
TRANSCRIPT
Balance recovery of a planar biped
using the Foot Placement Estimator
T.M. Assman
DC 2010.007
Open Space Project
Coach(es): dr. D. Kostic
Supervisor: prof.dr. H. Nijmeijer
Technische Universiteit EindhovenDepartment Mechanical EngineeringDynamics and Control Technology Group
Eindhoven, March, 2010
Contents
1 Introduction 4
2 The Foot Placement Estimator 52.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 The principle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.3 The Capture Point, an alternative method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3 A 2DOF biped with massless legs 73.1 Derivation of the model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2 Derivation of the FPE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
4 2DOF biped with mass at its legs 114.1 Modeling the biped . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
4.1.1 Unconstrained equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124.1.2 Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124.1.3 Constrained equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124.1.4 Impact . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134.1.5 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134.1.6 Simulation parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.2 FPE based on massless legs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144.2.1 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144.2.2 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4.3 FPE with leg mass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3.1 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3.2 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.4 FPE for unleveled terrain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.4.1 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.4.2 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
5 Conclusions and Future Work 23
2
Abstract
In literature, different methods define a point where an unbalanced robot should take a step in order to restorebalance. Often, these methods are based on simplified models, while their contributors claim that they are alsoapplicable on more complex bipeds. In this assignment the first objective is to carry out analysis of nonlinear dy-namics of a biped system in order to clarify the principle of the Foot Placement Estimator as a systematic approachto recover balance. The second objective is to contribute to the Humanoid Robotics project at the Eindhoven Uni-versity of Technology, by applying the obtained knowledge in simulation and creating an algorithm that can be usedfor experimental case studies. To get experience and gain analytic understanding, first a 2DOF biped with masslesslegs is considered. After successful implementation, it is tested on a biped with mass in the legs. Then, two moresophisticated estimators are proposed, that also take the leg mass into account, resulting in more accurate predictionof the FPE-angle.
3
Chapter 1
Introduction
Humanoid robotics is an emerging technology that will become part of our daily life in the coming decades [1], [2].Researchers and engineers are already developing humanoid robots that feature various human-like characteristics,since these robots should substitute people in a variety of tasks in industry, household, services, care, etc. As human-like appearance is the most obvious link between humans and humanoids, it is generally accepted that walking ontwo legs should be the underlying principle of humanoid locomotion. Humanoid robots that walk on two legs arecalled bipeds.
The Dynamics and Control Group of the Eindhoven University of Technology is involved in the Dutch initiative tocreate biped humanoid robots (site.dutchrobocup.com, humanoid.tue.nl). This initiative faces several research chal-lenges, such as control of a large number of robot degrees of freedom to ensure stable biped locomotion. To comeup with systematic control design that would enable safe robot operation of high autonomy, it is important to havesolid understanding of principles of biped locomotion. Given the desired path of the robot in the world coordinateframe, there is a challenge of distributing motions among the robot joints such as to ensure stable locomotion alongthis path. Here, stability is defined as preventing the robot from falling down, i.e., avoiding that any part of the robot,besides the soles of the feet, touches the ground. Stability of walking should be guaranteed even if external forces acton the biped. Having in mind that bipedal robots are nonlinear and under-actuated dynamical systems that featuredifferent modes depending on the number of contact points with the ground, robustness against disturbing forcescan practically be guaranteed only for forces that act in specific directions and are constrained in magnitude.
In this assignment, the principle of the Foot Placement Estimator will be studied as a method to ensure robustnessof the biped locomotion against the class of disturbing forces that are known as push forces. Robustness againstpush disturbances is called push recovery. If the robot is subject to the push, it should avoid a fall. The point wherethe robot should step in order to avoid the fall is called the Foot Placement Estimator point.
The report is organized as follows: In Chapter 2, the theory behind the Foot Placement Estimator is explained. Thistheory is used in Chapter 3 to find an expression for the FPE-angle assuming that the leg mass can be neglected.A simulation based on the inverted pendulum model shows that for a 2DOF biped with massless legs this FPE-angle is correct. Then a new 2DOF biped is simulated using Lagrange-Euler in Chapter 4, and when its leg mass isincreased, the original FPE-angle turns out to be less accurate. Therefore, two new methods are proposed and testedin simulation, which take the leg mass into account and even allow for the biped to walk on unleveled terrain. Resultsof this project will be beneficial for simulation, dynamical analysis, and experimental control of biped walking. Thesewill help us getting real humanoids walk in realistic environments, such as on rough terrains and in the presence ofdisturbing forces.
4
Chapter 2
The Foot Placement Estimator
2.1 Motivation
The most popular approach in the literature to achieve stability of bipedal locomotion is known as the "Zero MomentPoint" (ZMP). Introduced by Vukobratovic [3], this approach determines the nominal trajectories of the robot jointssuch as to maintain the ZMP within the support polygon. This polygon is a convex set of the points at the groundof the robot which depends on the number of contacts between the robot feet and the ground. The limitation ofthis approach is that if the ZMP moves to the edge of the support polygon or leaves the polygon completely, therobot tips over, which is according to the ZMP criterion a situation where the robot loses balance. In practice itis possible to restore balance if the robot can move the ZMP back to the stable region or if the robot takes one orseveral steps. Unfortunately, the ZMP criterion does not give a hint what movements of the robot are needed tomaintain balance. That is why in this assignment a systematic approach to restore balance is investigated, whichis applicable even if the ZMP leaves the support polygon. This approach is based on the principles of the FootPlacement Estimator (FPE) introduced by Wight [4]. Besides being less conservative than the ZMP method, the FPEapproach has potential to provide a more human-like movement of the robot, which is in line with the very ideaof designing TUlip, as the robot should mimic human-like locomotion as close as possible. Besides maintainingbalance, the considered approach can also be used for design of stable walking gaits, as well as for keeping balancewhile starting and stopping the walking.
2.2 The principle
The principle of the FPE method will now be explained. Consider the simplified planar model of a humanoid robotshown in Figure 2.1. It has two straight legs which are assumed massless and have no inertia. Each leg has a de-gree of freedom by rotating with respect to the torso, which is the only inertial element and modeled as a point mass.
In Figure 2.1a, a biped is standing on its left leg and has potential energy P1. A push is given, so that it gets kineticenergy K1 and starts to rotate around its left foot. When the right foot touches the ground, impact occurs (Figure2.1b). From the old velocity vector perpendicular to the old left stance leg vt1, the component parallel to the new rightstance leg vg is counteracted by the support forces in the ground. For a coefficient of restitution of zero (inelastic col-lision), the component vt2 perpendicular to the new right stance leg is the only velocity left. The amount of velocitychange is dependent on the angle between the legs β. When the angle β is set at a specific value called the FPE-angle, for that case in Figure 2.1c all kinetic energy is converted into potential energy (K3 = 0, P3 = P2 +K2), andthe potential energy is equal to the maximal possible potential energy when standing on the right leg (P3 = Ppeak).With the center of mass above its support foot and the velocity equal to zero, the biped is balanced but at an unstableequilibrium, Figure 2.1d. Another way of defining the leg angle β is specifying a point on the ground where theswing foot should be placed, called the FPE-point (shown in Figure 2.1b).
In Figure 2.1e, a smaller push than in Figure 2.1a is applied, with the same value for the leg angle β as in Figure 2.1b.This leg angle β is larger than the required FPE-angle for this situation, so in other words larger than necessary torecover from the push. The biped then steps over the FPE-point (Figure 2.1f). All kinetic energy is already convertedinto the potential energy (K3 = 0) before the peak potential energy is reached (P3 < Ppeak in Figure 2.1g). Sincethe center of mass is not exactly above its support foot, the biped falls back and eventually comes to rest on both feet(Figure 2.1h).
5
P3<Ppeak,K3=0 P4<Ppeak,K4=0P2+K2
FPEβ
P1+K1
P4<Ppeak,K4=0P3=Ppeak,K3>0P2+K2
FPEβ
P1+K1
P3=Ppeak,K3=0P2+K2 P4=Ppeak,K4=0
FPEβ
P1+K1vt2
vt1vg
a b c d
e f g h
i j k l
Figure 2.1: Different cases explain the FPE principle
In Figure 2.1i, a larger push than in Figure 2.1a is applied, so the biped steps before the FPE-point (Figure 2.1j).At the moment when the potential energy is at the maximum (equal to the peak potential energy in Figure 2.1k(P3 = Ppeak)), there is still kinetic energy left (K3 > 0), so the robot tips over and falls on the ground (Figure 2.1l).
Having in mind illustrations given in Figure 2.1, the principle of the Foot Placement Estimator can be formulated as:
"The angle between the legs should ensure that after impactthe sum of kinetic and potential energy is equal to the maximal possible potential energy"
or in terms of the variables used in Figure 2.1
"Find β such that the sum P2 +K2 is equal to Ppeak"
2.3 The Capture Point, an alternative method
An alternative method to recover balance from a push is proposed by Pratt [5]. This method introduces a pointknown as the "Capture Point" (CP). This point is similar to the FPE point, but it is determined based on a morecomplicated model. In this model, the torso is described as a flywheel which allows swing correcting of the torsoand in turn, instead of a single Capture Point, provides a region of points where the biped may step to recover froma push. Unfortunately, due to the limited time available for an Open-space project, this method was not furtherstudied.
6
Chapter 3
A 2DOF biped with massless legs
3.1 Derivation of the model
Figure 3.1: Parameters of the biped
A simple inverted pendulum model will now be presented, which is used to determine the position of the FPE. Thismodel is overtaken from the paper of Wight [4]. For now, the legs of the biped are assumed to be massless, so theirmotion will not cause additional dynamics to the system. Consequently, legs have no influence on the calculation ofthe FPE point. Later on in this report, it will be investigated if the legs do influence the FPE in the case when theyare not massless anymore while the mass of the robot torso is a dominant one.
Consider a simple biped with two straight legs of length L, point feet, body mass m, inertia about the center of massICOM , and leg separation angle β (Figure 3.1). The standing foot is pinned to the ground and the impact is assumednon-elastic and no slipping occurs.
This biped is modeled as an inverted pendulum first rotating about the point where the left foot makes contact withthe ground (point A in Figure 3.1). We begin with the Euler equation
∑τA = IAθA (3.1)
mgL sin(θA) = (ICOM +mL2)θA (3.2)(3.3)
θA =mgL sin(θA)ICOM +mL2
(3.4)
If the biped is rotating about the point where the right foot makes contact with the ground (point B in Figure 3.1),the equation becomes
θB =mgL sin(θB)ICOM +mL2
(3.5)
These two angles can be unified into a single coordinate (Figure 3.2), using
7
Figure 3.2: Relation between θa, θb and θ
θA = θ +β
2(3.6)
θB = θ − β
2(3.7)
(3.8)
The equations of motion become
θ =
mgL sin(θ + β/2)ICOM +mL2
θ < 0
mgL sin(θ − β/2)ICOM +mL2
θ > 0
mgL sin(θ + β/2)ICOM +mL2
θ = 0, θ < 0
mgL sin(θ − β/2)ICOM +mL2
θ = 0, θ > 0
0 θ = 0, θ = 0
(3.9)
Figure 3.3: Conservation of angular momentum
The initial velocity following an impact (change of equation) is found using conservation of angular momentum:
(HB)1 = (HB)2 (3.10)mLvt1 cos(β) + ICOM θ1 = mLvt2 + ICOM θ2 (3.11)
mL(Lθ1) cos(β) + ICOM θ1 = mL(Lθ2) + ICOM θ2 (3.12)
Which results in an expression of the angular velocity immediately following an impact (θ2) as function of theangular velocity just prior to impact (θ1)
θ2 =(L2m cos(β) + ICOM )θ1
L2m+ ICOM(3.13)
8
3.2 Derivation of the FPE
The FPE angle is the angle between the legs which lets that, after the impact, the total energy of the biped is equal tothe peak potential energy. When the kinetic energy is converted to potential energy such that it is equal to the peakpotential energy, there will be no kinetic energy left and the biped will be balanced. Starting point is the relationbetween energies according to the FPE principle:
K2 + P2 = Ppeak (3.14)
12(ICOM +mL2)θ22 +mgL cos(β/2) = mgL (3.15)
Using (3.13) to express the angular velocity after impact θ2 in terms of the angular velocity just before impact θ1,(3.15) can be re-written as:
1/2(ICOM +mL2)(mL2 cos(β) + ICOM )2θ21(ICOM +mL2)2
+mgL cos(β/2) = mgL (3.16)
(mL2 cos(β) + ICOM )2θ212(ICOM +mL2)
= mgL(1− cos(β/2)) (3.17)
2mgL(1− cos(β/2))(ICOM +mL2)(mL2 cos(β) + ICOM )2
− θ21 = 0 (3.18)
Because it is not known in advance what the angular velocity θ1 will be just prior to the impact, at each time instantthe current angular velocity is used to find the angle β. When the impact does occur, the current angular velocitywill be equal to θ1, and the leg angle β will be properly set.
9
3.3 Simulation results
The equations of motion from (3.9) are implemented in Matlab and numerically integrated using the ode45 solver.In simulations, the leg angle is fixed and initially chosen that the biped steps exactly on the FPE point. To get moreinsight into the FPE method, all important phases during simulation will now be discussed in detail.
(a) t = 0θ = −0.42 θ = 1.83
(b) t = 0.209θ = 0 θ = 2.30
(c) t = 1.319θ = 0.474 θ = 0
(d) t = 3.549θ = 0 θ = 0
Figure 3.4: Configurations of the biped shown at different time instants during simulation, where the stancelegis shown in red and swing leg is shown in green, the black line shows the orientation of the torso and the blackdot marks the FPE point
0 0.5 1 1.5 2 2.5 3 3.5 40
1
2
3
4
5
6
7
8
9
10
time [s]
Ene
rgy
[J]
KinPotPotpeak
(a) Energy balance
0 0.5 1 1.5 2 2.5 3 3.5 4−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
time [s]
angl
e [ra
d] o
r ang
ular
vel
ocity
[rad
/s]
theta [rad]thetadot [rad/s]legangle [rad]FPEangle [rad]
(b) The states of the biped
Figure 3.5: If the legangle of the biped with massless legs is equal to the FPE-angle in (b) when impact occurs,then the potential energy reaches the peak potential energy with zero kinetic energy in (a) as desired
At t=0 [s]:In Figure 3.4a the biped is standing on its left leg, and gets an initial angular velocity.In Figure 3.5a, in the first period, potential energy is converted into kinetic energy.In Figure 3.5b, θ increases because the biped is accelerating.
At t=0.209 [s]:In Figure 3.4b impact occurs and the biped steps on the FPE point.In Figure 3.5a, there is a drop in kinetic energy, and the potential energy reaches a minimum.In Figure 3.5b, θ is equal to zero, and θ drops due to energy dissipated into the ground.For the leg angle a constant is chosen in advance, such that it is exactly equal to the FPE angle when impact occurs.
At t=1.319 [s]:In Figure 3.4c the biped stands upright on its right feet.In Figure 3.5a, the kinetic energy is equal to zero and the biped has reached its maximum potential energy.In Figure 3.5b, θ is equal to zero.
At t=3.549 [s]:In Figure 3.4d the biped is at rest at both feet.In Figure 3.5a, the kinetic energy is zero and the biped has its minimum potential energy.In Figure 3.5b, θ and θ are both equal to zero. Because at every subsequent impact θ is smaller than at the previousimpact, the needed FPE angle will also become smaller and will not exceed the leg angle anymore.
10
Chapter 4
2DOF biped with mass at its legs
4.1 Modeling the biped
A biped with two actuated degrees of freedom and leg masses is now considered. The parameters and states aredefined in Figure 4.1. The position of the top of the biped with respect to the world frame is expressed by statesx and y. The orientation with respect to the right handed world frame is expressed by θ1. The legs are actuatedseparately, and the angles θ2 and θ3 are defined with respect to the torso as shown in this figure. The length of thetorso, left leg and right leg are l1, l2 = l3, respectively. The center of mass of the torso m0 is located at a distancec1 from the top. The position of the centers of mass of the legs are defined at distances c2 = c3 from the hip. Thegravity vector ~g points in the negative y direction.
+θ1
-θ2
+θ3
c1
c3
c2
l1
l2
l3
(x,y)
p0
p1
p2
y
x
z
g
Figure 4.1: Definition of the parameters and states
11
4.1.1 Unconstrained equations of motion
The positions of the centers of mass expressed in the states are
p0 =[x+ c1 sin(θ1)y − c1 cos(θ1)
](4.1)
p1 =[x+ l1 sin(θ1) + c2 sin(θ1 + θ2)y − l1 cos(θ1)− c2 cos(θ1 + θ2)
](4.2)
p2 =[x+ l1 sin(θ1) + c3 sin(θ1 + θ3)y − l1 cos(θ1)− c3 cos(θ1 + θ3)
](4.3)
The velocities v0x, v0y, v1x, v1y, v2x, v2y are computed by differentiating these positions with respect to time. Thekinetic and potential energies are found by the relations
K =12(m0(v2
0x + v20y) +m1(v2
1x + v21y) +m2(v2
2x + v22y) + I0θ
21 + I1θ
22 + I2θ
23) (4.4)
P = m0gp0y +m1gp1y +m2gp2y (4.5)
By defining
q = [x, y, θ1, θ2, θ3]T (4.6)q = [x, y, θ1, θ2, θ3]T (4.7)q = [x, y, θ1, θ2, θ3]T (4.8)
The Lagrangian unconstrained equations of motion are given by [6] [7]
d
dt(K, q)−K, q + P, q = Qnc (4.9)
These are rewritten to
Mq + Cq +G = τ (4.10)
4.1.2 Constraints
When constraints are active, the feet cannot go through the ground and cannot slip sideways.The positions of the feet are given by
h1 =[x+ l1 sin(θ1) + l2 sin(θ1 + θ2)y − l1 cos(θ1)− l2 cos(θ1 + θ2)
](4.11)
h2 =[x+ l1 sin(θ1) + l3 sin(θ1 + θ3)y − l1 cos(θ1)− l3 cos(θ1 + θ3)
](4.12)
The constraint matrices follow as
h = [h1x, h1y, h2x, h2y]T (4.13)W = h, q (4.14)w = ((Wq), q)q (4.15)
4.1.3 Constrained equations of motion
By defining
H = Cq +G; (4.16)
and computing the Lagrange multipliers12
λ = (WTM−1W )−1(WTM−1(H − τ)− w) (4.17)
the constrained equations of motion are found by
q = M−1(τ −H +Wλ); (4.18)
4.1.4 Impact
By again computing Lagrange multipliers [7] [8]
λ = −(WTM−1W )−1WT qmin (4.19)
the velocities after impact become
qplus = M−1(Wλ+Mqmin) (4.20)
4.1.5 Controller
A simple PD controller is implemented for actuation of θ2 and θ3, with proportional gains Kp1 and Kp2 and deriva-tive gains Kd1 and Kd2
τ =
000
Kp1(θ2ref − θ2) +Kd1(θ2ref − θ2)Kp2(θ3ref − θ3) +Kd2(θ3ref − θ3)
(4.21)
The angle θ3ref is the computed FPE angle plus a very small constant of 0.02, this is just enough to ensure thatwhen θ3 = θ3ref , after balancing at the peak configuration it will fall backward instead of tip over.
4.1.6 Simulation parameters
The simulations described in the next sections all use the constant parameters from Table 4.1 to make comparisonpossible
c1 0.5 [m]c2 0.5 [m]c3 0.5 [m]l1 0.5 [m]l2 1 [m]l3 1 [m]I0 0.1 [kgm2]I1 0.1 [kgm2]I2 0.1 [kgm2]g 9.81 [m/s2]
m0 10 [kg]
Table 4.1: Biped parameters used in all simulation case-studies
Notice that c1 = l1, which means the center of mass of the torso lies in the hip. By exciting the x state during thevery short time interval from t = 0[s] to t = 0.01[s] by a force of 500 Newton, an initial push is simulated to bringthe biped into motion.
13
4.2 FPE based on massless legs
In this section it is examined if the original FPE method with the assumption of massless legs is still applicable inthe case when the legs do have mass.
4.2.1 Derivation
θ3
q* , q*
(a) Configuration A
θ3ref = fpe(θ1*)
θ3ref
(b) Configuration B
θ3ref
K=0, P=Ppeak
(c) Configuration C
Figure 4.2: Using m0 and θ1 from config A to predict impact in config B and to reach config C
In Figure 4.2, when the biped is rotating around its left leg in configuration A, it is not yet known in advance whatthe exact velocities of the biped will be just before impact. Therefore, at every time instant t∗ the angular velocity θ1∗from configuration A is used as an approximation for the angular velocity that the biped will have in configurationB just before impact. From configuration B the method looks for a value θ3ref that guarantees that after impact, thebiped will reach the peak potential energy with zero kinetic energy in configuration C. In this energy balance onlythe mass in the torso m0 is considered, and the masses in the legs m1 and m2 are neglected.
The energy balance (3.18) derived in section 4.2 needs to be solved to find θ3ref , here repeated in terms of the vari-ables for the 2DOF biped depicted in Figure 3.1 and 3.2
2mgL(1− cos(β/2))(ICOM +mL2)(mL2 cos(β) + ICOM )2
− θ2 = 0 (4.22)
In terms of the variables for the 2DOF biped depicted in Figure 4.1, with θ2 locked at zero, (4.22) is now rewritten as
2m0gl2(1− cos(θ3/2))(I0 +m0l22)
(m0l22 cos(θ3) + I0)2− θ21 = 0 (4.23)
14
4.2.2 Simulation results
Small leg mass
(a) t = 0 (b) t = 0.15858 (c) t = 1.0086 (d) t = 2.1803
Figure 4.3: Configurations during simulation
0 0.5 1 1.5 2 2.50
50
100
150
200
250
time [s]
ener
gy [J
]
PotKinPotpeak
(a) Energy balance
0 0.05 0.1 0.15 0.2 0.25 0.30.6
0.7
0.8
0.9
1
1.1
1.2
1.3
time [s]
angl
e [ra
d]
θ3
θ3ref
(b) Setpoint tracking
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2−1
−0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
time [s]
posi
tion
[m] o
r ang
le [r
ad]
x [m]
y [m]
θ1 [rad]
θ2 [rad]
θ3 [rad]
(c) The states
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2−10
−5
0
5
10
15
20
25
time [s]
velo
city
[m/s
] or a
ngul
ar v
eloc
ity [r
ad/s
]
x [m/s]
y [m/s]
θ1 [rad/s]
θ2 [rad/s]
θ3 [rad/s]
(d) Time derivatives of the states
Figure 4.4: If the legangle θ3 is equal to the FPE-angle in (b) when impact occurs and the leg mass is insignificant,then the potential energy reaches the peak potential energy with zero kinetic energy in (a) as desired
In this simulation, in addition to parameters given in Table 4.1, the parameters m1 = m2 = 0.01 are used, withgains Kp1 = Kp2 = 65000 and Kd1 = Kd2 = 2000 to achieve satisfactory setpoint tracking. In Figure 4.3 theinitial-, impact-, peak- and end-position are shown. When the kinetic energy is zero, indeed the potential energy hasreached its maximum possible state in Figure 4.4a. This is again confirmed in Figure 4.4b where the angle betweenthe legs θ3 converges to the desired reference value. Figure 4.4c shows the states from the initial configuration untiljust after impact. In Figure 4.4d the changes in velocities due to impact can be studied. If the masses of the legs arenegligible relative to the torso mass, the original FPE method computes the FPE angle correctly.
15
Legs with significant masses
(a) t = 0 (b) t = 0.1707 (c) t = 0.55 (d) t = 1.0586
Figure 4.5: Configurations during simulation
0 0.5 1 1.5 2 2.50
50
100
150
200
250
time [s]
ener
gy [J
]
PotPotold
KinKinold
Potpeak
Potpeak−old
(a) Energy balance
0 0.2 0.4 0.6 0.8 10
2
4
6
8
10
12
time [s]
ener
gy [J
]
Potm1m2
(b) Potential energy of legs mass
0 0.2 0.4 0.6 0.8 10
5
10
15
20
25
30
35
40
45
50
time [s]
ener
gy [J
]
Kinm1m2 push
Kinm1m2 tracking
Kinm1m2 impactKinm1m2 fix
Kinm1m2 after impact
(c) Kinetic energy of legs mass
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2
0
2
4
6
8
10
12
time [s]
angu
lar v
eloc
ity [r
ad/s
]
θ3 push
θ3 tracking
θ3 impact
θ3 fix
θ3 after impact
(d) Influence of controller
Figure 4.6: Because the potential energy (b) and kinetic energy (c) of the leg masses are not taken into account,the FPE-angle is less accurate and the potential energy does not reach the peak potential energy in (a)
In this simulation, in addition to parameters given in Table 4.1, the masses of the legs are increased tom1 = m2 = 1,and gains Kp1 = Kp2 = 55000 and Kd1 = Kd2 = 2000 are chosen to meet the desired control performance. InFigure 4.5c the biped does not reach its highest possible configuration, which is also visible in Figure 4.6a where thesituation with and without significant leg mass can be compared.
The original method neglects masses of the legs, but when these masses become significant, they do have influenceon all three terms in the energy balance equation of (3.14), namely:
1. The kinetic energy term in (3.14) for m1 and m2
(a) Because a push force is applied on the biped at the beginning of the simulation, the whole biped gets16
angular velocity θ1 about the stance foot, thereby also creating a step in the kinetic energies of the massesm1 and m2 (shown by the blue line segment in Figure 4.6c)
(b) Because the controller applies a torque at the leg to track the reference, the leg gets angular velocity θ3with respect to the torso (shown by the green line segment in Figure 4.6d), creating a peak in the kineticenergy for the swing leg of mass m2 (shown by the green line segment in Figure 4.6c)
(c) Because the ground reaction forces act at the biped during impact, both the angular velocity of the wholebiped θ1 and a part of what is left of the swing leg angular velocity θ3 (shown by the red line segment inFigure 4.6d) are slowed down, causing a drop in the kinetic energy of the masses m1 and m2 (shown bythe red line segment in Figure 4.6c)
(d) Because the controller applies a torque at the swing leg to fix the leg angle after impact, the angularvelocity of the swing leg with respect to the torso θ3 is reduced to zero (the cyan line segment in Figure4.6d), causing a small drop in the kinetic energy for the swing leg mass m2 (shown by the cyan linesegment in Figure 4.6c)
(e) Because of gravity forces acting on the whole biped, the kinetic energy left after impact is converted intopotential energy and, when θ1 = 0, back again into kinetic energy (the purple line segment in Figure4.6c)
2. The potential energy term in (3.14) for m1 and m2, shown in Figure 4.6b
3. The peak potential energy term in (3.14), which should now not only include the potential energies of m1 andm2, but is also dependant on the angle between the legs θ3
When the leg masses become significant, it appears that the original FPE method becomes less accurate.
17
4.3 FPE with leg mass
In this section a more accurate method for computing the FPE angle is derived by also considering the kinetic andpotential energy of the masses in the legs. It predicts what the FPE angle at the impact configuration should be, ifactual velocities at this configuration would be known. Then this method is examined in a simulation.
4.3.1 Derivation
θ3
q* , q*
(a) Configuration A
θ3ref
θ3ref = fpe(qimpact,θ1*,θ2*,θ3*)
(b) Configuration B
θ3ref
K=0, P=Ppeak
(c) Configuration C
Figure 4.7: Using m0, m1 and m2 and angular velocities from config A to predict impact in config B
So a more detailed computation of the FPE point is desired. The suggested algorithm still tries to find a value for θ3until the energy after impact is balanced as
K2 + P2 = Ppeak (4.24)
To compute these quantities, the states q and velocities q from configuration A in Figure 4.7a should be transformedto configuration B by taking the following steps for each θ3:
• θ1 is computed for configuration B such that the feet are on the ground, by solving the equation
cos(θ1 + θ2)− cos(θ1 + θ3) = 0 (4.25)
• y then follows from θ1, (x does not play a role in this analysis)
y = l1 cos(θ1) + l2 cos(θ1 + θ2) (4.26)
• θ1 from configuration A is directly used in configuration B, and new resulting x and y are generated from
x = − cos(θ1)(l1 + l2)θ1 (4.27)y = − sin(θ1)(l1 + l2)θ1 (4.28)
The velocities after impact not only depend on the impact, but also the effect of fixing the leg by the controllersshould be predicted as seen in Figure 4.6c. This is done by the holonomic constraints on θ2 and θ3
h1 = θ2 (4.29)h2 = θ3 (4.30)
(4.31)18
resulting in the constraint matrices which are added to the constraint matrices in (4.14) and (4.15)
h =[h1
h2
](4.32)
W = h, q (4.33)
=[0 0 0 1 00 0 0 0 1
](4.34)
w = ((Wq), q)q (4.35)
=[00
](4.36)
to compute the velocities after impact with (4.19) and (4.20). From these velocities the kinetic energyK2 after impactis found using (4.4).
The potential energy is constant during the impact and P2 for configuration B is obtained from (4.5).
The peak potential energy is computed by finding the angle θ1 for which the potential energy has a maximumdetermined from (4.5) using y as given below to ensure that the right foot touches the ground
y = l1 cos(θ1) + l3 cos(θ1 + θ3) (4.37)
19
4.3.2 Simulation results
(a) t = 0 (b) t = 0.16597 (c) t = 0.90597 (d) t = 1.953
Figure 4.8: Configurations during simulation
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
50
100
150
200
250
300
time [s]
ener
gy [J
]
PotKinPotpeak
(a) Energy balance
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.20.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
1.5
1.6
time [s]
angl
e [ra
d]
θ3
θ3ref
(b) Setpoint tracking
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2−1
−0.5
0
0.5
1
1.5
2
2.5
3
3.5
time [s]
posi
tion
[m] o
r ang
le [r
ad]
x [m]
y [m]
θ1 [rad]
θ2 [rad]
θ3 [rad]
(c) The states
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2−10
−5
0
5
10
15
20
25
time [s]
velo
city
[m/s
] or a
ngul
ar v
eloc
ity [r
ad/s
]
x [m/s]
y [m/s]
θ1 [rad/s]
θ2 [rad/s]
θ3 [rad/s]
(d) Time derivatives of the states
Figure 4.9: If the legangle θ3 is equal to the FPE-angle based on legs with mass in (b) when impact occurs,then the potential energy reaches the peak potential energy with zero kinetic energy in (a) as desired
In this simulation, in addition to parameters given in Table 4.1, the masses of the legs are unchanged at m1 = m2 =1, and gains Kp1 = Kp2 = 65000 and Kd1 = Kd2 = 2000 are chosen to meet the desired control performance.With lower gains, a rapid leg correction could create backlash on the torso, even causing θ1 to change sign for a timeinstant, giving trouble to the FPE algorithm which assumes forward rotation. Figures 4.8c and 4.9a show that thismethod is more accurate and that the peak potential energy is indeed reached. But because of the initial big errorin Figure 4.9b, a big peak in θ3 is visible in Figure 4.9d, resulting in a tall sharp peak of kinetic energy in the first0.2 seconds in Figure 4.9a. Because θ1 is almost constant before impact in Figure 4.9d, the θ1 at configuration A inFigure 4.7 is indeed a good estimate for the θ1 just prior to impact in configuration B of Figure 4.7. In this methodit is assumed that the ground is flat and horizontal, by assuming in (4.25) that at the moment of impact both feetare at y = 0. In the next section a method is presented which removes this restriction and works for a more generalcase, allowing the ground to be at a certain slope or stepping on an obstacle, which does not need to be known inadvance.
20
4.4 FPE for unleveled terrain
In this section a less restrictive method for computing the FPE angle is derived which can be used at unleveledterrain, and predicts what the FPE angle should be when impact would occur at the next time instant with thecurrent configuration at the swing foot position. This method is then tested in a simulation.
4.4.1 Derivation
θ3
q* , q*
(a) Configuration A
θ3ref = fpe(q*,q*)
θ3ref
yo�set
(b) Configuration B
yo�set
θ3ref
K=0, P=Ppeak
(c) Configuration C
Figure 4.10: By changing only θ3 in configuration A, the impact is predicted at the corresponding foot position
The algorithm still tries to find values for θ3 for which the relation holds
K2 + P2 = Ppeak (4.38)
The current configuration A and current velocities in Figure 4.10a are equal to configuration B, except that the swingleg is at angle θ3ref , and the impact occurs at the new position of the swing foot, as if it steps on a platform in the air.When the biped reaches a configuration where an impact with the ground occurs, then configuration B will becomethe ground impact configuration.
The rest of the steps is equal to the steps taken after (4.28), except that instead of (4.37), we use here
y = yoffset + l1 cos(θ1) + l3 cos(θ1 + θ3) (4.39)
21
4.4.2 Simulation results
(a) t = 0 (b) t = 0.1490 (c) t = 1.1090 (d) t = 2.4119
Figure 4.11: Configurations during simulation
0 0.5 1 1.5 2 2.50
50
100
150
200
250
time [s]
ener
gy [J
]
PotKinPotpeak
(a) Energy balance
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.20.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
1.5
1.6
time [s]
angl
e [ra
d]
θ3
θ3ref
(b) Setpoint tracking
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2−1
−0.5
0
0.5
1
1.5
2
2.5
3
3.5
time [s]
posi
tion
[m] o
r ang
le [r
ad]
x [m]
y [m]
θ1 [rad]
θ2 [rad]
θ3 [rad]
(c) The states
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2−10
−5
0
5
10
15
20
25
time [s]
velo
city
[m/s
] or a
ngul
ar v
eloc
ity [r
ad/s
]
x [m/s]
y [m/s]
θ1 [rad/s]
θ2 [rad/s]
θ3 [rad/s]
(d) Time derivatives of the states
Figure 4.12: If the legangle θ3 is equal to the FPE-angle for unleveled terrain in (b) when impact occurs at theobstacle, then the potential energy reaches the peak potential energy with zero kinetic energy in (a) as desired
In this simulation, in addition to parameters given in Table 4.1, the masses of the legs are unchanged at m1 = m2 =1, and gains Kp1 = Kp2 = 59000 and Kd1 = Kd2 = 2000 are chosen to meet the desired control performance. Anobstacle of height of 0.1 m height is placed under the right foot, and is considered unknown during computation ofthe FPE. The FPE angle is still accurate, which is confirmed by the upright leg in Figure 4.11c and the kinetic andpotential energies in Figure 4.12a. The initial error between the leg angle θ3 and the desired θ3ref is much smallerin Figure 4.12b, causing a lower peak in θ3 in Figure 4.12d than the peak from the previous simulation shown inFigure 4.9d.
22
Chapter 5
Conclusions and Future Work
The studied Foot Placement Estimator method calculates the angle between the legs of a planar biped such as toguarantee that the biped reaches a balanced posture after being subjected to a push force. This method, as originallyformulated in [4], ignores the masses of the legs. In this report, we show that the original formulation of the FPEmethod may result in inaccurate prediction of the FPE point if the leg masses are not negligible relative to the massof the torso. We show that legs with masses introduce additional kinetic and potential energies that must be takeninto account when calculating the FPE point.
Two strategies proposed in this report take the kinetic and potential energies of the biped leg masses explicitly intoaccount. The first method assumes a leveled ground and always predicts impact for the same static posture whenthe both feet are on a flat horizontal surface of equal height. The second method allows for an unleveled groundand at each time instant predicts impact as if it would occur in its current configuration, at the location of the swingfoot when the swing leg is at the desired angle. We show in simulations that both methods give more accuratepredictions of the FPE points than the original one.
Experimental case studies are needed to clarify what is the best approach to compute the FPE point. The experiencefrom this project shows that computation time increases enormously with the new methods: it takes Matlab 104 sec-onds to compute the FPE angle for a simulation time period of 0.1 seconds on a 2.8 GHz computer. In the conductedsimulations, high gains were needed to guarantee good tracking performance. Although these might raise concernsabout practical applicability of the proposed methods, experimental evaluation still seems important and interesting.
In the method predicting the impact with the ground, the FPE angle was almost constant. So it needs to be investi-gated if only computing this FPE angle at the beginning and by adding a safe margin, the FPE angle will be sufficientat the moment of impact to prevent the biped from tipping over.
In the future, a walking gait should be simulated on a biped with knee and ankle joints. Also different controlalgorithms like the model with a flywheel [5] need to be studied, which can also be useful for sideways stabilization.Then this 2D stabilization method should be expanded and made applicable in 3D, for implementation on the robotNAO, and, eventually, on the robot Tulip (Figure 5.1).
(a) NAO (b) TULIP
Figure 5.1: The robots
23
Bibliography
[1] B. Choi, “Humanoid robots,” In-Tech, 2009.
[2] B. Siciliano and O. Khatib, Handbook of Robotics. Springer, 2008.
[3] M. Vukobratovic and B. Borovac, “Zero-moment point - thirty five years of its life,” Int. J. Humanoid Robotics,vol. 1, 2004.
[4] D. Wight, E. Kubica, and D. Wang, “Introduction of the foot placement estimator: A dynamic measure of balancefor bipedal robotics,” J. Comput. Nonlinear Dynam., vol. 3, 2008.
[5] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” Pro-ceedings of the 2006 IEEE-RAS International Conference on Humanoid Robots, Genoa, Italy, 2006.
[6] N. v. d. Wouw, “Multibody dynamics, lecture notes,” Eindhoven University of Technology, 2007.
[7] P. van Zutven, “Modeling, identication and stability of humanoid robots, with a case study on humanoid robottulip,” master thesis, Dynamics and Control Group, Dep. Mech. Eng., Technische Universiteit Eindhoven, 2009.
[8] M. Bachelier, A. Chemori, and S. Krut, “A control law for human like walking biped robot sherpa based on a con-trol and a ballistic phase - application on the cart-table model,” IEEE-RAS International Conference on HumanoidRobots, 2008.
24