Real time motion generation and control for biped robot -3rd report Dynamics error compensation
Real time motion generation and control for biped robot -3rd report Dynamics error compensation
1595
Only the third FF method is implemented in our system, but the Approximate
Detailed dynamics
model + xbody _ mdfd
other two FF methods are used for the ease of explaining. dynamics model xbody _ cmd F full + Fmdfd
Inverted +
A. Feedforward Dynamics Error Compensation without θ body _ cmd M full pendulum model
x pend
Ground Reaction Force Limits - Fpend
Pendulum Stability +
Fig. 4 is the block diagram of the first compensation method control
M pend _ stab M mdfd
which ignores the ground reaction force limits. This is
equivalent to Kajita et al. [7] except that we do not use preview Fig. 4 Feedforward dynamics error compensation
control. without ground reaction force limits
The desired ZMP trajectory x zmp , feet trajectory and upper body
xbody _ mdfd
position xbody _ cmd and inclination θ body _ cmd are designed using an +
+
xbody _ cmd F full Fmdfd
approximate dynamics model. These are transformed into +
θ body _ cmd M full x pend
desired angle for each joint through inverse kinematics on the
- Fpend
detailed dynamics model. Now, the moment about the desired
M pend _ stab Pendulum Stability +
M
ZMP, M full ,and the ground reaction force, F full can be control
ltd
pend _ stab z −1
Z −1 Fbias +
-
computed. M full is caused due to the difference between the
Fpend _ stab + unltd
Ftotal Ftotal
ltd z −1 M mdfd
1 -
two dynamics models and thus ideally is 0. The goal of the h + +
h
( = M ltd
pend _ stab )
dynamics compensation is to make M full to 0 using the inverted
Fig. 5 Feedforward dynamics error compensation with
pendulum model introduced earlier. This can be achieved by ground reaction force limits in discrete time
substituting − M full into M pend in Eq. (1) and adding resulting
x pend into xbody _ cmd . This usually causes the inverted pendulum B. Feedforward Dynamics Error Compensation with
Ground Reaction Force Limits
to diverge. To prevent it, a stabilizing control which outputs
Fig. 5 is the block diagram of the second compensation method,
M pend _ stab is enhanced to make x pend 0. The stabilizing
an extension from the first method to satisfy the ground reaction
controller can be a simple PD controller for example. An force limits.
example controller for running motion is introduced in [12]. According to Eq. (1)(2), if M ltd is increased by
pend _ stab ( = M mdfd )
The sum of requested dynamics compensation moment and
∆M ltd , then Fmdfd increases by ∆M ltd pend _ stab h
.
pendulum stabilizing moment, (− M full + M pend _ stab ) , is then pend _ stab
1596
+ xbody _ mdfd From Eq. (9)(10)(12),
+
xbody _ cmd F full Fmdfd M mdfd = M ltd
pend _ stab + M wheel _ stab
ltd (13)
θ body _ cmd M full x pend
+
Note that Fmdfd is obtained from Eq. (6).
Fpend
- In case the ground reaction force and horizontal moment do not
Pendulum
M pend _ stab
Stability
+
exceed the limits, the expected values of Fmdfd and M mdfd ,
control Z −1 Fbias +
-
unltd
( Ftotal unltd
, M total ) , can be computed as shown in Fig. 9. These
Fpend _ stab + Ftotal
unltd
Ftotal
ltd M mdfd
1 - KF
h values are input to the limiter which modifies them to Ftotal
ltd
and
h + + s Fpend
ltd
_ stab
M total so that they do not violate admissible values. The details
ltd
Fig. 6 Feedforward dynamics error compensation with
ground reaction force limits in continuous time of how the limiter works are explained in the next section.
Note that M ltd
pend _ stab
is computed in the same way as in the
+ xbody _ mdfd
xbody _ cmd F full +
Fmdfd second method and M wheel
ltd
_ stab
is computed as follows.
+ +
θ body _ cmd M full x pend
ltd
M wheel _ stab = M total − M pend _ stab
ltd ltd (14)
- Fpend The modified gait pattern has admissible values of Fmdfd and
+
M mdfd by designing M ltd
pend _ stab
and M wheel
ltd
_ stab
as explained
above.
- KF M mdfd
Fmax h + + xbody _ mdfd
+ s
+ + + θ
Fig. 7 Feedforward dynamics error compensation with xbody _ cmd F full +
body _ mdfd
+ Fmdfd
ground reaction force limits for Ftotal
unltd
exceeding Fmax +
θ body _ cmd M full
x pend
Note that M ltd
pend _ stab
is the desired ground reaction moment of M ltd - Fpend
pend _ stab
1597
D. Ground Reaction Force and Momentum Limiter dynamics model and the detailed dynamics model using an
The rectangle in Fig. 10 is the admissible range of the ground inverted pendulum and a flywheel as dynamics compensation
reaction force and moment. The limiter outputs the input models. However, the integrated dynamics of the approximate
(Ftotal
unltd unltd
, M total ) without modification if its coordinate is inside dynamics model, inverted pendulum model and flywheel model
do not strictly match that of the detailed dynamics model.
this rectangle. On the other hand, if the point (Ftotal
unltd unltd
, M total ) lies Fig. 11 is the block diagram for the FB method. The motions of
outside the triangle, M wheel _ stab is modified in such a way that
ltd
dynamics error models, an inverted pendulum and a flywheel,
ltd
M wheel becomes as close as possible to M wheel _ stab . Inputting are added to the original gait pattern and input to the detailed
_ stab
dynamics model. On the other hand, the motions of the
moments which differs from M wheel _ stab by large amount causes
dynamics error models in the FF methods are not input to the
the flywheel to diverge from its original state thus leading the detailed dynamics models. The desired ground reaction
upper body of the robot to incline largely. As shown in Fig. moment of the modified gait pattern, M mdfd , is set to
10(a), a line which goes through (Ftotal ) and has (M _ stab )
unltd unltd
, M total .
ltd
pend _ stab + M wheel
ltd
+
+ +
+ M mdfd bending dynamics. Fig. 12(a) is used to generate the original
Limiter
& Distributor lmtd
+
xwheel
gait pattern, Fig. 12(c) is the detailed dynamics model of our
M wheel M wheel
_ stab
θ wheel real robot and the Fig. 12(b) is the telescopic model. The
M pend _ stab
Pendulum Stability control
telescopic model has thigh, shin and upper body links. Each of
M wheel _ stab the links has the same mass and inertia as those of the
Wheel Stabilitycontrol
corresponding link of the detailed dynamics model. The CoG’s
of the thigh and shin links are located on the line which
Fig. 11 Feedback dynamics error compensation
connects the hip joint and ankle joint.
Note that the GoG of the upper body link of the gait pattern
E. Feedback Dynamics Compensation
designed with the approximate dynamics model is x s b and the
The previous section explained methods to modify gait patterns
angle from vertical line is θ s b . Also, the CoG of the thigh, shin
to compensate for the dynamics error between an approximate
and upper body links of the detailed dynamics model are
1598
θ cb
located at x c t [i ] , x c s[i ] and x c b ([i] is either [R] or [L] for left θ tb
and right leg respectively). The angles of each link are θ c t [i ] , hcb hc b
and θ t b . The vertical CoG of each link are hc t , hc s and h c b Fig. 13 Translational and rotational knee dynamics
respectively and assumed to be constant during motion. These compensation
values are determined from a standard upright posture of the
robot in which the knees are bent at some angle. The position of VI. RESULTS
the ankle joint of the detailed dynamics model and telescopic We conducted experiments using a biped robot who is 130 cm
model are identical to that of the approximate dynamics model. tall. Each leg of the robot has 6 degrees of freedom. Fig. 14 is
Note also that xt b and θ t b are identical to x s b and θ s b the moment generated about the desired ZMP trajectory
respectively. transformed into equivalent ZMP error while walking at 4 km/h.
We solve the following two kinematic constraints iteratively for As seen, the motion generated from the approximate dynamics
x c b and θ c b to compensate for the knee dynamics. x c b and θ c b error without compensation generates maximum error of 110
are the modified upper body position and angle respectively. mm(A). With the knee dynamics compensation, the maximum
error decreases to 90 mm(B). Furthermore, with the FF method
mb ( x c b − xt b ) + mt ∑ x c t [i ] − xt t [i ] ( ) following the knee dynamics error compensation and the FB
i
method, the maximum error is less than 20 mm. The result
(
+ ms ∑ x c s[i ] − xt s[i ] = Const. ) (15)
confirms that proposed methods successfully compensates for
i
mb h c b × ( x c b − x t b ) dynamics error of the approximate dynamics model and realizes
(
+ mt ∑ h c t [ i ] × ( x c t [ i ] − x t t [i ] ) ) motion with large margin of stability.
Fig. 15 shows the behavior of the inverted pendulum and
i
+ m ∑ (h s
c
s[ i ] × ( x c s[ i ] − x ) ) + I (θ − θ )
t
s[ i ] b
c
b
t
b
flywheel dynamics error compensation models with and
i without knee dynamics compensation while running at 6km/h.
+ I ∑ (θ
t
c
t[i ] − θ t t[i ] ) + I ∑ (θ − θ ) = Const.
s
c
s[ i ]
t
s[ i ] (16) Without the knee dynamics compensation, both dynamics error
i i compensation models diverge. With the knee dynamics, no
Regarding the difference of horizontal positions of each mass as divergence is observed. The result indicates effectiveness of the
velocity, Eq. (15) means that the total moment of the masses proposed knee dynamics error compensation technique. It also
stays constant (Fig. 13(a)(b)). Eq. (16) refers to constant total indicates that there is a limit on how much dynamics error the
angular momentum (Fig. 13(c)). From these kinematic feedforward and feedback compensation techniques can
computations, the dynamics error due to the knee bending compensate for as explained in previous sections.
motion is decreased. 120
(A)Dynamics model with three mass
(a) (b) (c)
θ t
b θ c
b 80
Ib mb (C)FF type
mb
xcb
xt b 40
ZMP error [mm]
θ t t [[ii ] It θ c t [i ]
mt
x t t [i ] x c t [i ] 0
ms
Is
x t s[[ii ] x c s[[ii ] -40
θ t s[[ii ] θ c s[i ] (D)FB type
(B)Knee bend
-80
Fig. 12 Telescopic model and knee dynamics model
-120
Double support phase (80) Single support phase (320) Time [ms
1599
60
Another method to compensate for the motion of knees which
our approximate dynamics model did not have is introduced. It
Pendulum position [mm]
FB type without knee bend FB type with knee bend
1600