Upper Body Joints Control for the Quasi static Stabilization of a Small-Size Humanoid Robot

Transcription

1 Upper Body Joints Control for the Quasi static Stabilization of a Small-Size Humanoid Robot Andrea Manni, Angelo di Noi and Giovanni Indiveri Dipartimento Ingegneria dell Innovazione, Università di Lecce via per Monteroni, 731 Lecce, Italy andrea.manni@unile.it, angelo.dinoi@unile.it and giovanni.indiveri@unile.it Abstract Given the intrinsic instability of walking humanoid robots, the design of controllers assuring robust stabilization of walking gaits is one of the most important goals. This paper describes a control architecture aiming at decoupling the problem of stable walking in the two relatively simpler problems of legs gait generation and upper body feedback control to guarantee dynamic stability. The upper body joints control law has the objective of stabilizing a proper dynamic stability index to a suitable reference value. Possible dynamic stability indexes include the ZMP (Zero Moment Point) or the FRI (Foot Rotation Index) points. As these points depend on joint accelerations and jerks that are not easily measured on a small size low cost platform, the presented implementation of the proposed architecture builds on the use of the GCoM point (Ground projection of the Center of Mass) that may be considered a suitable stability index only in quasi static robot states. The presented solution is particularly suited for small size robots having limited onboard computational power and limited sensor suits. The effectiveness of the proposed method has been validated through Matlab R simulations and experimental tests performed on a Robovie-MS VStone s platform. I. INTRODUCTION Interest in biped robots has rapidly increased over the last thirty years in view of developing robots capable of effectively moving in unstructured environments. In particular, anthropomorphic biped robots are potentially able to move in all the environments where humans do. In recent years, a number of humanoid robots capable of walking have been developed. These includes the Honda P3 and Asimo [1] or Sony Qrio [13], and Tokyo University s H6, whereas simpler designs include Vstone [18], Kondo [6] or RoboSapien [12], which has been developed for the toy market. Small humanoid robots can have from a few to over twenty actuated degrees of freedom and generally they carry some microcontroller electronics board for the low level control, i.e. to generate target signals for the actuators. The communication link between low and higher level control loops is most often of serial RS232 kind. The actuators are usually low power servo motors from the radio control (RC) models market. Biped locomotion is one of the most important issues to be faced. During walking, two different situations arise in sequence: the double support phase in which the mechanism is supported on both feet simultaneously, and the single support phase, when only one foot of the mechanism is in contact with the ground while the other is being transferred from the back to the front positions. Several methods have been presented in the literature to address the stability problem. Some of these are based on the inverted pendulum model for the biped legs [1], [14], [3]. Other techniques take directly into account dynamic stability indicators as the zero moment point (ZMP) [8], [7] or the foot rotation indicator (FRI) [4]. The ZMP was originally introduced by Vukobratović [17], [16] and is defined as the point in the ground plane about which the total moments due to the ground contacts become zero in the plane. The FRI, introduced by Goswami [5], is defined as the point on the foot/ground contact surface, within or outside the convex hull of the foot-support area, at which the resultant moment of the force/torque impressed on the foot is normal to the surface. Building on Lyapunov Theory based nonlinear control methods, in this paper we propose a control law for the upper body joints able to guarantee quasi static walking stability of a small size and low-cost humanoid robot. The presented control architecture allows to decouple the gait generation issue and the overall stability of the system. The analysis of stability is addressed on the basis of the Ground projection of the Center of Mass (GCoM) and the support polygon. The remainder of the paper is organized as follows: the proposed control method is derived in section II. Simulation and experimental results are presented in section III and, finally, conclusions are briefly discussed in section IV. II. CONTROL METHOD The general control architecture of the proposed method has been presented in [11] and it is shown in figure 1 for the sake of clarity. The basic idea is that the leg joints only are considered for locomotion planning while the upper body and arm joints are used for stabilization of the robot. This approach allows to decouple the gait generation and stabilization problems. Decoupling the gait planning and dynamic stabilization tasks is particularly important for small size robots that have limited computational power. In order for such decoupling to be effective, the cycle time of the stabilization controller needs to be suitably smaller than the gait period. Notice that this task division approach can be compared to [9] with the difference

2 Fig. 1. Control architecture. that in [9] the task division is implemented at an algorithmic level, while in the present case at a control architecture level. With reference to figure 1 q L, q L and q L are the acceleration, velocity and position, respectively, of the leg joints. Likewise q UB are the acceleration of the upper body joints, while q R and q R are the overall accelerations and positions, respectively, of the robot joints; v csp and r csp are the velocity and position of a target point in the ground plane to be used as reference value for a proper stability index as the ZMP, the FRI or the GCoM (Ground projection of the Center of Mass) that may be considered a suitable stability index only in quasi static robot states. Vectors q L and q UB are the estimates of the legs and upper body joint positions based upon the robots sensors; at last τ L and τ UB are the actuator leg torques and upper body torques. The gait generator block in figure 1 is a planner for the leg joints motions. The gait generator output is used to define the leg joint commands and it may use joint information also to perform obstacle avoidance planning or re-planning. As for the stability control, the direct kinematics model is used to compute the position of the center of the support polygon as a function of the joint values. The stabilization controller has as input the vector difference of the position of a target point inside the support polygon with the position of a proper stability index as the FRI (as reported in figure 1), the ZMP or the GCoM in quasi static cases. The control objective of this control system is to drive the above defined error to zero by acting on the upper body degrees of freedom only. A. Gait pattern generation In the literature the general issue of gait generation has been formulated in several ways including an optimization problem (eg. generation of energy optimal gait cycles), a path planning problem or a kinematics problem. In this work a kinematics approach is followed. After having derived the kinematics equations of the robots legs based upon Denavit-Hartenberg s convention, a point to point method [15] is used to generate joint trajectories. In order to generate a smooth motion, it is necessary that the boundary values of joint speeds and accelerations in the point to point time profile be all zero. To this extent a fifth order spline interpolation is used for each leg joint, namely q(t) = a 5 t 5 + a 4 t 4 + a 3 t 3 + a 2 t 2 + a 1 t + a (1) where t t f ; the a j determined as: : j = 1,..., 5 parameters can be a = M 1 q where a = [a a 1 a 2 a 3 a 4 a 5 ] T is the parameter vector and q = [q i q f q i q f q i q f ] T defines the desired polynomial boundary conditions. Matrix M R 6 6 is known by construction, and in particular, 1 t i t 2 i t 3 i t 4 i t 5 i 1 t f t 2 f t 3 f t 4 f t 5 f M = 1 2t i 3t 2 i 4t 3 i 5t 4 i 1 2t f 3t 2 f 4t 3 f 5t 4 f 2 6t i 12t 2 i t 3 i 2 6t f 12t 2 f t 3 f where q i = initial position, q f = final position t i = initial time, t f = final time q i =, q f =, q i =, q f =, t f > t i B. Implementation issues The stabilization feedback control loop described in figure 1 can be designed based upon a Lyapunov technique. Wanting the GCoM point to converge on a target point r t within the support polygon, a quadratic Lyapunov candidate function may be defined as: V = 1 2 (r t r GCoM ) T R (r t r GCoM ) (2) where R will be a symmetric positive defined matrix, and r t and r GCoM are the positions of a target point inside the support polygon and of the GCoM respectively. The time derivative of V will be given by: ) T ) V = (ṙ GCoM ṙ t R (r t r GCoM (3)

3 Fig. 3. Robot model in sagittal plane Calling q L, q UB the legs and upper body joint variables and J L (q), J UB (q) the legs and upper body Jacobian matrices such that ṙ GCoM = J L (q) q L + J UB (q) q UB, (4) equation (3) suggests to compute the reference value of the upper joint velocities as [ ] q UBd = J UB R (r t r GCoM ) J L q L + ṙ t (5) being J UB the weighted pseudo-inverse of matrix J UB. In case J UB should be full rank, J UB results in J ( ) UB = W 1 JUB T JUB W 1 JUB T 1 for some symmetric positive definite W of proper dimension. In case J UB should be rank deficient in some pose, J UB could be calculated on the basis of a singular value decomposition. Alternatively J UB could be taken to be a damped least squares inverse [15], hence avoiding singularity issues at the expense of control accuracy. Assuming J UB = W ( ) 1, 1 JUB T JUB W 1 JUB T the extra degrees of freedom provided by the entries of the positive definite weight matrix W can be eventually dynamically assigned in order to try avoiding link collisions and kinematics singularities. As for the q L, q L and ṙ t in the upper body joint control law (5), notice that q L and q L are known as they are generated by the legs gait controller and ṙ t is generated in such a way that r t is always within the support polygon. Generally ṙ t is designed such that during the single support phase r t moves within the support polygon in the same direction of the walking gait so that at the end of the single support phase, when the support polygon becomes the one of the double support phase, r t will be located approximately in its center. Considering only the sagittal plane, for the sake of simplicity, the robot model is represented in fig. 3. In this case the Jacobian matrices are reported in ( fig. 2 where M is the total mass of the robot and k 1 r m1 ) ( m 2 + m 3 + m 4 + m 5 + m 6 + m 7, k2 r m m ) ( 3 + m 4 + m 5 + m 6 + m 7, k 3 r m m ) ( 6 + m 7, k4 r m m ) 5, m k5 r 5 5 2, m k 6 r 6 6 2, k m 7 r and m i and r i are respectively the mass and the length of the i-th link as reported in Table I. III. SIMULATION AND EXPERIMENTAL RESULTS The presented control approach has been validated in the sagittal plane both in simulation and experimentally. The simulations have been performed in Matlab using the Robotics Toolbox realized by P. I. Corke [2]. The experimental implementation has been realized using the Robovie-MS VStone s platform. In figure 5 simulation results are displayed relative to a case where the robot starts from a double support phase: the right foot is moved upwards giving rise to a single support (on the left foot) phase during which the left knee is bend contributing to move the GCoM in the negative x direction. The target point to be used as reference for the GCoM is located at the center of the support polygon and it is assumed to have identically zero velocity. As expected, figure (5 (a)) shows that the error r t r GCoM is driven to zero by the action of the upper body joints (torso and arms). The parameters used for simulations are reported in Table I. In figures (5 (b)-(c)) the values of the upper body joints θ 3, θ 6, θ 7 and their velocities are reported. Panel (d) shows a stick diagram of the robot movement. In this simulation matrix W was the identity. In figure 6 simulation results are displayed relative to the same leg motion of the previous case, but with a different W matrix, namely: 1 W = 1 (6) 1 As expected, the torso moves less than in the previous simulation and GCoM error is driven to zero by the major action of arms joints. At last in figure 7 simulation results are displayed without upper body joints control, namely the upper body joints are kept still in their initial values, the same used in the previous cases. As expected the x coordinate of GCoM leaves the support polygon resulting in the loss of stability (i.e. the robot would fall). N Link Length (cm) Mass (g) 1 9, , , , , , , TABLE I ROBOVIE-MS PARAMETERS USED FOR THE SIMULATIONS. The robot used for the experimental validation is shown in figure 4. It is a Robovie-MS made by Vstone [18]. The robot has 17 degrees of freedom (DOFs): 5 in each leg, 3 in each arm and one in the head. It is 28 cm tall and has a total weight of about 86g. It has one 2 axis acceleration sensor and 17 joint angle sensors. The servos control board is composed by an H8 CPU at MHz, a 56KByte FLASH- ROM memory, a 4KByte RAM and a 128KByte External- EEPROM. Experimental results relative to the implementation of the proposed control law for the sagittal plane are reported in figure (8). Notice that, during the experimental validation tests, the robot needs to perform a dorsal plane movement (figure (8) (d)) to prevent the GCoM point from falling

4 J L (q)= 1 M 2 64 k 1 s 1 k 2 s 12 k 3 s 123 +k 4 s 1234 k 5 s k 6 s 1236 k 7 s 1237 k 2 s 12 k 3 s 123 +k 4 s 1234 k 5 s k 6 s 1236 k 7 s 1237 k 4 s 1234 k 5 s k 5 s k 1 c 1 +k 2 c 12 +k 3 c 123 k 4 c k 5 c k 6 c k 7 c 1237 k 2 c 12 +k 3 c 123 k 4 c k 5 c k 6 c k 7 c 1237 k 4 c k 5 c k 5 c k 3 s 123 +k 4 s 1234 k 5 s k 6 s 1236 k 7 s 1237 k 6 s 1236 k 7 s J UB (q)= 1 M 64 k 3 c 123 k 4 c k 5 c k 6 c k 7 c 1237 k 6 c 1236 k 7 c Fig. 2. Jacobian matrices to calculate ṙ GCoM. The notations s i...j, c i...j represent, respectively, sin(q i q j ), cos(q i q j ) fall in case the upper body joints were kept still did not cause the robot to fall when the upper body joints were controlled according to the presented strategy. Simulations performed at higher update frequencies and higher link velocities confirm the effectiveness of the presented solution. Near future work will focus on the implementation of this architecture using dynamic stability indexes as the ZMP or FRI that depend on the joint accelerations and jerks that are not easily available on low cost platforms. REFERENCES Fig. 4. Robovie-MS out of the single support phase support polygon in the z direction. Such dorsal plane motion is commanded in open loop. Moreover, as the joints are actuated by position servo motors, the control law (5) has been integrated in order to compute position commands for the upper body joints. Given the limited communication bandwidth with the joint position servo controllers, during the experimental tests the position commands (labeled as viapoints in figure (8)) were updated at very low frequency (approximately 1Hz). Nevertheless, as shown in figure (8), the sagittal plane component of the GCoM point converges to its target value. In particular, the experimental results reported in figure (8) refer to the same situation of simulations. Destabilizing x axis motion of the GCoM is automatically compensated for, in the sagittal plane, by the upper body joints controlled by the proposed law. Overall, the x coordinate of the GCoM converges to its constant target position approximately located at the center of the support polygon. IV. CONCLUSION A method to control the upper body joints of a humanoid robot in order to stabilize it, has been presented. This method allows to decouple the gait generation issue from the stabilization one. To the best of the author s knowledge, this approach appears to be novel. The proposed solution is particularly suited for small size, low cost humanoid systems having limited computational power. Although due to HW constraints the experimental validation was possible only at rather low control update frequencies (approximately 1Hz), extensive trials have shown that leg motions that would have caused the robot to [1] Honda (Inc), [2] P. I. Corke, A Robotics Toolbox for MATLAB, IEEE Robotics and Automation Magazine 3(1), pp [3] A. Goswami, B. Espiau and A. Keramane, Limit Cycles in a Passive Compass Gait Biped and Passivity-Mimicking Control Laws, Autonomous Robots, Vol. 4, pp , [4] A. Hoffman, M. Popovic, S. Massaquoi and H. Herr, A sliding controller for bipedal balancing using integrated movement of contact and noncontact limbs, in Proc. of the IEEE/RSJ International Conference on Intelligent Robots and System, (Sendal, Japan), 4. [5] A. Goswami, Postural Stability of Biped Robots and the Foot-Rotation Indicator (FRI) Point, The International Journal of Robotic Research, Vol. 18, No. 6, pp , [6] Kagaku (Co. Ltd), [7] R. Kurazume, T. Hasegawa and K. Yoneda, he Sway Compensation Trajectory for a Biped Robot, in Proc. of the 3 IEEE International Conference on Robotics and Automation, Taipei, Taiwan, 3. [8] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi and H. Hirukawa Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point, in Proc. of the 3 IEEE/RSJ Internationa Conference on Robotics and Automation, Taipei, Taiwan, 3. [9] O. Khatib, L. Sentis, J. Park and J. Warren Whole-Body Dynamic Behavior And Control Of Human-Like Robots, International Journal of Humanoid Robotics, Vol. 1, No. 1, pp , 4. [1] T. Tsuji and K. Ohnishi, A control of biped robot which applies inverted pendulum mode with virtual supporting point, AMC 2, Maribor, Slovenia, 2. [11] A. Manni, A. di Noi and G. Indiveri, A control architecture for dynamically stable gaits of small size humanoid robots, in Proc. of the 8th International IFAC Symposium on Robot Control SYROCO 6, September 6-8, Bologna (Italy) 6. [12] Wowwee (Robosapien), [13] Sony (Global), [14] M. Srinivasan and A. Ruina, Computer optimization of a minimal biped model discovers walking and running, Nature Letters to Editor, 11 Sept. 5. [15] L. Sciavicco and B. Siciliano, Modelling and Control of Robot Manipulators, Springer, Berlin,. [16] M. Vukobratović and B. Borovac, Zero-Moment Point - Thirty five years of its life, International Journal of Humanoid Robotics, Vol. 1, No. 1, pp , 4. [17] M. Vukobratović and D. Juricic, Contribution to the synthesis of biped gait, IEEE Trans. Bio-Medical Engineering, Vol. 16, No. 1, pp. 1-6, [18] Vstone (Co. Ltd),

5 x coordinate of GCoM (cm) Upper body joints (deg) 4 θ 3 θ 6 θ x coordinate of Ground projection of Center of Mass (a) (b) 3 Upper body joint velocities (deg/s) y (cm) dθ 3 dθ 6 dθ (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 final (dotted line) configuration and position of the center of the support polygon ( (hidden under )) and the position of the center of mass ( ) θ 3 θ θ 7 x coordinate of GCoM (cm) Upper body joints (deg) x coordinate of Ground projection of Center of Mass (a) (b) 3 25 Upper body joint velocities (deg/s) y (cm) dθ 3 dθ 6 dθ (c) x (cm) (d) Fig. 6. Simulation 2: (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 final (dotted line) configuration and position of the center of the support polygon ( (hidden under )) and the position of the center of mass ( )

6 8 x coordinate of GCoM (cm) x coordinate of Ground projection of Center of Mass Pos. Support Polygon y (cm) (a) x (cm) (b) Fig. 7. Simulation 3: (a) position of the x coordinate of GCoM (solid line) and position of target point (dashed line) and position of support polygon (dash-dot line); (b) stick diagram of start (solid line), middle (dashed line) and final (dotted line) configuration and position of the center of the support polygon ( ) and the position of the center of mass ( ) x coordinate of GCoM.7 Pos. CoM (cm) Viapoint Fig. 8. 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 final position