Autonomous swing up of a robot gymnast

E.E. Eldukhri, D.T. Pham, A.A. Fahmy, N.B. Zlatov

Manufacturing Engineering Centre, Cardiff University, Newport Road, Cardiff CF24 3AA, UK

Abstract In this paper the problem of swing up control of an under-actuated, three-link robot gymnast (Robogymnast) is discussed. Robogymnast mimics the human acrobat who hangs from a high bar and tries to swing up to an upsidedown position with his/her hands still on the bar. Unlike human acrobats, Robogymnasts hands are firmly attached to a freely rotating high bar mounted on ball bearings. Although this helps during the swing up phase, it poses a great challenge to balancing the robot at the upright position. The motion of Robogymnast was initiated by applying regularly changing sinusoidal torques to the two motors located at its shoulder and hip joints. As the swing angle increases, the amplitudes of the applied sinusoidal torques were increased while their respective frequency was reduced proportionately. Experimental results showed successful swing up of Robogymnast from the stable downward position to the inverted configuration. Keywords: Robot gymnast, swing up control, modelling, inverted pendulum.

1. Introduction Under-actuated mechanisms provide a useful testbed for evaluation and comparison of different control techniques. They are inherently nonlinear systems and present challenging problems [1, 2]. Many researchers studied the problem of balancing these mechanisms in the inverted position and controlling their motion along their unstable manifold. For example, Spong [3] investigated the problem of swing up control of a two-link robot (Acrobat). He based his control algorithm on the method of partial feedback linearization [4]. In this technique, the control linearises and decouples the motion of the second link from that of the first link in a nonlinear inner loop. The motion of the first link which is excited by the motion of the second link represented internal dynamics. He then designed an outer loop control to command the motion of the second link as a feedback function of the velocity of the first link. The combined strategy worked by creating unstable zero dynamics which

drove the first link away from its open loop stable position toward the inverted configuration achieving the swing up goal. The balancing of Acrobot in inverted position was accomplished by switching to a previously developed linear quadratic regulator (LQR) designed to capture and balance the robot at the top of its swing. Based on Spongs work [3], Brown and Passino [5] developed a combination of traditional and intelligent control techniques for swing up and balancing of Acrobot. They used classical, fuzzy and adaptive controllers to balance the acrobat in its inverted unstable equilibrium region. For the swing up control, they applied a proportional-derivative (PD) controller with inner loop partial feedback linearization, a state feedback and a fuzzy controller to swing up the acrobat from its equilibrium position to the inverted region where the balancing controller can catch it and balance it. In addition, they used genetic algorithms technique to further tune the balancing and swing up controllers. Pham et al., [6] used their newly developed Bees Algorithm (BA) to tune the parameters

of a fuzzy logic controller designed to stablise and balance Acrobot [3] in the upright position. The BA is a swarm-based optimisation algorithm that mimics the food foraging behaviour of honey bees [7]. To decide appropriate values for the parameters of the fuzzy membership functions and construct the rule base in order to achieve the required performance, they first used the Acrobot model [3] to develop a linear quadratic regulator to obtain the scaling gains needed to design the fuzzy logic controller. Nevertheless, the problem of balancing robot gymnasts in the upright position is typical to the problem of stabilising inverted pendulums which has been extensively studied for control education and research purpose [1, 2, 8, 9, 10, 11, 12, 13]. In particular, Medrano-Cerda [2] developed a robust digital state feedback controller for stabilising a triple inverted pendulum mounted on a cart using only one actuator (DC motor) to drive the cart. The controller was designed using linear quadratic regulator theory. It was based on an approximate discrete-time model of the pendulum-cart system. A Reduced-order observer was designed to reconstruct the missing (unmeasured) states. Eltohamy et al. [13] followed another approach to solve the problem addressed in [2]. Because the traditional linear design process could not incorporate the nonlinear dynamics of the inverted pendulum system, their controller design was formulated as an optimisation problem which accounted for the physical limitations and stability criterion of the system as infinite-dimensional nonlinear inequality constraints. A comprehensive historical survey concerning inverted pendulums is given in [14]. This paper addresses the challenging problem of autonomously swinging up a three-link, under-actuated robot gymnast (Robogymnast) which is attached to a freely rotating high bar (joint 1). It has two actuators (DC motors) located at the shoulder (joint 2) and hip (joint 3) joints to control its motion. The arms (without elbow and wrist joints), body (head, neck and trunk as a single rigid part) and legs (without knee joints) correspond to links 1, 2 and 3 respectively. The main part of each link is composed of two rigid carbon fibre tubes. The remainder of the paper is organised as follows. Section 2 describes the experimental setup of Robogymnast and its mathematical model. In section 3, the swing up control problem is investigated. Section 4 presents the experimental results. The conclusions and further work are given in section 5.

2. System description and mathematical modelling The Robogymnast is depicted in Fig. 1 and its schematic diagram is shown in Fig. 2. The main part of each link is made up of two 50 mm diameter carbon fibre tubes which are rigid, cheap and easy to cut and weigh 0.213 Kg/m. At both ends of each link, aluminium components (3 mm in thickness) are attached to provide the structures for mounting sensors and actuators [1]. The first joint consists of a steel shaft mounted on ball bearings. At one end of the shaft a potentiometer is mounted to measure the angle of link 1 (arms). The hinges for link 2 (head, neck and trunk) and link 3 (legs) are spilt into two sections. One section is a short steel shaft mounted on ball bearings and attached to a potentiometer which measures the relative angles between adjacent links. The second section is the output shaft of a DC motor/gearbox. Planetary gearboxes with a rated continuous capacity of 4.5 Nm were chosen. Backlash in the gearboxes (about 0.05 radians) can become a problem but the mechanical design avoids the use of timing belts and is flexible enough to allow changes in the lengths of the links as well as adding mass to the robot.

Arms

Shoulders

` Hip Ball bearings

Legs

Fig. 1. Robot Gymnast (Robogymnast)

Motor/gearbox

Pot

Couplings

a1
l1 1 a2 l2

m1, I1 T1 2

m2, I2 T2

0 0 0 0 1 0 0 B= , C = 0 15.19 0.74 0 28.92 0.62 1.32 16.21 & x =

0 0 0 0 0 1 0 0 0 0 0 1 0 0 0

motors ( u1 , u 2 10 V). y = [ 1 2 1 3 2 ]T
m3, I3

u = [u1

u 2 ]T , u1 and u 2 are the input voltages to the

& & & & 2 1 3 2 ,

a3

l3

Fig. 2. Schematic Representation of Robogymnast

For modelling purpose, Robogymnast is regarded as a triple pendulum in a stable equilibrium configuration (see Fig. 2). The mathematical model is derived via Lagranges equations along the same lines in [1, 10]. Details and parameter values are given in Appendix A. The linearised continuous-time model in terms of the generalised relative coordinates is calculated using Matlab/toolboxes and additional Mfiles developed by the authors. It is given by: & x = Ax + Bu (1) y = Cx (2) where 0 0 0 1 0 0 0 0 0 0 1 0 , A= 0 0 0 0 0 1 A22 A21

is the output vector. The block diagram of the experimental apparatus is shown in Fig. 3. The computer and system interface consists of amplifiers and first-order filters. These filters reduce aliasing effects introduce by sampling the systems outputs and smooth the control signals sent to the power amplifiers/motor drive units [1]. The filters dynamics were not included in the mathematical model given in equations 1 and 2. Matlab/toolboxes and additional M-files developed by the authors were employed to carry out simulations and analysis of the Robogymnast discrete-time model obtained from equations 1 and 2 using a sampling time of 25 ms. Power Amplifiers Robogymnast Outputs Filters Inputs D/A A/D Amplifiers/ filters

A21

A22

0.21 36.42 0.35 13.10 22.06 2.23, = 2.14 1.50 5.68 9.17 0.20 88.38 7.70 , = 0.20 168.29 0.02 7.69 201.45

Computer
Fig. 3. Block diagram representation of Robogymnast overall system 3. The swing up control The swing up control of Robogymnast involves commanding it to transition from the downward stable equilibrium position (Fig. 2) to an unstable inverted equilibrium configuration [5]. The basic idea behind the swing up strategy is to properly command the motions of the joint relative angles q2 = 2 1 and

q3 = 3 2 so that q1 = 1 moves away from its initial

4. Experimental results

position q1 = / 2 ( 3 / 2 ) to the inverted position q1 = / 2 [3]. The challenge is, therefore, the determination of appropriate input torques to the motors located at joints 2 and 3 in order to pump energy into the system in such a way that the amplitude of the first angle ( 1 ) increases with each swing. In this paper a new method is proposed that enables swing up of Robogymnast by manipulating frequency and amplitudes of sinusoidal functions applied to the two motors driving links 1 and 2. The frequency of the two control functions was tuned manually to obtain satisfactorily smooth sequence of oscillations. During each cycle, the amplitudes of the oscillating functions were also increased in inverse proportion to their respective frequency in order to enable the swing angle ( 1 ) increase with each cycle of oscillations. In contrast to Spongs approach [3], this technique does not require deriving the control signals ( u1 and u2 ) in terms of measurements of the link angular positions or velocities. The proposed swing up control was implemented in a PC equipped with appropriate AD/DA converters. The interface between the input/output components is enabled by a C++ programme which also includes codes to manipulate the frequency and amplitudes of the control signals ( u1 and u2 ) described below. u1 = A1 sin(1 ) (3) u 2 = A2 sin( 2 ) (4) For each cycle (multiple of sample intervals Ts depending on the value of ), 1 and 2 were varied between 0 and 2 with a step increment of applied during each sampling interval. By choosing a fixed value of , it was possible to reduce the oscillation frequency of the control signals u1 and u2 and increase their amplitudes with each increment of . After few iterations of tuning, the parameters in equations 3 and 4 to generate a reasonably smooth oscillation of the robot around its stable equilibrium position, the values of A1 , A2 and were fixed at 3.0, 2.5 and 0.3142 respectively. This helped simplify the tuning process and made it dependent on only one variable, which was initiated at 1.0 and then, at the end of each cycle ( 1 , 2 = 2 ), was incremented by 0.05 until Robogymnast was successfully swung to the upright position.

The discrete-time model of Robogymnast (obtained by discretising equations 1 and 2 at a sampling time of 0.025 s) was used to simulate, tune and validate the swing up control developed in section 3 before implementing it on the real system. Due to space restriction, the simulation results were not included. Figures 4, 5, 6, 7 and 8 showed the experimental results of the joint relative angles q1 , q 2 and q3 and control actions u1 and u2 respectively. Only responses during the first and last 25 second were presented. Fig. 4a and 4b showed how Robogymnast, autonomously starting from the stable equilibrium position ( q1 = / 2 ), swang pass the unstable inverted equilibrium configuration ( q1 = 3 / 2 ) in about 167 s. The experimental results also demonstrated how the manipulation of the frequency and amplitude of the control signals described in equations 3 and 4 (Figures 7 and 8) resulted in similar effect on the output responses (Figures 4, 5 and 6). Furthermore, the effect of nonlinearities mainly due to the backlash in the gearboxes was evident in all responses, in particular at the peaks of each cycle. This effect could have been partially eliminated by including compensating elements in the control actions. However, this approach will compromise the simplicity of the proposed method.
-80

-82

-84

-86

Deg

-88

-90

-92

-94 0

10 Time (s)

15

20

25

Fig. 4a. Measured swing angle, q1

100

14 12

0 10 -100 8 -200

Deg Upright position

145 150 155 Time (s) 160 165 170

6 4

Deg
-300

2 0

-400

-500 140

-2

10 Time (s)

15

20

25

Fig. 4.b Measured swing angle, q1

0 -2 -4 -6 100 80 60 40

Fig. 6a. Measured angular position, q3

Deg

-10 -12 -14 -16

Deg
0 5 10 Time (s) 15 20 25

-8

20 0 -20 -40 -60 140

145

150

155 Time (s)

160

165

170

Fig. 5a. Measured angular position, q2

20 8 6 4 2

Fig. 6b. Measured angular position, q3

10

Volts
145 150 155 Time (s) 160 165 170

-10

Deg

0 -2

-20

-30

-4 -6 -8

-40

-50 140

10 Time (s)

15

20

25

Fig. 5b. Measured angular position, q2

Fig. 7a. Control action applied to motor 1 ( u1)

10 8 6 4 2

0 -2 -4 -6 -8 -10 140

145

150

155 Time (s)

160

165

170

Fig. 7b. Control action applied to motor 1 ( u1)

6

motors mounted at the robots shoulder and hip joints. Robogymnast was modelled as a triple pendulum and linearised around the stable equilibrium position. The model was used to simulate the developed swing up control before implementing it on the real system. In contrast to previously developed swing up control techniques, this method does not require deriving the control signals in terms of measurements of joints angular positions or velocities. The experimental results showed the successful swing up of Robogymnast from stable equilibrium position to unstable inverted equilibrium configuration. Further work includes use of optimisation techniques such as Bees Algorithm [7] to automatically tune the parameters of the swing up control developed in this paper. In addition, capturing and stabilising Robogymnast at the upright position will be investigated.
Acknowledgements

Volts

-2

This work was supported by the EU-funded Innovative Production Machines and Systems (I*PROMS) FP6 Network of Excellence.
Appendix A.
5 10 Time (s) 15 20 25

Volts

-4

-6

-8 0

Fig. 8a. Control action applied to motor 1 ( u2)

10 8 6 4 2

Using the same approach in [1, 10], the Robogymnast model given in equations 1 and 2 is obtained as follows. & & 1 & 1 0 1 ~ && ~ & ~ ~ u1 M 2 + N 2 + P 2 + H = 0 u2 0 & & & 2 3 3 where

0 -2 -4 -6 -8 -10 140

J1 + I p1 ~ M = l1M 2 I p1 l1M 3
,
145 150 155 Time (s) 160 165 170

Volts

J 2 + I p1 + I p 2 l2 M 3 I p 2

l1M 2 I p1

l2 M 3 I p 2 J3 + I p2 l1M 3
0 C3 C p 2 C3 + C p 2

Fig. 8b. Control action applied to motor 1 ( u2)

C1 + C2 + C p1 C2 C p1 ~ N = C2 C p1 C2 + C3 + C p1 + C p 2 C3 C p 2 0
M 1 g ~ P= 0 0 0 M2g 0 0 G1 ~ 0 , H = G1 0 M 3 g

5. Conclusions and further work

In this paper a new method for swing up control of a robot gymnast (Robogymnast) was proposed and implemented. It involves manipulating frequency and amplitudes of oscillating functions applied to two

M1 = m1a1 + (m2 + m3 )l1, M 2 = m2 a2 + m3l2 ,

2 2 M 3 = m3a3 , J1 = I1 + m1a1 + (m2 + m3 )l1 ,

0 G2 G2

2 2 2 J 2 = I 2 + m2a2 + m3l2 , J 3 = I 3 + m3a3

G1(Nm/V)=1.333 K1=246:1

G2(Nm/V)=0.625 K2=110.6:1

& & & & & x = 1 2 1 3 2 1 2 1 3 2

The state equation in terms of the relative angle

]T isgiven

References
[1] Medrano-Cerda G. A., Eldukhri E. E., Cetin M., Balancing and attitude control of double and triple inverted pendulum, Trans. Inst. Meas. Cont, Vol. 17, No. 3, 1995, pp 143-154. [2] Medrano-Cerda G. A, Robust stabilisation of a triple inverted pendulum-cart, Int. J. Control, Vol. 68, No. 4, 1997, pp 849-865. [3] Spong M, Swing up control of the acrobot, Proc. of IEEE Conference on Robotics and Automation, San Diego, CA, May 1994, pp 2356-2361. [4] Isidori A., Nonlinear Control Systems (2nd edn), Springer-Verlag, Berlin, 1989. [5] Brown S. C., Passino K. M., Intelligent Control for an Acrobot, J. of Intelligent and Robotic Systems, Vol. 18, 1997, pp 209-248. [6] Pham D. T., Darwish A. H., Eldukhri E. E., Optimisation of a fuzzy logic controller using the Bees Algorithm, Int. J. Computer Aided Engineering and Technology, Vol. 1, No. 2, 2009, pp 250-264. [7] Pham D. T., Ghanbarzadeh A., Ko E., Otri, S., Rahim, S., Zaidi M., The Bees Algoritm a novel tool for complex optimisation problems, Proc. of Virtual International Conference on Intelligent Production Machines and Systems, IPROMS 2006, Pham, D. T., Eldukhri, E. E. and Soroka, A. J. (Eds), Elsevier, Oxford, Vol. 1, No. 2, 2009, pp 454-459. [8] Furuta K., Okutani T., Sone H.,Computer control of a double inverted pendulum, Computer and Electrical Engineering, Vol. 5, 1978, pp 67-84. [9] Furuta K., Hiroyuki K., Kosuge K.,Digital control of a double inverted pendulum on an inclined rail, Int. J. of Control, Vol. 32, 1980, pp 907-924. [10] Furuta K., Ochiai T., Ono N.,Attitude control of a triple inverted pendulum, Int. J. of Control, Vol. 39, 1984, pp 1351-1356. [11] Srinivasan B., Huguenin P., Bonvin, D., Global stabilisation on an inverted pendulum Control strategy and experimental verification, Automatica, Vol. 45, 2009, pp 265-269. [12] Inoue A., Deng M., Non-linear control of underactuated mechanical systems, Int. J. Modelling, Indentification and Control, Vol. 6, No. 1, 2009, pp 3239. [13] Eltohamy K. G., Kuo C-Y., Nonlinear optimal control of a triple link inverted pendulum with single control input, Int. J. Control, Vol. 69, No. 2, 1998, pp 239-256. [14] Larcombe P. J., on the control of a two-dimentional multi-link inverted pendulum: the form of the dynamic equations from choice of co-ordinate system, Int. J. Syst. Sci., Vol. 23, 1992, pp 2265-2289.

by:
03 & x= ~ ~ WM 1 P W 1 1 0 where W = 1 1 0 1 I3 0 3x 2 ~ ~ ~ 1 ~ u x + WM 1 NW 1 WM H 0 0 , 1

The nomenclatures are explained in Fig. 2 and Table 1.

Table 1: Nomenclature ui Input voltage from computer to ith driving motor li Length of ith link ai Centre of gravity of ith link mi Mass of ith link Ii Moment of inertia of ith link around its centre of gravity Ci Viscous friction coefficient of ith hinge i Angle of ith link from the vertical line Gi Static gain of ith motor/gearbox Cpi Viscous friction coefficient of ith motor/gearbox reflected at the output shaft of the gearbox Ipi Moment of inertia of ith motor/gearbox reflected at the output shaft of the gearbox Ki Ratio of the ith gearbox g Acceleration of gravity (9.81 m/s2)

A.1. Parameter characteristics

Mi, li ai, Ii Ci Ipi, Cpi, Gi

identification

and

model

Measured directly Determined analytically Links are swung freely, from the periods and the damping factor of their responses, the parameters are determined Calculated from values given in technical data of the motor/gearboxes

Parameter values are given in tables 2 and 3. Table 2: Parameters of Robogymnast

Link 1 l1(m)=0.155 a1(m)=0.0426 m1(Kg)=2.625 I1(Kgm2)=0.014 C1(Nms)=0.0172 Link 2 l2(m)=0.180 a2(m)=0.138 m2(Kg)=0.933 I2(Kgm2)=0.018 C2(Nms)=0.0272 Link 3 l3(m)=0.242 a3(m)=0.065 m3(Kg)=0.375 I3(Kgm2)=0.002 C3(Nms)=0.035

Table 3: Motor parameters

Motor 1 Ip1(Kgm2)=0.036 Cp1(Nms)=7.73 Motor 2 Ip2(Kgm2)=0.036 Cp2(Nms)=7.73