Attitude Control Experiments of Biped Walking Robot ... - CiteSeerX

c 1998 IEEE 70 '98 Int. Workshop. on Advanced Motion Control, Coimbra, Portugal, 0-7803-4484-7/98/$10.00

Attitude Control Experiments of Biped Walking Robot based on Environmental Force Interaction Yasutaka Fujimoto

Atsuo Kawamura

Dept. of System Design Eng. Keio University Yokohama 223-8522 Japan [email protected]

Dept. of Elec. & Comp. Eng. Yokohama National University Yokohama 240-8501 Japan [email protected]

Abstract

In this paper, the attitude control of the biped robot based on the robust reactive force control is proposed, which can locally suppresses the unknown disturbances on the terrain. The method is investigated through the experiments of the 14 axes biped robot. The position of the center of mass and the attitude of the body is well controlled within 60.03 [m] and 60.04 [rad] (= 6 2.3 [deg]) errors. 1 Introduction The biped walking machine is one of fundamental technology to realize the humanoid whose form is the best for the human living environment. But technique of the biped walking robot is still under development. The reasons are because the biped system is unstable, non-linear, and subject to restrictions caused by the environment. Concerning the third point of these characteristics, it is dicult to stabilize the biped robot when the characteristics of the environment is unknown. In this paper, the attitude control of the biped robot based on the robust reactive force control is proposed, which can locally suppresses the unknown disturbances on the terrain. In general, Fig. 1 illustrates a biped robot whose foot interacts to the ground and whose body receives the reactive force. The legged robot can be separated to two parts, the body and the legs. The controller calculates the actuator torque from joint angle and another sensor information, then the legs are moved. The legs interact to the environment, which generates the reactive force. Then the body is driven by the reactive force. The movement of the robot depends

Reactive Force

Environment

Position of Foot

Legged Robot Body

Position of body

+ +

Joint Torque

Legs

Controller

Position of Joint Angle

Figure 1: A block diagram of the legged system. on the reactive force. In this system, dynamics of the legged robot has restrictions because it does not have xed point on the ground and there are limitations of the force and the torque on the foothold. This point is dierent from a case of ordinary robot manipulators. Despite the information of the reactive force on the foot is very important, it is hardly used as a main index in the feedback control loop in the previous works. It is mostly used as a sub-index in the control loop[3] or a main index in the reference generation[9]. The conventional control systems calculate trajectories of joint angle or joint torque so as to approximately satisfy the stable contact condition. The whole dynamic equation of the robot and the contact condition is considered to generate joints references in [9], but it is o-line type planning due to the complexity of dynamics of the biped robot. Kajita et'al have proposed an linear inverted pendulum model[4]. The direct control of zero moment point is proposed in [8]. These approaches can be said to be model based control. Thus there are the modeling errors in the control systems, which yields lack of walking robustness. One of the solutions is in use of an adaptive

2

Fig. 2 shows the real 14-axis biped walking robot developed in our lab. The speci cation of the robot is as follows. 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. The freedom of arms is 1 for each arm. Dc servo motors with 50:1 harmonic gear is used as the actuators, which have rotary encoders of resolution 1000 [pulse/rev] on the motor shafts. The encoder counter enables the resolution four times as precise as original one. The 6DOF force/torque sensors are attached to both ankles. Also 3DOF gyro scope is attached to the body which senses the attitude of the body. All calculation of the control is done by DSP board with TMS320C32-50MHz. The programs can be written in the C language. The robot is about 1.2 [m] height and 20 [kg] weight. Fig. 3 shows the global system con guration. The host computer is used in cross compiling of DSP programs.

3

250 50

R402-011E18 (23W)

438

CSF-14-50

150

#13

200

#14

360

Giro Scope

R404-011E18 (40W)

R406-011E18 (60W) #1

#7

#2 CSF-14-50

CSF-14-50

#3

270

CSF-17-50 #9

#3

R402-011E18 (23W)

#10

#4

R404-011E18 (40W)

240

R301-011E17 (11W) CS-11-50

CSF-14-50

#11

220

CSF-14-50

#5

30

111

#6 R402-011E18 (23W)

20

F/T Sensor

35

140

40

180

70

Figure 2: 14-axis biped robot.

DSP TMS320C32 50MHz

DSP TMS320C32 50MHz

Implementation Aspects

Fig. 4 shows the hierarchical control originally proposed and investigated through the 3D precise simulator[1]. Although computing ability of recent micro processor progresses rapidly, it is not enough to implement all of the proposed control algorithm in realtime. The control includes the calculation of the quadratic programming in realtime, whose calculation costs very much. Then it is not suitable for realtime control for the present. Thus the simpli ed algorithms is proposed in this paper. As the result, the control period becomes 2 [ms]. In the experiment, the H1 joint servo controller is adopted on each joint. Then the workspace controller of each foot position is realized in use of Newton method. The robust force controller is applied over the work space position controller. The simpli ed attitude controller is realized over the force controller.

1211

240

Host Computer PC-AT

Experimental Equipment

310

method [2][5][6][7][10]. The other solution is a robust method. The biped walking based on the reactive force control have been proposed and investigated through 3D precise simulator[1]. In this paper, a new attitude control of the body of the biped robot based on the reactive force control on the foothold and the force distribution system is proposed and investigated through the experiment, which is the simpli ed version of the hierarchical control proposed in [1] from the implementation aspects. The method can improve the walking robustness because the robust force controller adopted on the support foot can locally suppresses unknown disturbances on the terrain.

Biped Walking Robot 12bit D/A Converter (14ch)

Motor Driver with Current Controller

16bit Counter (14ch)

3.1

Joint Actuator x14

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

Posture Control with Optimal Force Distribution

DC Servo Motor

The simpli ed method is introduced as follows. The equation of parallel motion of the COM (center of mass) of the robot and rotational motion of the body becomes:  + d = uc = Ku Mx (1) where x = [ pc  0 ]T (2)

Figure 3: System con guration. 71

f C = [fCx ; fCy ; fCz ]T is the force at the COM of the robot and nB = [nBx ; nBy ; nBz ]T is the torque around the body. On the other hand we obtain the force and torque at the origin as

Reactive Force Joint Angle and Body Attitude Free-leg Trajectory Planner with Discrete Inverted Pendulum Body Posture Reference Generator with Inverted Pendulum

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

= fCx = fCy f0z = fCz

f0y

Joint Angle and Body Attitude

= = n 0z =

n 0x

Figure 4: Biped walking control system. u = [ uR uL ]T uR = [ f R nR ]T uL = [ f L nL ]T   mI 3 H c12 M = H c21 H 22   I3 0 I3 0 K = [xRc 2] I 3 [xLc 2] I 3

n 0y

(3) (4) (5)

mpCz

mpCy

+ mg Jx !_ Bx Jy !_ By Jz !_ Bz

= = = = = =

fCx

fCy

fCz

nBx nBy

nBz

= fRx + fLx fCy = fRy + fLy fCz = fRz + fLz nBx = 0 xRz fRy + xRy fRz 0 xLz fLy + xLy fLz nBy = xRz fRx 0 xRx fRz + xLz fLx 0 xLx fLz nBz = 0 xRy fRx + xRx fRy 0 xLy fLx + xLx fLy

0 pBz fCy + pBy fCz + nBx 0 pBx fCz + nBy

(23) (24) + nBz : (25)

0 pBy fCx + pBx fCy

ZMPx = 0n0y =f0z ZMPy = n0x =f0z where ZMPx and ZMPy are in sagittal

(6) (7)

(26) (27)

and frontal planes, respectively. From the equations (20){(27) we have the equivalent system to (8){(13) as follows. m pCx

=

m pCy

=

mpCz

+ mg Jx !_ Bx Jy !_ By Jz !_ Bz

(8) (9) (10) (11) (12) (13)

fCz (pCx pCz fCz (pCy pCz fCz

0 ZMPx ) + 1fCx (28) 0 ZMPy ) + 1fCy (29)

= = pCz 1fCy + ndx = 0pCz 1fCx + ndy = nBz

(30) (31) (32) (33)

where 1fCx and 1fCy are new variables de ned by

1fCx 1fCy

where fCx

pBz fCx

Then the Zero Moment Point (ZMP) with respect to the origin are de ned by

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. d is a nonlinear term including gravity eect. f L and f R are the reactive forces at the center of left and right foot. nL and nR are the reactive torques at the same points. Consider the simpli ed model of the motion of the COM of the robot and the rotational motion of the body (1) as mpCx

(20) (21) (22)

f0x

(14) (15) (16)

=

0 npBy + (pCz 0 pBz )fCxp0 (pCx 0 pBx )fCz

=

nBx pCz

Cz

Cz

(34) (pCz 0 pBz )fCy 0 (pCy 0 pBy )fCz + pCz (35)

and ndx , ndy are the disturbances de ned by

+ nRx + nLx (17) + nRy + nLy (18) + nRz + nLz : (19) 72

= 0(pCz 0 pBz )fCy + (pCy 0 pBy )fCz(36) ndy = (pCz 0 pBz )fCx 0 (pCx 0 pBx )fCz : (37)

ndx

Then we can regard ZMPx , ZMPx , fCz , 1fCx, and nBz as the control inputs of the system (28){(33). Thus the feedback control law is designed

1fCy ,

as follows.

ZMPx

=

ZMPy

=

fCz

=

1fCy

=

1fCx

=

nBz

=

body is well controlled within 60.03 [m] and 60.04 [rad] (= 6 2.3 [deg]) errors. Also ZMPs are directly controlled as shown in Fig. 6 through the force controller where the ZMPs control the COM of the robot. Here, the length of foot is 0.18 [m]. The upper bound of the foot is +0:09 [m] and the lower bound is 00:09 [m] in sagittal plane Fig. 6 (a). In lateral plane Fig. 6 (b), the upper bound of the support domain is +0:155 [m] and the lower bound is 00:155 [m]. (Note that this case is in the double support.) Both in sagittal plane and in lateral plane the ZMPs are located within the support domain, i. e. this method guarantees the stability in a sense of the ZMP criterion. The parameters of the posture controller in (38){ (45) and (52) are shown in Table 1.

pCz m [K11 (pCx fCz

0 pref Cx ) + K12 p_Cx ] 0 pCx (38) pCz m ref [K21 (pCy 0 pCy ) + K22 p_ Cy ] 0 pCy fCz (39) ref 0m[K31 (pCz 0 pCz ) + K32 p_Cz ] + mg (40) Jx ref ) + K ! ] (41) 0 p [K41 (Bx 0 Bx 42 Bx Cz Jy ref ) + K ! ] [K51 (By 0 By (42) 52 By pCz ref ) + K ! ] 0Jz [K61 (Bz 0 Bz (43) 62 Bz

subject to the limitations ZMPlx  ZMPx ZMPly  ZMPy

Table 1: Parameters of posture controller.

 ZMPux  ZMPuy :

(44) (45)

Parameters m Jy K11 K21 K31 K41 K51 K61

Then the equivalent control input on the system (8){(13), that is the force input uc at the COM of the robot and around the body can be obtained by uc = [fCx ; fCy ; fCz ; nBx ; nBy ; nBz ]T (46) where fCx

=

fCy

=

= nBy =

nBx

fCz (pCx pCz fCz (pCy pCz pCz Cy

0 ZMPx )

(47)

0 ZMPy )

(48)

1f 0pCz 1fCx

ZMPlx ZMPly

w1 , w2 , w3 w4 , w 5 w6

(49) (50)

Parameters Values Jx 1.0 Jz 1.5 K12 2.0 K22 4.0 K32 20.0 K42 20.0 K52 20.0 K62 10.0 ZMPux 0.071 ZMPuy 0.148 w7 , w8 , w9 1.0 w10 , w11 0.25 w12 0.01

Fig. 7 shows the trajectories of the center of mass of the robot and the zero moment point in sagittal plane, where the ZMPs are used to control the COMs. The disturbance was imposed on the front of the robot at t = 5:0[sec] in Fig. 7 (a) and on the back at t = 4:0[sec] in Fig. 7 (b). The COMs recovered by the proposed control. Fig. 8 shows snapshots. The robot slightly changes its posture when the disturbances are imposed.

The force reference of both feet u is generated by the proposed simpli ed posture controller as follows. u = W 2 K T (KW 2 K T )01 uc (51) Here, K represents a transformation matrix from the force/torque of the both feet to the one of the body de ned in (7). This solution is the optimal in a sense that u has minimum square norm jjW 01 ujj2 . W is a weighting matrix de ned by W = diagfw1 ; w2 ; . . . ; w12 g:

Values 21.446 1.5 1.0 4.0 100.0 100.0 10.0 25.0 00:071 00:148 1.0 0.25 0.01

5

(52)

Conclusion

The hierarchical control of the biped robot based on the robust force control is proposed, which can locally suppresses the unknown disturbances on the terrain. In the experiments of the 14 axes biped robot, the attitude control of the body of the biped robot is realized.

4 Experimental Results The stable posture control of the robot is shown as Fig. 5. The position of COM and the attitude of the 73

0.08

0.04 0.03 0.02 0.01 0 −0.01

ZMP COM

0.06 ZMP, COM [m]

Position Error of COM [m]

0.05

x y z

−0.02 −0.03 −0.04 −0.05

0.04

disturbance

0.02 0 −0.02 −0.04 −0.06 −0.08

0

1

2

3

4 Time [sec]

5

6

7

8

0

(a) Error of COM position.

3

4 Time [sec]

5

6

0.08

roll pitch yaw

7

8

ZMP COM

0.06 ZMP, COM [m]

Angular Error of Body [rad]

2

(a) Disturbance on the front.

0.1

0.05

1

0

−0.05

0.04 0.02 0 −0.02 −0.04 disturbance

−0.06 −0.08

−0.1 0

1

2

3

4 Time [sec]

5

6

7

8

0

1

2

3

4 Time [sec]

5

6

7

8

(b) Error of rotation of body.

(b) Disturbance on the back.

Figure 5: Error of COM position and body rotation.

Figure 7: Trajectories of ZMP and COM in sagittal plane under disturbance.

ZMP in Saggital Plane [m]

0.08

References

ZMP in sagittal plane COM in sagittal plane

0.06 0.04

[1] Y. Fujimoto and A. Kawamura, \Robust Biped Walking with Active Interaction Control between Foot and Ground," Proc. IEEE Int. Conf. on Robotics and Automation, 1998. (to appear) [2] 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 Robotics and Automation, pp. 217{222, 1997. [3] J. Furusho and A. Sano, \Sensor-Based Control of a Nine-Link Biped," Int. J. Robotics Research, vol. 9, no. 2, pp. 83{98, 1990. [4] S. Kajita, T. Yamaura, and A. Kobayashi, \Dynamic Walking Control of a Biped Robot Along a Potential Energy Conserving Orbit," IEEE Trans. on Robotics and Automation, vol. 8, no. 4, pp. 431{438, 1992. [5] 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 Robotics and Automation, pp. 570{577, 1996. [6] A. Kun and W. T. Miller, III, \Adaptive Dynamic Balance of a Biped Robot using Neural Networks," Proc. IEEE Int. Conf. on Robotics and Automation, pp. 240{ 245, 1996. [7] 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. [8] 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

0.02 0 −0.02 −0.04 −0.06 −0.08 0

1

2

3

4 Time [sec]

5

6

7

8

(a) In sagittal plane 0.14

ZMP in lateral plane COM in lateral plane

0.12 0.1

ZMP in Lateral Plane [m]

0.08 0.06 0.04 0.02 0 −0.02 −0.04 −0.06 −0.08 −0.1 −0.12 −0.14 0

1

2

3

4 Time [sec]

5

6

7

8

(b) In frontal plane Figure 6: Measured ZMP trajectories. The position of COM and the attitude of the body is well controlled within 60.03 [m] and 60.04 [rad] (= 6 2.3 [deg]) errors. For the future work, the stepping control applied over the proposed control will be investigated through experiments, which will realizes the robust biped walking on any rough terrain. 74

(a) Without disturbance.

(c) With disturbance on the front.

(b) With disturbance on the side.

(d) Without disturbance.

(e) With disturbance on the back.

Figure 8: Snapshots of posture control. [10] J. Yamaguchi, A. Takanishi, and I. Kato, \Experimental Development of a Foot Mechanism with Shock Absorbing Material for Acquisition of Landing Surface Position Information and Stabilization of Dynamic Biped Walking," IEEE Int. Conf. Robotics and Automation, pp. 2892{2899, 1995.

Int. Conf. on Advanced Intelligent Mechatronics,

CDROM, 1997. [9] J. Yamaguchi, A. Takanishi and I. Kato, \Development of a Biped Walking Robot Compensating for ThreeAxis Moment by Trunk Motion," J. Robotics Society of Japan, vol. 11, no. 4, pp.581{586, 1993. (in Japanese)

75