Reverse Order Swing-up Control of Serial Double Inverted Pendulums

Reverse Order Swing-up Control of Serial Double Inverted Pendulums T.Henmi, M.Deng, A.Inoue, N.Ueki and Y.Hirashima Okayama University, 3-1-1, Tsushima-Naka, Okayama, Japan inoue@suri.sys.okayama-u.ac.jp Abstract: In this paper, we consider a reverse order swing up control of a serial double inverted pendulums. In the reverse order, the second pendulum is swung up first and, after that, the first pendulum is swung up. Reverse order swing-up control has three control steps. We propose a control scheme for each step, and switch the scheme to the next scheme according to the states of the system. A numerical simulation is given to show the effectiveness of the proposed scheme. Keywords: Serial double inverted pendulums, reverse order swing-up control, partial linearization 1. Introduction Inverted pendulums are typical examples of nonlinear and underactuated systems and well known in control engineering for verification and practice of various kinds of control theories. Inverted pendulum systems have several types, e.g., a single pendulum, parallel double pendulums and serial double pendulums, etc. Many control methods have been proposed to control each of the inverted pendulum systems, such as feedback stabilization 1)5), energy based control 1)5), bangbang control 3), sliding mode control 7), robust control ), hybrid control 5)8) (see 1) for more details). In spite of these existing methods, to control the inverted pendulums is still an open research topic. In particular, the serial double inverted pendulum is strongly nonlinear and underactuated than a single inverted pendulum and the control of the pendulum is a difficult problem. Hence a solution of this problem is applicable to solving other nonlinear and underactuated control problems. In this paper, we consider the swing-up and the stabilization problems of the serial double inverted pendulums. For swing-up control of the serial double inverted pendulums, various methods are already proposed, e.g., simultaneous swing-up which swings up two pendulums simultaneously 3), right order swinging up 3)7) which swings up the first pendulum first after that swings up the second pendulum. However, the method swinging up in reverse order, that is, swing-up the second pendulum first, then swing-up the first pendulum while stabilizing the second pendulum next is not succeeded yet, and requires stronger robustness to nonlinearity than the swing up in right order. Hence, in this paper, we consider the reverse order swing-up control of serial double inverted pendulums. The reverse order swing-up control has three control steps, step 1: to swing up the second pendulum only, step : to swing up the first pendulum while stabilizing the second pendulum at the upright position, and step 3: to stabilize two pendulums around the unstable equilibrium state. We propose a new control scheme for each step, and switch the scheme to the next one according to the states of system. In Section, we present the dynamics of the serial double inverted pendulum system as depicted in Fig.1. In Section 3, as a controller of Step 1, we give the control scheme which combines a controller to swing up second pendulum and a controller to stabilize the first pendulum. For the stabilizing controller of the first pendulum at the pendant position, we apply a state feedback stabilization controller by solving the LQR problems. And for swinging up the second pendulum, we modify the energy control methods described in 4) and 5). In Section 4, we propose a control scheme which carries out a periodic movement of the serial double inverted pendulum with stabilizing the second pendulum by partially stabilizing in the angle of the second pendulum. The scheme is applied to the Step. The partial linearization method is given to a control of Acrobot 4)6) which is similar to the serial double inverted pendulum in mechanisms. And we introduce a technique to avoid an input singular point. Moreover, zero dynamics of non-observable subspace is analyzed and the behavior of the dynamics is also explained to show the stability of the whole system. In Section 5, for a controller in Step 3, we use the sliding mode controller stabilizing both of the pendulums by linearizing the system around the unstable equilibrium state. In Section 6, a numerical simulation is given to show the effectiveness of the proposed scheme in this paper.. Dynamics The dynamics of the serial double inverted pendulumscart system as depicted in Fig.1 is given by: I θ θ + M1 cos (θ φ) φ + M φ 1 sin (θ φ) +(c 1 + c ) θ c φ + M g sin θ + M cos θ z = (1) I φ φ + M1 cos (θ φ) θ M θ 1 sin (θ φ) c ( θ φ)+m 3 g sin φ + M 3 cos φ z = ()

where I θ,i φ,m 1,M,M 3 are expressed as I θ = I 1 + m L + n L I φ = I + J n1 + J n, M 1 = m l L M = m 1 l 1 + m L + n L, M 3 = m l and the parameters of the pendulums are defined in Table 1, Table 1: Definitions of parameters z position of the cart θ angular position of the first pendulum from the vertical line φ angular position of the second pendulum from the vertical line m i (i =1, ) mass of the i th pendulum n i (i =1, ) mass of the i th pendulum s point mass l i (i =1, ) length from the joint to the center of mass of the i th pendulum J i (i =1, ) inertia of the i th pendulum around the center of gravity J ni (i =1, ) inertia of the i th pendulum s point mass around the center of gravity I i (i =1, ) inertia of the i th pendulum around the joint c i (i =1, ) viscosity of each joint L length of the first pendulum g gravity acceleration method consists of two control laws, to swing up the second pendulum, and to stabilize the first pendulum at the stable equilibrium point. First, the control law which stabilizes the first pendulum at the stable equilibrium point is considered. By neglecting φ in (1), the dynamics of θ becomes to I θ θ +(c1 + c ) θ M g sin θ + M cos θ z = (3) Now we define the input u as u = z (4) And choose the following state variables with x 1 =(x 11,x 1,x 13,x 14 ) T =(z,ż,θ, θ) T (5) With (5) and (4), the linearized state-space equation of (3) around the stable equilibrium point of the first pendulum ((θ, θ) =(, )) is ẋ 1 = 1 1 M g I θ c1+c I θ x 1 + 1 M I θ u (6) From (6), we can find a state feedback as a stabilizing controller by solving the following linear quadratic regulator (LQR) problem with performance index J = (x T 1 Qx 1 + ru )dt (7) t s being minimized, where Q and r>. Then Z First Pendulum Second Pendulum u 1 = B T Px 1 /r = K 1 u (8) where P is the stabilizing solution of the following Riccati equation PA+ A T P PBB T /r + Q = (9) L l1 m1g θ l φ mg Fig. 1: The serial double inverted pendulums 3. Swing up the second pendulum only (Step1) In this section, we propose a control method which swings up only the second pendulum. The proposed Next, the control law which swings up the second pendulum is considered. We modify the energy control method developed in 1). From () and (4) by neglecting θ and viscosity friction, the dynamics of φ is φ = M 3g J sin φ M 3 cos φu (1) J where J = I φ Jn1 = = I + J n. For the swing-up control, a control strategy is presented in 1) by using Lyapunov method. Define the Lyapunov candidate as V = 1 E p (11) where E p is the energy of the second pendulum given as E p = 1 J φ m l g(cos φ + 1) (1)

It follows that E p = when the pendulum is at the upright position, i.e., φ = π and φ =, and E p = m l g when the pendulum is at the pendant position, i.e., φ = and φ =. Calculating the derivative of V along the trajectory of (1) yields V = E p m l φ cos φ (13) From (13), to make V <, we give the control input as u = u a sign( φ cos φ) (u a > ) (14) From (13) and (14), the derivative of V is V = u a m l E p sign( φ cos φ) (15) It follows that V (t) is a non-increasing function. Thus, lim V (t) =, lim t E p(t) = (16) t are obtained. Therefore, using the input of (14), the energy of the second pendulum increases, and the second pendulum is swung up. It turns out that the input of (14) has the opposite sign to the swing up input of the a single pendulum shown as u a = u a sign( θ cos θ) (ua > ) (17) where θ is the angle of the single inverted pendulum and u a is the acceleration of the cart as input to the cart. Then, it is analyzed what influence the input to the cart has on the behavior of the second pendulum. If the positive constant input u = u a is given to a cart, a cart will move to the right horizontally and the first pendulum will incline to the left. At the moment, the joint portion between the first pendulum and the second pendulum moves to the right by the influence of reaction of the first pendulum. That is, the input is equivalent to the leftward input to the second pendulum. Therefore, the direction of the control input which swings up the second pendulum is opposite to the direction of (17). Until now, we calculated the control input which swings up the second pendulum, and it is explained that the control input has the opposite direction to the input of a single pendulum. When the angle of the second pendulum is large, the influence of the first pendulum is large and complex, and to decide a swing up input to the second pendulum only is difficult. Therefore we will consider to giving an input only just after the second pendulum passes through the vertical line. When the second pendulum passes through the vertical line rightward, the angle and the acceleration of the second pendulum are <φ<ɛ<π/, φ > (18) The input (14) at this moment is u = u a sign( φ cos φ) =u a > (19) When the pendulum passes leftward, the input has negative sign. Therefore, the swing up input for the second pendulum is u = u a sign(sin x 1 ) (sin φ φ >, cos φ>cos ɛ) (sin φ φ, cos φ>cos ɛ) (cos φ<cos ɛ) () By combining u 1 of (8) and u of (), we get the following controller which swings up only the second pendulum. u = u 1 + u (1) When the second pendulum swung up in the neighborhood φ = π by controller (1), we switch the controller to the one of Step. 4. Swing up the first pendulum while stabilizing the second pendulum (Step ) In this section, as the second step control, we obtain an input to realize a periodic movement 9) of a serial double inverted pendulum with stabilizing the second pendulum. And we introduce the technique of avoiding an input singular point. Moreover, zero dynamics of non-observable subspace is analyzed and the behavior is also explained. 4.1 Partial linearization of variables of the second pendulum From (1) and (), (φ, φ) =(π, ) is an unstable equilibrium point of the second pendulum to which the second pendulum will be stabilized. Choose x as the following state x =(x 1,x,x 3,x 4 ) T From (1),() and (), we obtain where =(θ, φ π, θ, φ) T () A 11 ẍ 1 + A 1 ẍ + h 1 = f 1 u (3) A 1 ẍ 1 + A ẍ + h = f u (4) A 11 = I θ A 1 = A 1 = M 1 cos (θ φ) A = I φ h 1 = M φ 1 sin (θ φ) (c 1 + c ) θ +c φ M g sin θ h = M θ 1 sin (θ φ)+c ( θ φ) M 3 g sin φ f 1 = M cos θ f = M 3 cos φ

We will find a controller to make state variable x be. First, from (3) and (4), we obtain where ẍ = 1 (A 11h A 1 h 1 + Ãu) (5) à = A 1 f 1 A 11 f =A 1 A 1 A 11 A θ, φ We assume that à holds so that ẍ can be directly controlled by u. The following Section 4. explains in detail the way to make Ã. Now, we assign a new control variable v as v := ẍ (6) From (3), (4) and (6), the relation between v and u is u = 1 à ( v + A 1h 1 A 11 h ) (7) Let v be then v = k p x k d ẋ (k p >,k d > ) (8) Since this equation is stable, ẍ + k d ẋ + k p x = (9) lim t x (t) =, lim ẋ (t) = (3) t 4. Avoiding input singular point From (5), we obtain à as à = I θ M 3 cos φ M 1 M cos (θ φ) cos θ (31) The control of Step starts at the state where the second pendulum stands at the upright position by the control law of Step 1, i.e., φ = π. Therefore, à can be linearized around φ = π, à = I θ M 3 + M 1 M cos (θ) (3) Consequently, the condition for à not being is M 1 M <I θ M 3 (33) Using parameters of Fig. 1, condition of (33) is equivalent to 3 4 L<l 1 (34) That is, the singular point of input (7) is avoidable by making the length from joint to a center of mass of the first pendulum be greater than 3/4 of the full length of first pendulum. For example, it is able to realize (33) by attaching a weight to the end point of the first pendulum. 4.3 Analysis of zero dynamics When x and ẋ are set to be by input u of (7) which satisfies the condition (34), dynamics of x 1 which is in non-observable subspace is called zero dynamics, and expresses behavior of the system when the partial linearization is attained. From (3) and (4), dynamics of x 1 under the condition x =ẋ =is ẍ 1 = h φ=π, φ= h 1 φ=π, φ= A 11 φ=π, φ= A 1 φ=π, φ= = M 1M ẋ 1 sin x 1 cos x 1 c ẋ 1 cos x 1 I θ M 3 + M 1 M cos x 1 +M 3(c 1 + c )ẋ 1 )+M M 3 g sin x 1 I θ M 3 + M 1 M cos x 1 (35) where I θ M 3 + M 1 M cos x 1 for all x 1. In order to analyze the behavior of zero dynamics, we obtain a phase diagram as shown in Fig.. From Fig., the behavior of zero dynamics has two modes, an oscillating mode and a rotation mode. In order to swing up the serial double inverted pendulums in the reverse order, we have to choose the rotation mode of zero dynamics. The expression for zero dynamics, (35) is independent of gain k d and k p used in the control (7). However, these gains and the initial conditions of systems determine the trajectory of the zero dynamics. That is to say, the tuning of these gains is crucial to the achievement of the rotation mode. If we choose these optimal gains, the serial double inverted pendulum system will be on an invariant manifold containing (θ, φ) =(π, π). Therefore, we can switch the controller to the stabilization controller of Step 3 in the state of (θ, φ) =(π, π). 15 1-5 -1 5-15 -15-1 -5 5 1 15 Fig. : Phase portrate of zero dynamics 5. Stabilize two pendulums (Step 3) The state (θ, θ, φ, φ) =(π,.π, ) is an unstable equilibrium point of both pendulums to which both pendulums

will be stabilized. Choose x 3 as the following state variables x 3 =(x 31,x 3,x 33,x 34 ) T =(θ π, θ, φ π, θ, φ) T (36) With (36) and (4), the linearized state-space equation of (1) and () around x 3 =(,,, ) is ẋ 3 = Ax 3 + Bu 1 = a 1 a a 3 a 4 1 x 3 + b u a 41 a 4 a 43 a 44 b 4 (37) where the elements of matrices A and B are given as follows. a 1 = M I φ g a = M 1c + I φ (c 1 + c ) a 3 = M 1M 3 g a 4 = I φc 1 + M 1 c a 41 = M 1M g a 4 = I θc + M 1 (c 1 + c ) a 43 = M 3I θ g a 44 = I θc + M 1 c 1 I φ I θ M 1 b = M 1M 3 M I φ I φ I θ M 1 b 4 = M 1M M 3 I θ I φ I θ M 1 By the control of Step, the state variables (x 31, x 33, x 34 ) are in the neighborhood of (,, ). However, the state variable x 3 is not in the neighborhood of, because the first pendulum is rotating. Therefore, we use a sliding mode controller having robustness for stabilization of two pendulums. The input control u which stabilizes (37) is u = (SB) 1 (SAx 3 + R 3 sign(σ)+k 3 σ) (38) σ = Sx 3 (39) where R 3 >,K 3 > and S is the solution of the following Riccati equation with e> P (A + ei)+(a + ei) T P PBB T P + Q = S = B T P 6. Numerical simulation In order to show the performance of the proposed scheme in this paper, we have simulated the reverse order swing up control of the serial double inverted pendulum successfully. The parameters in (1) and () are selected to be those of an experimental system of serial double inverted pendulums in our laboratory. These are given in Table. And the parameters of the control law which are given in Table 3 are chosen by the trial and error method. Simulation result is shown in Fig. 3. Fig. 3 illustrates the responses of θ, θ, φ, φ, z, u and number of steps. The start states of the system are given by (θ(), φ(), θ(), φ())=(π,.1,π,.1). And the times when controllers are switched are Step 1 Step : At the condition of ( φ π)+ φ <.31 Step Step 3: At the condition of ( θ π) <.3 The figure shows that, first, only the second pendulum has swung up by (1) of the controller of Step 1, and next, the first pendulum has swung up while stabilizing the second pendulum at the upright position by (7) of the controller of Step, finally both pendulums have stabilized at the upright position by (38) of the controller of Step 3. Especially, at Step, it turns out that zero dynamics, i.e., the dynamics of the first pendulum is in the rotation mode. Table : Value of system parameters m 1.18 [kg] m.1 [kg] n 1.78 [kg] n.5 [kg] l 1.9 [m] l.115 [m] J 1.5 [kgm ] J.44 [kgm ] J n1.8 1 5 [kgm ] J n. 1 6 [kgm ] I 1. [kgm ] I.18 [kgm ] c 1.1 [kgm /s] c. [kgm /s] L.38 [m] g 9.8 [m/s ] Table 3: Value of control parameters Step 1 u a =38.5, ɛ =π/3 K 1 =[.7755, 1.6156, 1.378,.655] Step k p = 4, k d = Step3 R 3 =3,K 3 =, e =1 7. Conclusions In this paper, a reverse order swing up control scheme of serial double inverted pendulums is proposed. This problem is divided into three steps, Step 1 is to swing

up the second pendulum only, Step is swinging up control of the first pendulum while stabilizing the second pendulum and Step 3 is stabilizing both pendulums at the upright position, and proposed the control laws for the three steps. In Step 1, proposed is a controller which combines the controller to swing up the second pendulum via the energy control method and the state feedback stabilizing controller to stabilize the first pendulum as controller of Step 1. In Step, a controller is proposed to realize a periodic movement of the serial double inverted pendulums while stabilizing the second pendulum by using partial stabilization around the angle of the second pendulum. And by analyzing zero dynamics, it is shown that the zero dynamics has two modes, an oscillating mode and a rotation mode. And to choose k p and k d to make zero dynamics into the rotation mode is shown to be necessary. Moreover, a scheme of avoiding an input singular point is given. In Step 3, a controller proposed to stabilize both pendulums via a sliding mode control method having strong robustness. Finally, a numerical simulation was given to show the effectiveness of the proposed scheme. u 5-5 5 1 3 4 5 6 7-5 5 1 3 4 5 6 7-5 1 3 4 5 6 7-1 3 4 5 6 7 Z - 1 1 3 4 5 6 7-1 3 1 3 4 5 6 7 Step 1 1 3 4 5 6 7 t References 1) K. J. Åström, K. Furuta: Swinging up a pendulum by energy control, Automatica, Vol. 36, No., pp. 87-95,. ) M. Yamakita, M. Iwasaki, Y. Sugahara and K. Furuta: Robust swing up control of double pendulum,proceeding of the American Control Conference, pp. 9 95, 1996. 3) M. Yamakita, K. Nonaka and K. Furuta: Swing up control of a double pendulum, Proceeding of the American Control Conference, pp. 9 33, 1993. 4) M. W. Spong: The swing up control problem for the acrobot, IEEE Control Systems Magazine, Vol. 15, No. 1, pp. 49 55, 1995. 5) M. Kaneda, X. Xin, T. Ochiiwa and T. Henmi: A new hybrid control of inverted pendulum systems, Proceedings of the 5th China-Japan International Symposium on Industrial Management, Beijing, pp. 114-119,. 6) M. Yoneda, X. Xin and M, Kaneda: An integrated design of parameters and controller of the acrobot, Proceedings of International Symposium on Advanced Control of Industial Processes, Kumamoto, pp. 643 648,. 7) T. Emoto, A. Inoue and Y. Hirashima: Swingin up contlol of serial double inverted pendulums using robustness of sliding mode control,proceedings of the 7th Meeting of Chugoku Branch of the Society of Instrument and Control Engineers, pp. 9 93, 1998, (In Japanese). 8) T. Henmi, X, Xin and M. Kaneda: Swing up control of an inverted pendulum consideration of motor s dynamics and cart s distance,proceedings of the 46th Annual Conference of the Institute of System, Control and Information Enginners, pp. 43 44,, (In Japanese). 9) T. Henmi, A, Inoue, Y. Hirashima, N. Ueki, and T. Wada, Realization of periodic movement of serial double inverted pendulum and application to the reverse order swing-up control, Proceedings of the 45th Japan Joint Automatic Control Conference, pp. 91 9, 3, (In Japanese). Fig. 3: Time responces of the reverse order swing up control of serial double inverted pendulums