Rotary Inverted Pendulum - SCCS Blogs
Rotary Inverted Pendulum Eric Liu 1 Aug 2013
1
1 1.1
State Space Derivations Electromechanical Derivation
Consider the given diagram. We note that the voltage across the motor can be described by:
eb = km ωm
(1.1)
where km is the back-emf constant of the motor, and ωm is the speed of the motor shaft. By Kirchoff’s Laws, the circuit shows that:
Vm − Im − Lm
dIm − km ωm = 0. dt
(1.2)
If we assume that the motor inductance Lm is insignificant, (1.2) can be reduced to: Vm − Im − km ωm = 0 Solving for Im , 2
(1.3)
Im =
Vm − km ωm Rm
(1.4)
The motor torque is proportional to the voltage applied and is described as: τm = ηm kt Im
(1.5)
where ηm is the motor efficiency and kt is the current-torque constant. If we consider the internal gear system as two gears with negligible friction and moment of inertia, we know that: τeq = ηg Kg τm
(1.6)
where ηg is the gearbox efficiency, and Kg is the total gear ratio. (Appendix, Figure (1)) Kg is provided in the manual, or otherwise can be calculated as the product of the exterior and interior gear ratios. That is, N4 N2 N3 N1 Additionally, we see that due to rotational alignments, θm = Kg θl , implying that: Kg = Kge Kgi =
ωm = Kg ωl
(1.7)
Examining the equivalent torque with respect to the load, we have:
τeq = Jeq
dωl + Beq ωl + rF dt
(1.8)
where F is the equivalent force to be applied to our pendulum, Be q is the viscous damping coefficient, and Jeq is the moment of interia as seen at the output, which can be also calculated as Jeq = Jl + ηg Jm Kg2 . (Jm is the moment of inertia of the internal gear.) Substituting in (1.4), (1.5), (1.6), and (1.7), we get:
ηm ηg kt Kg (
Vm − Kg km ωl dωl ) = Jeq + Beq ωl + rF Rm dt
3
(1.9)
1.2
Pendulum Motion Derivations
Examining the schematic (Appendix, Figure (2)), we note that if the pendulum arm must travel in a fixed circular trajectory with rotational movement solely tangential to its trajectory. Consequently, if we model the pendulum arm as a point mass at it’s center point with distance L from the joint, we can derive the following base equations. Case 1: Hanging pendulum
Fy = mg + m¨ y Fx = m¨ x τm + Fx y + Fy x = 0
(2.1) (2.2) (2.3)
Now fixing the coordinate system relative to the rotating arm’s base, we can also establish that: 1. y(t) = −Lcos α 2. x(t) = rθ(t) + Lsin α Where r is the length of the rotating arm. Substituting, our base equations can now be expressed as:
Fy = mg + m
d2 (−Lcos α) dt
(2.4)
d2 (rθ + Lsin α) dt Jp α + Fy Lsin α + Fx Lcos θ = 0
Fx = m
We note that for small angles, sin θ ≈ θ, cos θ ≈ 1. In addition, d2 cos θ = dt
dθ dt
2 cos θ − sin θ
and
4
d2 θ dt2
(2.5) (2.6)
d2 sin θ = − dt
dθ dt
2
d2 θ sin θ + cos θ 2 dt
Consequently our equations are now: Fy = mg + mL(α˙ 2 + αα ¨) Fx = m(rθ¨ + L(−α˙ 2 α + α ¨) Jp α ¨ + Fy Lα + Fx L = 0
(2.7) (2.8) (2.9)
Linearizing around α = 0, we note that α˙ 2 ≈ 0, αα ¨ ≈ 0, so: Fy = mg
(2.10)
Fx = m(rθ¨ + L¨ α) Jp α ¨ + Fy Lα + Fx L = 0
(2.11) (2.12)
We also know that Jp = 31 mL2 . Combining, we get: 1 mL2 α ¨ + mgLα + m(rθ¨ + L¨ α)L = 0 3
(2.13)
which we can rewrite as: 4 mL2 α ¨ + mgLα + mrLθ¨ = 0 3
(2.14)
Redefining equation (1.9) in terms of θ and setting the input force such that F = Fx , we have:
ηm ηg kt Kg (
Vm − Kg km θ˙ ) = mr2 θ¨ + mrL¨ α + Jeq θ¨ + Beq θ˙ Rm
We can solve for θ¨ and α ¨ by first rewriting the notation. Let 1. a = Jeq + mr2 2. b = mLr 3. c = 34 mL2 5
(2.15)
4. d = mgL 5. E = ac − b2 6. G =
ηm ηg kt km Kg2 +Beq Rm Rm
Then we have: c¨ α + dα + bθ¨ = 0 ηm ηg kt Kg Vm − Gθ˙ = aθ¨ + b¨ α Rm
(2.16) (2.17)
solving, we get: ηm ηg kt Kg ad bG ˙ θ−b α+ Vm E E Rm E bd ηm ηg kt Kg cG ˙ θ¨ = α − θ+c Vm E E Rm E
α ¨=−
which consequently can be written in state space form: 0 0 1 0 0 θ θ˙ 0 0 0 1 α˙ 0 cG bd α + ηm ηg kt Kg Vm = − 0 θ˙ c θ¨ 0 Rm E E E η η k K ad bG α˙ α ¨ −b mRgm Et g 0 − + 0 E E
(2.18) (2.19)
(2.20)
Case 2: Inverted Pendulum In the case of the inverted pendulum, we start with slightly different base equations: Fy = mg + m¨ y Fx = m¨ x τm − Fx y − Fy x = 0
(3.1) (3.2) (3.3)
Fixing the coordinate system relative to the rotating arm’s base, we establish that: 1. y(t) = Lcos α 6
2. x(t) = rθ(t) − Lsin α Following a similar derivation to the hanging pendulum, we solve to get: 4 mL2 α ¨ − mgLα − mrLθ¨ = 0 3
(3.4)
and
ηm ηg kt Kg (
Vm − Kg km θ˙ α + Jeq θ¨ + Beq θ˙ ) = mr2 θ¨ − mrL¨ Rm
(3.5)
Doing the same substiutions as mentioned before, we have: c¨ α − dα − bθ¨ = 0 ηm ηg kt Kg Vm − Gθ˙ = aθ¨ − b¨ α Rm
(3.6) (3.7)
solving, we get: ad bG ηm ηg kt Kg α− θ+b Vm E E Rm E ηm ηg kt Kg bd cG ˙ θ+c θ¨ = α − Vm E E Rm E
α ¨=
which leads to the state space equation: 0 0 1 0 0 θ θ˙ 0 0 0 1 α˙ 0 α bd cG + ηm ηg kt Kg Vm = 0 − 0 ˙ θ¨ c Rm E θ E E ad bG α˙ α ¨ b ηmRηgmkEt Kg 0 − 0 E E
7
(3.8) (3.9)
(3.10)
2
Model Verification
Using our state space model, we can evaluate the stability of both the hanging pendulum equilibrium and the inverted pendulum equilbrium by using MATLAB (Appendix 2.1, 2.2) to calculate the transfer function. For the hanging equilibruim, the transfer function is:
Rm
(−Gdb2
+
EKg bηg ηm kt s + EGcs2 + Eads + Gacd)
E 2 s3
which can be rearranged as
Rm
(E 2 s3
EKg bηg ηm kt s + EGcs2 + Eads − Gdb2 + Gacd)
Looking at the transfer function, we note that all eigenvalues are negative, indicating that the equilibrium is stable, as expected. For the inverted equlibrium, the transfer function is:
−
EKg bηg ηm kt s Rm (E(ads − Gcs2 ) − E 2 s3 − Gbd + Gacd)
which can be rearranged as Kg bηg ηm kt s Rm (E 2 s3 + Gcs2 − Eads + Gbd − Gacd) Looking at the transfer function, we note that not all eigenvalues are negative, indicating that the equilibrium is unstable, which is also as expected.
8
Subsequently, we can attempt to confirm the validity of our model by calculating the theoretical pulse response of the hanging pendulum system and comparing it to experimental results, using variable values obtained from the SRV02 User Manual (Appendix, Figure (3)) combined with values obtained from the SRV02 Rotary Pendulum Manual (Appendix, Figure (4)). Using MATLAB’s Simulink and subsequent data collection methods (Appendix, Figure (5), Appendix 2.3 ), we plot the experimental response, response using SRV02 user manual’s suggested state space value’s, and our own model’s response.
9
From these charts, we see that the experimental results indicate far less damping than expected in the manual’s model, and significantly less than our model as well. However, the period length and initial amplitude are reasonably close. If we decrease the damping by changing the motor armature resistance, Rm from 2.6Ω to 0.6Ω in our model, it closely mirror the experimental results.
Unfortunately, we could not confirm the actual motor armature resistance at the time, and thus did not make the change in our systems model. However, it is noted that the electrical control system is sufficent for controlling either scenario. 10
3 3.1
Control Hanging Pendulum
We now use a design procedure known as pole placement. If we express the state system model as: ~ x(t) + Bu(t) ~ ~x(t) ˙ + A~ ~ x(t) y(t) = C~
(3.1) (3.2)
and u(t) is the output and r(t) = 0 is the control system’s input, we can express u(t) as a function of the form ~ x(t) u(t) = f [~x(t)] = K~
(3.3)
~ is a 1 x n vector of constant gains. This function is a control law where K that allows all poles of the closed-looped system below to be placed in any desirable locations. The rule can be expressed as u(t) = −K1 x1 (t) − K2 x2 (t) − ..... − Kn xn (t)
11
(3.4)
We can use such an regulatory control system to control our hanging pendulum, by setting the desired location and positioning of the pendulum as ”zero”, and hence our equilibrium point. The control system will force the physical system to settle into its equilibrium at a pace determined by the desired poles. In our system, we picked the poles to be: P = 5 ∗ [−1 − 2 − 1/5 − 2.5]
(3.5)
Subsequently we use MATLAB to solve for the corresponding K-values by using the ”place” command (Appendix 2.4). Once we have the corresponding K-values, we can fully implement the control system using simulink (Appendix, Figure (6)), and compare results with a theoretical simulation (Appendix, Figure (7)). We note that in order to compare the theoretical model to the actual system, there exists formulaic transformation between α , θ and the measured voltage output from the equipment sensors. In our test trial, we accordingly adjust the values by: π 5.42 −π α = (Vα − 2.05) ∗ 2
θ = (Vθ + 0.08) ∗
12
(3.6) (3.7)
3.1.1 Resulting Scopes : Theoretical θ and α:
13
Experimental θ and α:
14
We notice that the experimental result has slightly less damping that our theoretical. In practice, the motor requires a minumum amount of voltage in order to move. It must overcome this minumum ”requirement” barrier in order to respond. Hence, for small oscillations after the system has essentially reached equilibrium, the actual system is incapable of implementing further control measures. However, this control system acheived the desired effects of moving the pendulum system to equilibrium. In addition, Figure (8) in the appendix is an example of an model used to alternate the equilbrium by adding a pulse input to the output system.
15
3.2
Inverted Pendulum
In the case of the inverted pendulum, we note that it’s equilibrium is unstable, and is quicker to fall out of the linear region defined by the model derivation. To ensure an appropriate system reaction time, we adjusted the poles to decrease the time constants by half: P = 10 ∗ [−1 − 2 − 1/5 − 2.5];
(3.8)
Subsequently, we used MATLAB again to solve for the corresponding Kvalues, and tested a theoretical simulink model to check for expected output (Appendix, Figure (9)). Upon confirmation that the theoretical model exhibited expected behavior, we then implemented the control system using simulink as well (Appendix, Figure (10)).
The experimental simulink model settings were re-optimized to allow for sample time of 0.0025 seconds, an upgrade from the previous 0.01 sample time used for the hanging pendulum system. In addition, restrictions were set to limit current flow to the motor only when the rotary arm was less than 90 degrees from equilibrium and the voltage was less than 8 V.
In addition, several minor precautions were taken prior to experimentation. First, the pendulum connecting pin was re-checked to ensure minimal slipping. Secondly, the voltmeter measuring the equlibrium point was reset to ensure accuracy. Finally, the rotary arm was held initially in place in order to give the system enough time to begin reacting.
Although this model succeeded in attaining equilibrium, it still exhibited some negative effects, primarily initial instability and subsequent overreaction to any type of input. While this may be in part caused by sensory limitation, we would still like to re-evaluate our pole-placement to produce a more optimal response reaction. Hence we turn to LQR (Linear Quadratic Regulator) algorithm.
16
3.3
LQR control
LQR sets the poles of the system based on a mathematical algorithm that minimizes a cost function with weighting factors determined by the user. Thus for our system: ~ x(t) + Bu(t) ~ ~x(t) ˙ + A~ ~ x(t) y(t) = C~
(3.1) (3.2)
We consider the weighting factors to be our state variables ~x(t) and input u(t). In general notation, the minimization function can be written as: Z min J =
∞
(~x(t)T Q~x(t) + ~u(t)T R~u(t))dt
(3.9)
o
where Q and R are appropriate matricies containing weights used to penalize their corresponding factor. We also note that for practical purposes, Q and R must be chosen such that the terms in the minimization function are positive definitive, or occasionally semi-positive definitive. In our control system, if we have a feedback response such that ~u(t) = K~x(t), we can solve the Riccati Equation: AT P + P A − P BR−1 B T P + Q = 0
(3.10)
for P, from which we can evaluate K, where K = R−1 B T P . Then our transition matrix (A − BK) has stable eigenvalues that minimize the cost function J. For the inverted pendulum, we choose to weight only α, θ and our input voltage. Thus our Q and R matrices are: q1 0 0 q2 Q= 0 0 0 0
0 0 0 0
0 0 and R = r1 0 0 17
(3.11)
The questions remains as to how to pick the values within the Q and R matrices. For our physical model, we would prefer that input voltage to be under 10 v, with priority set for quicker response to α over θ, and an overall adequate settling time. We use LQR as an iterative process, picking initial weight values and adjusting to fit our criterion. Since we know that the total range of both angles is 2π, we picked initial values: 1 2 ) 2π 1 q2 = ( )2 2π 1 r1 = ( )2 10
q1 = (
(1) (2) (3)
We then solved for the corresponding K-values and examined the theoretical response using a simulink model, (Appendix, Figure (9)). Testing various Q and R matrices, we can produce the following table:
Trial 8 fit our specifications best, so we proceeded to implement it’s corresponding K-values into our sinusoidal equilbrium system using the same simulink model (Appendix, Figure (10)). Thus we complete our optimal control system for the Inverted Pendulum, which can be observed in application.
18
4 4.1
Appendix Figures
Figure 1: Gear motors
Figure 2: Pendulum Derivation Diagram
19
Figure 3: SRV02 Motor Specifications
Note: length of bar load can vary between 0.1525 m and 0.175 m depending on placement of pendulum arm, but has little impact on overall results. Figure 4: SRV02 Pendulum Specifications
20
Figure 5: Model Checking Simulink Diagram
Figure 6: Simulink for Hanging Equilibrium
21
Figure 7: Simulink for Hanging Theoretical Comparison
Figure 8: Example of Alternating Equilibrium
22
Figure 9: Simulink for Inverted Theoretical Comparison
Figure 10: Simulink for Inverted Equilibrium
23
4.2
MATLAB code
2.1 Transfer function (Downwards) 1 2
% Ruisen ( E r i c ) Liu % C a l c u l a t e t h e t r a n s f e r f u n c t i o n f o r pendulum ’ s downwards e q u i l i b r i u m
3 4
clear all
5 6
% Downwards system
7 8
syms s a b c d E F G eta m e t a g k t K g R m
9 10
sIA = [ s 0 −1 0 ; 0 s 0 −1; 0 −b∗d/E s+c ∗G/E 0 ; 0 a ∗d/E −b∗G/E s ] ;
11 12
B = [ 0 ; 0 ; c ∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ; −b∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ] ;
13 14
p hi = i n v ( sIA ) ;
15 16
pretty ( phi )
17 18
% Want alpha v a l u e
19 20
C = [0 1 0 0 ] ;
21 22
D = 0;
23 24
% T r a n s f e r Function − no D matrix
25 26
Trans = C∗ p hi ∗B ;
27 28
p r e t t y ( s i m p l e ( Trans ) )
24
2.2 Transfer function (Upright) 1 2
% Ruisen ( E r i c ) Liu % C a l c u l a t e t h e t r a n s f e r f u n c t i o n f o r pendulum ’ s upright equilibrium .
3 4 5
clear all
6 7
% Upright system
8 9
syms t s a b c d E F G eta m e t a g k t K g R m
10 11
A = [ s 0 −1 0 ; 0 s 0 −1; 0 −b∗d/E s+c ∗G/E 0 ; 0 −a∗ d/E b∗G/E s ] ;
12 13
B = [ 0 ; 0 ; c ∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ; b∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ] ;
14 15
p hi = i n v (A) ;
16 17
pretty ( phi )
18 19
% Want alpha v a l u e
20 21
C = [0 1 0 0 ] ;
22 23
D = 0;
24 25
% T r a n s f e r Function − no D matrix
26 27
Trans = C∗ p hi ∗B ;
28 29
p r e t t y ( s i m p l e ( Trans ) )
25
2.3 Model Check 1
% Ruisen ( E r i c ) Liu
2 3
% C o n f i r m a t i o n o f Model Accuracy
4 5
% S i m u l i n k Experimental P l o t t i n g
6 7 8 9 10 11 12 13 14 15 16
% Run Inverted Pendulum . mdl S i m u l i n k f i l e and use : t s i m = PendulumAngle . time ; z = PendulumAngle . s i g n a l s . v a l u e s ; figure (1) plot ( t sim , z ) x l a b e l ( ’ Time ( S ) ’ ) ; y l a b e l ( ’ V o l t a g e (V) ’ ) ; t i t l e ( ’ Experimental R e s u l t s ’ ) ; a x i s ( [ 0 , 10 , 1.75 2 . 5 ] ) ; g r i d on
17 18
%Check Step Response from Model
19 20
%v a r i a b l e s t s a b c d E F G eta m e t a g k t K g R m J e q m r L B eq
21 22 23 24
25 26 27 28
29
30 31
32 33
% Given v a l u e s R m = 0 . 6 ; % Motor R e s i s t a n c e k t = 7 . 6 8 ∗ 10ˆ( −3) ; % motor c u r r e n t −t o r q u e constant K g = 7 0 ; % High−g e a r t o t a l g e a r r a t i o eta m = 0 . 6 9 ; %Motor e f f i c i e n c y e t a g = 0 . 9 0 ; % Gearbox e f f i c i e n c y J e q = 9 . 7 6 ∗ 10ˆ( −5) ; % High−g e a r e q u i v a l e n t moment w/o l o a d B eq = 0 . 0 1 5 ; % High−g e a r e q u i v a l e n t v i s c o u s damping c o e f f i c i e n t m = 0 . 1 2 8 ; % mass o f s h o r t pendulum r = . 1 5 2 5 ; % r a d i u s ( when pendulum f i x e d i n shortest position ) L = . 1 7 5 ; % h a l f l e n g t h o f s h o r t pendulum g = 9 . 8 1 ; % g r a v i t a t i o n a l constant
34
26
35
%Combined v a l u e s
36 37 38 39 40 41 42 43
a b c d E F G
= = = = = = =
J e q + m∗ r ˆ 2 ; m∗L∗ r ; 4/3∗m∗L ˆ 2 ; m∗g∗L ; a∗ c − b ˆ 2 ; a∗ c + b ˆ 2 ; ( eta m ∗ e t a g ∗ k t ˆ2∗ K g ˆ2 + B eq ∗R m) /R m ;
44 45
%E v a l u a t i o n o f S t a t e Space
46 47
48
49 50
A = [ 0 0 1 0 ; 0 0 0 1 ; 0 b∗d/E −c ∗G/E 0 ; 0 −a∗d/E b∗G/E 0 ] ; B = [ 0 ; 0 ; c ∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ; −b∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ] ; C = [ 0 1 0 0 ] ; % want alpha v a l u e D = 0;
51 52
% C a l c u l a t e d and P l o t Step Response
53 54 55 56
57
s y s = s s (A, B, C,D) ; t=l i n s p a c e ( 0 , 1 0 , 5 0 0 ) ; v i n= 2 ∗ ( ( t >1) . ∗ ( t 1) . ∗ ( t
1
1 1.1
State Space Derivations Electromechanical Derivation
Consider the given diagram. We note that the voltage across the motor can be described by:
eb = km ωm
(1.1)
where km is the back-emf constant of the motor, and ωm is the speed of the motor shaft. By Kirchoff’s Laws, the circuit shows that:
Vm − Im − Lm
dIm − km ωm = 0. dt
(1.2)
If we assume that the motor inductance Lm is insignificant, (1.2) can be reduced to: Vm − Im − km ωm = 0 Solving for Im , 2
(1.3)
Im =
Vm − km ωm Rm
(1.4)
The motor torque is proportional to the voltage applied and is described as: τm = ηm kt Im
(1.5)
where ηm is the motor efficiency and kt is the current-torque constant. If we consider the internal gear system as two gears with negligible friction and moment of inertia, we know that: τeq = ηg Kg τm
(1.6)
where ηg is the gearbox efficiency, and Kg is the total gear ratio. (Appendix, Figure (1)) Kg is provided in the manual, or otherwise can be calculated as the product of the exterior and interior gear ratios. That is, N4 N2 N3 N1 Additionally, we see that due to rotational alignments, θm = Kg θl , implying that: Kg = Kge Kgi =
ωm = Kg ωl
(1.7)
Examining the equivalent torque with respect to the load, we have:
τeq = Jeq
dωl + Beq ωl + rF dt
(1.8)
where F is the equivalent force to be applied to our pendulum, Be q is the viscous damping coefficient, and Jeq is the moment of interia as seen at the output, which can be also calculated as Jeq = Jl + ηg Jm Kg2 . (Jm is the moment of inertia of the internal gear.) Substituting in (1.4), (1.5), (1.6), and (1.7), we get:
ηm ηg kt Kg (
Vm − Kg km ωl dωl ) = Jeq + Beq ωl + rF Rm dt
3
(1.9)
1.2
Pendulum Motion Derivations
Examining the schematic (Appendix, Figure (2)), we note that if the pendulum arm must travel in a fixed circular trajectory with rotational movement solely tangential to its trajectory. Consequently, if we model the pendulum arm as a point mass at it’s center point with distance L from the joint, we can derive the following base equations. Case 1: Hanging pendulum
Fy = mg + m¨ y Fx = m¨ x τm + Fx y + Fy x = 0
(2.1) (2.2) (2.3)
Now fixing the coordinate system relative to the rotating arm’s base, we can also establish that: 1. y(t) = −Lcos α 2. x(t) = rθ(t) + Lsin α Where r is the length of the rotating arm. Substituting, our base equations can now be expressed as:
Fy = mg + m
d2 (−Lcos α) dt
(2.4)
d2 (rθ + Lsin α) dt Jp α + Fy Lsin α + Fx Lcos θ = 0
Fx = m
We note that for small angles, sin θ ≈ θ, cos θ ≈ 1. In addition, d2 cos θ = dt
dθ dt
2 cos θ − sin θ
and
4
d2 θ dt2
(2.5) (2.6)
d2 sin θ = − dt
dθ dt
2
d2 θ sin θ + cos θ 2 dt
Consequently our equations are now: Fy = mg + mL(α˙ 2 + αα ¨) Fx = m(rθ¨ + L(−α˙ 2 α + α ¨) Jp α ¨ + Fy Lα + Fx L = 0
(2.7) (2.8) (2.9)
Linearizing around α = 0, we note that α˙ 2 ≈ 0, αα ¨ ≈ 0, so: Fy = mg
(2.10)
Fx = m(rθ¨ + L¨ α) Jp α ¨ + Fy Lα + Fx L = 0
(2.11) (2.12)
We also know that Jp = 31 mL2 . Combining, we get: 1 mL2 α ¨ + mgLα + m(rθ¨ + L¨ α)L = 0 3
(2.13)
which we can rewrite as: 4 mL2 α ¨ + mgLα + mrLθ¨ = 0 3
(2.14)
Redefining equation (1.9) in terms of θ and setting the input force such that F = Fx , we have:
ηm ηg kt Kg (
Vm − Kg km θ˙ ) = mr2 θ¨ + mrL¨ α + Jeq θ¨ + Beq θ˙ Rm
We can solve for θ¨ and α ¨ by first rewriting the notation. Let 1. a = Jeq + mr2 2. b = mLr 3. c = 34 mL2 5
(2.15)
4. d = mgL 5. E = ac − b2 6. G =
ηm ηg kt km Kg2 +Beq Rm Rm
Then we have: c¨ α + dα + bθ¨ = 0 ηm ηg kt Kg Vm − Gθ˙ = aθ¨ + b¨ α Rm
(2.16) (2.17)
solving, we get: ηm ηg kt Kg ad bG ˙ θ−b α+ Vm E E Rm E bd ηm ηg kt Kg cG ˙ θ¨ = α − θ+c Vm E E Rm E
α ¨=−
which consequently can be written in state space form: 0 0 1 0 0 θ θ˙ 0 0 0 1 α˙ 0 cG bd α + ηm ηg kt Kg Vm = − 0 θ˙ c θ¨ 0 Rm E E E η η k K ad bG α˙ α ¨ −b mRgm Et g 0 − + 0 E E
(2.18) (2.19)
(2.20)
Case 2: Inverted Pendulum In the case of the inverted pendulum, we start with slightly different base equations: Fy = mg + m¨ y Fx = m¨ x τm − Fx y − Fy x = 0
(3.1) (3.2) (3.3)
Fixing the coordinate system relative to the rotating arm’s base, we establish that: 1. y(t) = Lcos α 6
2. x(t) = rθ(t) − Lsin α Following a similar derivation to the hanging pendulum, we solve to get: 4 mL2 α ¨ − mgLα − mrLθ¨ = 0 3
(3.4)
and
ηm ηg kt Kg (
Vm − Kg km θ˙ α + Jeq θ¨ + Beq θ˙ ) = mr2 θ¨ − mrL¨ Rm
(3.5)
Doing the same substiutions as mentioned before, we have: c¨ α − dα − bθ¨ = 0 ηm ηg kt Kg Vm − Gθ˙ = aθ¨ − b¨ α Rm
(3.6) (3.7)
solving, we get: ad bG ηm ηg kt Kg α− θ+b Vm E E Rm E ηm ηg kt Kg bd cG ˙ θ+c θ¨ = α − Vm E E Rm E
α ¨=
which leads to the state space equation: 0 0 1 0 0 θ θ˙ 0 0 0 1 α˙ 0 α bd cG + ηm ηg kt Kg Vm = 0 − 0 ˙ θ¨ c Rm E θ E E ad bG α˙ α ¨ b ηmRηgmkEt Kg 0 − 0 E E
7
(3.8) (3.9)
(3.10)
2
Model Verification
Using our state space model, we can evaluate the stability of both the hanging pendulum equilibrium and the inverted pendulum equilbrium by using MATLAB (Appendix 2.1, 2.2) to calculate the transfer function. For the hanging equilibruim, the transfer function is:
Rm
(−Gdb2
+
EKg bηg ηm kt s + EGcs2 + Eads + Gacd)
E 2 s3
which can be rearranged as
Rm
(E 2 s3
EKg bηg ηm kt s + EGcs2 + Eads − Gdb2 + Gacd)
Looking at the transfer function, we note that all eigenvalues are negative, indicating that the equilibrium is stable, as expected. For the inverted equlibrium, the transfer function is:
−
EKg bηg ηm kt s Rm (E(ads − Gcs2 ) − E 2 s3 − Gbd + Gacd)
which can be rearranged as Kg bηg ηm kt s Rm (E 2 s3 + Gcs2 − Eads + Gbd − Gacd) Looking at the transfer function, we note that not all eigenvalues are negative, indicating that the equilibrium is unstable, which is also as expected.
8
Subsequently, we can attempt to confirm the validity of our model by calculating the theoretical pulse response of the hanging pendulum system and comparing it to experimental results, using variable values obtained from the SRV02 User Manual (Appendix, Figure (3)) combined with values obtained from the SRV02 Rotary Pendulum Manual (Appendix, Figure (4)). Using MATLAB’s Simulink and subsequent data collection methods (Appendix, Figure (5), Appendix 2.3 ), we plot the experimental response, response using SRV02 user manual’s suggested state space value’s, and our own model’s response.
9
From these charts, we see that the experimental results indicate far less damping than expected in the manual’s model, and significantly less than our model as well. However, the period length and initial amplitude are reasonably close. If we decrease the damping by changing the motor armature resistance, Rm from 2.6Ω to 0.6Ω in our model, it closely mirror the experimental results.
Unfortunately, we could not confirm the actual motor armature resistance at the time, and thus did not make the change in our systems model. However, it is noted that the electrical control system is sufficent for controlling either scenario. 10
3 3.1
Control Hanging Pendulum
We now use a design procedure known as pole placement. If we express the state system model as: ~ x(t) + Bu(t) ~ ~x(t) ˙ + A~ ~ x(t) y(t) = C~
(3.1) (3.2)
and u(t) is the output and r(t) = 0 is the control system’s input, we can express u(t) as a function of the form ~ x(t) u(t) = f [~x(t)] = K~
(3.3)
~ is a 1 x n vector of constant gains. This function is a control law where K that allows all poles of the closed-looped system below to be placed in any desirable locations. The rule can be expressed as u(t) = −K1 x1 (t) − K2 x2 (t) − ..... − Kn xn (t)
11
(3.4)
We can use such an regulatory control system to control our hanging pendulum, by setting the desired location and positioning of the pendulum as ”zero”, and hence our equilibrium point. The control system will force the physical system to settle into its equilibrium at a pace determined by the desired poles. In our system, we picked the poles to be: P = 5 ∗ [−1 − 2 − 1/5 − 2.5]
(3.5)
Subsequently we use MATLAB to solve for the corresponding K-values by using the ”place” command (Appendix 2.4). Once we have the corresponding K-values, we can fully implement the control system using simulink (Appendix, Figure (6)), and compare results with a theoretical simulation (Appendix, Figure (7)). We note that in order to compare the theoretical model to the actual system, there exists formulaic transformation between α , θ and the measured voltage output from the equipment sensors. In our test trial, we accordingly adjust the values by: π 5.42 −π α = (Vα − 2.05) ∗ 2
θ = (Vθ + 0.08) ∗
12
(3.6) (3.7)
3.1.1 Resulting Scopes : Theoretical θ and α:
13
Experimental θ and α:
14
We notice that the experimental result has slightly less damping that our theoretical. In practice, the motor requires a minumum amount of voltage in order to move. It must overcome this minumum ”requirement” barrier in order to respond. Hence, for small oscillations after the system has essentially reached equilibrium, the actual system is incapable of implementing further control measures. However, this control system acheived the desired effects of moving the pendulum system to equilibrium. In addition, Figure (8) in the appendix is an example of an model used to alternate the equilbrium by adding a pulse input to the output system.
15
3.2
Inverted Pendulum
In the case of the inverted pendulum, we note that it’s equilibrium is unstable, and is quicker to fall out of the linear region defined by the model derivation. To ensure an appropriate system reaction time, we adjusted the poles to decrease the time constants by half: P = 10 ∗ [−1 − 2 − 1/5 − 2.5];
(3.8)
Subsequently, we used MATLAB again to solve for the corresponding Kvalues, and tested a theoretical simulink model to check for expected output (Appendix, Figure (9)). Upon confirmation that the theoretical model exhibited expected behavior, we then implemented the control system using simulink as well (Appendix, Figure (10)).
The experimental simulink model settings were re-optimized to allow for sample time of 0.0025 seconds, an upgrade from the previous 0.01 sample time used for the hanging pendulum system. In addition, restrictions were set to limit current flow to the motor only when the rotary arm was less than 90 degrees from equilibrium and the voltage was less than 8 V.
In addition, several minor precautions were taken prior to experimentation. First, the pendulum connecting pin was re-checked to ensure minimal slipping. Secondly, the voltmeter measuring the equlibrium point was reset to ensure accuracy. Finally, the rotary arm was held initially in place in order to give the system enough time to begin reacting.
Although this model succeeded in attaining equilibrium, it still exhibited some negative effects, primarily initial instability and subsequent overreaction to any type of input. While this may be in part caused by sensory limitation, we would still like to re-evaluate our pole-placement to produce a more optimal response reaction. Hence we turn to LQR (Linear Quadratic Regulator) algorithm.
16
3.3
LQR control
LQR sets the poles of the system based on a mathematical algorithm that minimizes a cost function with weighting factors determined by the user. Thus for our system: ~ x(t) + Bu(t) ~ ~x(t) ˙ + A~ ~ x(t) y(t) = C~
(3.1) (3.2)
We consider the weighting factors to be our state variables ~x(t) and input u(t). In general notation, the minimization function can be written as: Z min J =
∞
(~x(t)T Q~x(t) + ~u(t)T R~u(t))dt
(3.9)
o
where Q and R are appropriate matricies containing weights used to penalize their corresponding factor. We also note that for practical purposes, Q and R must be chosen such that the terms in the minimization function are positive definitive, or occasionally semi-positive definitive. In our control system, if we have a feedback response such that ~u(t) = K~x(t), we can solve the Riccati Equation: AT P + P A − P BR−1 B T P + Q = 0
(3.10)
for P, from which we can evaluate K, where K = R−1 B T P . Then our transition matrix (A − BK) has stable eigenvalues that minimize the cost function J. For the inverted pendulum, we choose to weight only α, θ and our input voltage. Thus our Q and R matrices are: q1 0 0 q2 Q= 0 0 0 0
0 0 0 0
0 0 and R = r1 0 0 17
(3.11)
The questions remains as to how to pick the values within the Q and R matrices. For our physical model, we would prefer that input voltage to be under 10 v, with priority set for quicker response to α over θ, and an overall adequate settling time. We use LQR as an iterative process, picking initial weight values and adjusting to fit our criterion. Since we know that the total range of both angles is 2π, we picked initial values: 1 2 ) 2π 1 q2 = ( )2 2π 1 r1 = ( )2 10
q1 = (
(1) (2) (3)
We then solved for the corresponding K-values and examined the theoretical response using a simulink model, (Appendix, Figure (9)). Testing various Q and R matrices, we can produce the following table:
Trial 8 fit our specifications best, so we proceeded to implement it’s corresponding K-values into our sinusoidal equilbrium system using the same simulink model (Appendix, Figure (10)). Thus we complete our optimal control system for the Inverted Pendulum, which can be observed in application.
18
4 4.1
Appendix Figures
Figure 1: Gear motors
Figure 2: Pendulum Derivation Diagram
19
Figure 3: SRV02 Motor Specifications
Note: length of bar load can vary between 0.1525 m and 0.175 m depending on placement of pendulum arm, but has little impact on overall results. Figure 4: SRV02 Pendulum Specifications
20
Figure 5: Model Checking Simulink Diagram
Figure 6: Simulink for Hanging Equilibrium
21
Figure 7: Simulink for Hanging Theoretical Comparison
Figure 8: Example of Alternating Equilibrium
22
Figure 9: Simulink for Inverted Theoretical Comparison
Figure 10: Simulink for Inverted Equilibrium
23
4.2
MATLAB code
2.1 Transfer function (Downwards) 1 2
% Ruisen ( E r i c ) Liu % C a l c u l a t e t h e t r a n s f e r f u n c t i o n f o r pendulum ’ s downwards e q u i l i b r i u m
3 4
clear all
5 6
% Downwards system
7 8
syms s a b c d E F G eta m e t a g k t K g R m
9 10
sIA = [ s 0 −1 0 ; 0 s 0 −1; 0 −b∗d/E s+c ∗G/E 0 ; 0 a ∗d/E −b∗G/E s ] ;
11 12
B = [ 0 ; 0 ; c ∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ; −b∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ] ;
13 14
p hi = i n v ( sIA ) ;
15 16
pretty ( phi )
17 18
% Want alpha v a l u e
19 20
C = [0 1 0 0 ] ;
21 22
D = 0;
23 24
% T r a n s f e r Function − no D matrix
25 26
Trans = C∗ p hi ∗B ;
27 28
p r e t t y ( s i m p l e ( Trans ) )
24
2.2 Transfer function (Upright) 1 2
% Ruisen ( E r i c ) Liu % C a l c u l a t e t h e t r a n s f e r f u n c t i o n f o r pendulum ’ s upright equilibrium .
3 4 5
clear all
6 7
% Upright system
8 9
syms t s a b c d E F G eta m e t a g k t K g R m
10 11
A = [ s 0 −1 0 ; 0 s 0 −1; 0 −b∗d/E s+c ∗G/E 0 ; 0 −a∗ d/E b∗G/E s ] ;
12 13
B = [ 0 ; 0 ; c ∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ; b∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ] ;
14 15
p hi = i n v (A) ;
16 17
pretty ( phi )
18 19
% Want alpha v a l u e
20 21
C = [0 1 0 0 ] ;
22 23
D = 0;
24 25
% T r a n s f e r Function − no D matrix
26 27
Trans = C∗ p hi ∗B ;
28 29
p r e t t y ( s i m p l e ( Trans ) )
25
2.3 Model Check 1
% Ruisen ( E r i c ) Liu
2 3
% C o n f i r m a t i o n o f Model Accuracy
4 5
% S i m u l i n k Experimental P l o t t i n g
6 7 8 9 10 11 12 13 14 15 16
% Run Inverted Pendulum . mdl S i m u l i n k f i l e and use : t s i m = PendulumAngle . time ; z = PendulumAngle . s i g n a l s . v a l u e s ; figure (1) plot ( t sim , z ) x l a b e l ( ’ Time ( S ) ’ ) ; y l a b e l ( ’ V o l t a g e (V) ’ ) ; t i t l e ( ’ Experimental R e s u l t s ’ ) ; a x i s ( [ 0 , 10 , 1.75 2 . 5 ] ) ; g r i d on
17 18
%Check Step Response from Model
19 20
%v a r i a b l e s t s a b c d E F G eta m e t a g k t K g R m J e q m r L B eq
21 22 23 24
25 26 27 28
29
30 31
32 33
% Given v a l u e s R m = 0 . 6 ; % Motor R e s i s t a n c e k t = 7 . 6 8 ∗ 10ˆ( −3) ; % motor c u r r e n t −t o r q u e constant K g = 7 0 ; % High−g e a r t o t a l g e a r r a t i o eta m = 0 . 6 9 ; %Motor e f f i c i e n c y e t a g = 0 . 9 0 ; % Gearbox e f f i c i e n c y J e q = 9 . 7 6 ∗ 10ˆ( −5) ; % High−g e a r e q u i v a l e n t moment w/o l o a d B eq = 0 . 0 1 5 ; % High−g e a r e q u i v a l e n t v i s c o u s damping c o e f f i c i e n t m = 0 . 1 2 8 ; % mass o f s h o r t pendulum r = . 1 5 2 5 ; % r a d i u s ( when pendulum f i x e d i n shortest position ) L = . 1 7 5 ; % h a l f l e n g t h o f s h o r t pendulum g = 9 . 8 1 ; % g r a v i t a t i o n a l constant
34
26
35
%Combined v a l u e s
36 37 38 39 40 41 42 43
a b c d E F G
= = = = = = =
J e q + m∗ r ˆ 2 ; m∗L∗ r ; 4/3∗m∗L ˆ 2 ; m∗g∗L ; a∗ c − b ˆ 2 ; a∗ c + b ˆ 2 ; ( eta m ∗ e t a g ∗ k t ˆ2∗ K g ˆ2 + B eq ∗R m) /R m ;
44 45
%E v a l u a t i o n o f S t a t e Space
46 47
48
49 50
A = [ 0 0 1 0 ; 0 0 0 1 ; 0 b∗d/E −c ∗G/E 0 ; 0 −a∗d/E b∗G/E 0 ] ; B = [ 0 ; 0 ; c ∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ; −b∗ eta m ∗ e t a g ∗ k t ∗K g / (R m∗E) ] ; C = [ 0 1 0 0 ] ; % want alpha v a l u e D = 0;
51 52
% C a l c u l a t e d and P l o t Step Response
53 54 55 56
57
s y s = s s (A, B, C,D) ; t=l i n s p a c e ( 0 , 1 0 , 5 0 0 ) ; v i n= 2 ∗ ( ( t >1) . ∗ ( t 1) . ∗ ( t
