A Control Architecture for Dynamically Stable Gaits of Small Size Humanoid Robots

A CONTROL ARCHITECTURE FOR DYNAMICALLY STABLE GAITS OF SMALL SIZE HUMANOID ROBOTS

Andrea Manni ∗,1, Angelo di Noi ∗ and Giovanni Indiveri ∗

∗ Dipartimento di Ingegneria dell’Innovazione, Universit`a di Lecce, via per Monteroni, 73100 Lecce, Italy, Fax: +39 0832 297279, email:{andrea.manni, giovanni.indiveri, angelo.dinoi}@unile.it

Abstract: From the 1970’s biped robots have had a large attention from the robotic research community. Yet the issue of controlling dynamically stable walking for arbitrary biped robots is still open. We propose a simple control architecture based on the use of the FRI (Foot Rotation Indicator) point and the support polygon. The major advantage of the proposed architecture is that motion planning (and eventually sensor based re-planning (slower feedback loop)) is limited to the leg joints whereas the trunk and arm degrees of freedom are controlled in closed loop (faster feedback loop) to achieve overall dynamic stability. Such architecture allows to decouple the problem of dynamic stable walking in the two relatively simpler problems of gait generation and robot stabilization. This architecture is particularly suited for small size robots having limited onboard computational power and limited sensor suits. The eﬀectiveness of the proposed method has been validated through Matlabr simulations and experimental tests performed on a Robovie-MS platform. Copyright °c 2006 IFAC

Keywords: control architecture, dynamically stable gait, foot rotation indicator, support polygon.

1. INTRODUCTION over twenty actuated degrees of freedom and gen- erally they carry some microcontroller electronics Humanoid robots are enjoying increasing popular- board for the low level control, i.e. to generate ity as their anthropomorphic body allows the in- target signals for the actuators. Higher level con- vestigation of human-like motion and multimodal trol loops need by far more computational power communication. Currently, examples of advanced and are usually implemented either on industrial humanoid robots include Asimo (Honda, Inc) or controllers as, by example, PC104 or on handheld Qrio (Sony, Global), whereas simpler designs in- computers (Behnke et al., July 2005a)(Behnke et clude Vstone (Vstone, Co. Ltd), Kondo (Kagaku, al., May 2005b), such as Pocket PC and alike. The Co. Ltd) or RoboSapien (Wowwee, Robosapien), communication link between low and higher level which has been developed for the toy market. control loops is most often of serial RS232 kind. Small humanoid robots can have from a few to The actuators are usually low power servo motors from the radio control (RC) models market. 1 Corresponding author Fig. 1. Robovie-MS Fig. 2. Definition of the support polygon (single Biped locomotion is one of the most important (a) and double (b) support phases) issues to be faced: the basic characteristics of all biped locomotion systems are (Vukobratovićand joint trajectories for dynamic walking is an im- Borovac, 2004): portant research area: several methods have been presented in the literature. Some of these are (1) the possibility of rotation of the overall sys- based on the inverted pendulum model for the tem about one of the feet edges caused by biped legs (Tsuji and Ohnishi, 2002), (Goswami strong disturbances; et al., 1997). Other more complicated techniques (2) gait repeatability; take directly into account dynamic stability in- (3) regular interchangeability of single and dou- dicators as the zero moment point (ZMP) (Zhou ble support phases. et al., 2004) or the foot rotation indicator (FRI) In this paper, we propose a control architec- (Hoffman et al., 2004). We consider a control ture for the motion control of a humanoid robot architecture based on the FRI (Foot rotation In- that allows to decouple the gait generation issue dicator) (Goswami, 1999), which is a point on and the overall dynamic stability of the system. the foot/ground contact surface where the net The analysis of dynamic stability is addressed on ground reaction force would have to act to keep the basis of the Foot Rotation Indicator (FRI) the foot stationary. To ensure the absence of foot (Goswami, 1999). The remainder of the paper is rotations around any axis laying in the ground organized as follows: the used robot is described in plane, the FRI point must remain within the con- the section 2, the proposed architecture in section vex hull of the foot support area. The proposed 3. Implementation issue are discussed in section 4. control architecture is shown in figure 3. The basic Simulation and experimental results are presented idea, is that the leg joints only are considered for in section 5 and, finally, conclusions are briefly locomotion planning while the upper body and discussed in section 6. arm joints are used for dynamic stabilization. The aim is to design an architecture that allows to decouple the gait generation and dynamic stabilization problems. In essence, this architecture is 2. ROBOT DESCRIPTION inspired by classical task based control architec- tures (Sciavicco and Siciliano, 2000) of industrial The considered robot, shown in figure 1, is the robots that allow to design separate control laws Robovie-MS, a small humanoid robot kit made for concurrent, but different tasks. As for the gait by Vstone (Vstone, Co. Ltd). The robot has 17 generation, basically this will be commanded to degrees of freedom (DOFs): 5 in each leg, 3 in the leg joints based on an off line (or ”loose” feed- each arm and one in the head. It is 28 cm tall back) planning phase. Any of the standard gait and has a total weight of about 860g. It has planning approaches described in the literature one 2 axis acceleration sensor and 17 joint angle may be considered, eventually including obstacle sensors. The servos control board is composed avoidance tasks as suggested in figure 3. by an H8 CPU at 20 MHz, a 56KByte FLASH- ROM memory, a 4KByte RAM and a 128KByte With reference to figure 3, the gait generator is a External-EEPROM. planner for the leg joints only generating either leg joint torques (in case of a dynamic planner) or leg joint reference velocities (in case of a kinematics 3. CONTROL ARCHITECTURE planner). In either case the gait generator output is used to define the leg joint commands. As In this section we present the control architecture pictorially represented in figure 3, such planner proposed to obtain a dynamically stable gait for may use joint information to perform obstacle the biped robot. The issue of planning desired avoidance planning or re-planning. As for the Fig. 3. Control architecture. dynamic stability control, the direct kinematics be compared to (Khatib et al., 2004) with the model will be used to compute the velocity and difference that in (Khatib et al., 2004) the task position of the center of the support polygon as a division is implemented at an algorithmic level, function of the joint values. The support polygon while in the present case at a control architecture can be defined as the contact area between the level. humanoid robot and the ground; therefore in the single support phase, it is given by the sole in contact with the ground (seen figure 2(a)), while 4. IMPLEMENTATION ISSUES in the double support phase, it is given by the convex hull of contact points between the soles The dynamic stabilization feedback control loop and the ground (seen figure 2(b)). described in section 3 and in figure 3 can be de- The dynamic stabilization controller will have signed based upon a Lyapunov technique. Want- as input the vector difference of the position of ing the FRI point to converge on a target point r t a target point inside the support polygon with within the support polygon, a quadratic Lyapunov the position of the FRI, and its aim will be to candidate function may be defined as: drive this error to zero by acting on the upper body degrees of freedom. Indeed if FRI is in the 1 V = (r − r )T R (r − r ) (2) support polygon, we are sure that the robot’s 2 t F RI t F RI motion is dynamically stable. It should be noticed where R will be a symmetric positive defined that according to its definition (Goswami, 1999), matrix, and r and r are the positions of the FRI tends to the ground projection of the F RI t the FRI and of a target point inside the support center of mass (GCoM, in the sequel) as the joint polygon respectively. The time derivative of V will accelerations tend to zero. Namely indicating with be given by: r F RI and r GCoM the position of the FRI and of the ground projection of the center of mass ˙ T respectively, the following holds: V = (˙r t − r˙ F RI ) R (r t − r F RI ) = (3) ³ ´T ³ ´ ˙ δ := r FRI − r GCoM =⇒ lim δ = 0 (1) = − r˙ GCoM − r˙ t + δ R r t − r GCoM − δ ai,ω˙ i→0 being δ defined in equation (1). Calling q , q being a andω ˙ the linear and angular acceler- L UB i i the legs and upper body joint variables and J (q), ations of each link. Notice that δ is a continu- L J (q) the legs and upper body Jacobian matri- ous function of the link accelerations and that if UB ces such that |aj| < g ∀ j, then kδk is upper bounded. Decoupling the gait planning and dynamic sta- r˙ GCoM = JL(q)q ˙L + JUB(q)q ˙UB, (4) bilization tasks is particularly important for small size robots that have limited computational equation (3) suggests to compute the reference power. In order for such decoupling to be effec- value of the upper joint velocities as tive, the cycle time of the dynamic stabilization h i controller needs to suitably smaller than the gait † q˙UBd = J R (r − r ) − JLq˙L +r ˙ (5) period. Moreover the legs contribution to the FRI UB t GCoM t position will be regarded as a disturbance (namely † a non manipulable input) by the upper body con- being JUB the Moore Penrose pseudo inverse of troller. Notice that this task division approach can matrix JUB. Notice that the use of the ground projection of the center of mass in place of the FRI in the control law makes V˙ equal to

V˙ = − (r − r ) RT R (r − r ) + t ³ GCoM ´ t GCoM ˙ ˙ T T T + δ R δ − δ − δ R R (r t − r GCoM ) that is negative definite only in the limit of van- ˙ ishing δ and δ. Nevertheless the use of r GCoM in place of r F RI makes the control law compu- tationally much simpler as according to the FRI definition (Goswami, 1999),r ˙ F RI will depend on the joint accelerations and jerks. As for the qL, q˙L andr ˙ t in the upper body joint control law (5), Fig. 4. Robot model in sagittal plane notice that q andq ˙ are known as they are L L the Robovie-MS platform. generated by the legs gait controller andr ˙ is t In figure 5 simulation results are displayed relative generated in such a way that r is always within t to a case where the robot executes a complete step the support polygon. Generallyr ˙ is designed such t in the positive x axis direction. Initially the two that during the single support phase r moves t feet have a common x coordinate and the target within the support polygon in the same direction point r is at the center of the (initial) double of the walking gait so that at the end of the t support phase support polygon. While one of the single support phase, when the support polygon two feet stays on the ground, the other one moves becomes the one of the double support phase, r t forward giving rise to the single support phase. will be located approximately in its center. During this phase, the target point velocityr ˙ t is Considering only the sagittal plane, for the sake constant along the positive x axis direction until of simplicity, the robot model is represented in fig. r t reaches the border of the single support phase 2 4. In this case the Jacobian matrices are : support polygon wherer ˙ t is set to zero. As ex-   −k s −k s −k s −k s pected, figure (5) shows that the error r t −r GCoM 1 1 1 2 12 4 1234 5 12345 J (q) =  k c k c k c k c  is driven to zero by the action of the upper body L M 1 1 2 12 4 1234 5 12345 0 0 0 0 joints (torso and arms). The parameters used for simulations are reported in Table 1.

and N◦ Link Length (cm) Mass (g) 1 9,554 102 2 5,808 110 J (q) = UB   3 13,095 173 −k3s123 − k4s1234 −k6s1236 −k7s1237 4 5,808 110 1 5 9,554 102 =  k3c123 + k4c1234 k6c1236 k7c1237  M 6 15,343 121.5 0 0 0 7 15,343 121.5 Table 1. Robovie-MS parameters used where M¡ is the total mass of the robot and k , r m1 + m + m + m + m + m + for the simulations. 1 ¢ 1 2 ¡ 2 3 4 5 6 ¢ m , k , r m2 + m + m + m + m + m , 7 2 ¡ 2 2 3 ¢ 4 5 ¡ 6 7¢ m3 m4 k3 , r3 2 + m6 + m7 , k4 , −r4 2 + m5 , Experimental results relative to the implementa- m5 m6 m7 k5 , r5 2 , k6 , r6 2 , k7 , r7 2 and mi and tion of the proposed control law for the sagittal ri are respectively the mass and the length of the plane of the Robovie-MS platform are reported in i-th link as reported in Table 1. figure (6). Notice that, during the experimental validation tests, the robot needs to perform a dorsal plane movement (figure (6) (d)) to prevent 5. SIMULATION AND EXPERIMENTAL the GCoM point from falling out of the single RESULTS support phase support polygon in the z direction. Such dorsal plane motion is commanded in The presented control approach has been vali- open loop. Moreover, as the joints are actuated dated in the sagittal plane both in simulation by position servo motors, the control law (5) and experimentally. The simulations have been has been integrated in order to compute position performed in Matlab using the Robotics Toolbox commands for the upper body joints. Given the realized by P. I. Corke (Corke, 1996). The exper- limited communication bandwidth with the joint imental implementation has been realized using position servo controllers, during the experimental tests the position commands (labeled as viapoints 2 The notations si...j , ci...j represent, respectively, sin(qi+ in figure (6)) were updated at very low frequency ... + qj ), cos(qi + ... + qj ) (approximately 1Hz). Nevertheless, as shown in figure (6), the sagittal plane component of the 1st International Conference on Dextrous Au- GCoM point converges to its target value. In par- tonomous Robots and Humanoids (darh2005) ticular, the experimental results reported in figure Yverdon-les-Bains - Switzerland. May 2005. (6) refer to a situation where the robot starts from Corke, P.I. (1996). A robotics toolbox for MAT- a double support phase: the right foot is moved LAB. IEEE Robotics and Automation Maga- upwards giving rise to a single support (on the zine 3(1), 24–32. left foot) phase during which the left knee is bend Goswami, A. (1999). Postural stability of biped contributing to move the GCoM in the negative x robots and the foot-rotation indicator (fri) direction. Such destabilizing x axis motion of the point. The International Journal of Robotic GCoM is automatically compensated for, in the Research Vol. 18, No. 6, 523–533. sagittal plane, by the upper body joints controlled Goswami, A., B. Espiau and A. Keramane (1997). by the proposed law. Overall, the x coordinate Limit cycles in a passive compass gait biped of the GCoM converges to its constant target and passivity-mimicking control laws. Au- position approximately located at the center of tonomous Robots 4, 273–286. the support polygon. Hoffman, A., M. Popovic, S. Massaquoi and H. Herr (2004). A sliding controller for bipedal balancing using integrated movement 6. CONCLUSION of contact and non-contact limbs. in Pro- ceedings of the IEEE/RSJ International Con- A humanoid robot control architecture has been ference on Intelligent Robots and System, presented that allows to decouple the gait gen- (Sendal, Japan). eration issue from the dynamic stabilization one. Honda (Inc). www.world.honda.com/asimo. To the best of the author’s knowledge, this ap- Kagaku (Co. Ltd). www.kondo-robot.com. proach appears to be novel. The proposed so- Khatib, O., L. Sentis, J. Park and J. Warren lution is particularly suited for small size, low (2004). Whole-body dynamic behavior and cost humanoid systems having limited computa- control of human-like robots. International tional power. Although due to HW constraints Journal of Humanoid Robotics Vol. 1, No. the experimental validation was possible only at 1, 29–43. rather low control update frequencies (approx- Sciavicco, L. and B. Siciliano (2000). Modelling imately 1Hz), extensive trials have shown that and Control of Robot Manipulators. Springer. leg motions that would have caused the robot to Berlin. fall in case the upper body joints were kept still Sony (Global). www.sony.net/sonyinfo/qrio/. did not cause the robot to fall when the upper Tsuji, T. and K. Ohnishi (2002). A control of body joints were controlled according to the pre- biped robot which applies inverted pendolum sented strategy. Simulations performed at higher mode with virtual supporting point. AMC update frequencies and higher link velocities con- 2002, Maribor, Slovenia. firm the effectiveness of the presented solution. Vstone (Co. Ltd). www.vstone.co.jp. Several open issues need to be addressed in future Vukobratović,M. and B. Borovac (2004). Zero- work. These include handling of: (i) dynamic ef- moment point - thirty five years of its life. fects by estimating δ := r F RI − r GCoM and its International Journal of Humanoid Robotics time derivative, (ii) possible link collisions and Vol. 1, No. 1, 157–173. (iii) jacobian singularities. A possible strategy to Wowwee (Robosapien). www.wowwee.com. handle link collisions and jacobian singularities Zhou, C., P. K. Yue, J. Ni and S. B. Chan is to replace the pseudo-inverse of JUB in equa- (2004). Dynamically stable gait planning for tion (5) with a weighted pseudo-inverse such as ¡ ¢ a humanoid robot to climb sloping surface. −1 T −1 T −1 W JUB JUBW JUB for some symmetric Proceedings of the 2004 IEEE Conference positive definite W of proper dimension. The ex- on Robotics, Automation and Mechatronics, tra degrees of freedom provided by W can be used Singapore. to try avoiding collisions and kinematics singularities.

REFERENCES Behnke, S., J. Muller and M. Schreiber (July 2005a). Playing soccer with robosapien. In: In Proceedings of The 9th RoboCup Interna- tional Symposium, Osaka, Japan. July 2005. Behnke, S., J. Muller and M. Schreiber (May 2005b). Using handheld computers to control humanoid robots. In: In Proceedings of 4.5 60

4 40 3.5

3 20 θ 3 θ 2.5 6 0 θ 7 2

−20 1.5 Upper body joints (deg) x coordinate of GCoM (cm) 1 −40

0.5 −60 0 x coordinate of Ground projection of Center of Mass Pos. target point −0.5 −80 0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7 time (sec) (a) time (sec) (b) 50

−50

15 y (cm)

−100 10 Upper body joint velocities (deg/s)

−150 dθ /dt 3 5 dθ /dt 6 dθ /dt 7 −200 0 0 1 2 3 4 5 6 7 −15 −10 −5 0 5 10 15 time (sec) (c) x (cm) (d)

Fig. 5. Simulation 1: (a) position of the x coordinate of GCoM (solid line) and position of target point (dashed line); (b) θ3 , θ6 and θ7 position, (c) θ3 , θ6 and θ7 velocity, (d) stick diagram of start (solid line), middle (dashed line) and ﬁnal (dotted line) conﬁguration and position of the center of the support polygon (∗) and the position of the center of mass (¤)

Fig. 6. Experimental results 1: (a) position of the x coordinate of GCoM (*) and position of target point (dashed line); (b) Robovie-MS initial position, (c) Robovie-MS middle position, (d) Robovie-MS ﬁnal position