Professional Documents
Culture Documents
July 2006
1. Objectives
Control of an inverted pendulum is one of the most interesting and classical problems for control
engineering. The objective of this project is to design a controller which is capable of driving the pendulum
from its “hanging-down” position to upright position and then holding it there. The experiment system, as
shown in Fig. 1, consists of a vertical pendulum, a horizontal arm, a gear chain, and a servomotor which
drives the pendulum through the gear transmission system. The rotating arm is mounted on the output gear
of the gear chain. An encoder is attached to the arm shaft to measure the rotating angle of the arm. At the
end of the rotating arm there is a hinge instrumented with an encoder. The pendulum is attached to the
hinge.
2. System requirements
To complete this project, the following hardware and software are required:
1) A Quanser UPM 1503 universal power module
2) A Quanser Q4 data acquisition card
3) A Quanser terminal card
3) A Quanser SRV02-ET servomotor and the geared driving unit
4) A Quanser rotary inverted pendulum
2
5) A PC
6) Quanser’s WinCon 5.0 software
7) Matlab/Simulink and control system toolbox
All of the above-mentioned hardware and software are provided in the Robotics lab (JH608).
Pendulum
α L
y
α&
x
arm A
z r
θ
O θ&
3
Ax x
A B Pendulum
z
θ Arm
y
mg
θ& α
Beqθ& Ox
Tl O
α&
Oz Ax A x
Ay
(a) F.B.D. of Arm (b) F.B.D. of Pendulum
Fig. 3 Free Body Diagram of the rotary inverted pendulum system
Differentiating (2) with respect to time, we will get the acceleration of the point B
Applying Euler’s Equation to the rotational motion of the pendulum about point B , we obtain
1
J Bα&& = ∑ M B ⇒ m(2 L) 2 α&& = Ax L cos(α ) + Ay L sin(α )
12 (6)
1
⇒ mL2α&& = Ax L cos(α ) + Ay L sin(α )
3
Applying the Euler’s Equation to the rotational motion of the arm about point O , we obtain
1 2
mL α&& = (mrθ&& + mL sin(α )α& 2 − mL cos(α )α&&) L cos(α )
3
+ (mg − mL cos(α )α& 2 − mL sin(α )α&&) L sin(α )
1
⇒ mL2α&& = (mLr cos(α )θ&& + mL2 sin(α ) cos(α )α& 2 − mL2 cos 2 (α )α&&) (8)
3
+ (mgL sin(α ) − mL2 sin(α ) cos(α )α& 2 − mL2 sin 2 (α )α&&)
4
⇒ −mLr cos(α )θ&& + mL2α&& − mgL sin(α ) = 0
3
4
J eqθ&& = Tl − Beqθ& − (mrθ&& + mL sin(α )α& 2 − mL cos(α )α&&)r
⇒ J eqθ&& = Tl − Beqθ& − (mr 2θ&& + mLr sin(α )α& 2 − mLr cos(α )α&&) (9)
⇒ ( J eq + mr 2 )θ&& − mLr cos(α )α&& + mLr sin(α )α& 2 = Tl − Beqθ&
Combining (8) and (9), we obtain the motion of equation of the system
( J eq + mr 2 )θ&& − mLr cos(α )α&& + mLr sin(α )α& 2 = Tl − Beqθ&
4 (10)
− mLr cos(α )θ&& + mL2α&& − mgL sin(α ) = 0
3
1 1 1
J eqθ& 2 + m( x& B + y& B ) + J Bα& 2
2 2
T=
2 2 2
1 1 1
= J eqθ& 2 + m[(rθ& − L cos(α )α& ) 2 + (− L sin(α )α& ) 2 ] + J Bα& 2 (11)
2 2 2
1 2
= ( J eq + mr 2 )θ& 2 + mL2α& 2 − mLr cos(α )θ&α&
2 3
1 1
where J B = m(2 L) 2 = mL2 is the moment of inertia of the pendulum about its center of mass. Taking
12 3
the horizontal plane where the arm lies as the datum plane, the only potential energy in the mechanical
system is gravity, i.e.,
V = mgL cos(α ) (12)
Since we have two generalized coordinates, θ and α , we therefore have two equations according to
Lagrangian Formulation
d ∂T ∂T ∂V
( )− + = Tl − Beqθ&
dt ∂θ& ∂θ ∂θ
(13)
d ∂T ∂T ∂V
( )− + =0
dt ∂α& ∂α ∂α
Substituting (11) and (12) into (13), we obtain the motion of equation of the system (10) as expected.
From Project I, we have known that the output torque of the driving unit on the load shaft is
Tl = η g K g (Tm − J mθ&&m )
= η g K g (η m K t I m − J m K g θ&&)
Vm − K m K g θ&
= η mη g K t K g − η g K g2 J mθ&& (14)
Rm
η mη g K t K g η mη g K t K g2 K m &
= Vm − θ − η g K g2 J mθ&&
Rm Rm
Substituting (14) into (10), we obtain the nonlinear model of the system as follows:
where
5
a = J eq + mr 2 + η g K g2 J m
b = mLr
4 2
c= mL
3
d = mgL (16)
2
η mη g K t K K mg
e = Beq +
Rm
η mη g K t K g
f =
Rm
Linearizing (15) under the assumption that α ≈ 0 and α& ≈ 0 , we get the linearized model as follows:
aθ&& − bα&& + eθ& = fVm
(17)
− bθ&& + cα&& − dα = 0
Solving (15) for the two accelerations α&& and θ&& , we obtain the solution from the nonlinear model below:
1 − b sin(α )α& 2 − eθ& + fVm − b cos(α )
θ&& = 2 2
ac − b cos (α ) d sin(α ) c
1
= (−bc sin(α )α& 2 + bd sin(α ) cos(α ) − ceθ& + cfVm )
ac − b cos2 (α )
2
(18)
1 a − b sin(α )α& 2 − eθ& + fV m
α&& =
ac − b 2 cos2 (α ) − b cos(α ) d sin(α )
1
= (ad sin(α ) − b 2 sin(α ) cos(α )α& 2 − be cos(α )θ& + bf cos(α )Vm )
ac − b 2 cos2 (α )
Solving (17) for the two accelerations α&& and θ&& , we find the solution from the linearized model as follows:
1 − eθ& + fVm −b 1
θ&& = 2
= (bdα − ceθ& + cfVm )
ac − b dα c ac − b 2
(19)
1 a − eθ& + fVm 1
α&& = 2
= (adα − beθ& + bfVm )
ac − b − b dα ac − b 2
To obtain the transfer function of the linearized system equations (17) analytically, we need to take the
Laplace transformation of it, namely,
In the above the initial conditions have been assumed zero. The transfer function relates the variation from
the desired position of the pendulum to the input voltage of the motor. Since we are interested in the angle
α , we want to eliminate Θ( s ) from (20)
Α( s) bfs 2
= (21)
Vm (s) (ac − b 2 )s 4 + ces 3 − ads 2 − des
From the above transfer function, it can be seen that there is a pole and a zero at the origin. They can be
canceled from each other and the resulting transfer function becomes
Α( s ) bfs
= (22)
Vm ( s) (ac − b 2 ) s 3 + ces 2 − ads − de
This is our final model of the inverted pendulum system for designing the controller. Note that this transfer
function represents the linearized model only.
6
4. Verification of the mathematical model
7
Fig. 6 Diagram of the linear model
60
Linear
40 Nonlinear
θ (deg)
20
-20
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8
Time (s)
250
Linear
200
Nonlinear
150
α (deg)
100
50
0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8
Time (s)
Fig. 7 Overlapped plots of the simulation outputs from the linear and nonlinear models
8
1
θ&& = (bdα − ceθ& + cfVm )
ac − b 2 (24)
1
α&& = (−adα + beθ& − bfVm )
ac − b 2
The main Simulink diagram of comparing both the linear and nonlinear models with the data measured
from the physical system is shown in Fig. 8. The interface to the inverted pendulum system is shown in Fig.
9. The linear model and the nonlinear model, as shown in Figs. 10 and 11, are created according to (23) and
(24) respectively.
Fig. 8 Main diagram of verifying both the linear model and the nonlinear model
9
Fig. 10 Liner model of the inverted pendulum system
In the experiment, a ramp signal with a slope of 2, as shown in Fig. 12, is used as the input voltage of
the system. From the plots of θ and α , as shown in Fig. 13, we can see that the responses of the models
are similar to these of the physical system. They have almost the same shape. The responses of the physical
system have some delay due to the friction in the physical system as explained in project I.
10
2.5
0.5
0
0 0.2 0.4 0.6 0.8 1 1.2
Time (s)
80
Linear model
Nonlinear model
60
Physical system
θ (deg)
40
20
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
Time (s)
0
Linear model
Nonlinear model
-2
Physical system
α (deg)
-4
-6
-8
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
Time (s)
Fig. 13 Plots of the verifying both the linear model and the nonlinear model with the physical system
5. Destabilizing controller
The controller of the whole system consists of three parts: destabilizing controller, stabilizing
controller, and mode controller. The destabilizing controller, as the name implies, oscillates the arm until it
has built up enough energy to break the initial stable (hanging-down) state and get the pendulum into an
almost upright but unstable state. Then the stabilizing controller is turned on to stabilize the pendulum in its
11
upright sate. The mode controller determines when to switch between the destabilizing controller and
stabilizing controller. We will discuss the design and implementation of the destabilizing controller in this
section. The mode controller and the stabilizing controller will be discussed in the next two sections.
Destabilizing controller will essentially drive the position of the arm in order to get away from the
stable “hanging-down” position of the pendulum. It simply makes sense that, by moving the arm back and
forth strongly enough, it can eventually swing up the pendulum. Hence, the first thing we need to do is to
design a position controller which can swing the arm to achieve the destabilizing goal.
The pendulum in the system has a length of 2 L = 0.335 (m) and its center of mass is located at its
geometric center. Thus the natural frequency for small oscillations of the pendulum is given by
mgL 3g
ωp = = = 6.628 (rad/s ) (25)
IA 4L
where I A is the mass moment of inertia of the pendulum about point A . We want the arm to react to these
movements. Therefore the closed-loop response of the arm should be considerably faster than the natural
frequency of the pendulum. It would then be reasonable to design a closed-loop controller for the arm
position which has the following specifications
where %OS is the maximum overshoot of the response for a step input.
For the arm to track the desired position, we design a PD control law
Vm = K p (θ d − θ ) − K vθ& (27)
This is a position control loop that controls the voltage applied to the motor so that θ tracks θ d with zero
desired velocity. Now we need to determine K p and K v according the above defined specifications (26).
θ η gη m K g K t K p
= (28)
θ d J eq R m s 2 + ( Beq R m + η g η m K m K t K g2 + η gη m K g K t K v ) s + η g η m K g K t K p
Comparing it with the standard transfer function of a second order system (see Section 5-3 of the textbook)
we have
Beq Rm + η gη m K m K t K g2 + η gη m K g K t K v
= 2ω n ζ
J eq Rm
(29)
η gη m K g K t K p 2
=ω n
J eq Rm
12
2ω n ζJ eq Rm − Beq Rm − η gη m K m K t K g2
Kv = = 0.585
η gη m K g K t
(30)
ω n2 J eq Rm
Kp = = 19.612
η gη m K g K t
With these values of the control grains, we expect the arm tracks the desired position and velocity with the
required specifications.
The main Simulink diagram of the simulation of the position controller is shown in Fig. 14 and the
Simulink diagram of the servomotor and gear transmission system is shown in Fig. 15. Given the required
specifications ω n = 26.512 (rad/s), %OS = 2% , the step response of the closed-loop system is shown in Fig.
17. We can see that the response has a maximum overshoot of 2% and the first peak at 0.189 second. So the
position controller meets the required specifications.
6
Control signal (V)
-2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (s)
Fig. 16 Control signal of the system
13
25
System reponse
15
10
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (s)
The main Simulink diagram of the implementation of the position controller is shown in Fig. 18 and the
interface to the servomotor and gear transmission system is shown in Fig. 19. The model of the servomotor
and gear transmission system is shown in Fig. 15. An example step response of the system is shown in Fig.
21. In this example, the desired angular position of the arm is set to 20 degrees.
14
8
Simulated control signal
6 Measured control signal
-2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)
Fig. 20 Simulated and real control signals for the position control experiment
25
Load shaft position (deg)
20
15
10
Input command
5 Simulated position
Measured position
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)
Fig. 21 Simulated and real dynamic responses of the system to a step input
15
Fig. 22 Main diagram of the Destabilizing controller using positive feedback
200
100
α (deg)
-100
-200
0 0.5 1 1.5 2 2.5
Time (s)
Fig. 24 Plot of alpha during the pendulum swing-up motion under the destabilizing control
30
20
10
θ (deg)
-10
-20
-30
0 0.5 1 1.5 2 2.5
Time (s)
Fig. 25 Plot of theta during the pendulum swing-up motion under the destabilizing control
16
6. Mode controller
The purpose of the mode controller is to track the pendulum angle α and facilitate switching between
the destabilizing controller and stabilizing controller. This controller is to be enabled when α is in the
neighborhood of zero, within the threshold of α (currently set to 10 degrees).
The Simulink diagram of the simulation of the mode controller is shown in Fig. 26. From the
simulation results shown in Fig. 27 we can see that the mode controller works very well. It output 1 when
15
Mode signal
10 Input signal
5
-5
-10
-15
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time (s)
Fig. 27 Plots of the input signal and the mode signal
where θ d is the desired position of the rotating arm after balancing. The PD controller for α is
where α d ≡ 0 . The control signal (i.e., the input voltage of the motor) is then given by
u = alpha _ u − theta _ u (34)
Based on the experiment, we choose
17
theta _ K p = 2.2; theta _ K d = 2.0; alpha _ K p = 21.1; alpha _ K d = 2.9 (35)
One can adjust these four parameters to obtain a better stabilizing controller.
To simulate the stabilizing controller, the Simulink diagram shown in Fig. 28 is created. The rotary
inverted pendulum model is shown in Fig. 6. An impulse disturbance with amplitude of 5 degrees and
period of 5 seconds is added to the measured alpha in Fig. 29. We can see that the stabilizing controller is
good enough to maintain the pendulum in the upright position and keep it there stably. The same impulse
disturbance is added to the measured theta in Fig. 30. The stabilizing controller can also maintain the
pendulum in the upright position and keep it stable.
18
50
-50
-100
0 2 4 6 8 10 12 14 16 18 20
40
20
θ (deg)
-20
-40
0 2 4 6 8 10 12 14 16 18 20
10
0
α (deg)
-10
-20
-30
0 2 4 6 8 10 12 14 16 18 20
Time (s)
Fig. 29 Plots of control signal, theta, and alpha when alpha has an impulse disturbance
20
Control signal (V)
-20
-40
-60
0 2 4 6 8 10 12 14 16 18 20
Time (s)
50
θ (deg)
-50
0 2 4 6 8 10 12 14 16 18 20
Time (s)
10
0
α (deg)
-10
-20
-30
0 2 4 6 8 10 12 14 16 18 20
Time (s)
Fig. 30 Plots of control signal, theta, and alpha when theta has an impulse disturbance
19
8. Implementation of the whole system by feeding back both θ and α
The main Simulink diagram of the implementation of the rotary inverted pendulum system is shown in
Fig. 31. The diagrams of the three sub-controllers, namely, the destabilizing controller, the mode controller
and the stabilizing controller are shown in Figs. 32-34, respectively. The implementation of the sevomotor
and gear transmission system is shown in Fig. 23. From the experiment, we can see that the pendulum is
destabilized in the “hanging-down” position, brought upright and maintained in the inverted position. Note
that the pendulum has some small oscillations (see Fig. 35) in the inverted position due to the errors in the
mathematical model and the controller, the friction in the hardware, the vibration of the experiment table,
etc.
20
Fig. 34 Diagram of the stabilizing controller
0.5
α (deg)
-0.5
-1
0 1 2 3 4 5 6 7 8 9 10
Time (s)
Fig. 35 Plot of the alpha angle after balancing in the upright position
0
θ (deg)
-2
-4
-6
0 1 2 3 4 5 6 7 8 9 10
Time (s)
Fig. 36 Plot of the theta angle after balancing in the upright position
21
Kd s2 + K p s + Ki
C (s) = K c × (37)
s
we get
To simulate the stabilizing controller by feeding back α only, the main Simulink diagram shown in
Fig. 38 was created. The rotary inverted pendulum model is shown in Fig. 6. From the simulation results
shown in Figs. 39 and 40, we can see that the pendulum can reject a pulse disturbance, but the rotating arm
will rotate crazily if we feed back α only.
22
Fig. 38 Simulink diagram for simulating the stabilizing controller
0
α (deg)
-5
Feedback α only
Feedback α and θ
-10
0 1 2 3 4 5 6 7 8 9 10
Time (s)
200
Feedback α only
150 Feedback α and θ
100
θ (deg)
50
-50
0 1 2 3 4 5 6 7 8 9 10
Time (s)
23
Appendix A: Nomenclature of the System
Nominal
MATLAB
Symbol Description Value
Variable
(SI Units)
X component of the action force exerted on the pendulum at
Ax --- ---
point A by the arm
Y component of the action force exerted on the pendulum at
Ay --- ---
point A by the arm
α Pendulum position --- ---
α& Pendulum velocity --- ---
α&& Pendulum acceleration --- ---
J eq Moment of inertia of the arm and pendulum about the axis of θ Jeq 0.0035842
Jl Moment of inertia of the arm and pendulum about the axis of θ l --- ---
Jm Moment of inertia of the rotor of the motor Jm 3.87e-7
24
θ&&l Load shaft acceleration --- ---
25