Robust Biped Walking with Active Interaction Control ... - CiteSeerX
Proc. of the 1998 IEEE Int. Conf. on Robotics & Automation, Leuven, Belgium - May 1998
2030
Robust Biped Walking with Active Interaction Control between Foot and Ground Yasutaka Fujimoto
Satoshi Obata
Dept. of System Design Eng. Keio University Yokohama 223 JAPAN [email protected]
Atsuo Kawamura
Dept. of Elec. & Comp. Eng. Yokohama National University Yokohama 240 JAPAN [email protected]
This paper describes a biped walking control system based on the reactive force interaction control at the foothold. 1) robust control of reactive force/torque interaction at the foothold based on Cartesian space motion controller. 2) the posture control considering the physical constraints of the reactive force/torque at the foothold by quadratic programming. The proposed approach realizes the robust biped locomotion because the environmental interaction is directly controlled. The control is applied to the 20 axes simulation model, and the stable biped locomotion is realized even if unknown small slope exists. The stable attitude control is con rmed by 14-axis biped robot experiments.
force control on the foothold and the force distribution system is proposed in order to improve the walking robustness. The physical constraints of the contact force on the foothold are precisely considered in the force distribution system. Then the robust force controller of the support foot locally suppresses unknown disturbances on the terrain. First in this paper, the plant formulation are shown in the section 2. The control strategy with hierarchical system are described in the section 3. The proposed control system is applied to the 20 axes simulation model and the results of stable legged locomotion are shown in the section 4. The experimental results are shown in the section 5. The section 6 concludes this paper.
1 Introduction
2
Abstract
A number of biped walking systems have been proposed in the previous works[1]{[14]. The control objective of the biped walking is to carry the body of the robot in use of reactive force of foot with foot placement planning. The unknown disturbances are, however, exist in the terrain and also the reactive force is subject to nonlinear physical constraints such as Zero Moment Point conditions and friction conditions. Thus the conventional control systems calculate trajectories of joint angle or joint torque so as to approximately satisfy the stable contact condition[1]{[3]. The approximation, however, yields lack of walking robustness. The whole dynamic equation of the robot and the contact condition is considered in the generation of joints references in [3], but it is o -line type planning due to the complexity of dynamics of the biped robot. To improve the walking robustness, an adaptive method is introduced[7]. In this paper, robust approach is adopted. A hierarchical control system based on the robust reactive
Model of Biped Robot
Consider the basic equation of the biped robot 3 2 3 3 2 32 0 b1 p 0 H 11 H 12 H 13 4 H 21 H 22 H 23 5 4 !_ 0 5 + 4 b2 5 = 4 0 5 + b3 q H 31 H 32 H 33 2 2 3 3 I3 0 I3 0 4 [xR 2] I 3 5 f R + 4 [xL2] I 3 5 f L nR nL J TL1 J TL2 J TR1 J TR2 (1) where p0 : 3 2 1 vector specifying position of the body ! 0 : 3 2 1 vector specifying angular velocity 2
of the body
N 2 1 vector specifying joint angle N 2 1 torque vector generated by actuator f L , f R : 3 2 1 vector of reactive force
q: :
nL , nR :
at the center of left or right foot
3
2 1 vector of reactive torque
at the center of left or right foot
H ij and bi are inertia matrices and a non-linear term, respectively. J TRi are transposal Jacobian matrices which transform reactive force and torque at the center of right foot into torque at joint coordinates. J TLi are those about left foot. xR is a tip position of the right leg with respect to the origin of p0 (see Fig. 1). xL is that of the left leg. [a2] is a matrix representing a cross product, and I n is a n 2 n identity matrix. In addition, there are physical constraints on reactive force and torque. Let f TR = [fRx fRy fRz ], nTR = [nRx nRy nRz ], f TL = [fLx fLy fLz ], and nTL = [nLx nLy nLz ]. The contact force and torque between foot and ground must satisfy the pressure condition, the friction condition, and the Zero Moment Point (ZMP) condition. First of all the normal component of the reactive force on the ground plane is not attractive but repulsive, which yield the following non-negative conditions. fRz
0;
fLz
0
Figure 1: 20-axis simulation model and 14-axis real robot.
(2)
The friction force, i. e., the tangent component of the reactive force on the ground plane always exists within the friction cone. q
2 fRx
2 f + fRy Rz
jnRz j 0fRz
q
2 fLx
2 + fLy
foot with foot placement planning. However, the relation between the input torque of the actuator and reactive force and torque f 3i , n3i is hard to solve because the reactive force and torque depend on the nonlinear static characteristics (2){(6) and the unknown dynamic ones. Thus the conventional control systems calculate trajectory of joint angle or joint torque so as to approximately satisfy static characteristics (2){(6). The approximation, however, yields lack of walking robustness. In this paper, a control system based on the reactive force control and the force distribution system is proposed in order to improve the walking robustness, in which the physical constraints of the contact force are precisely considered. Fig. 2 shows the overview of the control system.
fLz (3)
jnLz j 0 fLz
(4)
where and 0 are friction coecients. It is possible to break out slips at the contact points when the equality in the (3){(4) is realized. The tangent component of the reactive torque at the center of the foot on the ground plane is also limited due to niteness of the contact area.
jnRx j dy fRz jnRy j dx fRz
jnLxj dy fLz jnLy j dx fLz
(5) (6)
where dx and dy are halves of length and width of the foot, respectively. (5){(6) are equivalent to the Zero Moment Point conditions. The rst and second row of (1) represent the parallel and rotational motion of the body of the robot, respectively. The third row of (1) corresponds to the motion of the joint. Regarding the reactive force and torque f R , f L , nR , nL as an indirect control input, the position and attitude of the body p0 , ! 0 become controllable.
Reactive Force Joint Angle and Body Attitude Free-leg Trajectory Planner with Discrete Inverted Pendulum Body Posture Reference Generator with Inverted Pendulum
3 Hierarchical Control System
Yaw Joint Angle References of arms Moment Compensator Yaw Mom. Tip Position Ref. Reference of free-leg Body Reactive Workspace Biped Posture Force Position Robot Reactive Controller Controller Joint Position Controller Force Tip Position Reference Torque Reference Reference of of Center support-leg of Mass Reactive Force Joint Angle and Body Attitude
Figure 2: Biped walking control system.
The control objective of the biped walking is to carry the body of the robot in use of reactive force of 2031
3.1
Qf (s); P fn (s)01 Qf (s); P fn (s)(I 0 Qf (s))d(s); (I 0 Qf (s))r (s) 2 RH 1 . Here, RH 1 expresses a set of proper and stable transfer function matrices. These conditions are obtained from the internal stability and the output regulation. In a case of the force control, the free parameter Qf (s) and the nominal plant model P fn (s) can be set Qf (s) = diagfQf 1 (s); . . . ; Qf 6 (s)g and P fn (s) = diagfPfn1 (s); . . . ; Pfn6 (s)g where
Reactive Force Controller
The hybrid position/force control is applied to each leg, in which the force control mode is activated when the leg is in the support phase, otherwise the position control mode is activated. First in the system, the workspace position controller is applied as shown in Fig. 2 which consists of the H1 robust servo control with inertia compensation[16] = H n (q )C q (s)(q r 0 q )
(7)
Qfi (s)
and the inverse kinematics by the simpli ed Newton method
q r (t+Ts ) = qr (t)+
JR JL
01
g Rx (q r (t)) 0 xRr (8) g Lx (q r (t)) 0 xLr
where qr is the reference of joint angles, H n (q) is the nominal inertia matrix, C q (s) is the H1 controller, Ts is the sampling period, g Rx(1) is the function of the right foot position given joint angles, and xRr is the position reference of the right foot. Assuming the transfer characteristics of the Cartesian position control system is almost unity by the robust controller, the hybrid position/force controller is easily applied to the upper layer of the system. The following discussion in this section is in a case of right foot support. The force controller is simply given by
f Rr + -
Mx + d = uc = Ku
(13)
where 0 ]T uL ]T nR ]T nL ]T mI 3 H c12 M = H c21 H 22 I3 0 I3 0 K = [xRc 2] I 3 [xLc2] I 3 d = [ mg b2 ]T
x u uR uL
fR
= = = =
[ [ [ [
pc uR fR fL
(14) (15) (16) (17) (18) (19) (20)
and pc is COM of the robot. xRc is a tip position of the right leg with respect to the origin of COM pc . xLc is that of the left leg. m is total mass of the robot. H c12 = H Tc21 is non-diagonal term of the inertia matrix. The objective here is to make the COM of the robot and the attitude of the body converge its given reference trajectories with consideration of the physical constraints (2){(6). The control input of this system is the reactive force reference which is realized by the force controller. While the degree-of-motion-freedom
Figure 3: Force control system. When the nominal plant model P fn (s) is given, a very simple parameterization of the robust servo controller can be obtained as follows C f (s) = P fn (s)01 (I 0 Qf (s))01 Qf (s)
Body Posture Controller
In this section, the method to control the parallel and rotational motion of the body is presented. The rst row and second row of (1) can be transformed into the equation of parallel motion of the Center Of Mass (COM) and rotational one of the body.
(9)
P f (s)
= 1; 2; . . . ; 6. Thus, the robust force controller is obtained from (10) as C f (s) = diagfCf 1 (s); . . . ; Cf 6(s)g where 3 i2 s2 + 3 i s + 1 (12) Cfi (s) = 3 i s3 (mi s2 + bi s + ki ) 3.2
where f R and f Rr are the 6 2 1 force/torque vector and its reference on the right foot (the support foot), respectively. The con guration of the force control system is shown in Fig. 3. The plant system P f (s) includes the dynamics of the environment and the Cartesian position control system, whose control input is xRr , the Cartesian position reference of the support foot. d + C f (s) x Rr
3 i2 s2 + 3 i s + 1 2 ; Pfni (s) = mi s +bi s+ki (11) ( i s + 1)3
i
xRr = C f (s)(f Rr 0 f R )
=
(10)
Here, Qf (s) is the free parameter representing a complementary sensitivity function and is subject to 2032
of (13) is 6, the degree of control input is time-variant, which becomes 12th in double support phase and 6th in single support phase. The ideal force input at the COM of the robot and torque input around the body u3c is determined by the state feedback.
Table 1: Parameters of biped robot.
subject to
Jmain
+ Jsub
Au b
weight [kg]
0.99
28.744
arm
Due to the physical limitations (2){(6), the ideal force u3c at the COM and torque at the body is not always realized by the reactive force and torque. Thus the following performance indices Jmain , Jsub are introduced. 1 (u 0 u3c )T C (uc 0 u3c ) (22) Jmain = 2 c 1 Jsub = (u 0 uL )T (uR 0 uL ) (23) 2 R The index Jmain corresponds to the square error between the ideal force and torque and the realizable ones. The index Jsub corresponds to the square error between the force and torque of the left foot and those of the right one. The reactive force and torque input u is determined by quadratic programming, which minimizes the performance index under the linearized constraints of (2){(6). u
size [m]
all head
ref ]+ d(21) u3c = M [K p (xref 0 x)+ K d (x_ ref 0 x_ )+ x
min
parts 0.14
2 0.14 2 0.14 (d 2 w 2 h)
2.744
0.3
3.5
body
0.4
8
thigh
0.2
2
shin
0.2
foot
0.2
2 0.1 (d 2 w)
2 1.5
[s]. After that, the walking motion starts. Fig. 5 shows the trajectory of zero moment point (ZMP). The walking motion becomes more robust when the yaw axis moment is compensated by the arm swing motion[17].
Walking on Unknown Slope Fig. 6 shows the biped walking simulation with slope environment whose information is not used in the controller. The controller used in the simulation is exactly as same as that of in the previous section. Thus the proposed control algorithm is robust against the environmental uncertainty. The slope is set to 5 [deg] up, which is not used in the controller. Fig. 6 shows the trajectory of the ZMP. Due to the slope, the trajectory of the ZMP shifts to the heel in Fig. 6 (a) and (b), compared with the at terrain case in Fig. 5 (a) and (b).
(24) (25)
where is a small positive real number. The main performance index Jmain approaches the solution to the ideal force and torque u3c given by the state feedback. The sub performance index Jsub distributes the inner force and torque to the both foot in balance. Because is very small, the sub performance index does not almost have in uence on the main performance index. The optimization problem (24){(25) is equivalent to the quadratic programming problem. The reactive force and torque reference can be obtained by solving a quadratic programming problem for each sampling period.
5
Experiments
Fig. 1 shows a photo of the real 14-axis biped walking robot developed in our lab. The speci cation of the robot is as follows. 6-degree-of-freedom for each leg. Dc servo motor with 50:1 harmonic gear. Rotary encoder sensor for each joints with resolution 4000 [pulse/rev] on motor shaft. 6DOF force/torque sensor on each ankle. 3DOF giro scope on body. Controller: DSP (TMS320C32-50MHz) The robot has 6 joints for each leg so that the position and orientation of the foot can be chosen any posture in the 3-dimensional space. All calculation of the control is done by DSP board with TMS320C32-50MHz. The programs are written in the C language. The robot is about 1.2 [m] height and 20 [kg] weight. Fig. 7 shows the global system con guration. The host computer is used in cross compiling the DSP programs.
4 Simulations The proposed control is applied to 20 axes humantype biped robot and is investigated by a precise simulator[15]. The parameters of the robot is shown in Table 1. The QP is solved by the algorithm in [18]. The snapshots of the simulation is shown in Fig. 4. The initial movement of COM is nished in 0 < t < 1 2033
Figure 4: Snapshots of biped walking simulation. ZMP of Left Leg
ZMP of Right Leg 0.15
0.1
0.1
0.05
0.05 Pzmp_x [m]
Pzmp_x [m]
0.15
0
0
−0.05
−0.05
−0.1
−0.1
−0.15
−0.15 4
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
4
(a) ZMP of left foot in sagittal plane.
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
(b) ZMP of right foot in sagittal plane.
Figure 5: Trajectory of zero moment point in sagittal plane. (simulation) ZMP of Left Leg
ZMP of Right Leg 0.15
0.1
0.1
0.05
0.05 Pzmp_x [m]
Pzmp_x [m]
0.15
0
−0.05
0
−0.05
−0.1
−0.1
−0.15
−0.15 4
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
4
(a) ZMP of left foot in sagittal plane.
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
(b) ZMP of right foot in sagittal plane.
Figure 6: Trajectory of zero moment point in slope environment. (simulation)
Host Computer PC-AT
DSP TMS320C32 50MHz
DSP TMS320C32 50MHz
is not enough to implement all of the proposed control algorithm in real-time for the present. Especially the calculation of the quadratic programming costs very much. Here the simpli ed version of the force distribution instead of (24){(25) is proposed as follows.
Biped Walking Robot 12bit D/A Converter (14ch)
Motor Driver with Current Controller
16bit Counter (14ch)
Joint Actuator x14 DC Servo Motor Rotary Encoder
12bit A/D Converter (5ch)
Giro Scope Controller
Giro Scope on Body (roll, yaw, pitch)
Digital Output (3bit)
Force/Torque Sensor Controller
Force/Torque Sensor on Foot x2
u = W 2 K T (KW 2 K T )01 u3c
(26)
where u is the force reference of both feet and u3c is required force/torque at the body obtained by (21). This solution is the optimal in a sense that u has minimum square norm jjW 01 ujj2 . W is a weighting matrix W = diagfw1 ; w2 ; . . . ; w12 g. The stable posture control of the robot is shown as Fig. 8. The position of COM and the attitude of the body is well controlled within 60.03 [m] and 60.04
Figure 7: System con guration.
Implementation Aspects
Although computing ability of recent micro processor progresses rapidly, it 2034
[rad] (= 6 2.3 [deg]) errors. Also ZMPs are used to control COM of the robot as shown in Fig. 9.
[2] J. Furusho and A. Sano, \Sensor-Based Control of a NineLink Biped," Int. J. Robotics Research, vol. 9, no. 2, pp. 83{98, 1990. [3] J. Yamaguchi, A. Takanishi and I. Kato, \Development of a Biped Walking Robot Compensating for Three-Axis Moment by Trunk Motion," J. Robotics Society of Japan, vol. 11, no. 4, pp.581{586, 1993. (in Japanese) [4] M. H. Raibert, Legged Robots That Balance, Cambridge, MA, MIT Press, 1986. [5] A. Kun and W. T. Miller, III, \Adaptive Dynamic Balance of a Biped Robot using Neural Networks," Proc. IEEE Int. Conf. on RA, pp. 240{245, 1996. [6] J. K. Hodgins, \Three-Dimensional Human Running," Proc. IEEE Int. Conf. on RA, pp. 3271{3276, 1996. [7] J. Yamaguchi, N. Kinoshita, A. Takanishi, and I. Kato, \Development of a Dynamic Biped Walking System for Humanoid, | Development of a Biped Walking Robot Adapting to the Human's Living Floor |," Proc. IEEE Int. Conf. on RA, pp. 232{239, 1996. [8] S. Kajita and K. Tani, \Adaptive Gait Control of a Biped Robot based on Realtime Sensing of the Ground Pro le," Proc. IEEE Int. Conf. on RA, pp. 570{577, 1996. [9] H. Minakata and Y. Hori, \Development of Biped Bike Prototype `Ostrich-I&II'," Proc. Asian Control Conference, vol. 3, pp. 319{322, 1997. [10] A. W. Salatian, K. Y. Yi, and Y. F. Zheng, \Reinforcement Learning for a Biped Robot to Climb Sloping Surfaces," J. Robotic Systems, vol. 14, no. 4, pp. 283{296, 1997. [11] S. Kawaji, N. Matsunaga and M. Arao, \Hierarchical Control of Biped Locomotion Robot", Proc. IEEE Int. Workshop on Advanced Motion Control, pp. 421-430, 1994. [12] K. Sorao, T. Murakami, and K. Ohnishi, \A Uni ed Approach to ZMP and Gravity Center Control in Biped Dynamic Stable Walking," Proc. IEEE/ASME Int. Conf. on Advanced Intelligent Mechatronics, CD-ROM, 1997. [13] T. Fukuda, Y. Komata, and T. Arakawa, \Stabilization Control of Biped Locomotion Robot based Learning with GAs having Self-adaptive Mutation and Recurrent Neural Networks," Proc. IEEE Int. Conf. on RA, pp. 217{222, 1997. [14] J. Pratt, P. Dilworth, and G. Pratt, \Virtual Model Control of a Bipedal Walking Robot," Proc. IEEE Int. Conf. on RA, pp. 193{198, 1997. [15] Y. Fujimoto and A. Kawamura, \Autonomous Control and 3D Dynamic Simulation of Biped Walking Robot Including Environmental Force Interaction," to appear in IEEE RA Magazine, June 1997. [16] Y. Fujimoto and A. Kawamura, \An Inertia Fluctuation Insensitive Robust Control of Robot Manipulators Based on a Combination of Inertia Torque Computation Filter and H1 Control," Trans. IEE of Japan, vol. 117-D, no. 4, pp. 493{500, 1997. (in Japanese) [17] Y. Fujimoto and A. Kawamura, \Robust Control of Biped Walking Robot with Yaw Moment Compensation by Arm Motion," Proc. Asian Control Conference, vol. 3, pp. 327{ 330, 1997. [18] R. W. Cottle and G. B. Dantzig, \Complementary Pivot Theory of Mathematical Programming," Linear Algebra and Its Applications, vol. 1, pp. 103{125, 1968.
Position Error of COM [m]
0.05 0.04 0.03 0.02 0.01 0 −0.01
x y z
−0.02 −0.03 −0.04 −0.05 0
1
2
3
4 Time [sec]
5
6
7
8
Angular Error of Body [rad]
0.1 roll pitch yaw
0.05
0
−0.05
−0.1 0
1
2
3
4 Time [sec]
5
6
7
8
Figure 8: Error of COM position and body rotation. ZMP in Saggital Plane [m]
0.08
ZMP in sagittal plane COM in sagittal plane
0.06 0.04 0.02 0 −0.02 −0.04 −0.06 −0.08 0
1
2
3
4 Time [sec]
5
6
7
8
Figure 9: Measured ZMP in sagittal and lateral plane.
6 Conclusion In this paper the following hierarchical control system is proposed. 1) robust control of reactive force/torque interaction at the foothold based on Cartesian space motion controller. 2) the posture control considering the physical constraints of the reactive force/torque on the foot by quadratic programming. The proposed control system is applied to the 20axis simulation model, and the stable biped locomotion is realized. The stable attitude control is con rmed by 14-axis biped robot experiments. Finally, the authors would like to note that part of this research is carried with the subsidy of the Scienti c Research Fund of the Ministry of Education.
References [1] S. Kajita, T. Yamaura, and A. Kobayashi, \Dynamic Walking Control of a Biped Robot Along a Potential Energy Conserving Orbit," IEEE Trans. RA, vol. 8, no. 4, pp. 431{438, 1992.
2035
2030
Robust Biped Walking with Active Interaction Control between Foot and Ground Yasutaka Fujimoto
Satoshi Obata
Dept. of System Design Eng. Keio University Yokohama 223 JAPAN [email protected]
Atsuo Kawamura
Dept. of Elec. & Comp. Eng. Yokohama National University Yokohama 240 JAPAN [email protected]
This paper describes a biped walking control system based on the reactive force interaction control at the foothold. 1) robust control of reactive force/torque interaction at the foothold based on Cartesian space motion controller. 2) the posture control considering the physical constraints of the reactive force/torque at the foothold by quadratic programming. The proposed approach realizes the robust biped locomotion because the environmental interaction is directly controlled. The control is applied to the 20 axes simulation model, and the stable biped locomotion is realized even if unknown small slope exists. The stable attitude control is con rmed by 14-axis biped robot experiments.
force control on the foothold and the force distribution system is proposed in order to improve the walking robustness. The physical constraints of the contact force on the foothold are precisely considered in the force distribution system. Then the robust force controller of the support foot locally suppresses unknown disturbances on the terrain. First in this paper, the plant formulation are shown in the section 2. The control strategy with hierarchical system are described in the section 3. The proposed control system is applied to the 20 axes simulation model and the results of stable legged locomotion are shown in the section 4. The experimental results are shown in the section 5. The section 6 concludes this paper.
1 Introduction
2
Abstract
A number of biped walking systems have been proposed in the previous works[1]{[14]. The control objective of the biped walking is to carry the body of the robot in use of reactive force of foot with foot placement planning. The unknown disturbances are, however, exist in the terrain and also the reactive force is subject to nonlinear physical constraints such as Zero Moment Point conditions and friction conditions. Thus the conventional control systems calculate trajectories of joint angle or joint torque so as to approximately satisfy the stable contact condition[1]{[3]. The approximation, however, yields lack of walking robustness. The whole dynamic equation of the robot and the contact condition is considered in the generation of joints references in [3], but it is o -line type planning due to the complexity of dynamics of the biped robot. To improve the walking robustness, an adaptive method is introduced[7]. In this paper, robust approach is adopted. A hierarchical control system based on the robust reactive
Model of Biped Robot
Consider the basic equation of the biped robot 3 2 3 3 2 32 0 b1 p 0 H 11 H 12 H 13 4 H 21 H 22 H 23 5 4 !_ 0 5 + 4 b2 5 = 4 0 5 + b3 q H 31 H 32 H 33 2 2 3 3 I3 0 I3 0 4 [xR 2] I 3 5 f R + 4 [xL2] I 3 5 f L nR nL J TL1 J TL2 J TR1 J TR2 (1) where p0 : 3 2 1 vector specifying position of the body ! 0 : 3 2 1 vector specifying angular velocity 2
of the body
N 2 1 vector specifying joint angle N 2 1 torque vector generated by actuator f L , f R : 3 2 1 vector of reactive force
q: :
nL , nR :
at the center of left or right foot
3
2 1 vector of reactive torque
at the center of left or right foot
H ij and bi are inertia matrices and a non-linear term, respectively. J TRi are transposal Jacobian matrices which transform reactive force and torque at the center of right foot into torque at joint coordinates. J TLi are those about left foot. xR is a tip position of the right leg with respect to the origin of p0 (see Fig. 1). xL is that of the left leg. [a2] is a matrix representing a cross product, and I n is a n 2 n identity matrix. In addition, there are physical constraints on reactive force and torque. Let f TR = [fRx fRy fRz ], nTR = [nRx nRy nRz ], f TL = [fLx fLy fLz ], and nTL = [nLx nLy nLz ]. The contact force and torque between foot and ground must satisfy the pressure condition, the friction condition, and the Zero Moment Point (ZMP) condition. First of all the normal component of the reactive force on the ground plane is not attractive but repulsive, which yield the following non-negative conditions. fRz
0;
fLz
0
Figure 1: 20-axis simulation model and 14-axis real robot.
(2)
The friction force, i. e., the tangent component of the reactive force on the ground plane always exists within the friction cone. q
2 fRx
2 f + fRy Rz
jnRz j 0fRz
q
2 fLx
2 + fLy
foot with foot placement planning. However, the relation between the input torque of the actuator and reactive force and torque f 3i , n3i is hard to solve because the reactive force and torque depend on the nonlinear static characteristics (2){(6) and the unknown dynamic ones. Thus the conventional control systems calculate trajectory of joint angle or joint torque so as to approximately satisfy static characteristics (2){(6). The approximation, however, yields lack of walking robustness. In this paper, a control system based on the reactive force control and the force distribution system is proposed in order to improve the walking robustness, in which the physical constraints of the contact force are precisely considered. Fig. 2 shows the overview of the control system.
fLz (3)
jnLz j 0 fLz
(4)
where and 0 are friction coecients. It is possible to break out slips at the contact points when the equality in the (3){(4) is realized. The tangent component of the reactive torque at the center of the foot on the ground plane is also limited due to niteness of the contact area.
jnRx j dy fRz jnRy j dx fRz
jnLxj dy fLz jnLy j dx fLz
(5) (6)
where dx and dy are halves of length and width of the foot, respectively. (5){(6) are equivalent to the Zero Moment Point conditions. The rst and second row of (1) represent the parallel and rotational motion of the body of the robot, respectively. The third row of (1) corresponds to the motion of the joint. Regarding the reactive force and torque f R , f L , nR , nL as an indirect control input, the position and attitude of the body p0 , ! 0 become controllable.
Reactive Force Joint Angle and Body Attitude Free-leg Trajectory Planner with Discrete Inverted Pendulum Body Posture Reference Generator with Inverted Pendulum
3 Hierarchical Control System
Yaw Joint Angle References of arms Moment Compensator Yaw Mom. Tip Position Ref. Reference of free-leg Body Reactive Workspace Biped Posture Force Position Robot Reactive Controller Controller Joint Position Controller Force Tip Position Reference Torque Reference Reference of of Center support-leg of Mass Reactive Force Joint Angle and Body Attitude
Figure 2: Biped walking control system.
The control objective of the biped walking is to carry the body of the robot in use of reactive force of 2031
3.1
Qf (s); P fn (s)01 Qf (s); P fn (s)(I 0 Qf (s))d(s); (I 0 Qf (s))r (s) 2 RH 1 . Here, RH 1 expresses a set of proper and stable transfer function matrices. These conditions are obtained from the internal stability and the output regulation. In a case of the force control, the free parameter Qf (s) and the nominal plant model P fn (s) can be set Qf (s) = diagfQf 1 (s); . . . ; Qf 6 (s)g and P fn (s) = diagfPfn1 (s); . . . ; Pfn6 (s)g where
Reactive Force Controller
The hybrid position/force control is applied to each leg, in which the force control mode is activated when the leg is in the support phase, otherwise the position control mode is activated. First in the system, the workspace position controller is applied as shown in Fig. 2 which consists of the H1 robust servo control with inertia compensation[16] = H n (q )C q (s)(q r 0 q )
(7)
Qfi (s)
and the inverse kinematics by the simpli ed Newton method
q r (t+Ts ) = qr (t)+
JR JL
01
g Rx (q r (t)) 0 xRr (8) g Lx (q r (t)) 0 xLr
where qr is the reference of joint angles, H n (q) is the nominal inertia matrix, C q (s) is the H1 controller, Ts is the sampling period, g Rx(1) is the function of the right foot position given joint angles, and xRr is the position reference of the right foot. Assuming the transfer characteristics of the Cartesian position control system is almost unity by the robust controller, the hybrid position/force controller is easily applied to the upper layer of the system. The following discussion in this section is in a case of right foot support. The force controller is simply given by
f Rr + -
Mx + d = uc = Ku
(13)
where 0 ]T uL ]T nR ]T nL ]T mI 3 H c12 M = H c21 H 22 I3 0 I3 0 K = [xRc 2] I 3 [xLc2] I 3 d = [ mg b2 ]T
x u uR uL
fR
= = = =
[ [ [ [
pc uR fR fL
(14) (15) (16) (17) (18) (19) (20)
and pc is COM of the robot. xRc is a tip position of the right leg with respect to the origin of COM pc . xLc is that of the left leg. m is total mass of the robot. H c12 = H Tc21 is non-diagonal term of the inertia matrix. The objective here is to make the COM of the robot and the attitude of the body converge its given reference trajectories with consideration of the physical constraints (2){(6). The control input of this system is the reactive force reference which is realized by the force controller. While the degree-of-motion-freedom
Figure 3: Force control system. When the nominal plant model P fn (s) is given, a very simple parameterization of the robust servo controller can be obtained as follows C f (s) = P fn (s)01 (I 0 Qf (s))01 Qf (s)
Body Posture Controller
In this section, the method to control the parallel and rotational motion of the body is presented. The rst row and second row of (1) can be transformed into the equation of parallel motion of the Center Of Mass (COM) and rotational one of the body.
(9)
P f (s)
= 1; 2; . . . ; 6. Thus, the robust force controller is obtained from (10) as C f (s) = diagfCf 1 (s); . . . ; Cf 6(s)g where 3 i2 s2 + 3 i s + 1 (12) Cfi (s) = 3 i s3 (mi s2 + bi s + ki ) 3.2
where f R and f Rr are the 6 2 1 force/torque vector and its reference on the right foot (the support foot), respectively. The con guration of the force control system is shown in Fig. 3. The plant system P f (s) includes the dynamics of the environment and the Cartesian position control system, whose control input is xRr , the Cartesian position reference of the support foot. d + C f (s) x Rr
3 i2 s2 + 3 i s + 1 2 ; Pfni (s) = mi s +bi s+ki (11) ( i s + 1)3
i
xRr = C f (s)(f Rr 0 f R )
=
(10)
Here, Qf (s) is the free parameter representing a complementary sensitivity function and is subject to 2032
of (13) is 6, the degree of control input is time-variant, which becomes 12th in double support phase and 6th in single support phase. The ideal force input at the COM of the robot and torque input around the body u3c is determined by the state feedback.
Table 1: Parameters of biped robot.
subject to
Jmain
+ Jsub
Au b
weight [kg]
0.99
28.744
arm
Due to the physical limitations (2){(6), the ideal force u3c at the COM and torque at the body is not always realized by the reactive force and torque. Thus the following performance indices Jmain , Jsub are introduced. 1 (u 0 u3c )T C (uc 0 u3c ) (22) Jmain = 2 c 1 Jsub = (u 0 uL )T (uR 0 uL ) (23) 2 R The index Jmain corresponds to the square error between the ideal force and torque and the realizable ones. The index Jsub corresponds to the square error between the force and torque of the left foot and those of the right one. The reactive force and torque input u is determined by quadratic programming, which minimizes the performance index under the linearized constraints of (2){(6). u
size [m]
all head
ref ]+ d(21) u3c = M [K p (xref 0 x)+ K d (x_ ref 0 x_ )+ x
min
parts 0.14
2 0.14 2 0.14 (d 2 w 2 h)
2.744
0.3
3.5
body
0.4
8
thigh
0.2
2
shin
0.2
foot
0.2
2 0.1 (d 2 w)
2 1.5
[s]. After that, the walking motion starts. Fig. 5 shows the trajectory of zero moment point (ZMP). The walking motion becomes more robust when the yaw axis moment is compensated by the arm swing motion[17].
Walking on Unknown Slope Fig. 6 shows the biped walking simulation with slope environment whose information is not used in the controller. The controller used in the simulation is exactly as same as that of in the previous section. Thus the proposed control algorithm is robust against the environmental uncertainty. The slope is set to 5 [deg] up, which is not used in the controller. Fig. 6 shows the trajectory of the ZMP. Due to the slope, the trajectory of the ZMP shifts to the heel in Fig. 6 (a) and (b), compared with the at terrain case in Fig. 5 (a) and (b).
(24) (25)
where is a small positive real number. The main performance index Jmain approaches the solution to the ideal force and torque u3c given by the state feedback. The sub performance index Jsub distributes the inner force and torque to the both foot in balance. Because is very small, the sub performance index does not almost have in uence on the main performance index. The optimization problem (24){(25) is equivalent to the quadratic programming problem. The reactive force and torque reference can be obtained by solving a quadratic programming problem for each sampling period.
5
Experiments
Fig. 1 shows a photo of the real 14-axis biped walking robot developed in our lab. The speci cation of the robot is as follows. 6-degree-of-freedom for each leg. Dc servo motor with 50:1 harmonic gear. Rotary encoder sensor for each joints with resolution 4000 [pulse/rev] on motor shaft. 6DOF force/torque sensor on each ankle. 3DOF giro scope on body. Controller: DSP (TMS320C32-50MHz) The robot has 6 joints for each leg so that the position and orientation of the foot can be chosen any posture in the 3-dimensional space. All calculation of the control is done by DSP board with TMS320C32-50MHz. The programs are written in the C language. The robot is about 1.2 [m] height and 20 [kg] weight. Fig. 7 shows the global system con guration. The host computer is used in cross compiling the DSP programs.
4 Simulations The proposed control is applied to 20 axes humantype biped robot and is investigated by a precise simulator[15]. The parameters of the robot is shown in Table 1. The QP is solved by the algorithm in [18]. The snapshots of the simulation is shown in Fig. 4. The initial movement of COM is nished in 0 < t < 1 2033
Figure 4: Snapshots of biped walking simulation. ZMP of Left Leg
ZMP of Right Leg 0.15
0.1
0.1
0.05
0.05 Pzmp_x [m]
Pzmp_x [m]
0.15
0
0
−0.05
−0.05
−0.1
−0.1
−0.15
−0.15 4
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
4
(a) ZMP of left foot in sagittal plane.
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
(b) ZMP of right foot in sagittal plane.
Figure 5: Trajectory of zero moment point in sagittal plane. (simulation) ZMP of Left Leg
ZMP of Right Leg 0.15
0.1
0.1
0.05
0.05 Pzmp_x [m]
Pzmp_x [m]
0.15
0
−0.05
0
−0.05
−0.1
−0.1
−0.15
−0.15 4
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
4
(a) ZMP of left foot in sagittal plane.
4.2
4.4
4.6
4.8
5 time [sec]
5.2
5.4
5.6
5.8
6
(b) ZMP of right foot in sagittal plane.
Figure 6: Trajectory of zero moment point in slope environment. (simulation)
Host Computer PC-AT
DSP TMS320C32 50MHz
DSP TMS320C32 50MHz
is not enough to implement all of the proposed control algorithm in real-time for the present. Especially the calculation of the quadratic programming costs very much. Here the simpli ed version of the force distribution instead of (24){(25) is proposed as follows.
Biped Walking Robot 12bit D/A Converter (14ch)
Motor Driver with Current Controller
16bit Counter (14ch)
Joint Actuator x14 DC Servo Motor Rotary Encoder
12bit A/D Converter (5ch)
Giro Scope Controller
Giro Scope on Body (roll, yaw, pitch)
Digital Output (3bit)
Force/Torque Sensor Controller
Force/Torque Sensor on Foot x2
u = W 2 K T (KW 2 K T )01 u3c
(26)
where u is the force reference of both feet and u3c is required force/torque at the body obtained by (21). This solution is the optimal in a sense that u has minimum square norm jjW 01 ujj2 . W is a weighting matrix W = diagfw1 ; w2 ; . . . ; w12 g. The stable posture control of the robot is shown as Fig. 8. The position of COM and the attitude of the body is well controlled within 60.03 [m] and 60.04
Figure 7: System con guration.
Implementation Aspects
Although computing ability of recent micro processor progresses rapidly, it 2034
[rad] (= 6 2.3 [deg]) errors. Also ZMPs are used to control COM of the robot as shown in Fig. 9.
[2] J. Furusho and A. Sano, \Sensor-Based Control of a NineLink Biped," Int. J. Robotics Research, vol. 9, no. 2, pp. 83{98, 1990. [3] J. Yamaguchi, A. Takanishi and I. Kato, \Development of a Biped Walking Robot Compensating for Three-Axis Moment by Trunk Motion," J. Robotics Society of Japan, vol. 11, no. 4, pp.581{586, 1993. (in Japanese) [4] M. H. Raibert, Legged Robots That Balance, Cambridge, MA, MIT Press, 1986. [5] A. Kun and W. T. Miller, III, \Adaptive Dynamic Balance of a Biped Robot using Neural Networks," Proc. IEEE Int. Conf. on RA, pp. 240{245, 1996. [6] J. K. Hodgins, \Three-Dimensional Human Running," Proc. IEEE Int. Conf. on RA, pp. 3271{3276, 1996. [7] J. Yamaguchi, N. Kinoshita, A. Takanishi, and I. Kato, \Development of a Dynamic Biped Walking System for Humanoid, | Development of a Biped Walking Robot Adapting to the Human's Living Floor |," Proc. IEEE Int. Conf. on RA, pp. 232{239, 1996. [8] S. Kajita and K. Tani, \Adaptive Gait Control of a Biped Robot based on Realtime Sensing of the Ground Pro le," Proc. IEEE Int. Conf. on RA, pp. 570{577, 1996. [9] H. Minakata and Y. Hori, \Development of Biped Bike Prototype `Ostrich-I&II'," Proc. Asian Control Conference, vol. 3, pp. 319{322, 1997. [10] A. W. Salatian, K. Y. Yi, and Y. F. Zheng, \Reinforcement Learning for a Biped Robot to Climb Sloping Surfaces," J. Robotic Systems, vol. 14, no. 4, pp. 283{296, 1997. [11] S. Kawaji, N. Matsunaga and M. Arao, \Hierarchical Control of Biped Locomotion Robot", Proc. IEEE Int. Workshop on Advanced Motion Control, pp. 421-430, 1994. [12] K. Sorao, T. Murakami, and K. Ohnishi, \A Uni ed Approach to ZMP and Gravity Center Control in Biped Dynamic Stable Walking," Proc. IEEE/ASME Int. Conf. on Advanced Intelligent Mechatronics, CD-ROM, 1997. [13] T. Fukuda, Y. Komata, and T. Arakawa, \Stabilization Control of Biped Locomotion Robot based Learning with GAs having Self-adaptive Mutation and Recurrent Neural Networks," Proc. IEEE Int. Conf. on RA, pp. 217{222, 1997. [14] J. Pratt, P. Dilworth, and G. Pratt, \Virtual Model Control of a Bipedal Walking Robot," Proc. IEEE Int. Conf. on RA, pp. 193{198, 1997. [15] Y. Fujimoto and A. Kawamura, \Autonomous Control and 3D Dynamic Simulation of Biped Walking Robot Including Environmental Force Interaction," to appear in IEEE RA Magazine, June 1997. [16] Y. Fujimoto and A. Kawamura, \An Inertia Fluctuation Insensitive Robust Control of Robot Manipulators Based on a Combination of Inertia Torque Computation Filter and H1 Control," Trans. IEE of Japan, vol. 117-D, no. 4, pp. 493{500, 1997. (in Japanese) [17] Y. Fujimoto and A. Kawamura, \Robust Control of Biped Walking Robot with Yaw Moment Compensation by Arm Motion," Proc. Asian Control Conference, vol. 3, pp. 327{ 330, 1997. [18] R. W. Cottle and G. B. Dantzig, \Complementary Pivot Theory of Mathematical Programming," Linear Algebra and Its Applications, vol. 1, pp. 103{125, 1968.
Position Error of COM [m]
0.05 0.04 0.03 0.02 0.01 0 −0.01
x y z
−0.02 −0.03 −0.04 −0.05 0
1
2
3
4 Time [sec]
5
6
7
8
Angular Error of Body [rad]
0.1 roll pitch yaw
0.05
0
−0.05
−0.1 0
1
2
3
4 Time [sec]
5
6
7
8
Figure 8: Error of COM position and body rotation. ZMP in Saggital Plane [m]
0.08
ZMP in sagittal plane COM in sagittal plane
0.06 0.04 0.02 0 −0.02 −0.04 −0.06 −0.08 0
1
2
3
4 Time [sec]
5
6
7
8
Figure 9: Measured ZMP in sagittal and lateral plane.
6 Conclusion In this paper the following hierarchical control system is proposed. 1) robust control of reactive force/torque interaction at the foothold based on Cartesian space motion controller. 2) the posture control considering the physical constraints of the reactive force/torque on the foot by quadratic programming. The proposed control system is applied to the 20axis simulation model, and the stable biped locomotion is realized. The stable attitude control is con rmed by 14-axis biped robot experiments. Finally, the authors would like to note that part of this research is carried with the subsidy of the Scienti c Research Fund of the Ministry of Education.
References [1] S. Kajita, T. Yamaura, and A. Kobayashi, \Dynamic Walking Control of a Biped Robot Along a Potential Energy Conserving Orbit," IEEE Trans. RA, vol. 8, no. 4, pp. 431{438, 1992.
2035
