Embed Size (px)
Citation preview
ME 452 Course Project II
Rotary Inverted Pendulum
(Full Version)
Project Instructor: Xiumin Diao
Advisor: Dr. Ou Ma
Department of Mechanical Engineering
New Mexico State University
July 2006
2
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.
Fig. 1 Rotary inverted pendulum system
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
3
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).
3. Modeling of the inverted pendulum system
The inverted pendulum (mechanical part only) is sketched in Fig. 2, θα and are employed as the
generalized coordinates to describe the inverted pendulum system. The pendulum is displaced with a given
α while the arm rotates an angle of θ . In this project, we assume that lθθ ≡ where the latter has been
used in Project I. We assume the pendulum to be a lump mass at point B which is located at the geometric
center of the pendulum. The xyz frame is fixed to the arm at point A . For a complete listing of the symbols
used in the math formulation, please refer to Appendix A.
A x
z
y
O
B
α
θ
α&
θ&
Pendulum
arm
r
L
Fig. 2 Simplified model of the rotary inverted pendulum system
3.1 Using Free Body Diagram method
The Free Body Diagram of the inverted pendulum (mechanical part only) system is shown in Fig. 3.
Note that the arm rotates in the horizontal plane (xz plane) only and the pendulum rotates in the vertical
plane (xy plane) only, we can draw the forces in these two planes only to simplify the drawing.
Referring back to Fig. 2, we notice that the velocity of point B on the pendulum relative to that of point
A on the arm is
αα
αα
&&
&&
)sin(
)cos(
Ly
Lx
BA
BA
−=
−= (1)
We also know that the pendulum is also moving with the rotating arm at a rate of θ&r . Thus, the absolute
velocity of the point B on the pendulum can be expressed as
αα
ααθ
&&
&&&
)sin(
)cos(
Ly
Lrx
B
B
−=
−= (2)
4
A x
y
B
α
α&
(b) F.B.D. of Pendulum
Ay
Ax
mg
A x
zθ
θ&
O
Ax
Tl
(a) F.B.D. of Arm
Ox
Oz
Beqθ&
Arm
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
αααα
ααααθ
&&&&&
&&&&&&&
)sin()cos(
)cos()sin(
2
2
LLy
LLrx
B
B
−−=
−+= (3)
Applying Newton’s 2nd Law to the pendulum in x direction, we obtain
xxB AmLmLmrFxm =−+⇒= ∑ ααααθ &&&&&&& )cos()sin( 2 (4)
Applying Newton’s 2nd Law to the pendulum in y direction, we obtain
y
yyB
AmLmLmg
mgAmLmLFym
=−−⇒
−=−−⇒= ∑
αααα
αααα
&&&
&&&&&
)sin()cos(
)sin()cos(
2
2
(5)
Applying Euler’s Equation to the rotational motion of the pendulum about point B , we obtain
)sin()cos(3
1
)sin()cos()2(12
1
2
2
ααα
αααα
LALAmL
LALALmMJ
yx
yxBB
+=⇒
+=⇒= ∑
&&
&&&&
(6)
Applying the Euler’s Equation to the rotational motion of the arm about pointO , we obtain
rABTJMJ xeqleqOO −−=⇒= ∑ θθθ &&&&& (7)
Substituting (4) and (5) into (6), we are left
0)sin(3
4)cos(
))(sin)cos()sin()sin((
))(cos)cos()sin()cos((3
1
)sin())sin()cos((
)cos())cos()sin((3
1
2
2222
22222
2
22
=−+−⇒
−−+
−+=⇒
−−+
−+=
ααθα
αααααα
αααααθαα
ααααα
αααααθα
mgLmLmLr
mLmLmgL
mLmLmLrmL
LmLmLmg
LmLmLmrmL
&&&&
&&&
&&&&&&&
&&&
&&&&&&&
(8)
Substituting (4) into (7), we are left
5
θααααθ
ααααθθθ
ααααθθθ
&&&&&&
&&&&&&&&
&&&&&&&&
eqleq
eqleq
eqleq
BTmLrmLrmrJ
mLrmLrmrBTJ
rmLmLmrBTJ
−=+−+⇒
−+−−=⇒
−+−−=
22
22
2
)sin()cos()(
))cos()sin((
))cos()sin((
(9)
Combining (8) and (9), we obtain the motion of equation of the system
0)sin(3
4)cos(
)sin()cos()(
2
22
=−+−
−=+−+
ααθα
θααααθ
mgLmLmLr
BTmLrmLrmrJ eqleq
&&&&
&&&&&&
(10)
3.2 Using Lagranian Formulation
The kinetic energy of the mechanical system arising from the rotating arm and pendulum is
αθααθ
αααααθθ
αθ
&&&&
&&&&&
&&&&
)cos(3
2)(
2
1
2
1]))sin(())cos([(
2
1
2
1
2
1)(
2
1
2
1
2222
2222
2222
mLrmLmrJ
JLLrmJ
JyxmJT
eq
Beq
BBBeq
−++=
+−+−+=
+++=
(11)
where 22
3
1)2(
12
1mLLmJ B == is the moment of inertia of the pendulum about its center of mass. Taking
the horizontal plane where the arm lies as the datum plane, the only potential energy in the mechanical
system is gravity, i.e.,
)cos(αmgLV = (12)
Since we have two generalized coordinates, θ and α , we therefore have two equations according to
Lagrangian Formulation
0)(
)(
=∂
∂+
∂
∂−
∂
∂
−=∂
∂+
∂
∂−
∂
∂
ααα
θθθθ
VTT
dt
d
BTVTT
dt
deql
&
&
& (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
θηθηηηη
θηθ
ηη
θηη
θη
&&&
&&&
&&
&&
mgg
m
mgtgm
m
m
gtgm
mgg
m
gmm
gtgm
gmmtmgg
mmmggl
JKR
KKKV
R
KK
JKR
KKVKK
KJIKK
JTKT
2
2
2
)(
)(
−−=
−−
=
−=
−=
(14)
Substituting (14) into (10), we obtain the nonlinear model of the system as follows:
0)sin()cos(
)sin()cos( 2
=−+−
=++−
ααθα
θααααθ
dcb
fVebba m
&&&&
&&&&&&
(15)
where
6
m
gtgm
m
mgtgm
eq
mggeq
R
KKf
R
KKKBe
mgLd
mLc
mLrb
JKmrJa
ηη
ηη
η
=
+=
=
=
=
++=
2
2
22
3
4
(16)
Linearizing (15) under the assumption that 0≈α and 0≈α& , we get the linearized model as follows:
0=−+−
=+−
ααθ
θαθ
dcb
fVeba m
&&&&
&&&&&
(17)
Solving (15) for the two accelerations θα &&&& and , we obtain the solution from the nonlinear model below:
))cos()cos()cos()sin()sin(()(cos
1
)sin()cos(
)sin(
)(cos
1
))cos()sin()sin(()(cos
1
)sin(
)cos()sin(
)(cos
1
22
22
2
22
2
22
2
22
m
m
m
m
Vbfbebadbac
db
fVeba
bac
cfVcebdbcbac
cd
bfVeb
bac
αθαααααα
αα
θαα
αα
θααααα
α
αθαα
αθ
+−−−
=
−
+−−
−=
+−+−−
=
−+−−
−=
&&
&&&&
&&
&&&&
(18)
Solving (17) for the two accelerations θα &&&& and , we find the solution from the linearized model as follows:
)(11
)(11
22
22
mm
mm
bfVbeadbacdb
fVea
bac
cfVcebdbaccd
bfVe
bac
+−−
=−
+−
−=
+−−
=−+−
−=
θαα
θα
θαα
θθ
&&
&&
&&
&&
(19)
To obtain the transfer function of the linearized system equations (17) analytically, we need to take the
Laplace transformation of it, namely,
0)()()(
)()()()(
22
22
=Α−Α+Θ−
=Θ+Α−Θ
sdsscssb
sfVssessbssa m (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)
desadscessbac
bfs
sV
s
m −−+−=
Α2342
2
)()(
)( (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
deadscessbac
bfs
sV
s
m −−+−=
Α232 )()(
)( (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.
7
4. Verification of the mathematical model
4.1 Verification of the linear model against the nonlinear model
In order to get some sense about how well the linearized model represents the original nonlinear system,
we are going to simulate the dynamics of the system using both the linear and non-linear models and then
compare their simulation outputs. The simulation will not only verify the linear model, but also establish a
threshold for us to know the threshold (on α ) of the linear model.
The main Simulink diagram of comparing the linear model with the nonlinear model is shown in Fig. 4.
The nonlinear and linear models are shown in Figs. 5 and 6, respectively. α is given an initial condition
( )( 00001.0 rad ) and thus the pendulum is allowed to fall. As one can see from the simulation results shown
in Fig. 7, the linear model correctly depicts the motion of the pendulum for the first 1.4 seconds and then
begin to break down. Zooming in the plots around 1.4 seconds, we saw that the linear model quite
accurately described the system for the first 15 degrees and then began to diverge from the actual motion.
Fig. 4 Main diagram of verifying the linear model
Fig. 5 Diagram of the nonlinear model
8
Fig. 6 Diagram of the linear model
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8-20
0
20
40
60
Time (s)
θ (deg)
Linear
Nonlinear
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.80
50
100
150
200
250
Time (s)
α (deg)
Linear
Nonlinear
Fig. 7 Overlapped plots of the simulation outputs from the linear and nonlinear models
4.2 Verification of both linear and nonlinear models against hardware
In order to get some sense about how well the dynamics models represent the real hardware system, we
are going to compare the outputs of both the linear model and the nonlinear model with hardware data. To
simplify the verification, we do the experiment near the “hanging-down” position. Replacing α with
πα + in (18), we obtain the nonlinear model of the pendulum system at the “hanging-down” position:
))cos()cos()cos()sin()sin(()(cos
1
))cos()sin()sin(()(cos
1
22
22
2
22
m
m
Vbfbebadbac
cfVcebdbcbac
αθαααααα
α
θααααα
θ
−+−−−
=
+−+−
=
&&&&
&&&&
(23)
Linearizing (23) under the assumption that 0≈α and 0≈α& , we get the corresponding linearized model
9
)(1
)(1
2
2
m
m
bfVbeadbac
cfVcebdbac
−+−−
=
+−−
=
θαα
θαθ
&&&
&&&
(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
Fig. 9 Interface to the inverted pendulum system
10
Fig. 10 Liner model of the inverted pendulum system
Fig. 11 Nonliner 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.
11
0 0.2 0.4 0.6 0.8 1 1.20
0.5
1
1.5
2
2.5
Time (s)
Input Voltage (V)
Fig. 12 Input voltage of the experiment
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.90
20
40
60
80
Time (s)
θ (deg)
Linear model
Nonlinear model
Physical system
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9-8
-6
-4
-2
0
Time (s)
α (deg)
Linear model
Nonlinear model
Physical system
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
12
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.
5.1 Position controller
5.1.1 Design of the position controller
The pendulum in the system has a length of (m) 335.02 =L and its center of mass is located at its
geometric center. Thus the natural frequency for small oscillations of the pendulum is given by
)(rad/ 628.64
3s
L
g
I
mgL
A
p ===ω (25)
where AI 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
0.780 (rad/s), 512.26or %2%OS ,4 ==== ζωωω npn (26)
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
θθθ &vdpm KKV −−= )( (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 vp KK and according the above defined specifications (26).
The closed-loop transfer function of the input and output is
ptgmgvtgmggtmmgmeqmeq
ptgmg
d KKKsKKKKKKRBsRJ
KKK
ηηηηηη
ηη
θ
θ
++++=
)(22
(28)
Comparing it with the standard transfer function of a second order system (see Section 5-3 of the textbook)
we have
2
2
2
n
meq
ptgmg
n
meq
vtgmggtmmgmeq
RJ
KKK
RJ
KKKKKKRB
ωηη
ζωηηηη
=
=++
(29)
Solving (29), we obtain
13
612.19
585.02
2
2
==
=−−
=
tgmg
meqn
p
tgmg
gtmmgmeqmeqn
v
KK
RJK
KK
KKKRBRJK
ηη
ω
ηη
ηηζω
(30)
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 %2%OS (rad/s), 512.26 ==nω , 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.
Fig. 14 Main diagram for simulation of the position controller
Fig. 15 Model of the servomotor and gear transmission system
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-2
0
2
4
6
8
Time (s)
Control signal (V)
Fig. 16 Control signal of the system
14
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
5
10
15
20
25
Time (s)
Load shaft position (deg)
System reponse
Input command
Fig. 17 System response due to a step input
5.1.3 Implementation of the position controller
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.
Fig. 18 Main diagram for implementation of the position controller
Fig. 19 Interface to the servomotor and gear transmission system
15
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2
0
2
4
6
8
Time (s)
Control signal (V)
Simulated control signal
Measured control signal
Fig. 20 Simulated and real control signals for the position control experiment
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
5
10
15
20
25
Time (s)
Load shaft position (deg)
Input command
Simulated position
Measured position
Fig. 21 Simulated and real dynamic responses of the system to a step input
5.2 Destabilizing controller
Many schemes can be devised to gradually swing up the pendulum. In this project we will design a
positive feedback controller to destabilize the pendulum and eventually swing up it. Notice that we have a
useful application of positive feedback here. Assume the arm position can be commanded via dθ . Then the
feedback
ααθ &DPd += (31)
can be made to destabilize the system with the proper choice of the gains DP and . 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 DP and are crucial in bring up the pendulum smoothly. Based on the
experiment, we choose )deg/(deg/s 00001.0 and (deg/deg) 5.0 == DP . 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.
16
Fig. 22 Main diagram of the Destabilizing controller using positive feedback
Fig. 23 Interface to the rotary inverted pendulum system
0 0.5 1 1.5 2 2.5-200
-100
0
100
200
Time (s)
α (deg)
Fig. 24 Plot of alpha during the pendulum swing-up motion under the destabilizing control
0 0.5 1 1.5 2 2.5-30
-20
-10
0
10
20
30
Time (s)
θ (deg)
Fig. 25 Plot of theta during the pendulum swing-up motion under the destabilizing control
17
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
degrees 10≤α and 0 when degrees 10>α .
Fig. 26 Simulink diagram of the mode controller
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-15
-10
-5
0
5
10
15
Time (s)
Mode signal
Input signal
Fig. 27 Plots of the input signal and the mode signal
7. Stabilizing controller by feeding back both αθ and
If we can feed back both αθ and angles, we can calculate the control signal using both of them.
Assuming the pendulum is almost upright, two PD controllers can be implemented to maintain it at the
upright position (capable of rejecting disturbances up to a certain extent). The PD controller for θ is
θθθ &ddp KthetaKthetautheta _)(__ +−= (32)
where dθ is the desired position of the rotating arm after balancing. The PD controller for α is
ααα &ddp KalphaKalphaualpha _)(__ +−= (33)
where 0≡dα . The control signal (i.e., the input voltage of the motor) is then given by
uthetaualphau __ −= (34)
Based on the experiment, we choose
18
9.2_ ;1.21_ ;0.2_ ;2.2_ ==== dpdp KalphaKalphaKthetaKtheta (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.
Fig. 28 Diagram of the simulation of the stabilizing controller
19
0 2 4 6 8 10 12 14 16 18 20-100
-50
0
50
Control signal (V)
0 2 4 6 8 10 12 14 16 18 20-40
-20
0
20
40
θ (deg)
0 2 4 6 8 10 12 14 16 18 20-30
-20
-10
0
10
Time (s)
α (deg)
Fig. 29 Plots of control signal, theta, and alpha when alpha has an impulse disturbance
0 2 4 6 8 10 12 14 16 18 20-60
-40
-20
0
20
Time (s)
Control signal (V)
0 2 4 6 8 10 12 14 16 18 20-50
0
50
Time (s)
θ (deg)
0 2 4 6 8 10 12 14 16 18 20-30
-20
-10
0
10
Time (s)
α (deg)
Fig. 30 Plots of control signal, theta, and alpha when theta has an impulse disturbance
20
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.
Fig. 31 Main diagram of the rotary inverted pendulum system
Fig. 32 Diagram of the destabilizing controller
Fig. 33 Diagram of the mode controller
21
Fig. 34 Diagram of the stabilizing controller
0 1 2 3 4 5 6 7 8 9 10-1
-0.5
0
0.5
1
Time (s)
α (deg)
Fig. 35 Plot of the alpha angle after balancing in the upright position
0 1 2 3 4 5 6 7 8 9 10-6
-4
-2
0
2
4
Time (s)
θ (deg)
Fig. 36 Plot of the theta angle after balancing in the upright position
9. Stabilizing controller by feeding back α 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
s
ss
s
sssC
200302
)10)(20(2)(
2 ++×=
++×= (36)
Comparing to the standard PID controller
22
s
KsKsKKsC
ipd
c
++×=
2
)( (37)
we get
1,200,30,2 ==== dipc KKKK (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.
Fig. 37 Design of the PID controller using SISO Design Tool
23
Fig. 38 Simulink diagram for simulating the stabilizing controller
0 1 2 3 4 5 6 7 8 9 10-10
-5
0
5
Time (s)
α (deg)
Feedback α only
Feedback α and θ
Fig. 39 Plot of alpha when there is a pulse disturbance in alpha
0 1 2 3 4 5 6 7 8 9 10-50
0
50
100
150
200
Time (s)
θ (deg)
Feedback α only
Feedback α and θ
Fig. 40 Plot of theta when there is a pulse disturbance in alpha
24
Appendix A: Nomenclature of the System
Symbol Description MATLAB
Variable
Nominal
Value
(SI Units)
xA X component of the action force exerted on the pendulum at
point A by the arm --- ---
yA Y component of the action force exerted on the pendulum at
point A by the arm --- ---
α Pendulum position --- ---
α& Pendulum velocity --- ---
α&& Pendulum acceleration --- ---
eqB Equivalent viscous damping coefficient Beq 0.004
g Gravity acceleration g 9.81
mI Current in the armature circuit --- ---
BJ Moment of inertia of the pendulum about its center of mass --- ---
eqJ Moment of inertia of the arm and pendulum about the axis of θ Jeq 0.0035842
lJ Moment of inertia of the arm and pendulum about the axis of lθ --- ---
mJ Moment of inertia of the rotor of the motor Jm 3.87e-7
gK SRV02 system gear ratio (motor -> load) Kg 70 ( 514× )
mK Back-emf constant Km 0.00767
tK Motor-torque constant Kt 0.00767
L Half length of the pendlum L 0.1675
mL Armature inductance --- ---
m Mass of pendulum m 0.125
xO X component of the action force exerted on the arm at point
O by the base of the motor --- ---
zO Z component of the action force exerted on the arm at point
O by the base of the motor --- ---
r Rotating arm length r 0.215
mR Armature resistance Rm 2.6
lT Torque applied to the load --- ---
mT Torque generated by the motor --- ---
θ Load shaft position --- ---
θ& Load shaft velocity --- ---
θ&& Load shaft acceleration --- ---
lθ Angular position of the arm --- ---
lθ& Load shaft velocity --- ---
25
lθ&& Load shaft acceleration --- ---
mθ Motor shaft position --- ---
mθ& Motor shaft velocity --- ---
mθ&& Motor shaft acceleration --- ---
emfV Motor back-emf voltage --- ---
mV Input voltage of the armature circuit --- ---
BAx& X component of the velocity of point B on the pendulum
relative to point A on the arm --- ---
BAy& Y component of the velocity of point B on the pendulum
relative to point A on the arm --- ---
Bx& Velocity of pendulum center of mass in X direction --- ---
By& Velocity of pendulum center of mass in Y direction --- ---
Bx&& Acceleration of pendulum center of mass in X direction --- ---
By&& Acceleration of pendulum center of mass in Y direction --- ---
gη Gearbox efficiency Eff_G 0.9
mη Motor efficiency Eff_M 0.69
