Experiment 106. pendulum 1
Stabilisation of a rotary inverted
Objective
This experiment is aimed at providing you with hands on experience of the design of a stabilising controller for a rotary inverted pendulum.
2
Introduction
The rotary motion inverted pendulum, which is shown in Figure 1, is driven by a rotary servo motor system (SRV-02) that is illustrated in Figure 2. The servo motor drives an independent output gear whose angular position is measured by an encoder. The rotary pendulum arm is mounted to an output gear. At the end of the pendulum arm is a hinge instrumented with another encoder. The pendulum attaches to the hinge. The second encoder measures the angular position of the pendulum. The system is interfaced by R R means of a data acquisition card and driven by Matlab° /Simulink° based real time software. Notice that the pendulum has two equilibrium points. A stable equilibrium at the point where the rod is vertical and pointing down. An unstable equilibrium at the point where the rod is vertical and pointing up. In this practical session, a control scheme will be designed to stabilise the pendulum around the unstable equilibrium point.
3 3.1
Mathematical modelling Nonlinear model
Consider the diagram shown in Figure 3. A nonlinear model of this system, which can be derived by means of the Euler-Lagrange equations, is given by: (mp r2 + Jb )θ¨ + mp rα ¨ lp cos(α) − mp rα˙ 2 lp sin α = T ¨ − mp lp sin(α)α˙ θr ˙ + mp α mp lp cos(α)θr ¨ l2 − mp glp sin α = 0
(1)
p
where the symbols used are described in Table 1. Note that the zero position for α and θ are defined as the pendulum being vertical “up”. A positive angle in the pendulum α is defined for a fall to the right when looking at the pendulum from the motor shaft. A positive rotation of the arm θ is defined as clockwise when looking at the SRV-02 from above. Also note that parameter lp is half the pendulum length.
1
Figure 1: The rotary inverted pendulum
3.2
Linearised model
A linearised form of Equation (1) around the vertical (up) unstable equilibrium point is given by: mp rg 1 θ¨ = − α+ T Jb Jb (2) 2 g(Jb + mp r ) r α ¨= α− T lp Jb lp Jb This model is valid for small values of α. In the Laplace domain, it is possible to write the transformed form of (2) as follows: 1 mp rg α(s) + T (s) Jb Jb 2 g(J + m r ) r b p s2 α(s) = α(s) − T (s) l p Jb lp Jb s2 θ(s) = −
2
(3)
Table 1: Description of the mathematical model variables and parameters Variable Description Units Value T input torque from motor Nm mp mass of rod kg 0.125 lp distance from arm to the centre of gravity of rod m 0.1714 r length of pendulum arm m 0.2159 Jb Inertia of arm and gears kg m 0.0038 g Acceleration of gravity m/s2 9.8 θ Deflection of arm from zero position rad α Deflection of pendulum from vertical UP position rad ˙θ Angular velocity of θ rad/s ¨ θ Angular acceleration of θ rad/s2 α˙ Angular velocity of α rad/s α ¨ Angular acceleration of α rad/s2
Figure 2: SRV02 servo-motor with the pendulum arm and connections where s is the Laplace variable, θ(s), α(s) and T (s) represent the Laplace transforms of θ, α and T , respectively. Define the following auxiliary parameters: mp rg Jb 1 b= Jb g(Jb + mp r2 ) c= l p Jb r d= lp Jb
a=
3
(4)
Pendulum rod
α 2l p Pendulum arm
θ
r
Figure 3: Diagram of the rotary inverted pendulum Then, Equation (3) can be rewritten as follows: s2 θ(s) = −aα(s) + bT (s) s2 α(s) = cα(s) − dT (s)
(5)
From Equation (5), we have that α(s) can be written as follows: α(s) = −
s2
d T (s) − p2
(6)
√ where p = c. Substituting in the first sub-equation of (3), results in the following equation for θ(s): θ(s) =
b(s2 − z 2 ) T (s) s2 (s2 − p2 )
where
r z=
bc − ad b
4
(7)
(8)
Table 2: Variables and parameters associated with the servo motor Variable Description Units Value T Torque generated by the servo-motor Nm v Voltage applied to the armature of the motor V R Armature resistance Ω 2.6 Km Motor torque constant N m/A 0.00767 Kg Gear ratio 70 ˙θ Angular velocity of the output gear rad/s
3.3
Torque equation
The torque T is generated by the DC servo-motor, which is activated by a voltage v applied to its armature. The generated torque is given by: T = t1 v − t2 θ˙
(9)
where
Km Kg R (10) 2 Km Kg2 t2 = R and the parameters and variables are described in Table 2. In the Laplace domain, the torque equation (9) can be written as follows: t1 =
T (s) = t1 v(s) − t2 sθ(s)
(11)
where T (s), v(s) and θ(s) are the Laplace transforms of T , v and θ, respectively.
3.4
Transfer function from v(s) to θ(s)
Substituting Equation (9) into Equation (7) and solving for θ(s) gives: t1 b(s2 − z 2 ) v(s) s(s3 + t2 bs2 − p2 s − t2 bz 2 ) So that the transfer function from v(s) to θ(s) is: θ(s) =
Gv,θ (s) =
3.5
t1 b(s2 − z 2 ) s(s3 + t2 bs2 − p2 s − t2 bz 2 )
(12)
(13)
Transfer function from θ(s) to α(s)
If we divide equation (6) by equation (7), we obtain the transfer function from θ(s) to α(s): ( db )s2 α(s) Gθ,α (s) = =− 2 (14) θ(s) s − z2 5
u(s)
α(s)
Gv ,θ ( s)
Gθ ,α ( s )
θ ( s)
Figure 4: Block diagram of the linearised model of the inverted pendulum including the servomotor, gearbox and pendulum arm
3.6
Block diagram of the linearised model
A block diagram of the linearised model is shown in Figure 4.
4
Procedure
You should sketch all the root locus diagrams and write down the transfer functions found R with Matlab° as you work through the different tasks on your log book. Please note that the servo and pendulum can rotate quite quickly, so be careful when starting and operating the system.
4.1
Task 1
Introduce the values of the parameters given in Tables 1 and 2 as relevant variables in R the Matlab° workspace. Calculate the values of the auxiliary parameters a, b, c, d, z, p, t1 and t2 . Use the tf command1 to define the transfer functions Gv,θ and Gθ,α as objects in R the Matlab° workspace (see Equations (13) and (14) ). R Hint: To do this, it is useful to define the Laplace variable s in the Matlab° workspace as follows: >> s= tf( [1, 0], [0, 1]); R Then, transfer function Gv,θ may be defined in Matlab° , as follows: >> G_V_theta=b*t1*(s^2-z^2)/(s*(s^3+t2*b*s^2-p^2*s-t2*b*z^2));
4.2
Task 2
Consider the block diagram given in Figure 5, where Cθ (s) is a Proportional+Derivative (PD) compensator, with the following transfer function: µ ¶ Tθ,d s Kθ,d s Cθ (s) = Kθ,p 1 + 1 = Kθ,p + 1 (15) s+1 s+1 N N 1
Type >> help tf for help on this command
6
A
θ (s)
v(s)
+
-
Gv ,θ ( s )
B
α(s)
Gθ ,α ( s )
Cθ ( s )
Figure 5: Block diagram of the inner control loop on θ where Kθ,p , is the proportional gain, Tθ,d is the derivative time constant and Kθ,d = Kθ,p Tθ,d is the derivative gain. Note that the derivative action has a lag with time constant 1/N , which is included to avoid amplifying high frequency signals. Assume that the values of the compensator parameters are Kθ,p = −1 and Kθ,α = −1.42 and N = 50. Use the feedback command2 to compute the closed loop transfer function GA,B between points A and B of Figure 5 including the transfer function Gv,θ and the controller Cθ (s).
4.3
Task 3
Consider the block diagram given in Figure 6, where Cα (s) is a Proportional+Derivative (PD) controller, with the following transfer function: ¶ µ Kα,d s Tα,d s Cα (s) = Kα,p 1 + 1 = Kα,p + 1 (16) s+1 s+1 N N where Kα,p , is the proportional gain, Tα,d is the derivative time constant and Kα,d = Kα,p Tα,d is the derivative gain. Compute the open loop transfer function seen by the α-controller by using the product (’*’) operator: GA,α = GA,B (s)Gθ,α (s) (17) Notice that this transfer function exhibits pole-zero cancellations. To see this, use the zpk command3 to convert the transfer function to pole-zero form. To eliminate the common poles and zeros you must use the minreal command4 (it is important to do this). Now assume for the moment that Kα,p = 1 and Tα,p = 0 and use the product operator (OL) (’*’) to compute the open loop transfer function GR,α as follows: (OL)
GR,α (s) = Cα (s)GA,α (s) 2
Type >> help feedback for help on this command type >> help zpk for help on this command 4 type >> help minreal for help on this command 3
7
(18)
R(s) +
Cα ( s)
-
A +
-
v(s)
θ (s )
Gv ,θ ( s )
B
α(s)
Gθ ,α ( s )
Cθ ( s)
Figure 6: Block diagram of the control scheme including the inner control loop on θ and the outer control loop on α Use the rlocus command5 to compute the root locus based on the above values of Kα,p and Tα,d . Write your observations on your log book. Now let Kα,p = −1 and Tα,p = 0 and (OL) recalculate GR,α and the root locus. Write your observations on your log book. Repeat this procedure leaving Kα,p = −1 but for different values of Tα,d = {0.1, 0.5, 1}. Write your observations on your log book, and indicate what value of Tα,d is best and why. Using the root locus for the best value of Tα,d compute the best value of the gain Kα,p to achieve a good damping ratio for the dominant complex poles ( ζ ≈ 0.7 is reasonable; you can do this by placing the pointer and clicking the mouse on different points of the loci and reading the values from the screen.) and take a note of the value of the gain Kα,p and compute the corresponding derivative gain using Kα,d = Kα,p Tα,d . Using the same root locus, find a value of the gain Kα,p for which the system is unstable. Take a note of this other value of Kα,p and find the corresponding value of the derivative gain.
4.4
Task 4
R R Open the Simulink° file exp106.mdl and assign values to the following Matlab° variables from the command prompt:
>> >> >> >> >>
ktp = −1.0; ktd = −1.4207; kap = (Kα,p found in Task 3 for ζ ≈ 0.7) kad = (Kα,d found in Task 3 for ζ ≈ 0.7) N = 50;
Switch on the Universal Power Module (See Figure 7; the switch is located at the back R of the module), hold the pendulum steady at the upright position, from the Simulink° window called exp106 choose the menu option WinCon/Build. Then, from the small window 5
Type >> help rlocus for help on this command
8
Figure 7: The Universal Power Module and associated connections entitled WinCon Server press the start button, release the pendulum and observe the results. Apply a slight tap to perturb the pendulum using your hand and see if it recovers. After a while, press the Stop button in the WinCon server window.
4.5
Task 5
R Assign values to the following Matlab° variables from the command prompt:
>> kap = (Kα,p found in Task 3 for unstable behaviour) >> kad = (Kα,d found in Task 3 for unstable behaviour) R Again, hold the pendulum steady at the upright position, from the Simulink° window called exp106 choose the menu option WinCon/Build. Then, from the small window entitled WinCon Server press the start button, release the pendulum and observe the results.
5
Conclusions
Your conclusions must address the following points. 1. How robust is this control scheme to large perturbations on the pendulum and why? 9
Figure 8: The terminal board and associated connections 2. How accurate was the root locus approach based on the linearised model in predicting the stability/unstability of the system?
Appendix Set up and connections The required setup and connections will be made by the demonstrator. This experiment is performed with the SRV-02 in the high gear ratio configuration. Attach the pendulum arm to the central gear of the SRV-02 using the two brass thumb screws. The connections are indicated in Figures 2, 7 and 8. The required connections are listed below: 1. Encoder output of SRV-02 to encoder input No. 0 of terminal board (white cable). 2. Encoder output of pendulum arm to encoder input No. 1 of terminal board (white cable). 3. Analog ouput No. 0 of terminal board to connector labelled From D/A on Universal Power Module (black cable). 4. Output labelled To Load on Universal Power Module to left connector (as seen from the back) of SRV-02 (black cable).
10
Written by Dr. V. M. Becerra, October 2002.
11