Internal Model Control for Improving the Gait tracking of a Compliant Humanoid Robot Luca Colasanto, Nikos G. Tsagarakis, Zhibin Li and Darwin G. Caldwell 

Abstract—This paper reports on the modelling and trajectory generation of an intrinsically compliant humanoid robot. To achieve adequate gait tracking performance in a compliant robot is not trivial and cannot be addressed with the traditional control approaches used for stiff robots. To permit the development of effective gait generators which take into account the additional dynamic effects due to intrinsic compliance, an appropriate model which can predict the robot motion dynamics is required. In this work, we propose a model which combines the inverted pendulum model approach with a compliant model (Cartesian) at the level of the COM. Based on this model which permits to predict the motion of the centre of mass (COM) of the compliant robot an Internal Model Control strategy is adopted to improve the gait tracking performance. The derivation of the model is introduced followed by experimental validation which demonstrates the tracking performance achieved by the proposed reduced model. The Internal Model Control is subsequently discussed and validated on the COmpliant huMANoid COMAN using a series of ZMP based walking gaits.

level and ability to adapt to interaction uncertainties. To permit the widespread of humanoid systems and allow the development of applications within domestic human living environment, humanoid robots should exhibit large adaptability to interaction uncertainly which may occur with humans or environment. To achieve this, the emphasis in the mechatronic design has been recently placed on the development of humanoid systems powered by actuators with intrinsic physical elasticity. The work in [8] introduced series elastic actuators to a biped walking robot and more recently this has been applied to the lower body of humanoid robots [9][10]. Recent studies have also demonstrated how these actuators can provide enhanced safety [11] and energy efficient [12]. COMAN robot [10], Fig. 1, is a bipedal robot developed following this approach with some of its joints equipped with intrinsically compliant actuators [13].

I. INTRODUCTION The complexity of the mechatronic design, the natural and adaptive locomotion and the human like behaviour and performance are some of the challenges that have driven the research and rapid growth of humanoid robots during the past decades with several prototypes developed by robotics community [1-7]. Looking in particular on the actuation system of these highly complex machines it can be observed that the predominant approach is the use of non-backdrivable, stiff transmission systems, combined with high gain PID controllers. This highly nonbackdrivable and large stiffness approach has been mostly inspired from industrial robotics designs where the necessity for precision movement and high load disturbance rejection is predominant. Although this industrial approach gives to the humanoid robots high precision, the resulting large output mechanical impedance makes these inherently unsafe when interacting with humans or environment. Safety is not the only feature being compromised in these “stiff” humanoids. Despite the fact these stiff humanoid designs represent outstanding engineering prototypes, when compared to biological systems they have significant performance handicaps relating particularly to their energy efficiency, peak power Luca Colasanto ([email protected]), Nikos G. Tsagarakis ([email protected]), Zhibin Li ([email protected]) and Darwin G. Caldwell ([email protected]) are with the Department of Advanced Robotics, Istituto Italiano di Tecnologia (IIT), Genova 16163, Italy

Fig.1 The mechanical assembly of the “COMAN” lower body

The dynamic behaviour of such a compliant humanoid significantly differs from that of the stiff robot. During walking, compliance contributes to the reduction of the impact disturbance during the take off and touchdown of the swing foot since it introduces a proportional action to the position error (like an intrinsic proportional controller). However, at the same time it deteriorates the foot position tracking during the rest of the walking cycle of the robot, which may result in larger uncertainties on the tracking of reference trajectories such as the COM reference, or, in case of ZMP based trajectory, the ZMP reference [14]. Therefore, while ZMP based gait trajectories have been successfully applied to stiff humanoid where high precision tracking performance is possible, their application to compliant humanoid systems is not trivial due to the deterioration of the joint tracking

performance. To permit the application of these trajectory generation techniques also to compliant systems as well to develop control schemes for these compliant robots appropriate models which can effectively predict the dynamic behaviour of these robots are required. To estimate the dynamic motion behaviour of the compliant robot we propose a model which combines an inverted pendulum model [15] with a compliant model at the level of the COM. Based on this model an Internal Model Control (IMC) strategy is adopted to improve the gait tracking performance. The proposed model and control scheme were implemented and experimentally validated on the humanoid robot COMAN. The presentation of the work is organized as follows: Section II introduces some details of the compliant humanoid COMAN. Section III reports on the system modelling starting from the model of the compliant joint existed in COMAN and progressively presenting the equivalent compliant Cartesian model at the level of the COM. Results from the experimental of the model are also reported. In Section IV the IMC controller and its implementation is explained while the experimental validation of the controller is discussed in Section V. Finally, Section VI addresses the conclusions.

the hips) of 176 mm and 110 mm, respectively. The total lower body weight is 17.3 kg, with each leg weighing approximately 5.9 kg, and the waist, including the hip flexion motors, weighing 5.5 kg. III. COMPLIANT HUMANOID MODELLING To permit the effective control of the compliant humanoid a model which can accurately describe the system behaviour is required. Following a bottom up approach, the model of the compliant joint is initially identified and subsequently used to derive a Cartesian spring mass damper model at the level of the centre of mass (COM). A. Joint Model COMAN robot had a modular structure. Two different types of joints exist: The first type is a “stiff” joint actuated by a motor and a harmonic reduction drive group. The passive compliance in this joint is due to the elasticity of the harmonic gearbox drive ( ). The second type of joint is called “compliant” and incorporates an additional physical elasticity. The additional elastic mechanism is in series with the harmonic drive and is characterized by stiffness .

II. COMPLIANT HUMANOID COMAN The mechanical structure of the leg of COMAN humanoid and an overview of its kinematics with the location of the D.O.F are illustrated in Fig. 2.

Fig. 3 Joint model

Fig. 3 presents a schematic model of the joint. According to this representation and by adapting the model form of [16] the joint can be described by the following equations: =

(1) (2)

where , and are the position, velocity and torque of the motor respectively reflected at the link side after the gear reduction: (3) (4) Fig.2 COMAN leg assembly and kinematics

The lower body includes the lower torso (housing the 3 DOF waist module), and the two leg assemblies with 6 DOF each [10]. The leg of COMAN incorporates two series elastic (SEA) actuation units, which are placed at the knee flexion and the ankle dorsiflexion joints. These SEA units are based on a compliant actuator developed in [13]. Each joint is equipped with three position sensors and one torque sensor. This permits to measure the position of each joint before and after the elastic transmission. Moreover, two six axis force/torque sensors are mounted below the ankle in order to measure the interaction forces with the ground. The height of the COMAN lower body, from the foot to the waist, is 671 mm, with a maximum width and depth (at

where N is the gear ratio (N=100:1), and are position and torque of the motor. and are inertia and damping of the motor reflected to the link side as follow: (5) (6) where and are the torque sensitivity and back EMF constant, is the stator resistance and is the physical damping of the motor. Finally, , , , and are the position, velocity, torque, inertia and damping of the link respectively and is the resultant joint stiffness (

for the stiff joint and

for the case of a

compliant joint). In the case of the compliant joint the resultant joint stiffness can be approximated to since

is much larger than . The overall leg joint stiffness and damping matrices can then be defined as 6x6 diagonal matrices positive-definite , , i={1,6}. B. Cartesian Model at the COM Given the compliant joint model introduced in the previous section, the compliant robot dynamics are approximated in this work by an equivalent Cartesian spring mass model at the level of the COM. The resultant Cartesian stiffness matrix at the pelvis level (COM) can be obtained as a function of the joint stiffness matrix .

where are sub matrices of related to the linear motion along x, y and z. In case of diagonal matrices x, y and z dynamic are completely decoupled, however, this is not the case for the matrices in (7) and (8) in which the off diagonal elements are different from zero. It means that decoupling the movement of the robot is no feasible.

(7) where is the inverse of transpose of the Jacobian matrix of the COM with respect to the feet. In a similar manner, the Cartesian damping matrix at the pelvis level (COM) can be obtained from the diagonal joint damping matrix as follows: (8) The reduced dynamical model of the compliant robot is developed with the following considerations in mind: (A1) The joints positions before the elastic transmission are controlled with a stiff PID loop. (A2) The elasticity in the joint transmission system is due to the harmonic drive compliance as well due to additional physical elasticity integrated in some of the pitch joints of the leg (knee and ankle), see Section II. (A3) A single mass approximation is used for the robot model. Due to the first assumption it is possible to reduce the complexity of the model. Assuming an ideally stiff position control (motor position error equal to zero) combined with a high reduction ratio (minimum back-drivability) the dynamics of the motor described by (1) can be ignored when the robot is subject to external force perturbations. In this case, (2) can approximate the overall joint/link dynamic because the dynamics of the controlled actuator is much faster than the dynamics of the transmission. The consequence of (A2) is that the level of compliance is high in sagittal plane of the humanoid robot (due to additional elasticity in the knee and ankle pitch joints) while in lateral direction the robot is stiffer (only the compliance of the harmonic reduction drive contributes to this). Experiment results confirm the above by demonstrating large deviations of the pelvis (COM) position during the walk along the sagittal and vertical directions x and z and smaller deviations along the lateral direction y according to foot frame shown in Fig.4. Because of that, in y direction the movement can be approximated by a stiff system. Let now considered the forces generated at the pelvis (COM) frame when the COM position is deflected from its reference position vector to a position . The generated forces can be related to the COM deflection and velocity as follows: (9)

Fig. 4 Robot model, COM and associated support feet reference frame

Although as all humanoids COMAN is also a distributed mass system in this work we approximate the robot with a single mass model. This is a common approach which has been used in trajectory generation and control of humanoid robot [3]. Our interest is to validate if this is also applicable in the case of an intrinsically compliant humanoid. Therefore, according to (A3), the dynamics of the robot is approximated by the dynamics of the single mass placed at the pelvis (COM location). Due to the intrinsic joint elasticity and passive damping the linear passive dynamics of the single mass model can be described by the following expression: (10) where mass

with being the total placed at the COM. is the Cartesian forces given by (9) and represents the gravity. By omitting the passive dynamics along y (lateral direction, physical elasticity is not present in the joints contributing to that direction) and considering only the passive dynamics along the sagittal and vertical directions, equation (10) can be written in a matrix form as follows of

the

robot

(11) where represent the forces along x and z directions due to the equivalent Cartesian stiffness and damping. Considering (9), the model in (11) can be further extended as follows: (12)

where , , , , , ,

and are the relevant elements of are the relevant elements of ,

230

Force right leg Force left leg Average value

Z force [N]

180 130

(14)

Table 1 reports all the values of the estimated coefficients after tuning showing only minor adjustments on the estimated model. TABLE I MODEL PARAMETERS 0.96

0.99

1.05

1.00

0.99

0.99

1.00

1.05

Fig. 6 compares the model response with the experimental data recorded from the robot. The solid blue line is the reference trajectory sent to the robot and used as an input reference of the model. Moreover the model receives the initial position and velocity at the beginning of the first double support phase. The experimental motion of the COM (black broken line) was derived from the joint angles measured by the joint encoders. The solid red line is the model response. 0.06 0.04

X [m]

The model in (13) is used during the double support phase. The scaling coefficients and are computed from experimental data which evaluates the x and z forces measured by the force/torque sensors installed at the feet of the robot. In Fig. 5 the measured contact forces along the vertical direction z during a single walking cycle are presented. The red line is the average value of the right leg vertical force component during the single support and the two double support phases. The force along z direction measured by the sensor of the left leg is represented by the dot blue curve. It has almost the same profile with that of the right leg (solid blue) with an expected phase lag.

C. Model Tuning and Experimental Verification The developed model is based on some assumptions (Section III) which allow reducing the model complexity but at the same time they may affect its accuracy. In order to match the approximated model to the real system, each parameters of the model and has been scaled by a coefficient as follows:

0.02 0 Reference Real Model

-0.02 -0.04 0

0.5

1

1.5

2

0.47

2.5 Time [s]

3

3.5

4

4.5

0.46

Z [m]

end are Cartesian position reference of the COM, and , , , , are position, velocity and acceleration of the COM when it is subject to external loads. The reference trajectories used in this work are fixed height ZMP based trajectories. The gait of each trajectory can be described in terms of step length (sl), single support duration ( ) and double support duration ( ). Equation (12) describes the system during single support. In fact during this phase the robot stands on a single leg with the compliant joints of the support leg mostly affecting the robot movement. During the double support both legs are on the ground. In this configuration the passive elasticity from both legs affect the robot dynamics. To take into account the effect of the second leg during the double support phase a similar procedure as the one described in this section can be used for the second leg in order to derive the forces at the COM along x and z as generated due to the deflection of the second leg. However to reduce the overall model complexity a different approach was adopted in this work. During the double support phase it is assumed that the two feet on the ground do not move relative to each other. The forces developed from the two legs are different because of the different configuration of the two legs. It is possible though to include the effect of the second leg by scaling (11) as follows: (13)

0.45 Reference Real Model

0.44 0.43 0

0.5

1

1.5

2

2.5 Time [s]

3

3.5

4

4.5

Fig. 6 Model and real system comparison gait sl=0.06m, =0.6s, =0.2s

80 30 -20 0

0.2

0.4

0.6

0.8

1

1.2

Fig. 5 Force along z: gait sl=0.03m,

=0.5s,

=0.2s

Time [s]

1.4

According to the average force value in the two legs the z force distribution was estimated approximately as 58% to the back leg and 42% to the front leg. Accordingly, during the first double support phase is set to and during the second double support phase is equal to . Similarly, for the x direction during the first double support phase and during the second support phase .

The gait used in this experiment was different from the gait used to tune the parameter of the model. The model was also tested with different gaits (0.03m
respectively. The reference signal denotes the desired trajectory, is the control signal and is the true trajectory. If the model of the plant P is perfectly known C is the exact representation of the inverse of the plant model. In a real system a feedback loop is generally added to compensate for errors in the plant model.

where the subscript k represent the time instant , and are the x and z desired COM position, and their first and second derivate are approximate using the Euler’s backward differentiation method. B. Controller Implementation Fig. 8 presents the schematic of IMC strategy implemented in the robot.

Fig. 7 Schematic of Feed forward IMC

A. Inverse Model The model in (14) can be expressed as follow: (15) where model, is the input and model can be expanded as follow:

is the state of the is the output. The

Fig. 8 IMC control scheme

The reference trajectories for the robot were generated from the Trajectory Generator block which is based on the ZMP approach. The controller block implements equation (18). To compute and , the block needs to know the walking phase. The phase detector block identifies the actual robot phase using the measurements of the force/torque sensors mounted on the feet of the robot, see Fig. 9. Right Left

200

4

(16) Z force [N]

Phase

3

2

1 0 0

0.5

1

1.5

2

2.5

Time [s]

Fig. 9 Phase detector: Phase 1 (Right single support); Phase 2 and 4 (double support); Phase 3 (left single support)

For simplicity, in (16) and in the rest of the paper we omit the tuning coefficients introduced in (14). The model of the system is time continuous, non linear (note that the elements of and matrix are not linearly related to the robot configuration and depends also from the actual phase of the robot) and time invariant. According to (15), the inverse model can be express as follows: (17) where is the pseudoinverse of matrix. Referring to (17) the discrete function implemented in the controller can be expressed as follows:

(18)

Finally the reference trajectories of the joints were computed from the reference COM position ( , , ) through inverse kinematics. V. EXPERIMENT RESULTS The control framework described in section IV.B was evaluated on the robot demonstrating good matching performance. Fig. 10 reports one of the trials performed with the COMAN robot. The black dashed line is the experimental trajectory of the COM derived from the joint angles measured by the joint encoders. The blue solid line is the desired trajectory developed by the trajectory generator and the red solid line is the controller modified reference signal. During the first four seconds the controller was not enabled and the desired trajectory has sent directly to the robot as reference. During this uncontrolled walking the robot did not fall down as the desired trajectory was designed with a very restricted stability constraint (theoretical ZMP trajectory well inside an area far from the feet edges). However the movement of

the COM demonstrated large deviations from the desired. After the initial four seconds the controller was activated and the modified reference was sent to the robot. 0.08

Desired Reference Real

X CoM [m]

0.06 0.04 0.02 0 -0.02 -0.04 0

1

2

3

4 Time [s]

5

6

7

8

0.47

Z CoM [m]

0.46 0.45 0.44 0.43

Desired Reference Real

0.42 0.41 0

1

2

3

4 Time [s]

5

6

7

8

Fig. 10 Controlled and uncontrolled walking gait sl=0.04m, =0.6s, =0.2s

The improved tracking performance of the desired COM trajectory after the activation of the IMC controller can be confirmed in the second half (4-8sec) of Fig. 10. The deviation of the reference input from the desired one due to the activation of the IMC can be also easily noticed. This modified reference compensates for the system dynamics according to the model introduced in the previous sections and finally results in the improvement of the desired COM trajectory tracking. To mention here that this improvement is achieved by only employing the open loop Inverse Model Control without the use of feedback control. The above results demonstrate also the effectiveness of the reduced model employed in the IMC strategy. VI. CONCLUSION AND FUTURE WORKS In this work, a reduced model and control method to improve the trajectory tracking of a compliant robot during walking has been proposed. A model of the compliant dynamics at the level of the COM of the robot has been presented and experimentally validated. Based on that model an Internal Model Control strategy has been implemented and experimentally validated on the COMAN robot. The proposed model have shown good efficacy in representing the dynamics of the compliant biped. Based on the derived model the IMC strategy in a feed forward scheme was subsequently applied to the COMAN robot and its performance was experimentally evaluated demonstrating improved gait tracking performance. To further improve the tracking performance and the robustness of the controlled system, a closed loop IMC will be evaluated in the future work. ACKNOWLEDGMENT This work is supported by the FP7 European project AMARSI (ICT-248311).

REFERENCES [1]

K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka, “The development of Honda humanoid robot,” IEEE International Conference on Robotics and Automation, pp. 1321–1326, 1998. [2] R. Hirose and T. Takenaka, “Development of the humanoid robot ASIMO,” Honda R&D Technical Review, vol. 13, no. 1, pp. 1–6, 2001. [3] S. Kajita, M. Morisawa, K. Miura, S. Nakaoka, K. Harada, K. Kaneko, F. Kanehiro, and K. Yokoi, “Biped walking stabilization based on linear inverted pendulum tracking,” in IEEE/RSJ Intelligent Robots and Systems, pp. 4489–4496, 2010. [4] Y. Ogura, H. Aikawa, K. Shimomura, A. Morishima, H. Lim, and A. Takanishi, “Development of a new humanoid robot wabian-2,” in IEEE Robotics and Automation, pp. 76–81, 2006. [5] N.G. Tsagarakis, G. Metta, G. Sandini, D. Vernon, R. Beira, F. Becchi, L. Righetti, J.S. Victor, A.J. Ijspeert, M.C. Carrozza and D.G. Caldwell, “iCub: the design and realization of an open humanoid platform for cognitive and neuroscience research,” Advanced Robotics, vol. 21, No. 10, pp. 1151 – 1175, 2007. [6] N.G. Tsagarakis, B. Vanderborght, M. Laffranchi , and D. G. Caldwell, “The mechanical design of the new lower body for the child humanoid robot ‘iCub’”, IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4962–4968, 2009. [7] N.G. Tsagarakis, F. Becchi , M. Singlair, G. Metta, D.G. Caldwell and G. Sandini, “Lower body realization of the baby humanoid‘iCub’,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3616–3622, 2007. [8] Pratt, J., Pratt, G. 1998 “Intuitive Control of a Planar Bipedal Walking Robot”in IEEE International Conference on Robotics and Automation (ICRA '98), Leuven, Belgium 1998. [9] Jerry E. Pratt, Ben Krupp, Victor Ragusila, John Rebula, Twan Koolen, Niels van Nieuwenhuizen, Chris Shake, Travis Craig, John Taylor, Greg Watkins, Peter Neuhaus, Matthew Johnson, Steve Shooter, Keith Buffinton, Fabian Canas, John Carff, William Howell. “The Yobotics-IHMC Lower Body Humanoid Robot” in IEEE/RSJ International Conference on Intelligent Robots and Systems, October 11-15, 2009 St. Louis, USA [10] N. Tsagarakis, Z. Li, J. Saglia, and D. G. Caldwell, “The design of the lower body of the compliant humanoid robot “cCub”,” in IEEE International Conference on Robotics and Automation, pp. 2035 2040, 2011 [11] M. Laffranchi, N. G. Tsagarakis, and D. G. Caldwell, "Safe human robot interaction via energy regulation control," Proc. of IEEE Int. Conf. on Intelligent Robots and Systems, St. Louis, US, 2009, pp. 35-41.on Robotics and Automation, May, 2012, USA. [12] B. Vanderborght, R. Van Ham, B. Verrelst, M. Van Damme, and D. Lefeber, "Overview of the lucy project: Dynamic stabilization of a biped powered by pneumatic artificial muscles," Advanced Robotics, vol. 22, no. 25, pp. 1027-1051, 2008. [13] N. Tsagarakis, M. Laffranchi, B. Vanderborght, and D. Caldwell, “A compact soft actuator unit for small scale human friendly robots,” in IEEE International Conference on Robotics and Automation. IEEE, pp. 4356–4362, 2009. [14] M. Vukobratovic and B. Borovac, Zero-moment point - thirty ¯ve years of its life, Journal of Humanoid Robotics 1-1 (2004) 157-173. [15] S. Kajita, T. Nagasaki, K. Yokoi, K. Kaneko, and K. Tanie, “Running pattern generation for a humanoid robot,” in Proc. IEEE ICRA, 2002, vol. 3, pp. 2755–2761 [16] M. Spong, “Modelling and control of elastic joint robots,” Trans. ASME : J. Dyn. Syst., Meas., Control, vol. 109, pp. 310–319, 1987. [17] Constantin G. Economou, Manfred Morari,, “Internal Model Control. Design Procedure for Multivariable System” in Ind. Eng. Chem. Process Des. Dev., 1985, 24, pp 472–484 [18] Constantin G. Economou, Manfred Morari, Bernhard O. Palsson, “Internal Model Control. Extension to Nonlinear System” in Ind. Eng. Chem. Process Des. Dev., 1986, 25 (2), pp 403–411 [19] A. Henson and E. Seborg, “An internal model control strategy for nonlinear systems” in AIChE Jl, 37 (1991), pp. 1065–1081