Control of the Double Inverted Pendulum

on Cart

Mechatronics - MECH 6741

Course Project

Professor
Dr. Chun-Yi Su

By

Amir Sayadi Shervin Foroughi Saud M. Altamimi

Student ID# 40106366 Student ID# 40038869 Student ID# 40059341

Winter 2019
Table of Contents
1 Introduction...........................................................................................................................1

2 Mathematical modeling of double inverted pendulum.........................................................1

3 System identification............................................................................................................5

3.1 DIP setup and specification...........................................................................................5

3.1.1 The linear motion servo plant (Cart)......................................................................6

3.1.2 Power Module........................................................................................................7

3.1.3 The I/O board.........................................................................................................7

3.1.4 Pendulums..............................................................................................................8

3.1.5 Encoders.................................................................................................................8

3.2 Identification approach..................................................................................................8

4 Verification of the results...................................................................................................11

5 Control Design....................................................................................................................13

5.1 History.........................................................................................................................13

5.2 Overview of DIP system control strategies.................................................................14

5.3 Swing-up control.........................................................................................................14

5.4 Closed loop stabilization control.................................................................................15

6 Experimental Results..........................................................................................................18

7 Conclusion..........................................................................................................................19

8 Contributions......................................................................................................................19

9 References...........................................................................................................................20

Appendix A
List of Figures

Figure 1: Schematic of the Double Inverted Pendulum on a Cart............................................................2

Figure 2: Double inverted pendulum setup..............................................................................................6

Figure 3: Cart with servomotor drive......................................................................................................6

Figure 4: Power module..........................................................................................................................7

Figure 5: I/O board..................................................................................................................................7

Figure 6: DIP system encoders................................................................................................................8

Figure 7: Open-loop input signals.........................................................................................................10

Figure 8: Block diagram of MATLAB Simulink model for calculating velocities of the DIP system...10

Figure 9: Comparison between simulated and measured values of state parameters x 0, θ 1, θ 2..........12

Figure 10: Comparison between simulated and measured values of state parameters x 0, θ 1, θ 2........12

Figure 11: Case studies for LQR performance......................................................................................16

Figure 12: Extracted results during the LQR controller operation for 10 seconds.................................17

Figure 13: Results for system responses θ 1 and θ 2 with respect to applied step function....................17

Figure 14: Block diagram of MATLAB Simulink model for DIP control system.................................18

Figure 15: The inserted pins for limiting the rotation of second pendulum...........................................19
1 Introduction
Double inverted pendulum (DIP) on a cart is one of the nonlinear control problems which can
be used for system-based experiments in order to test the linear and nonlinear control laws such
as stabilization of the dynamic systems [1, 2]. Nonlinearity, being multivariable and fast reaction
as well as instability are the essential characteristics of this system [2]. The instability and
nonlinearity of this system makes it the perfect model for examining the various control
algorithms such as PID controls, LQR controls, neural network, fuzzy control, etc. [2].
Commonly, there are two different this system, double inverted pendulum-cart system and the
rotary double inverted pendulum. All these systems consist of two inverted pendulums that are
assembled on a cart or servomotor as a motion generators and rotate about their pivot points.
Pendulums are controlled and stabilized by applying external force or torque to the motion
generators. The double pendulum system consists of one input which is the cart excitation force
and three outputs: angular position of rods and linear position of the cart.
The objectives of this project are (i) formulation of dynamic model of DIP system (ii)
identification of constant parameters of the dynamic model (iii) verification of obtained state
parameters from simulation and real system measurement (iv) swing up the pendulum rods with
utilizing the control function and (v) stabilizing the rods at their unstable equilibrium point.
However, choosing the appropriate control algorithm and developing the system based on that
are the final goal of this project.

2 Mathematical modeling of double inverted pendulum

In this section the mathematical modeling of the DIP will be performed in order to provide a
precise description of the system. This model possess sets of nonlinear equations of system
motions including the constant parameters of the system. The model simulation is performed by
utilizing the MATLAB Simulink software. Table 1 shows the DIP system constants’ notations
and units.
Table 1 Notation and units of constant parameters of the DIP system
Parameter Unit Description
mc [Kg] Mass of the cart
m1, m2 [Kg] Mass of the pendulums
L1, L2 [m] Length of the pendulums
Distance from pivot joints to the
l1, l2 [m]
pendulums’ center of mass

1
 I1, I2 [Kg m2] Pendulums’ moment of Inertia
g m/s2 Gravity constant
x0 [m] Cart position
θ1 , θ2 [rad] Pendulums’ angles

Figure 1 displays the schematic of DIP system on a cart which will be studied in this project.

Figure 1: Schematic of the Double Inverted Pendulum on a Cart

In this study the desired equations of motion are found by implementing Lagrange’s equations:
https://www.youtube.com/watch?v=W6niOz9qEQk – ovjasnjena Lagranzova jednacina
d ∂L ∂L
( )−
dt ∂ q̇ ∂ qi
=Qi i=1,2 , … … ,r ( 2-0 )

where the q i (i =1, 2, 3,….., r) is a set of generalized coordinates, Q i denotes generalized forces
or moments acting in direction of coordinates q and L is the lagrangian function defined as:

L=T ( q1 , q2 , … , qr , q˙1 , … , q̇ r )−V ( q1 , … , qr ) ( 2-0 )

in which T is the kinetic energy and V presents the potential energy. For the presented DIP
system kinetic energy T and potential energy P are obtained by summation of individual
components’ energies. For the system shown in Figure 1 the independent coordinate parameters
are defined as x 0 ,θ1 and θ2. Therefore, by considering the individual external force u exerted to
the system along x coordinate and neglecting the friction of the system, equation (2-1) can be
expanded as follow:
d ∂L ∂L
( ) −
dt ∂ ẋ 0 ∂ x0
=u ( 2-0 )

2
 d ∂L ∂L
( ) −
dt ∂ θ̇1 ∂ θ1
=0 ( 2-0)

d ∂L ∂L
dt ( ∂ θ̇ ) ∂ θ
− =0 ( 2-0)
2 2

Since the system has three structural elements (cart and two pendulum rods), system energies
are defined as follow:
T =T c +T 1 +T 2 ( 2-0 )
P=P c + P1 + P2 ( 2-0)
st nd
In which indices c, 1 and 2 denote the cart, 1 and 2 pendulum rods respectively. Assuming
the components of system, energy is calculated as follow:
1
T c = mc ẋ 20 ( 2-0 )
2
Pc =0 ( 2-0)
1 1
T 1= m 1 ẋ 20+ ( m 1 l 21 + I 1 ) θ̇12+m 1 l1 ẋ 0 θ̇1 cos θ1 ( 2-0 )
2 2

P1=m1 g l 1 cos θ1 ( 2-0 )

1 1 1
T 2= m 2 ẋ 20 + m2 L21 θ̇12+ ( m2 l 22+ I 2 ) θ̇22 +m 2 ẋ 0 ( L1 θ̇ 1 cos θ1 +l 2 θ̇2 cos θ2 )
2 2 2 ( 2-0 )
+m2 L1 l 2 θ̇1 θ̇2 cos ( θ1−θ 1 )

P2=m 2 g ( L1 cos θ 1+l 2 cos θ2 ) ( 2-0 )

Substituting energy equations in equation 2-2, the Lagrangian function of the system becomes:
1 1 1
L= ( mc +m 1+ m 2) ẋ 20 + (m ¿ ¿ 1 l21 +m 2 L12+ I 1) θ̇21 + ( m 2 l 22 + I 2 ) θ̇22 ¿
2 2 2
+ ( m1 l 1+ m2 L1) cos θ 1 ẋ 0 θ̇ 1+ m2 l 2 cos θ 2 ẋ 0 θ̇ 2+ m2 L1 l 2 cos ( θ1−θ 2 ) θ̇ 1 θ̇2 ( 2-0 )
−( m 1 l 1 +m 2 L1 ) g cos θ1−m2 l 2 g cos θ2
Using the obtained result for L, equations 2-3, 2-4 and 2-5 can be rewritten as follow:
u=( ∑ mi ) ẍ 0+ ( m1 l 1+ m2 L1 ) cos θ1 θ̈ 1+ m2 l 2 cos θ 2 θ̈2 −( m1 l 1 +m 2 L1 ) sin θ1 θ̇12
( 2-0 )
−m 2 l 2 sin θ2 θ̇ 22
0=( m 1 l 1 +m 2 L1 ) cos θ1 ẍ 0+(m¿¿ 1l 21 +m 2 L21 + I 1) θ̈1+ m 2 L1 l 2 cos ( θ1−θ2 ) θ̈ 2 ¿
( 2-0)
+m2 L1 l 2 sin ( θ 1−θ2 ) θ̇22−( m1 l1 +m2 L1) g sin θ1
0=m2 l 2 cos θ2 ẍ 0 +m2 L1 l 2 cos ( θ1−θ2 ) θ̈1 + ( m2 l 22+ I 2 ) θ̈2−m2 l 2 g sin θ2
( 2-0)
−m 2 L1 l 2 sin ( θ1 −θ2 ) θ̇ 21
These equations are nonlinear due to the presence of the general terms θ̇2, sin θ,cos θ. The
equation 2-18 presents the compact matrix form of the last three equations:
3
 D ( θ ) θ̈ +C ( θ , θ̇ ) θ̇+G ( θ )=Hu ( 2-0 )
where:

θ=( x 0 θ1 θ 2) T ( 2-0 )

d1 d 2 cos θ1 d 3 cos θ2

(
D ( θ )= d 2 cos θ1 d4
d 3 cos θ2 d5 cos ( θ 1−θ2 )
d 5 cos ( θ1−θ2 )
d6 ) ( 2-0 )

0 −d 2 sin θ1 θ̇1 −d 3 sin θ2 θ̇ 2

(
C ( θ , θ̇ ) = 0 0
0 −d 5 sin ( θ1−θ 2) θ̇1
d 5 sin ( θ 1−θ2 ) θ̇2

0
0 ) ( 2-0 )

(
G ( θ )= −f 1 sin θ1
−f 2 sin θ2 ) ( 2-0 )

T
H= (1 0 0 ) ( 2-0 )
Since in Figure 1 it is assumed that the centers of mass of the rods locate at the geometrical
center, for both rods “l” and “I” will be “L/2” and “m i L2i /12” respectively. As a result, by
substituting magnitudes of “l” and “I” parameters d i (i=1,2 , … , 6) will be defined as:

d 1=mc +m 1+ m 2 (a)

d 2= ( 12 m + m ) L
1 2 1 (b)

1
d 3= m 2 L2 (c)
2

d4= ( 13 m +m ) L
1 2
2
1 (d)
( 2-0 )
1
d 5 = m 2 L 1 L2 (e)
2
1
d 6 = m2 L22 (f)
3

f 1= ( 12 m +m ) L g
1 2 1 (g)

1
f 2 = m 2 L2 g (h)
2

4
 So, considering the equation 2-18 and multiplying it by the matrix D −1 ( θ ) as well as defining
the state variables and substituting in this equation, the nonlinear form of state equation will be
obtained as below:
β̇ 4 β4

()
β̇ 6
−1

β6() −1 −1
β̇5 =−D ( θ ) C ( θ , θ̇ ) β 5 + ( D ( θ ) Hu−D ( θ ) G ( θ ) ) (a)

β 1=x 0 , β 2=θ 1 , β 3=θ2 , β 4= ẋ 0 , β 5=θ̇ 1 , β 6=θ̇2 , (b)

β̇ 1=β 4 (c)
( 2-0 )
β̇ 2=β 5 (d)
β̇ 3=β 6 (e)
β̇ 4 = ẍ0 (f)
β̇ 5=θ̈1 (g)
β̇ 6=θ̈ 2 (h)
The equation 2-26 (a) could be written in a following compact form as well [1]:

β̇=
(00
−D (θ )
I
−1 −1
C (θ , θ̇ ) ) β+
( −1
0
D (θ) H ) u+
( 0
−D (θ ) G ( θ ) )
( 2-0 )

So far, the dynamic model of the DIP system as well as nonlinear form of state equation have
been derived by implementing the Lagrangian function. In next section the identification of this
system will be performed in order to find the parameters presented in equation 2-24.

3 System identification
In order to identify the unknown parameters presented in equations 2-24 (
d 1 , d 2 , d 3 , d 4 , d5 , d 6 , f 1 , f 2 ¿, the on-line system identification will be performed by utilizing the
energy theorem. As a result, it is needed to form equations that can be solved for d i. In this
approach the on-line data needs to be collected from the real system. In the following, the DIP
setup used in this project is introduced in detail.
3.1 DIP setup and specification
This section introduces the hardware and system components used in order to successfully
erect and balance the pendulum. Figure 2 illustrates the system setup used in this project.

5
 Power supply
Module

Rack
2nd pendulum encoder
Cart Pendulums

(a) (b)
Figure 2: Double inverted pendulum setup

The main parts of this setup are linear motion servo plant (Cart), Power supply module, I/O
board, Pendulums and encoders that are introduced. Some of detail information are extracted
from reference [3].
3.1.1 The linear motion servo plant (Cart)
Figure 3 displays the cart of the DIP system. The servomotor connects to the power supply
module and provides the cart motion. The input voltage limitation of the servomotor is ±6 volts.
The rack and pinion mechanism of the system converts the rotary motion of the motor to the
linear motion.

Pendulum axis connected

to 1st pendulum encoder

Position pinion connected

to Cart encoder

Pinion connected to Cart

motor axis

Rack
1st pendulum
connector

Figure 3: Cart with servomotor drive

6
3.1.2 Power Module
Figure 4 displays the power module with a linear power operational amplifier along with a
dual output DC power supply set.

To motor

Figure 4: Power module

The DC power supply has three ports +12V,-12V and GND, which supply the power to the
components. The operational amplifier has found binding posts, it can be used in standard op-
amp configurations. Its two inputs are labeled (-) and (+) for the inverting input and non-
inverting inputs respectively.
3.1.3 The I/O board
Figure 5 shows the Q4 board which has multiple I/O ports and supports digital and analog
sensors and is responsible for data acquisition. It provides four quadrature encoder inputs, used
for transferring the encoders of the cart and pendulums data to the computer unit.

Signal port of 2nd

pendulum’s encoder

Signal port of 1st

pendulum’s encoder

Signal port of
cart’s encoder

Figure 5: I/O board

7
 3.1.4 Pendulums
As it was shown in Figure 2 the DIP system includes two pendulums. The socket joint
connects the system of jointed pendulums to the cart’s axis. These pendulums are free to rotate
about their joints due to the cart motion.
3.1.5 Encoders
The DIP system include three optical encoders. The cart and pendulums’ positions are
measured with these three sensors. Using the encoder provides the possibility for sensing the
wide range of pendulum angular positions up to inverted position [3]. The cart’s linear position
encoder performs the measurement through the rack-pinion system. Figure 6 displays these
encoders assembled in the system.

Cart’s encoder

1st pendulum’s
encoder

2nd pendulum’s encoder

a) Cart’s and 1st pendulum encoders b) Encoder for 2nd pendulum

Figure 6: DIP system encoders
3.2 Identification approach
According to the energy theorem, the work done by the system in a certain period is equal to
the total energy change of the system. By neglecting the effect of the friction, in the DIP
mechanism, the motor performs the work and the energy change of the system is equal to the
summation of energy change for each individual elements, two pendulums and the cart. The
equation 3-1 displays the energy theorem:
t1
W m|t =( K ( t 2 ) +V ( t 2) )−( K ( t 1 ) +V ( t 1 ) )=E ( t 2 )−E ( t 1 )
2
( 3-0 )

which the W m is the work done by motor, K, V and E are kinetic energy, potential energy and
total energy of the system respectively at the times t 1 and t 2. For the DIP system, by using

8
equations (2-8 … 2-13) and substituting the unknown parameters, the total energy equation of
the system at the time t is obtained as:
1 1 1
E= d 1 ẋ 20 + d 4 θ̇21 + d 6 θ̇ 22+ d 2 cos θ 1 ẋ 0 θ̇ 1+ d3 cos θ 2 ẋ 0 θ̇ 2+ d 5 cos ( θ 1−θ2 ) θ̇1 θ̇ 2
2 2 2 ( 3-0 )
+ f 1 cos θ1+ f 2 cos θ2
This equation is linear in parameters d i. The work done by the motor between two times t 1 and
t 2 is determined as below:
t2
t1
W m|t =∫ K m V ẋ0 dt ( 3-0 )
2
t1
in which K m is the motor constant that is considered 0.6, V is the supplied voltage to the motor
and ẋ 0 is the velocity of the cart.
Next, by utilizing the developed MATLAB Simulink model and identification code following
steps will be applied:
 Driving the real system setup from fully extended downward resting position, with 2
open-loop input signals, 1 and 1.25 volts, as shown in Figure 7. Each of input signals
applied for duration of 15s
 Recording the displacement parameters x 0, θ1, θ2 reported by the setup’s encoders at
certain time steps (t i) during driving the system. The time steps have been considered
equal to 0.05s
 Calculating the velocities ẋ 0, θ̇1 and θ̇2 as well as energy of the system E ( t i ) by
implementing stored data at each time step (t i)
 Calculation of servomotor’s work, by substituting the ẋ 0 in equation 3-3 for all
successive time intervals

9
 1.5

0.5

-0.5

-1

-1.5
0 5 10 15 20 25 30

Figure 7: Open-loop input signals

Figure 8 displays the block diagram of the MATLAB Simulink model.

Figure 8: Block diagram of MATLAB Simulink model for calculating velocities of the DIP system

Finally, rewriting the equation 3-1 for all time intervals and substituting the recorded and
calculated data from previous mentioned steps, results in forming number of linear equations
with 8 unknowns (d 1 , d 2 , d 3 , d 4 , d5 , d 6 , f 1 , f 2 ¿. In order to identify the optimized solution for
these unknowns, the least square minimization method has been implemented in another
developed MATLAB code, see appendix A. Table 2 shows the obtained results from the
optimization algorithm.

10
 Table 2 Identified unknowns from optimization algorithm
d1 d2 d3 d4 d5 d6 f1 f2
1.2432 0.0601 0.0489 0.0012 0.0071 0.0039 0.3216 0.2910
4 Verification of the results
In order to provide the possibility of solving nonlinear system dynamic equation 2-26
numerically, verifying the results and comparing those with the measured data, this equation
should be linearized by considering following assumptions:
 For stabilizing the pendulums along the vertical line, the angles θ1 and θ2 must be kept
small which leads to: sin θ ≈ θ and cos θ ≈ 1.
2 2
 The θ̇1 and θ̇2 will be kept small, so the θ̇1 and θ̇2 will be negligible.
Considering the above assumptions, the state space equation is modified to following form:
β̇= A β + Bu ( 4-0 )
where:
0 I
A=
(−D ( 0 )
∂ G(0)
−1

∂θ
0 ) ( 4-0 )

0
B=
(
D ( 0 )−1 H ) ( 4-0 )

By substituting the identified unknown parameters from Table 2 into the system dynamic
equations, 4-1, 2 and 3, the equation 4-1 is solved numerically with RK45 method in MATLAB
for calculating the x 0, θ1, θ2, ẋ 0, θ̇1, θ̇2 . On the other hand, the responses of the system to the
input signals, which were introduced in Figure 2, have been measured and recorded in order to
compare with the last numerically identified results. Figure 4 and 5 illustrate the comparison
between the simulated and measured values of state parameters x 0, θ1, θ2, ẋ 0, θ̇1, θ̇2 during the 30
seconds.

11
 Simulated Response Comparison
0.01
Simulated Measured
0.005

-0.005

-0.01

0.2 Simulated Measured

0.1

-0.1

-0.2

0.5
Simulated Measured

-0.5
5 10 15 20 25 30
Time (seconds)

Figure 9: Comparison between simulated and measured values of state parameters x 0, θ1, θ2

Simulated Response Comparison

0.1
Simulated Measured

0.05

-0.05

-0.1

2 Simulated Measured

-1

-2

5
Simulated Measured

-5
5 10 15 20 25 30
Time (seconds)

Figure 10: Comparison between simulated and measured values of state parameters ẋ 0, θ̇1, θ̇2
Results show that the responses behavior of identified model is in good agreement with
measured responses from driving the on-line DIP system. The difference between the magnitude

12
of responses are due to the neglecting of friction in simulation model, error of data capturing by
the encoders and linearizing of the system dynamic model. In order to identify the error of each
state parameters x 0, θ1, θ2, ẋ 0, θ̇1, θ̇2 resulted from simulation with respect to one measured from
real system the standard error has been calculated. The implemented equation of standard error is
presented in equation 4-4.
N

SE=
√∑ |
k=1
2
β m−β s|
( 4-0 )
N
where N is the number of recorded samples, β m and β s are the measured and simulated state
parameters respectively.
Table 3 displays the calculated values of the standard errors.
Table 3 Standard deviation of state parameters
x c (m) θ1 (rad) θ2 (rad) ẋ c (m/s) θ̇1 (rad/s) θ̇2 (rad/s)
0.0063 0.0035 0.0489 0.0610 0.0344 0.0839
As it has been mentioned so far, these errors obtained due to neglecting of the friction in
simulation model, accuracy in data capturing by the encoders and linearizing of the system
dynamic model.
So far, formulation of dynamic model of DIP system, identification of constant parameters of
the dynamic model as well as verification of obtained state parameters from simulation and real
system measurement have been performed. In following sections, the swing up and stabilization
control systems for DIP will be designed, utilized and verified in order to identify the accuracy
of them.

5 Control Design
5.1 History
There are number of different controllers, which have been developed in order to achieve the
desired response and performance of the systems. The Proportional-Integral-Derivative (PID)
control is a loop feedback controller that still is used in industrial fields and despite of its
simplicity it provides the efficient solution to simple problems. But, in case of occurrence of
complexity in the system such as nonlinearity it cannot work effectively. This weakness could be
solved by combination of PID with other type of controllers such as adaptive, fuzzy, LQR, etc.

13
 With the improvements made in the computing hardware and advent of efficient algorithms,
the usage of optimal control has facilitated [4]. This control deals with optimizing a certain
criteria for finding the control law for a system [5]. The linear quadratic regulator (LQR) and
optimal neural network control are examples of this control system.
Adaptive control is another control method. In this method, the system repeatedly updates the
controller parameters in order to minimize the error between actual system behavior and the
model response [6].
The last two algorithms, LQR and Adaptive control, are the control methods which will be
used in this research in order to control the performance of DIP on a cart system dynamics.
5.2 Overview of DIP system control strategies
The control design of the DIP system is performed in two steps. First, designing the controller
for the swing-up, next controller design for the stabilization of pendulums in an upright position
by implementing MATLAB Simulink model. During this process of control, the swing-up
controller is supposed to bring up the pendulums upright close to the stability point. Once the
angular position of the pendulums reaches the certain predefined range of angles the system
control will be switched to the closed loop stabilization controller that is supposed to take care of
the system stabilization and maintains the DIP at the equilibrium position.
5.3 Swing-up control
In this step, the energy method has been performed to derive an energy based swing-up
controller for the DIP system. The energy equation of the DIP is defined as equation 5-1:
1 1 1
E= d 1 ẋ 20 + d 4 θ̇21 + d 6 θ̇ 22+ d 2 cos θ 1 ẋ 0 θ̇ 1+ d3 cos θ 2 ẋ 0 θ̇ 2
2 2 2
( 5-0 )
+d 5 cos ( θ 1−θ2 ) θ̇1 θ̇ 2+ f 1 cos θ1 +f 2 cos θ2

Rewriting the energy equation to the matrix form, the equation 5-2 is obtained:
d1 d 2 cos θ1 d 3 cos θ2 ẋ0
1
2 [
E= [ ẋ0 θ̇1 θ̇2 ] d 2 cos θ1 d4
d 3 cos θ2 d 5 cos ( θ1−θ2 )
d 5 cos ( θ1−θ2 ) θ̇1
d6 θ̇2 ][ ] ( 5-0 )

+ f 1 cos θ1+ f 2 cos θ2

Using the dynamic equation (2-18) and taking first derivative of E, the Ė will become:
Ė=θ̇T ( D ( θ ) θ̈+C ( θ , θ̇ ) θ̇+G (θ ) )=θ̇ T Hu= ẋ0 u ( 5-0 )

14
Defining the following Lyapunov function candidate:
1
V = ( E−E uu)2 ( 5-0 )
2
in which the Euuis a potential energy at upright equilibrium point where θ=0 and θ̇=0and is
equal to E.
Euu=f 1 +f 2
( 5-0 )
So, V at stabilization point will be equal to zero and V̇ is defined as
V̇ = ẋ 0 ( E−Euu )u ( 5-0 )
if the second term in right hand of above equation be chosen such that
u=−K V ẋ 0 ( E−E uu) ( 5-0 )
Where K V is a positive constant, then

V̇ =−K V ẋ20 (E−Euu )2 ≤ 0 ( 5-0 )

which means that the stability condition is satisfied. The control force u is applied to the system
by a DC-motor mounted on the cart.
5.4 Closed loop stabilization control
In order to stabilize pendulums in the upright position the linear quadratic regulator (LQR)
design is performed to design the proper controller. The LQR design will effectively return the
state feedback gains needed to ensure stability of the system. However, to minimize the error of
the cart position, a tracking controller is added by integration of cart position error with respect
to the setting point over time. The gain adjustment of the integration result allows control over
the zero steady state error convergence time. Since the LQR is applicable for the linear system
dynamics, the linearized form of equation 2-26 for the DIP system around β=0, has been
introduced in equation 4-1, will be utilized to derive an approximate linear solution to the
optimal control problem. The linearized equation is recalled again in following.
β̇= A β + Bu
where:
0 I
A=
(−D ( 0 )
−1 ∂ G(0)
∂θ
0 )
0
B=
( D ( 0 )−1 H )
15
 Next, the quadratic performance index function which should be minimized by choosing the
proper control signal u, is defined as follow:
∞ ( 5-0 )
J=∫ ( βT Qβ+ uT Ru ) dt
0
where Q and R are positive definite matrices. In case of applying disturbance to the system or
offset from the equilibrium point β=0, the control signal u should maintain the equilibrium state
of the system at β=0 while the J is minimal. The input signal u is defined as following:
u ( t )=−R−1 B T P ( t ) β ( t )=−Kβ ( t ) ( 5-0 )
where P(t) is the solution of Riccati equation and K is the linear optimal feedback matrix.
In order to prove the successful performance of the LQR controller, Figure 11 shows the
results extracted from Simulink model for three different cases for angle θ1 and θ2.

x0 θ1 θ2
a ¿ θ1=θ 2 ∈[0 5]

x0 θ1 θ2
b ¿ θ1 >θ2 when θ1=10 °∧θ2 =5°

x0 θ1 θ2
c ¿ θ1< θ2 whenθ1 =0 °∧θ2=5 °

16
 Figure 11: Case studies for LQR performance
The graphs show that the accurate performance of the LQR around the upright position for
angles in the interval [0 10].
In another case study, the pendulums were brought up manually and were left close the
stabilization point at upright position. The response of the LQR controller during the stabilization
of the DIP have been recorded for 10 seconds. Figure 12 shows the results.

20 20

0 0

-20 -20

-40 -40

-60 -60

(deg)
(deg)

-80 -80

2
1

-100 -100

-120 -120

-140 -140

-160 -160

-180 -180
0 5 10 15 0 5 10 15
Time(sec) Time(sec)

θ1 θ2
Figure 12: Extracted results during the LQR controller operation for 10 seconds

Figure 13 illustrates the system responses θ1 and θ2 to the applied step function about the
stabilization point.

10
10

8
8

6 6

4 4

2 2
(deg)

(deg)

0 0
1

-2 -2

-4 -4

-6 -6

-8
-8
-10
-10 0 5 10 15
0 5 10 15
Time(sec)
Time(sec)

Applied step function θ1 θ2

Figure 13: Results for system responses θ1 and θ2 with respect to applied step function

Results shows a well performance of the LQR in order to maintain the stability of the system
around the equilibrium point.

17
6 Experimental Results
In order to ensure effective performance of the designed controllers, the integrated MATLAB
simulink model, included both controllers, was developed, as shown in Figure 14.

Figure 14: Block diagram of MATLAB Simulink model for DIP control system

Then, the DIP system on a cart was run with the cart’s motor input power 6 volts. The total
cart movement on a rack was limited to 70 centimeters based on the total length of the rack.
Since the possibility of working with each controllers, integrated or individually, have been
considered in the structure of Simulink model, first, two controllers were examined individually
which showed that both of them were working well. Next, the coupled controllers were tested. In
this test, The swing-up happened successfully and the controller brought up the pendulums until
the predefined angle 20◦, but the LQR was not able to stabilize the system at upright position and
failed. By studying and performing multiple experiments it was observed that, whenever the
control of the system switched to the LQR, the relative angle between second pendulum and the
first rod was large. Therefore, due to the angular momentum of the second pendulum near the
equilibrium point, the LQR controller was not able to reduce the large relative angle, so the
system passed the stability point and kept rotating. This issue could happen because of the
fabricated structural restriction against the rotation of second pendulum, which was made by
inserting two screws at both sides of the second rod’s root, as shown in Figure 15, which
prevents the proper balance of pendulums angular energy during the swing-up process.

18
 Screws
Screws

Figure 15: The inserted pins for limiting the rotation of second pendulum
In fact, the existing limitation in rotation of second pendulum imposes an unexpected
nonlinearity in the linearized system, which causes to instability of the system.

7 Conclusion
In this research, the control system for manipulation and stabilization of double inverted
pendulum on a cart was designed and tested. The integrated controller included the swing-up and
LQR controllers. Results from experiments showed the accurate and acceptable performance of
each controllers. In case of using both controllers to perform the swing-up and stabilization
continuously, the LQR was not able to maintain and stabilized the system at the upright position
due to the structural restriction applied for rotation of second pendulum. To overcome this issue
removing the motion limitation or using more than one controller for the swing-up in order to
compensate the imposed disturbance to the system during swinging from the structural limitation
are the tasks that can be proposed as future works.

8 Contributions
Amir, Shervin and Saud put efforts on this project equally. All project tasks were defined at
the beginning and distributed between them. Following table shows the tasks distribution.

Task Responsible person/s Helped by

Literature survey All
Simulink Model Amir Shervin and Saud
Report Shervin Amir and Saud
Presentation Saud Shervin and Amir
Experiments All

19
9 References
[1] Bogdanov, Alexander. “Optimal control of a double inverted pendulum on a
cart.” Oregon Health and Science University, Tech. Rep. CSE-04-006, OGI School of
Science and Engineering, Beaverton, OR (2004).
[2] Yadav, Sandeep Kumar, Sachin Sharma, and Narinder Singh. “Optimal control of double
inverted pendulum using LQR controller.” International Journal of Advanced Research in
Computer Science and Software Engineering 2, no. 2 (2012).
[3] C. Su, T. Wen, G. Huard, “Mechatronics MECH 474 Project Manual”, 2015.
[4] Oskar Stattin, “Optimal Control of Inverted Pendulum,”' B.S. thesis, Dept. Math., Royal
Institute of Technology Univ., Stockholm, Sweden, 2015.
[5] Alexander Bogdanov, “Optimal Control of a Double Inverted Pendulum on a Cart”, OGI
School of Science & Engineering, Hillsboro, OR, Tech. Rep. CSE-04-006, 2004.
[6] Steven A. Frank, “Control Theory Tutorial: Basic Concepts Illustrated by Software
Examples”, SpringerBriefs in Applied Sciences and Technology, Chapter 11, pp. 85-89,
2018.

20
 Appendix A
clc
clear all

% Load data from workspace

load('doublePend.mat');
clear 'dGama'; clear 'ltq';
Rm = 2.6;
Kt = 0.00767;
rmp = 6.35e-3;
tau = V * Kt/(Rm*rmp);
g=9.81;
theta2_f = theta1_f + theta2_f;

% Define coeficient array

Gama = [0.5*x_dot_c.^2,...
cos(theta1_f).*(x_dot_c.*theta1_dot),...
cos(theta2_f).*(x_dot_c.*theta2_dot),...
0.5*theta1_dot.^2,...
cos(theta1_f-theta2_f).*theta1_dot.*theta2_dot,...
0.5*theta2_dot,...
cos(theta1_f),...
cos(theta2_f)];
wm = tau .* x_dot_c;
step = 10;
for i = 1:(length(Gama) - step):2
dGama(i,:) = Gama(i+step,:) - Gama(i,:);
ltq(i,1) = trapz(time(i:i+step,1),wm(i:i+step,1));
end

% Define lower bound for optimization

lb = [ 0.5 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ];

% Define upper bound for optimization

ub = [ 2 ; 0.1 ; 0.1 ; 0.01 ; 0.01 ; 0.01 ; 0.5 ; 0.5 ];

% Use lsqlin to find parameters

options = optimoptions('lsqlin','PrecondBandWidth',inf);
phi = lsqlin(dGama,ltq,[],[],[],[],lb,ub,[],options)

dt= 0.05 ; starttime = 15/dt; endtime = 45/dt;

d1 = phi(1);
d2 = phi(2);
d3 = phi(3);
d4 = phi(4);
d5 = phi(5);
d6 = phi(6);
f1 = phi(7);
f2 = phi(8);

21
% Define matrices using identified params at zero point
t1 = 0; t2 = 0;
d0=[d1, d2*cos(t1), d3*cos(t2);
d2*cos(t1), d4, d5*cos(t1-t2);
d3*cos(t2), d5*cos(t1-t2), d6];
dg=[0,0,0;0,-f1,0;0,0,-f2];

% Define the State Space System

A=[zeros(3),eye(3);-inv(d0)*dg,zeros(3)];
B=[zeros(3,1);inv(d0)*[1;0;0]];
C=eye(6); D=zeros(6,1);
sys=ss(a,b,eye(6),0);
Q =diag([5 50 50 20 700 700]); R=1;

% Compute the feedback k

k=lqr(sys,Q,R);

% Define the new system

sysnew=ss(a-b*k,b,c,0);

% simulation and comparing data in plot

[y,t,xi]=lsim(sysnew,V(starttime:endtime),time(starttime:endtime));

figure
Simulated = iddata(y(:,1:3),V(starttime:endtime),0.05);
Measured = iddata([-
xc(starttime:endtime),theta1_f(starttime:endtime),theta2_f(starttime:endtime
)],V(starttime:endtime),0.05);
compare(Simulated,Measured)

figure
Simulated = iddata(y(:,4:6),V(starttime:endtime),0.05);
Measured = iddata([-
x_dot_c(starttime:endtime),theta1_dot(starttime:endtime),theta2_dot(starttim
e:endtime)],V(starttime:endtime),0.05);
compare(Simulated,Measured)

err_beta=[norm(xc(starttime:endtime)),
norm(theta1_f(starttime:endtime)),
norm(theta2_f(starttime:endtime)),
norm(x_dot_c(starttime:endtime)),
norm(theta1_dot(starttime:endtime)),
norm(theta2_dot(starttime:endtime))];

% error estimation for each state

SE = err_beta/size(y,1);

22