# Rotary Inverted Pendulum - Swarthmore...

date post

08-Jul-2018Category

## Documents

view

214download

0

Embed Size (px)

### Transcript of Rotary Inverted Pendulum - Swarthmore...

Rotary Inverted Pendulum

Eric Liu

1 Aug 2013

1

1 State Space Derivations

1.1 Electromechanical Derivation

Consider the given diagram. We note that the voltage across the motor canbe described by:

eb = kmm (1.1)

where km is the back-emf constant of the motor, and m is the speed of themotor shaft.

By Kirchoffs Laws, the circuit shows that:

Vm Im LmdImdt

kmm = 0. (1.2)

If we assume that the motor inductance Lm is insignificant, (1.2) can bereduced to:

Vm Im kmm = 0 (1.3)

Solving for Im,

2

Im =Vm kmm

Rm(1.4)

The motor torque is proportional to the voltage applied and is described as:

m = mktIm (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 = gKgm (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 asthe product of the exterior and interior gear ratios. That is,

Kg = KgeKgi =N4N3

N2N1

Additionally, we see that due to rotational alignments, m = Kgl, implyingthat:

m = Kgl (1.7)

Examining the equivalent torque with respect to the load, we have:

eq = Jeqdldt

+Beql + rF (1.8)

where F is the equivalent force to be applied to our pendulum, Beq is theviscous damping coefficient, and Jeq is the moment of interia as seen at theoutput, which can be also calculated as Jeq = Jl + gJmK

2g . (Jm is the

moment of inertia of the internal gear.)

Substituting in (1.4), (1.5), (1.6), and (1.7), we get:

mgktKg(Vm Kgkml

Rm) = Jeq

dldt

+Beql + rF (1.9)

3

1.2 Pendulum Motion Derivations

Examining the schematic (Appendix, Figure (2)), we note that if the pendu-lum arm must travel in a fixed circular trajectory with rotational movementsolely tangential to its trajectory. Consequently, if we model the pendulumarm as a point mass at its center point with distance L from the joint, wecan derive the following base equations.

Case 1: Hanging pendulum

Fy = mg +my (2.1)

Fx = mx (2.2)

m + Fxy + Fyx = 0 (2.3)

Now fixing the coordinate system relative to the rotating arms base, we canalso 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 +md2

dt(Lcos ) (2.4)

Fx = md2

dt(r + Lsin ) (2.5)

Jp + FyLsin + FxLcos = 0 (2.6)

We note that for small angles, sin , cos 1.In addition,

d2

dtcos =

(d

dt

)2cos sin d

2

dt2

and

4

d2

dtsin =

(d

dt

)2sin + cos

d2

dt2

Consequently our equations are now:

Fy = mg +mL(2 + ) (2.7)

Fx = m(r + L(2 + ) (2.8)Jp + FyL + FxL = 0 (2.9)

Linearizing around = 0, we note that 2 0, 0, so:

Fy = mg (2.10)

Fx = m(r + L) (2.11)

Jp + FyL + FxL = 0 (2.12)

We also know that Jp =13mL2 . Combining, we get:

1

3mL2 +mgL +m(r + L)L = 0 (2.13)

which we can rewrite as:

4

3mL2 +mgL +mrL = 0 (2.14)

Redefining equation (1.9) in terms of and setting the input force such thatF = Fx , we have:

mgktKg(Vm Kgkm

Rm) = mr2 +mrL + Jeq +Beq (2.15)

We can solve for and by first rewriting the notation. Let

1. a = Jeq +mr2

2. b = mLr

3. c = 43mL2

5

4. d = mgL

5. E = ac b2

6. G =mgktkmK2g+BeqRm

Rm

Then we have:

c + d + b = 0 (2.16)

mgktKgRm

Vm G = a + b (2.17)

solving, we get:

= adE +

bG

E bmgktKg

RmEVm (2.18)

=bd

E cG

E + c

mgktKgRmE

Vm (2.19)

which consequently can be written in state space form:

=

0 0 1 00 0 0 1

0bd

EcGE

0

0 adE

+bG

E0

+

00

cmgktKgRmE

bmgktKgRmE

Vm (2.20)

Case 2: Inverted Pendulum

In the case of the inverted pendulum, we start with slightly different baseequations:

Fy = mg +my (3.1)

Fx = mx (3.2)

m Fxy Fyx = 0 (3.3)

Fixing the coordinate system relative to the rotating arms base, we establishthat:

1. y(t) = Lcos

6

2. x(t) = r(t) Lsin

Following a similar derivation to the hanging pendulum, we solve to get:

4

3mL2mgLmrL = 0 (3.4)

and

mgktKg(Vm Kgkm

Rm) = mr2 mrL + Jeq +Beq (3.5)

Doing the same substiutions as mentioned before, we have:

c d b = 0 (3.6)mgktKg

RmVm G = a b (3.7)

solving, we get:

=ad

E bG

E + b

mgktKgRmE

Vm (3.8)

=bd

E cG

E + c

mgktKgRmE

Vm (3.9)

which leads to the state space equation:

=

0 0 1 00 0 0 1

0bd

EcGE

0

0ad

EbGE

0

+

00

cmgktKgRmE

bmgktKgRmE

Vm (3.10)

7

2 Model Verification

Using our state space model, we can evaluate the stability of both the hang-ing pendulum equilibrium and the inverted pendulum equilbrium by usingMATLAB (Appendix 2.1, 2.2) to calculate the transfer function.

For the hanging equilibruim, the transfer function is:

EKgbgmkts

Rm(Gdb2 + E2s3 + EGcs2 + Eads+Gacd)

which can be rearranged as

EKgbgmkts

Rm(E2s3 + EGcs2 + EadsGdb2 +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:

EKgbgmktsRm(E(adsGcs2) E2s3 Gbd+Gacd)

which can be rearranged as

Kgbgmkts

Rm(E2s3 +Gcs2 Eads+GbdGacd)

Looking at the transfer function, we note that not all eigenvalues are nega-tive, indicating that the equilibrium is unstable, which is also as expected.

8

Subsequently, we can attempt to confirm the validity of our model by cal-culating the theoretical pulse response of the hanging pendulum system andcomparing it to experimental results, using variable values obtained from theSRV02 User Manual (Appendix, Figure (3)) combined with values obtainedfrom the SRV02 Rotary Pendulum Manual (Appendix, Figure (4)).Using MATLABs Simulink and subsequent data collection methods (Ap-

pendix, Figure (5), Appendix 2.3 ), we plot the experimental response, re-sponse using SRV02 user manuals suggested state space values, and our ownmodels response.

9

From these charts, we see that the experimental results indicate far lessdamping than expected in the manuals model, and significantly less thanour model as well. However, the period length and initial amplitude arereasonably close. If we decrease the damping by changing the motor arma-ture resistance, Rm from 2.6 to 0.6 in our model, it closely mirror theexperimental results.

Unfortunately, we could not confirm the actual motor armature resistance atthe 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 eitherscenario.

10

3 Control

3.1 Hanging Pendulum

We now use a design procedure known as pole placement. If we express thestate system model as:

~x(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 systems input, we canexpress 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 lawthat allows all poles of the closed-looped system below to be placed in anydesirable locations. The rule can be expressed as

u(t) = K1x1(t) K2x2(t) .....Knxn(t) (3.4)

11

We can use such an regulatory control system to control our hanging pen-dulum, by setting the desired location and positioning of the pendulum aszero, and hence our equilibrium point. The control system will force thephysical system to settle into its equilibrium at a pace determined by thedesired 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 byusing the place command (Appendix 2.4).

Once we have the corresponding K-values, we can fully implement the controlsystem using simulink (Appendix, Figure (6)), and compare results with atheoretical 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 voltageoutput from the equipment sensors. In our test trial, we accordingly adjustthe values by:

= (V + 0.08)

5.42(3.6)

= (V 2.05) 2

(3.7)

12

3.1.1 Resulting Scopes:Theoretical and :

13

Experimental and :

14

We notice that the experimental result has slightly less damping that ourtheoretical. In practice, the motor requires a minumum amount of voltagein order to move. It must overcome this minumum requirement barrier inorder to respond. Hence, for small oscillations after the system has essentiallyreached equilibrium, the actual system is incapable of implementing furthercontrol measures. However, this control system acheived the desired effectsof moving the pendulum system to equilibrium. In addition, Figure (8) inthe appendix is an example of an model used to alternate the equilbrium byadding a pulse input to the output system.

15

3.2 Inverted Pendulum

In the case of the inverted pendulum, we note that its 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 todecrease 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 ex-hibited expected behavior, we then implemented the control system usingsimulink as well (Appendix, Figure (10)).

The experimental simulink model settings were re-optimized to allow forsample time of 0.0025 seconds, an upgrade from the previous 0.01 sampletime used for the hanging pendulum system. In addition, restrictions wereset to limit current flow to the motor only when the rotary arm was less than90 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 slip-ping. Secondly, the voltmeter measuring the equlibrium point was reset toensure accuracy. Finally, the rotary arm was held initially in place in orderto give the system enough time to begin reacting.

Although this model succeeded in attaining equilibrium, it still exhibitedsome negative effects, primarily initial instability and subsequent overreac-tion to any type of input. While this may be in part caused by sensorylimitation, we would still like to re-evaluate our pole-placement to producea more optimal response reaction. Hence we turn to LQR (Linear QuadraticRegulator) algorithm.

16

3.3 LQR control

LQR sets the poles of the system based on a mathematical algorithm thatminimizes a cost function with weighting factors determined by the user.Thus for our system:

~x(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 inputu(t).

In general notation, the minimization function can be written as:

min J =

o

(~x(t)TQ~x(t) + ~u(t)TR~u(t))dt (3.9)

where Q and R are appropriate matricies containing weights used to penalizetheir corresponding factor. We also note that for practical purposes, Q and Rmust be chosen such that the terms in the minimization function are positivedefinitive, 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:

ATP + PA PBR1BTP +Q = 0 (3.10)

for P, from which we can evaluate K, where K = R1BTP . Then our transi-tion matrix (ABK) has stable eigenvalues that minimize the cost functionJ .

For the inverted pendulum, we choose to weight only , and our inputvoltage.

Thus our Q and R matrices are:

Q =

q1 0 0 00 q2 0 00 0 0 00 0 0 0

and R = (r1) (3.11)17

The questions remains as to how to pick the values within the Q and Rmatrices. For our physical model, we would prefer that input voltage to beunder 10 v, with priority set for quicker response to over , and an overalladequate settling time. We use LQR as an iterative process, picking initialweight values and adjusting to fit our criterion. Since we know that the totalrange of both angles is 2, we picked initial values:

q1 = (1

2)2 (1)

q2 = (1

2)2 (2)

r1 = (1

10)2 (3)

We then solved for the corresponding K-values and examined the theoreticalresponse using a simulink model, (Appendix, Figure (9)). Testing various Qand R matrices, we can produce the following table:

Trial 8 fit our specifications best, so we proceeded to implement its cor-responding K-values into our sinusoidal equilbrium system using the samesimulink model (Appendix, Figure (10)).

Thus we complete our optimal control system for the Inverted Pendulum,which can be observed in application.

18

4 Appendix

4.1 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 mdepending on placement of pendulum arm, but has little impact onoverall 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 % Ruisen ( Er ic ) Liu2 % Calcu la te the t r a n s f e r func t i on f o r pendulum s

downwards equ i l i b r i um3

4 c l e a r a l l5

6 % Downwards system7

8 syms s a b c d E F G eta m eta g k t K g R m9

10 sIA = [ s 0 1 0 ; 0 s 0 1; 0 bd/E s+cG/E 0 ; 0 ad/E bG/E s ] ;

11

12 B = [ 0 ; 0 ; ceta m e ta g k t K g /(R mE) ; beta m e ta g k t K g /(R mE) ] ;

13

14 phi = inv ( sIA ) ;15

16 pre t ty ( phi )17

18 % Want alpha value19

20 C = [ 0 1 0 0 ] ;21

22 D = 0 ;23

24 % Trans fe r Function no D matrix25

26 Trans = CphiB;27

28 pre t ty ( s imple ( Trans ) )

24

2.2 Transfer function (Upright)

1 % Ruisen ( Er ic ) Liu2 % Calcu la te the t r a n s f e r func t i on f o r pendulum s

upr ight equ i l i b r i um .3

4

5 c l e a r a l l6

7 % Upright system8

9 syms t s a b c d E F G eta m eta g k t K g R m10

11 A = [ s 0 1 0 ; 0 s 0 1; 0 bd/E s+cG/E 0 ; 0 ad/E bG/E s ] ;

12

13 B = [ 0 ; 0 ; ceta m e ta g k t K g /(R mE) ; beta m e ta g k t K g /(R mE) ] ;

14

15 phi = inv (A) ;16

17 pre t ty ( phi )18

19 % Want alpha value20

21 C = [ 0 1 0 0 ] ;22

23 D = 0 ;24

25 % Trans fe r Function no D matrix26

27 Trans = CphiB;28

29 pre t ty ( s imple ( Trans ) )

25

2.3 Model Check

1 % Ruisen ( Er ic ) Liu2

3 % Confirmation o f Model Accuracy4

5 % Simulink Experimental P l o t t i ng6

7 % Run Inverted Pendulum . mdl Simulink f i l e and use :8 t s im = PendulumAngle . time ;9 z = PendulumAngle . s i g n a l s . va lue s ;

10 f i g u r e (1 )11 p lo t ( t s im , 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 Resu l t s ) ;15 a x i s ( [ 0 , 10 , 1 .75 2 . 5 ] ) ;16 g r id on17

18 %Check Step Response from Model19

20 %v a r i a b l e s t s a b c d E F G eta m eta g k t K gR m J eq m r L B eq

21

22 % Given va lue s23 R m = 0 . 6 ; % Motor Res i s tance24 k t = 7.68 10(3) ; % motor currenttorque

constant25 K g = 70 ; % Highgear t o t a l gear r a t i o26 eta m = 0 . 6 9 ; %Motor e f f i c i e n c y27 e ta g = 0 . 9 0 ; % Gearbox e f f i c i e n c y28 J eq = 9.76 10(5) ; % Highgear equ iva l en t

moment w/o load29 B eq = 0 . 0 1 5 ; % Highgear equ iva l en t v i s c ou s

damping c o e f f i c i e n t30 m = 0 . 1 2 8 ; % mass o f shor t pendulum31 r = . 1 5 2 5 ; % rad iu 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 = . 1 7 5 ; % h a l f l ength o f shor t pendulum33 g = 9 . 8 1 ; % g r a v i t a t i o n a l constant34

26

35 %Combined va lue s36

37 a = J eq + m r 2 ;38 b = mL r ;39 c = 4/3mL2 ;40 d = mgL ;41 E = ac b 2 ;42 F = ac + b 2 ;43 G = ( eta m e ta g k t 2K g2 + B eqR m) /R m;44

45 %Evaluat ion o f State Space46

47 A = [ 0 0 1 0 ; 0 0 0 1 ; 0 bd/E cG/E 0 ; 0 ad/EbG/E 0 ] ;

48 B = [ 0 ; 0 ; ceta m e ta g k t K g /(R mE) ; beta m e ta g k t K g /(R mE) ] ;

49 C = [ 0 1 0 0 ] ; % want alpha value50 D = 0 ;51

52 % Calcu lated and Plot Step Response53

54 sys = s s (A,B,C,D) ;55 t=l i n s p a c e (0 ,10 ,500) ;56 v in= 2 ( ( t>1) . ( t

70

71 % Compare to Manual Model Values72

73 % Downwards system t e s t va lue s74 A 2 = [ 0 0 1 0 ; 0 0 0 1 ; 0 39 .32 14.52 0 ; 0

81.78 13 .98 0 ] ;75 B 2 = [ 0 ; 0 ; 25 .54 ; 24 .59 ] ;76 C 2 = [ 0 1 0 0 ] ;77 D 2 = 0 ;78

79 % Manual s Step Response80

81 sy s 2 = s s ( A 2 , B 2 , C 2 , D 2 ) ;82 v in manual= 5 ( ( t>1) . ( t

2.4 K-value Finder for Hanging Pendulum

1 %Ruisen ( Er ic ) Liu2 % Kvalue Finder3

4 %Def ine system5

6 R m = 2 . 6 ;7 k t = 7.68 10(3) ;8 K g = 70 ;9 eta m = 0 . 6 9 ;

10 e ta g = 0 . 9 0 ;11 J eq = 9.76 10(5) ;12 B eq = 0 . 0 1 5 ;13 m = 0 . 1 2 8 ; % mass o f shor t pendulum14 r = . 1 5 2 5 ; % rad iu 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 = . 1 7 5 ; % h a l f l ength o f shor t pendulum16 g = 9 . 8 1 ; % g r a v i t a t i o n a l constant17

18 %Combined va lue s19

20 a = J eq + m r 2 ;21 b = mL r ;22 c = 4/3mL2 ;23 d = mgL ;24 E = ac b 2 ;25 F = ac + b 2 ;26 G = ( eta m e ta g k t 2K g2 + B eqR m) /R m;27

28 %Evaluat ion o f State Space29

30 A = [ 0 0 1 0 ; 0 0 0 1 ; 0 bd/E cG/E 0 ; 0 ad/EbG/E 0 ] ;

31 B = [ 0 ; 0 ; ceta m e ta g k t K g /(R mE) ; beta m e ta g k t K g /(R mE) ] ;

32 C = [ 0 1 0 0 ; 1 0 0 0 ] ; % want alpha value33 D = 0 ;34

35 sys = s s (A,B,C,D) ;36

29

37 %Evaluate K va lue s38

39 P = 5[1 2 1.5 2 .5 ] ;40 K = place (A,B,P)41

42 A f = A BK;43

44 s y s f = s s ( A f , B, C, D) ;45

46 i n i t i a l ( sys , s y s f , [ 0 1 0 0 ] )

30

2.5 K -value Finder for Inverted Pendulum

1 %Def ine system2

3 R m = 2 . 6 ;4 k t = 7.68 10(3) ;5 K g = 70 ;6 eta m = 0 . 6 9 ;7 e ta g = 0 . 9 0 ;8 J eq = 9.76 10(5) ;9 B eq = 0 . 0 1 5 ;

10 m = 0 . 1 2 8 ; % mass o f shor t pendulum11 r = . 1 5 2 5 ; % rad iu 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 = . 1 7 5 ; % h a l f l ength o f shor t pendulum13 g = 9 . 8 1 ; % g r a v i t a t i o n a l constant14

15 %Combined va lue s16

17 a = J eq + m r 2 ;18 b = mL r ;19 c = 4/3mL2 ;20 d = mgL ;21 E = ac b 2 ;22 F = ac + b 2 ;23 G = ( eta m e ta g k t 2K g2 + B eqR m) /R m;24

25 %Evaluat ion o f State Space26

27 A = [ 0 0 1 0 ; 0 0 0 1 ; 0 bd/E cG/E 0 ; 0 ad/E bG/E 0 ] ;

28 B = [ 0 ; 0 ; ceta m e ta g k t K g /(R mE) ; beta m e ta g k t K g /(R mE) ] ;

29 C = [ 0 1 0 0 ; 1 0 0 0 ] ; % want alpha value30 D = 0 ;31

32 sys = s s (A,B,C,D) ;33

34 %Evaluate K va lue s35

36 P = 10[1 1.5 2 2 .5 ] ; % works with m u l t i p l i c i t y

31

o f 1037 K = place (A,B,P)38

39 %LQR40

41 % Pena l i z e alpha much more than theta42 Q = [ ( 1 / ( 2 pi ) ) 2 0 0 0 ; 0 (10/(2 pi ) ) 2 0 0 ; 0 0

0 0 ; 0 0 0 0 ] ;43 R = 0 . 0 0 1 ;44

45 %Theo 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 a l low f o r V < 7 .8 , theta