Rotary Inverted Pendulum

Transcription

1 Rotary Inverted Pendulum Eric Liu 1 Aug

2 1 State Space Derivations 1.1 Electromechanical Derivation Consider the given diagram. We note that the voltage across the motor can be described by: e b = k m ω m (1.1) where k m 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: V m I m L m di m dt k mω m = 0. (1.2) If we assume that the motor inductance L m is insignificant, (1.2) can be reduced to: Solving for I m, V m I m k m ω m = 0 (1.3) 2

3 I m = V m k m ω m R m (1.4) The motor torque is proportional to the voltage applied and is described as: τ m = η m k t I m (1.5) where η m is the motor efficiency and k t 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 K g τ m (1.6) where η g is the gearbox efficiency, and K g is the total gear ratio. (Appendix, Figure (1)) K g is provided in the manual, or otherwise can be calculated as the product of the exterior and interior gear ratios. That is, K g = K ge K gi = N 4 N 2 N 3 N 1 Additionally, we see that due to rotational alignments, θ m = K g θ l, implying that: ω m = K g ω l (1.7) Examining the equivalent torque with respect to the load, we have: τ eq = J eq dω l dt + B eqω l + rf (1.8) where F is the equivalent force to be applied to our pendulum, B e q is the viscous damping coefficient, and J eq is the moment of interia as seen at the output, which can be also calculated as J eq = J l + η g J m Kg 2. (J m is the moment of inertia of the internal gear.) Substituting in (1.4), (1.5), (1.6), and (1.7), we get: η m η g k t K g ( V m K g k m ω l dω l ) = J eq R m dt + B eqω l + rf (1.9) 3

4 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 F y = mg + mÿ (2.1) F x = mẍ (2.2) τ m + F x y + F y x = 0 (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: F y = mg + m d2 ( Lcos α) (2.4) dt F x = m d2 (rθ + Lsin α) dt (2.5) J p α + F y Lsin α + F x Lcos θ = 0 (2.6) We note that for small angles, sin θ θ, cos θ 1. In addition, and d 2 dt cos θ = ( ) 2 dθ cos θ sin θ d2 θ dt dt 2 4

5 Consequently our equations are now: ( ) d 2 2 dθ dt sin θ = sin θ + cos θ d2 θ dt dt 2 F y = mg + ml( α 2 + α α) (2.7) F x = m(r θ + L( α 2 α + α) (2.8) J p α + F y Lα + F x L = 0 (2.9) Linearizing around α = 0, we note that α 2 0, α α 0, so: F y = mg (2.10) F x = m(r θ + L α) (2.11) J p α + F y Lα + F x L = 0 (2.12) We also know that J p = 1 3 ml2. Combining, we get: which we can rewrite as: 1 3 ml2 α + mglα + m(r θ + L α)l = 0 (2.13) 4 3 ml2 α + mglα + mrl θ = 0 (2.14) Redefining equation (1.9) in terms of θ and setting the input force such that F = F x, we have: η m η g k t K g ( V m K g k m θ R m ) = mr 2 θ + mrl α + Jeq θ + Beq θ (2.15) We can solve for θ and α by first rewriting the notation. Let 1. a = J eq + mr 2 2. b = mlr 3. c = 4 3 ml2 5

6 4. d = mgl 5. E = ac b 2 6. G = ηmηgktkmk2 g +BeqRm R m Then we have: solving, we get: c α + dα + b θ = 0 (2.16) η m η g k t K g V m G R θ = a θ + b α m (2.17) α = ad E α + bg E θ b η mη g k t K g R m E V m (2.18) θ = bd E α cg E θ + c η mη g k t K g R m E V m (2.19) which consequently can be written in state space form: θ α θ 0 θ = bd 0 cg E E 0 α θ + 0 α α 0 ad E +bg E 0 Case 2: Inverted Pendulum c ηmηgktkg R me b ηmηgktkg R me V m (2.20) In the case of the inverted pendulum, we start with slightly different base equations: F y = mg + mÿ (3.1) F x = mẍ (3.2) τ m F x y F y x = 0 (3.3) Fixing the coordinate system relative to the rotating arm s base, we establish that: 1. y(t) = Lcos α 6

7 2. x(t) = rθ(t) Lsin α Following a similar derivation to the hanging pendulum, we solve to get: and 4 3 ml2 α mglα mrl θ = 0 (3.4) η m η g k t K g ( V m K g k m θ R m ) = mr 2 θ mrl α + Jeq θ + Beq θ (3.5) Doing the same substiutions as mentioned before, we have: solving, we get: c α dα b θ = 0 (3.6) η m η g k t K g V m G R θ = a θ b α m (3.7) α = ad E α bg E θ + bη mη g k t K g R m E V m (3.8) θ = bd E α cg E θ + c η mη g k t K g R m E V m (3.9) which leads to the state space equation: θ α θ θ = bd 0 E cg E 0 α θ + α α 0 ad E bg E c ηmηgktkg R me b ηmηgktkg R me V m (3.10) 7

8 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: EK g bη g η m k t s R m ( Gdb 2 + E 2 s 3 + EGcs 2 + Eads + Gacd) which can be rearranged as EK g bη g η m k t s R m (E 2 s 3 + EGcs 2 + Eads Gdb 2 + 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: EK g bη g η m k t s R m (E(ads Gcs 2 ) E 2 s 3 Gbd + Gacd) which can be rearranged as K g bη g η m k t s R m (E 2 s 3 + Gcs 2 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

9 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

10 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, R m 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

11 3 Control 3.1 Hanging Pendulum We now use a design procedure known as pole placement. If we express the state system model as: ẋ(t) + A x(t) + Bu(t) (3.1) y(t) = C x(t) (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 u(t) = f[ x(t)] = K x(t) (3.3) where K is a 1 x n vector of constant gains. This function is a control law 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) = K 1 x 1 (t) K 2 x 2 (t)... K n x n (t) (3.4) 11

12 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: θ = (V θ ) π 5.42 α = (V α 2.05) π 2 (3.6) (3.7) 12

13 3.1.1 Resulting Scopes : Theoretical θ and α: 13

14 Experimental θ and α: 14

15 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

16 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 K- values, 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 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

17 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: ẋ(t) + A x(t) + Bu(t) (3.1) y(t) = C x(t) (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: min J = o ( x(t) T Q x(t) + u(t) T R u(t))dt (3.9) 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: A T 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: q Q = 0 q and R = ( ) r (3.11) 17

18 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: q 1 = ( 1 2π )2 (1) q 2 = ( 1 2π )2 (2) r 1 = ( 1 10 )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

19 4 4.1 Appendix Figures Figure 1: Gear motors Figure 2: Pendulum Derivation Diagram 19

20 Figure 3: SRV02 Motor Specifications Note: length of bar load can vary between m and m depending on placement of pendulum arm, but has little impact on overall results. Figure 4: SRV02 Pendulum Specifications 20

21 Figure 5: Model Checking Simulink Diagram Figure 6: Simulink for Hanging Equilibrium 21

22 Figure 7: Simulink for Hanging Theoretical Comparison Figure 8: Example of Alternating Equilibrium 22

23 Figure 9: Simulink for Inverted Theoretical Comparison Figure 10: Simulink for Inverted Equilibrium 23

24 4.2 MATLAB code 2.1 Transfer function (Downwards) 1 % Ruisen ( Eric ) Liu 2 % C a l c u l a t e the 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 c l e a r a l l 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 s 0 1; 0 b d/e s+c G/E 0 ; 0 a d/e b G/E s ] ; 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) ] ; phi = inv ( sia ) ; p r e t t y ( phi ) % Want alpha value C = [ ] ; D = 0 ; % Transfer Function no D matrix Trans = C phi B; p r e t t y ( simple ( Trans ) ) 24

25 2.2 Transfer function (Upright) 1 % Ruisen ( Eric ) Liu 2 % C a l c u l a t e the t r a n s f e r f u n c t i o n f o r pendulum s upright e q u i l i b r i u m c l e a r a l l 6 7 % Upright system syms t s a b c d E F G eta m e t a g k t K g R m 11 A = [ s ; 0 s 0 1; 0 b d/e s+c G/E 0 ; 0 a d/e b G/E s ] ; 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) ] ; phi = inv (A) ; p r e t t y ( phi ) % Want alpha value C = [ ] ; D = 0 ; % Transfer Function no D matrix Trans = C phi B; p r e t t y ( simple ( Trans ) ) 25

26 2.3 Model Check 1 % Ruisen ( Eric ) Liu 2 3 % Confirmation o f Model Accuracy 4 5 % Simulink Experimental P l o t t i n g 6 7 % Run Inverted Pendulum. mdl Simulink f i l e and use : 8 t s i m = PendulumAngle. time ; 9 z = PendulumAngle. s i g n a l s. v a l u es ; 10 f i g u r e ( 1 ) 11 p l o t ( t sim, z ) 12 x l a b e l ( Time ( S ) ) ; 13 y l a b e l ( Voltage (V) ) ; 14 t i t l e ( Experimental Results ) ; 15 a x i s ( [ 0, 10, ] ) ; 16 g r i d on %Check Step Response from Model %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 eq m r L B eq % Given v a l u e s 23 R m = 0. 6 ; % Motor R e s i s t a n c e 24 k t = ˆ( 3) ; % motor current torque constant 25 K g = 7 0 ; % High gear t o t a l gear r a t i o 26 eta m = ; %Motor e f f i c i e n c y 27 e t a g = ; % Gearbox e f f i c i e n c y 28 J eq = ˆ( 5) ; % High gear e q u i v a l e n t moment w/o load 29 B eq = ; % High gear 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 30 m = ; % mass o f s h o r t pendulum 31 r = ; % r a d i u s ( when pendulum f i x e d in s h o r t e s t p o s i t i o n ) 32 L = ; % h a l f length o f s h o r t pendulum 33 g = ; % g r a v i t a t i o n a l constant 34 26

27 35 %Combined v a l u e s a = J eq + m r ˆ 2 ; 38 b = m L r ; 39 c = 4/3 m Lˆ 2 ; 40 d = m g L ; 41 E = a c b ˆ 2 ; 42 F = a c + b ˆ 2 ; 43 G = ( eta m e t a g k t ˆ2 K gˆ2 + B eq R m) /R m ; %Evaluation o f State Space A = [ ; ; 0 b d/e c G/E 0 ; 0 a d/e b G/E 0 ] ; 48 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) ] ; 49 C = [ ] ; % want alpha value 50 D = 0 ; % Calculated and Plot Step Response sys = s s (A,B,C,D) ; 55 t=l i n s p a c e ( 0, 1 0, 500) ; 56 v i n= 2 (( t >1). ( t <1.1) ) ; % d e f i n e input with gain o f 5 57 y = lsim ( sys, v in, t ) ; % optain y v alues step ( p u l s e ) response y converted = y 2/ pi ; % convert 0 to % Conversion from r a dians to v o l t a g e 61 % Need to confirm v o l t a g e s e t t i n g ( ) i n d i v i d u a l l y 62 % Negative s i g n f o r switched i n t e r p r e t a t i o n o f alpha /x p o s i t i o n i n g 63 f i g u r e ( 2 ) 64 p l o t ( t, y converted ) 65 x l a b e l ( Time ( S ) ) ; 66 y l a b e l ( Voltage (V) ) ; 67 t i t l e ( Model Step Response ) ; 68 a x i s ( [ 0, 10, ] ) ; 69 g r i d on 27

28 70 71 % Compare t o Manual Model Values % Downwards system t e s t values 74 A 2 = [ ; ; ; ] ; 75 B 2 = [ 0 ; 0 ; ; 24.59]; 76 C 2 = [ ] ; 77 D 2 = 0 ; % Manual s Step Response s y s 2 = s s ( A 2, B 2, C 2, D 2 ) ; 82 v in manual= 5 (( t >1). ( t <1.1) ) ; 83 y manual = lsim ( sys 2, v in, t ) ; 84 y new = ( y manual ) 2/ pi ; % same conversion 85 f i g u r e ( 3 ) 86 p l o t ( t, y new ) 87 x l a b e l ( Time ( S ) ) ; 88 y l a b e l ( Voltage (V) ) ; 89 t i t l e ( Manual Step Response ) ; 90 a x i s ( [ 0, 10, ] ) ; 91 g r i d on 28

29 2.4 K-value Finder for Hanging Pendulum 1 %Ruisen ( Eric ) Liu 2 % K value Finder 3 4 %Define system 5 6 R m = 2. 6 ; 7 k t = ˆ( 3) ; 8 K g = 7 0 ; 9 eta m = ; 10 e t a g = ; 11 J eq = ˆ( 5) ; 12 B eq = ; 13 m = ; % mass o f s h o r t pendulum 14 r = ; % r a d i u s ( when pendulum f i x e d in s h o r t e s t p o s i t i o n ) 15 L = ; % h a l f length o f s h o r t pendulum 16 g = ; % g r a v i t a t i o n a l constant %Combined v a l u e s a = J eq + m r ˆ 2 ; 21 b = m L r ; 22 c = 4/3 m Lˆ 2 ; 23 d = m g L ; 24 E = a c b ˆ 2 ; 25 F = a c + b ˆ 2 ; 26 G = ( eta m e t a g k t ˆ2 K gˆ2 + B eq R m) /R m ; %Evaluation o f State Space A = [ ; ; 0 b d/e c G/E 0 ; 0 a d/e b G/E 0 ] ; 31 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) ] ; 32 C = [ ; ] ; % want alpha value 33 D = 0 ; sys = s s (A,B,C,D) ; 36 29

30 37 %Evaluate K v a l u e s P = 5 [ ]; 40 K = p l a c e (A,B,P) A f = A B K; s y s f = s s ( A f, B, C, D) ; i n i t i a l ( sys, s y s f, [ ] ) 30

31 2.5 K -value Finder for Inverted Pendulum 1 %Define system 2 3 R m = 2. 6 ; 4 k t = ˆ( 3) ; 5 K g = 7 0 ; 6 eta m = ; 7 e t a g = ; 8 J eq = ˆ( 5) ; 9 B eq = ; 10 m = ; % mass o f s h o r t pendulum 11 r = ; % r a d i u s ( when pendulum f i x e d in s h o r t e s t p o s i t i o n ) 12 L = ; % h a l f length o f s h o r t pendulum 13 g = ; % g r a v i t a t i o n a l constant %Combined v a l u e s a = J eq + m r ˆ 2 ; 18 b = m L r ; 19 c = 4/3 m Lˆ 2 ; 20 d = m g L ; 21 E = a c b ˆ 2 ; 22 F = a c + b ˆ 2 ; 23 G = ( eta m e t a g k t ˆ2 K gˆ2 + B eq R m) /R m ; %Evaluation o f State Space A = [ ; ; 0 b d/e c G/E 0 ; 0 a d/e b G/E 0 ] ; 28 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) ] ; 29 C = [ ; ] ; % want alpha value 30 D = 0 ; sys = s s (A,B,C,D) ; %Evaluate K v a l u e s P = 10 [ ]; % works with m u l t i p l i c i t y 31

32 o f K = p l a c e (A,B,P) %LQR % P e n a l i z e alpha much more than theta 42 Q = [ ( 1 / ( 2 pi ) ) ˆ ; 0 (10/(2 pi ) ) ˆ2 0 0 ; ; ] ; 43 R = ; %T h e o r e t i c a l P r e d i c t i o n s : 46 %Current QR s e t t i n g s allow f o r V < 7. 8, theta < , alpha < % non p e n a l i z e d / p e n a l i z e d s e t t l e times are 1. 7 / 1. 8 / [K LQR, S, E] = l q r ( sys, Q, R) ; K LQR 32