Acta Polytechnica doi:10.14311/AP.2013.53.0883 Acta Polytechnica 53(6):883–889, 2013 © Czech Technical University in Prague, 2013 available online at http://ojs.cvut.cz/ojs/index.php/ap CONTROL OF THE DOUBLE INVERTED PENDULUM ON A CART USING THE NATURAL MOTION Zdeněk Neusser∗, Michael Valášek Department of Mechanics, Biomechanics and Mechatronics, Faculty of Mechanical Engineering, Czech Technical University in Prague, Technická 4,166 07 Prague, Czech Republic ∗ corresponding author: Zdenek.Neusser@fs.cvut.cz Abstract. This paper deals with controlling the swing-up motion of the double pendulum on a cart using a novel control. The system control is based on finding a feasible trajectory connecting the equilibrium positions from which the eigenfrequencies of the system are determined. Then the system is controlled during the motion between the equilibrium positions by the special harmonic excitation at the system resonances. Around the two equilibrium positions, the trajectory is stabilized by the nonlinear quadratic regulator NQR (also known as SDRE – the State Dependent Riccati Equation). These together form the control between the equilibrium positions demonstrated on the double pendulum on a cart. Keywords: underactuated systems, nonlinear control, mechanical systems. 1. Introduction The control of systems with fewer actuators than de- grees of freedom is a challenging task. Systems of this type are called underactuated systems. A more precise definition of underactuated systems should be based on the number of blocks in the Brunovsky canonical form [1]. Our study deals with the double pendulum on a cart in the gravity field between equi- librium positions passing through singularity positions. The mechanism can be controlled in its equilibrium positions (see [2]), but there are always singularity points in between. The system is not controllable by the NQR control [3, 4]) with zero velocity. The main aim of our work is to determine the behavior of the actuator for utilizing the swing-up motion. It is a challenging task (according to [5]) to move the mechanism over this region of singular positions, or even to perform trajectory tracking. This problem has already been investigated by many authors. The energy based approach used in the de- sign of stabilizing controllers constitutes the basis of the control applied in [6] and in [7], and this is another type of energy-based approach applied to the double inverted pendulum on a cart. A modified energy con- trol strategy using friction forces is used in [10]. A method adding the actuators to the underactuated joints and running the swing-up motion with optimiza- tion that minimizes the action of the added actuators is used in [8]. Partly stable controllers derived using the dynamic model of the manipulator in order to employ them under an optimal switching sequence is presented in [9]. Another approach is to use exact input-output linearization within a certain subman- ifold (direction) in which the system is controllable and the zero-dynamics is stable [10]. This approach is used in [11], where the feedfor- ward control strategy is combined with input-output linearization. The system acquires new input vari- ables that are suitable for controlling the actuated part together with the underactuated part. However, the system can be controlled by this new input only if the input functions are from a certain admissible class of functions. This admissible class of functions can be determined by the cyclic control described in [12] as periodic invariant functions (with respect to the actuated variables), without details. Another way to find suitable admissible functions for controlling underactuated mechanical systems be- tween equilibrium positions across a singularity area in the gravity field is described in [13]. These suit- able admissible functions are based on inverting the so-called natural motion, or on reusing specific knowl- edge gained from these natural motions. In brief, the natural motion is a motion of the system without control. The method is shown in [13] only for a dou- ble pendulum and a pendulum on a cart. Our paper deals with the application of this approach also for the swing-up motion of a double pendulum on a cart as a more complex system than that investigated in [13]. Our paper describes this different approach to the control of underactuated systems. It demonstrates a different solution of the problem explored in [7] and justifies the same results as in [7], but with broader validity and applicability. 2. Natural motion Natural motion [13] is the motion of an investigated mechanical system moving in the gravity field with zero or constant inputs (drive torques caused by grav- ity), from the unstable equilibrium position to the stable equilibrium position. To reach the desired qui- escent motion, it is necessary to add a passive member (a damper) into the actuator. The underactuated part moves freely, but due to the overall energy flow into 883 http://dx.doi.org/10.14311/AP.2013.53.0883 http://ojs.cvut.cz/ojs/index.php/ap Z. Neusser, M. Valášek Acta Polytechnica Figure 1. Double pendulum on a cart; left, lower position, right, upper position. the passive part the whole system is stabilized around the stable equilibrium point. The damping term (the added passive member) is chosen small, in order to remove the difference in potential energy between the equilibrium positions. The value of the damping term is optimized with respect to the settling time. The natural motion is later used to determine the eigenfrequencies of the natural motion. These are not influenced by the damping if it is small. Proposition 1. The control described here enables the system of the cart-double-pendulum to be moved from the lower position (Figure 1 left) to the upper position (Figure 1 right). Justification. The control consists of two phases. First, the system is moved from the lower position (Figure 1 left) to the vicinity of the upper position (Figure 1 right) with small final velocities. Second, the system is stabilized and controlled exactly into the equilibrium at the upper position by NQR control. The first phase is realized by proper selection of the amplitudes in equation (9)) in order to achieve values in the vicinity of the upper position and small veloci- ties. Amplitude values of this kind exist because each amplitude influences the particular different eigen- mode, and because the periods of all eigenmodes meet at one time instant. The excitation at the eigen- frequencies guarantees sufficient amplification of the excitation amplitudes to the angular amplitudes of the double-pendulum. The combination of two and more different eigenmodes enables any desired posi- tion to be reached, and the meeting of all periods at one time instant enables zero or near zero velocities to be reached. Proposition 2. The cart-double-pendulum system can be moved from any position to the upper position (Figure 1 right). u x ϕ � ϕ � m v I 1 , m 1 , l 1 , l t1 I 2 , m 2 , l 2 , l t2 Figure 2. Scheme of the double inverted pendulum on a cart with coordinates, parameters and external force. Justification. The control consists of two phases. First, the system is moved from any position to the lower position (Figure 1 left), and then the system is moved to the upper position, according to 1. The first phase is realized by active damping, i.e. the control applies damping of significant value to the cart, in order to damp out the energy and stop the system at stable equilibrium in the lower position (Figure 1 left). 3. Problem formulation Figure 2 presents the scheme of a double inverted pendulum on a cart. This mechanism has the cart moving horizontally, with two links rotationally con- nected, and there is a force acting on the cart. Using Lagrange equations of the second type, the dynamic equations of the double pendulum on a cart are ob- tained. Put M =  mv +m1+m2 A cos φ1 m2lt2 cos φ2A cos φ1 I1+m1l2t1+m2l21 B cos(φ1−φ2) m2lt2 cos φ2 B cos(φ1−φ2) I2+m2l2t2   , F =   (m1lt1+m2l1) sin φ1 φ̇21+m2lt2 sin φ2 φ̇22−m2l1lt2 sin(φ1−φ2)φ̇22+(m1lt1+m2l1) sin φ1 g m2l1lt2 sin(φ1−φ2)φ̇21+m2lt2 sin φ2 g   , M(φ1,φ2)   ẍφ̈1 φ̈2   = F(φ1, φ̇1,φ2, φ̇2) + u(t)  10 0   , (1) where A = (m1lt1 + m2l1) and B = m2l1lt2. Matrix M is the inertia matrix, and vector F contains the forces dependent on the velocities and external forces besides the control inputs. Actuator u is the horizontal force acting between the base frame and the cart. The goal is to control the mechanism from the stable equilibrium point (ϕ1 = π, ϕ2 = π) into the upper unstable equilibrium point (ϕ1 = 0, ϕ2 = 0). The procedure described in detail in [13] covering input- 884 vol. 53 no. 6/2013 Control of the Double Inverted Pendulum on a Cart output feedback linearization which forms the basis for subsequent adaptation of the system of equations. It is necessary to divide the coordinates of the mech- anism in such way that one part is controlled and another forms the zero dynamic. It is used the follow- ing division of the coordinates (but this choice is not unique, as is demonstrated in [13]): qm = x, qz = [ ϕ1 ϕ2 ] Index m denotes the actuated (controlled) part, and index z denotes the zero dynamics coordinates. Matrix M is decomposed in the following way: M = [ Mmm Mmz Mzm Mzz ] , where Mmm = [ mv + m1 + m2 ] , Mmz = [ (m1lt1 + m2l1) cos φ1, m2lt2 cos φ2 ] , Mzm = [ (m1lt1 + m2l1) cos φ1 m2lt2 cos φ2 ] , Mzz = [ I1 + m1l2t1 + m2l21 m2l1lt2 cos(φ1 − φ2) m2l1lt2 cos(φ1 − φ2) I2 + m2l2t2 ] . A necessary condition for matrix Mzz is that it must be invertible, and for nonzero moments of inertia this is the case. Vector F is as follows: F = [ Fm Fz ] , where Fm = (m1lt1 + m2l1) sin φ1 φ̇21 + m2lt2 sin φ2 φ̇ 2 2, Fz = [ −m2l1lt2 sin(φ1−φ2)φ̇22+(m1lt1+m2l1) sin φ1 g m2l1lt2 sin(φ1−φ2)φ̇21+m2lt2 sin φ2 g ] . The new control variable w is chosen as a second derivative of coordinate x: ẍ = w. (2) The remaining coordinates ϕ1 and ϕ2 can be expressed directly from equation (1) and its decomposition. The zero dynamics is represented by:[ ϕ1 ϕ2 ] = M−1zz (Fz − Mzmw). (3) The expression for actuator u is derived by decompos- ing equations (2) and (3): u(t) = Fm − MmzM−1zz Fz − (Mmm − MmzM−1zz Mzm)w. (4) The singular positions of the system are for ϕ1 = ±π/2, ϕ2 = ±π/2, and other states (velocities) zero. Then Mzm = 0. The system of equations (2)–(3) then takes the following form with zero velocity terms: ẍ = w, [ ϕ1 ϕ2 ] = M−1zz Fz, (5) where the control variable cannot influence coordi- nates ϕ1 and ϕ2 and the system cannot be controlled. 4. How to get new input w The problem of how to get a new input w to ensure that qz ends in the desired position together with qm has been addressed in [13]. It is easy to find such w, which controls qm, but it is not easy to find such w which simultaneously controls qm and qz. The new input w contains two parts according to [12]. It consists of the active part wA, which moves the qm coordinates from the initial position qm(0) to the desired final position qmd(T) in some chosen time T from equation (2), and the invariant part wI which is invariant to the final position of the motion of qm, but due to the dynamic connection through zero dynamics influences qz in a way that moves qz from the initial position qz(0) to the desired final position qzd(T) in time T : w = wA + wI. (6) The invariant wI is such that∫ T 0 wI dt = 0 and ∫ T 0 ∫ t 0 wI dtdt = 0. (7) In our case, due to conditions (7) the motion of x in the final velocity and position is influenced only by the active part wA of the control (6). This control is chosen arbitrarily just to fulfil the desired final values for the position and velocity of x. When the active part wA is chosen, it is a given function of time wA(t), and substitution of (6) into (3) results in[ ϕ1 ϕ2 ] = M−1zz ( Fz − Mzm(wA(t) + wI) ) (8) Now the motion of ϕ1 and ϕ2 can be controlled by the choice of wI just fulfilling the conditions (7). For invariant motion, use is made of so-called natural motions and their frequencies fi, wI = ∑ i Ai sin(2πfit + ϕi), (9) where amplitudes Ai and phases ϕi are some con- stants as a result of optimization. The frequencies fi are chosen and the amplitudes Ai and phases ϕi are optimized for coordinates ϕ1 and ϕ2 to reach the desired position and velocity at the chosen settling time T . If it holds 2πfiT = 2Kiπ (10) for some natural number Ki, then functions (9) satisfy conditions (7). How to determine suitable frequencies is explained in the next section. 5. Simulation of the natural motion During the simulation all states are recorded. Fourier analysis is applied to the recorded states in order to determine the eigenfrequencies of the considered non- linear system. The feedforward control is constructed 885 Z. Neusser, M. Valášek Acta Polytechnica −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 x [m] y [m ] cart motion end of the first link end of the second link Figure 3. Motion of the double pendulum on a cart from its upper position into the bottom position with the damping force between the cart and the base frame. using these eigenfrequencies and from equations (4), (6) and (9). The eigenfrequencies of the natural motion describe the vibrational motion of variables q that ends at equilibrium, where all swings meet at the final time. The conditions (10) are therefore satisfied. At the same time, the determined eigenfrequencies are in fact rational numbers, and the natural numbers Ki at the final time T for any set of eigenfrequencies fi therefore exist. Finally, the conditions (10) from the natural motion are to be satisfied just approximately in order to move the mechanical system into such vicinity of the upper equilibrium that the NQR control can sta- bilize the motion. The excitation of the system at its eigenfrequencies enables large motion of the system to be caused on the basis of resonance. The eigenfre- quencies allow the mechanism to be excited with less input energy than other frequencies, as excitation by eigenfrequencies enables the energy to be accumulated in the system. The system is nonlinear, and it is not easy to de- termine the eigenfrequencies. The usage of natural motion is therefore helpful. The following parameters are used for the simula- tion: the mass of the cart is 1 kg, the mass of the first and second link is 0.5 kg, their moments of inertia are 0.0417 kg/m2, and the length of each part is 1 m, with the center of gravity in the middle of each part. The natural motion is illustrated in Figure 3. The natural motion is achieved by simulating the equations of motion (1) with zero control and with damping instead of it, u(t) = −bx from the upper position to the lower position. The outer dotted line represents the end point of the second link, the dashed line is the trajectory of the joint between links, and the straight line represents the cart movements. It starts in the upper position with a slight deflection in the first and second angle ϕ1, ϕ2 and zero velocities. It falls into the bottom stable position with a quiescent motion. This behavior is caused by the damping force acting between the cart and the base frame, where the actuator is later placed. The damping coefficient (b = 11.92 N m s/rad) is received by the optimization with respect to the settling time. The behaviors of all states, including the accelerations, are recorded. 6. Control design The data from the natural fall motion are now used. The first frequencies from the angular acceleration of the first and the second link angular acceleration form the basis for the new input variable for the swing-up motion (Figure 4). The frequencies for the angular acceleration of the first link are 0.4053, 0.5722, 0.8583, 0.9537, 1.0490, 1.1444 Hz (Figure 4 middle row), the frequencies for angular acceleration of the second link are 0.4053, 0.6437, 0.8345, 0.9537, 1.0490, 1.1206 Hz (Figure 4 last row). The frequencies for the cart acceleration are 0.4053, 0.5722, 0.7629, 0.9060, 1.0014, 1.0967 Hz (Figure 4 first row). The input variable w consists of the active part and the invariant part (according to equation (6)). In this case, the active part is zero, because variable x has the same initial and final position. The sinusoidal behavior of the invariant part has frequencies from the movement of the two links, because w directly influences ϕ1 and ϕ2 through the zero dynamics in equation (3)). The amplitudes are proportional to the difference in potential energy between the initial and final position of the mechanism, but their values are the result of optimization. The difference of the potential energy is Ep = m1lt1g + m2(l1 + lt2)g. (11) All parameters and behaviors of the control variable w are included in the following system: A = [5 6, 1 12, 1 12 ]( m1lt1g + m2(l1 + lt2)g ) , ϕ = [ −π2 , π2, π 2 ] , f = [ 0.4053, 0.6437, 0.8345 ] , ẍ1 = w, wA = 0, wI = 3∑ i=1 Ai sin(2πfit + ϕi), w = wA + wI. (12) The conditions (10) are fulfilled only approximately (2/0.4053 = 4.93, 3/0.6437 = 4.66 and 4/0.8345 = 4.79), but near the upper position the control is switched to NQR. The usage of these frequencies is 886 vol. 53 no. 6/2013 Control of the Double Inverted Pendulum on a Cart 0 5 10 15 −40 −20 0 20 40 t [s] ẍ [m s− 2 ] 0 1 2 3 0 0.1 0.2 0.3 0.4 f [Hz] |ẍ |[ m s − 2 ] 0 5 10 15 −200 −100 0 100 200 t [s] ϕ̈ 1 [r a d ·s − 2 ] 0 1 2 3 0 1 2 3 f [Hz] |ϕ̈ 1 |[ r a d ·s − 2 ] 0 5 10 15 −200 −100 0 100 200 t [s] ϕ̈ 2 [r a d ·s − 2 ] 0 1 2 3 0 2 4 6 f [Hz] |ϕ̈ 2 |[ r a d ·s − 2 ] Figure 4. Accelerations (left) and their Fourier transformations. efficient, because they are the eigenfrequencies and the system is sensitive to the input on these frequencies. In the surroundings of the desired position it is required to use the local stabilization control. In our case, we use the nonlinear quadratic regulator (NQR or SDRE, see [3, 4]). However, the region of attraction of this control is also limited. In each time step, the NQR controller calculates the input torque u at the cart. The input variable u is therefore plotted in the same figure, based on the acceleration of the cart (4) when using NQR control and based on the NQR algorithm in the vicinity of the unstable equilibrium. 7. Simulation of the double inverted pendulum on the cart The control is designed, and is used for the simulation of the system described by equations (2)–(3). The simulated swing-up motion guided by the cart accel- eration ẍ1 = w described in equation (12) reaches the surroundings of the unstable equilibrium position (see Figure 5). The NQR algorithm is used for the stabilization around the upper equilibrium. This algorithm controls the system from the position in the surroundings of the upper unstable position to this unstable equilibrium. When the pendulum reaches the position where it can be controlled by NQR, it is switched on and it leads the mechanism to equilibrium. The entire trajectory of the end point of the pen- dulum and the cart is shown in Figure 5 left, with the starting position and the final position marked. Figure 5 right depicts the behavior of the input force acting on the cart. The control algorithm NQR starts at time 2.715 s. A big step in the input torque is shown in Figure 6 right, when the change in the con- trol strategies is made. For illustration, the behavior of the ϕ1, ϕ2 and x coordinates and their velocities are shown in Figure 7. 8. Conclusions This paper has described a novel control for the bottom-up motion of a double pendulum on a cart using eigenfrequencies determined from the natural motion. The set of equations describing the mechan- ical system is transformed using exact input-output feedback linearization. The control is based on the feedforward control strategy, using sinusoids with fre- quencies obtained from the falling trajectory (natural motion). The frequencies of the sinusoids are eigen- frequencies of this underactuated system. The am- plitudes of this control are related to the potential energy needed for the system to change its position in the field of gravity. The zero dynamics integrated in the underactuated mechanisms can be controlled by frequencies added to the signal of the control variable. The approach described here has great potential for extending this control into the field of underactuated systems. 887 Z. Neusser, M. Valášek Acta Polytechnica −3 −2 −1 0 1 −3 −2 −1 0 1 2 x [m] y [m ] 0 1 2 3 −10 −5 0 5 10 t [s] ẍ [m s − 2 ] Figure 5. Swing-up motion controlled by cart acceleration; left, mechanism end points in the working plane; right, input acceleration behavior. −2 0 2 4 −5 −4 −3 −2 −1 0 1 2 3 4 5 x [m] y [m ] 0 5 10 15 −20 0 20 40 60 80 t [s] u [N ] Figure 6. Final swing up motion for the double pendulum on the cart; left, motion in the plane; right, the input torque acting on the cart. Our work forms the basis for the control of systems with flexible parts to reduce unintentional residual structural vibrations during the motion. This kind of control is useful in controlling flexible robots. Acknowledgements This work was partially supported by project MSM 6840770003 ‘Algorithms for computer simulation and ap- plication in engineering’. References [1] Valášek, M.: Design and Control of Under-Actuated and Over-Actuated Mechanical Systems - Challenges of Mechanics and Mechatronics. Supplement to Vehicle System Dynamics, 40, 2004, p. 37–50. [2] Olfati-Saber, R.: Nonlinear Control of Underactuated Mechanical Systems with Application to Robotics and Aerospace Vehicles. PhD thesis, MIT, Boston, 2001. [3] Steinbauer, P.: Nonlinear control of nonlinear mechanical systems. PhD thesis, Czech Technical University in Prague, Prague, 2002. (in Czech) [4] Valášek, M., Steinbauer, P.: Nonlinear Control of Multibody Systems. In: “Euromech Colloquium 404, Advances in Computational Multibody Dynamics”, Lisboa: Instituto Technico Superior, Av. Rovisco Pais, 1999, p. 437–444. [5] Aneke, N. P. I.: Control of underactuated mechanical systems. PhD thesis, TU Eindhoven, Eindhoven, 2002. [6] Fantoni, I., Lozano, R.: Non-linear Control for 888 vol. 53 no. 6/2013 Control of the Double Inverted Pendulum on a Cart 0 5 10 15 −3 −2 −1 0 1 2 3 4 5 6 7 t [s] x [m ] ϕ 1 [r a d ] ϕ 2 [r a d ] x ϕ1 ϕ2 0 5 10 15 −10 −5 0 5 10 15 t [s] ẋ [m s− 1 ] ϕ̇ 1 [r a d ·s − 1 ] ϕ̇ 2 [r a d ·s − 1 ] x ϕ1 ϕ2 Figure 7. Upward movement of the double inverted pendulum on the cart, positions and velocities. Underactuated Mechanical Systems. London: Springer-Verlag, 2002. [7] Xin, X.: Analysis of the energy-based swing-up control for the double pendulum on a cart. International Journal of Robust and Nonlinear Control, 21, 2011, p. 387–403. [8] Rubi, J., Rubio, A., Avello, A.: Swing-up control problem for a self-erecting double inverted pendulum. IEE Proc.–Control Theory App., 149 (2), 2002, p. 169–175. [9] Udawatta, L., Watanabe, K., Izumi, K., Kuguchi, K.: Control of underactuated robot manipulators using switching computed torque method: GA based approach. Soft Computing, 8, 2003, p. 51–60. [10] Mahindrakar, A. D., Rao, S., Banavar, R. N.: A Point-to-point control of a 2R planar horizontal underactuated manipulator. Mechanism and Machine theory, 41, 2006, p. 838–844. [11] Valášek, M.: Exact Input-Output Linearization of General Multibody System by Dynamic Feedback. Eccomas Conference Madrid: Multibody Dynamics, 2005. [12] Valášek, M.: Control of Elastic Industrial Robots by Nonlinear Dynamic Compensation. Acta Polytechnica, 33 (1), 1993, p. 15–30. [13] Neusser, Z., Valášek, M.: Control of the underactuated mechanical systems using natural motion. Kybernetika; 48 (2), 2012, p. 223–241. 889 Acta Polytechnica 53(6):883–889, 2013 1 Introduction 2 Natural motion 3 Problem formulation 4 How to get new input w 5 Simulation of the natural motion 6 Control design 7 Simulation of the double inverted pendulum on the cart 8 Conclusions Acknowledgements References