Professional Documents
Culture Documents
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.
On the project, you are asked to do the following: Derive a nonlinear model of the rotary inverted pendulum system. Derive a linearized model of the same system in the neighborhood of the upright position. Verify the linear and non-linear models to see if they are indeed true models of the physical system over a specific operating range. Design, implement, and simulate a stabilizing controller which maintains the pendulum in the upright position with some robustness for small disturbance to the pendulum. Design and implement a mode controller which activates the stabilizing controller when the pendulum is placed in a small neighborhood of the upright position. Design and implement a destabilizing controller which swings up the pendulum to near upright position, so that the stabilizing controller can be activated to stabilize the pendulum.
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
5) A PC 6) Quansers 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
arm
A z r
&
A on the arm is
& & BA = L cos( ) x & & y BA = L sin( )
(1)
& . Thus, the absolute We also know that the pendulum is also moving with the rotating arm at a rate of r velocity of the point B on the pendulum can be expressed as & L cos( ) & & B = r x (2) & & B = L sin( ) y
Ax
A z
&
x Arm
Pendulum y
&
mg Ox
Beq& Tl
O Oz
Ax
Differentiating (2) with respect to time, we will get the acceleration of the point B
&& + L sin( ) & 2 L cos( ) && & &B = r x & 2 L sin( ) && & &B = L cos( ) y
(3)
(4)
(5)
Applying Eulers Equation to the rotational motion of the pendulum about point B , we obtain
&& = M B J B 1 && = Ax L cos( ) + Ay L sin( ) m(2 L) 2 12 1 && = Ax L cos( ) + Ay L sin( ) mL2 3
(6)
Applying the Eulers Equation to the rotational motion of the arm about point O , we obtain
&& = M J && = T B & J O O eq l eq Ax r
(7)
(8)
(9)
Combining (8) and (9), we obtain the motion of equation of the system && mLr cos( ) & && + mLr sin( ) & 2 = Tl Beq ( J eq + mr 2 )
&& + 4 mL2 && mgL sin( ) = 0 mLr cos( ) 3
(10)
(11)
where J B =
1 1 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 d T T V + ( ) =0 & dt
(13)
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
(14)
m g K t K g
Rm
2 Km & m g K t K g 2 && g K g J m
Rm
Substituting (14) into (10), we obtain the nonlinear model of the system as follows:
&& b cos( ) & = fV && + b sin( ) & 2 + e a m && + c && d sin( ) = 0 b cos( )
(15)
where
2 a = J eq + mr 2 + g K g Jm
b = mLr c= 4 2 mL 3 d = mgL
(16)
2 g
e = Beq + f =
m g K t K K m
Rm
m g K t K g
Rm
& 0 , we get the linearized model as follows: Linearizing (15) under the assumption that 0 and & & & && + e = fVm a b (17) & & && d = 0 b + c && , we obtain the solution from the nonlinear model below: && and Solving (15) for the two accelerations & + fV b cos( ) & 2 e b sin( ) 1 m && = 2 2 ac b cos ( ) d sin( ) c
= && = = 1 & + cfV ) & 2 + bd sin( ) cos( ) ce (bc sin( ) m ac b cos2 ( ) & + fV & 2 e a b sin( ) 1
2 m
(18)
ac b 2 cos2 ( ) b cos( )
d sin( )
1 & + bf cos( )V ) & 2 be cos( ) (ad sin( ) b 2 sin( ) cos( ) m ac b 2 cos2 ( ) && , we find the solution from the linearized model as follows: && and Solving (17) for the two accelerations
&& =
& + fV e 1 m 2 ac b d
(19)
To obtain the transfer function of the linearized system equations (17) analytically, we need to take the Laplace transformation of it, namely,
a( s) s 2 b(s) s 2 + e(s )s = fVm ( s) b( s)s 2 + c( s)s 2 d(s) = 0
(20)
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)
bfs 2 ( s) = Vm (s) (ac b 2 )s 4 + ces 3 ads 2 des
(21)
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 = Vm ( s) (ac b 2 ) s 3 + ces 2 ads de
(22)
This is our final model of the inverted pendulum system for designing the controller. Note that this transfer function represents the linearized model only.
60 40 Linear Nonlinear
(deg)
20 0 -20
0.2
0.4
0.6
1.2
1.4
1.6
1.8
(deg)
150 100 50 0 0 0.2 0.4 0.6 0.8 1 Time (s) 1.2 1.4 1.6 1.8
Fig. 7 Overlapped plots of the simulation outputs from the linear and nonlinear models
+ in (18), we obtain the nonlinear model of the pendulum system at the hanging-down position:
&& = 1 & + cfV ) & 2 + bd sin( ) cos( ) ce (bc sin( ) m ac b 2 cos2 ( ) 1 & bf cos( )V ) && = & 2 + be cos( ) (ad sin( ) b 2 sin( ) cos( ) m ac b 2 cos2 ( )
(23)
& 0 , we get the corresponding linearized model Linearizing (23) under the assumption that 0 and
&& =
(24)
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
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
0.2
0.4
0.8
1.2
60
(deg)
40
20
0.1
0.2
0.3
0.6
0.7
0.8
0.9
-2
(deg)
-4
-6
-8
0.1
0.2
0.3
0.6
0.7
0.8
0.9
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.
p =
mgL = IA
3g = 6.628 (rad/s ) 4L
(25)
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
(26)
& 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). The closed-loop transfer function of the input and output is
g m K g K t K p = 2 d J eq R m s 2 + ( Beq R m + g m K m K t K g + g m K g K t K v ) s + g m K g K t K p
(28)
Comparing it with the standard transfer function of a second order system (see Section 5-3 of the textbook) we have
2 + g m K g K t K v Beq Rm + g m K m K t K g
J eq Rm
= 2 n
(29)
g m K g K t K p
J eq Rm
Solving (29), we obtain
2 n
12
Kv =
2 2 n J eq Rm Beq Rm g m K m K t K g
g m K g K t
= 0.585
(30)
2 n 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.
5.1.2 Simulation of the position controller
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.
0.1
0.2
0.3
0.4
0.6
0.7
0.8
0.9
13
0.1
0.2
0.3
0.4
0.6
0.7
0.8
0.9
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
0.5
1.5
3.5
4.5
Fig. 20 Simulated and real control signals for the position control experiment
25 Load shaft position (deg) 20 15 10 5 0 Input command Simulated position Measured position 0 0.5 1 1.5 2 2.5 Time (s) 3 3.5 4 4.5 5
Fig. 21 Simulated and real dynamic responses of the system to a step input
(31)
can be made to destabilize the system with the proper choice of the gains P and D . This means that we want to command the arm based on the position and velocity of the pendulum. Moreover, by limiting d , we can ensure that the arm does not reach a position that will cause a collision with the nearby hardware (e.g., the table). The gains P and D are crucial in bring up the pendulum smoothly. Based on the experiment, we choose P = 0.5 (deg/deg) and D = 0.00001 deg/(deg/s) . One can tune the value of D to adjust the damping in the system. The main Simulink diagram of the destabilizing controller using positive feedback is shown in Fig. 22. The diagram of the rotary inverted pendulum is shown in Fig. 23. From the plot of alpha in Fig. 24, we can see that the pendulum is brought up (the curve passing through the = 0 line) in about 1.25 seconds.
15
200
100
(deg)
-100
-200
0.5
1 Time (s)
1.5
2.5
Fig. 24 Plot of alpha during the pendulum swing-up motion under the destabilizing control
30 20 10
(deg)
1.5
2.5
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 10 5 0 -5 -10 -15 0 0.5 1 1.5 2 2.5 Time (s) 3 3.5 4 4.5 5 Mode signal Input signal
(33)
u = alpha _ u theta _ u
Based on the experiment, we choose
(34)
17
theta _ K p = 2.2; theta _ K d = 2.0; alpha _ K p = 21.1; alpha _ K d = 2.9 One can adjust these four parameters to obtain a better stabilizing controller.
(35)
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
10
12
14
16
18
20
40 20
(deg)
0 -20 -40 0 2 4 6 8 10 12 14 16 18 20
10 0
(deg)
Fig. 29 Plots of control signal, theta, and alpha when alpha has an impulse disturbance
20 Control signal (V) 0 -20 -40 -60 0 2 4 6 8 10 Time (s) 12 14 16 18 20
50
(deg)
-50
10 Time (s)
12
14
16
18
20
10 0
(deg)
Fig. 30 Plots of control signal, theta, and alpha when theta has an impulse disturbance
19
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
0.5
(deg)
-0.5
-1
5 Time (s)
10
Fig. 35 Plot of the alpha angle after balancing in the upright position
4 2
(deg)
0 -2 -4 -6
5 Time (s)
10
Fig. 36 Plot of the theta angle after balancing in the upright position
only
The rotary inverted pendulum system has two degrees of freedom. In general, we cannot maintain the pendulum in the upright position if we feed back only. Using the SISO Design Tool in the Control System Toolbox, we can design a stabilizing (PID) controller for the system described by the transfer function in (22). As shown in Fig. 37, the controller can be
C (s) = 2 ( s + 20)(s + 10) s 2 + 30s + 200 = 2 s s
(36)
21
C (s) = K c
Kd s2 + K p s + Ki s
(37)
we get
K c = 2, K p = 30, K i = 200, K d = 1
(38)
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
(deg)
-5 Feedback only Feedback and -10 0 1 2 3 4 5 Time (s) 6 7 8 9 10
(deg)
100 50 0 -50
5 Time (s)
10
23
Ax
Ay
& &&
Beq
g Im JB
J eq
Jl Jm
Kg
Km Kt
L Lm
m
Ox Oz
r Rm Tl Tm
&
&& l
& l
24
&& l
Load shaft acceleration Motor shaft position Motor shaft velocity Motor shaft acceleration Motor back-emf voltage Input voltage of the armature circuit X component of the velocity of point B on the pendulum relative to point A on the arm Y component of the velocity of point B on the pendulum relative to point A on the arm Velocity of pendulum center of mass in X direction Velocity of pendulum center of mass in Y direction Acceleration of pendulum center of mass in X direction Acceleration of pendulum center of mass in Y direction Gearbox efficiency Motor efficiency
------------------------Eff_G Eff_M
------------------------0.9 0.69
m
& m && m
V emf
Vm
& BA x & BA y &B x &B y
25