Rotary Inverted Pendulum - Swarthmore...

32
Rotary Inverted Pendulum Eric Liu 1 Aug 2013 1

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 = kmωm (1.1)

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

By Kirchoff’s Laws, the circuit shows that:

Vm − Im − LmdImdt

− kmωm = 0. (1.2)

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

Vm − Im − kmωm = 0 (1.3)

Solving for Im,

2

Im =Vm − kmωm

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 = ηgKgτ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 asthe product of the exterior and interior gear ratios. That is,

Kg = KgeKgi =N4

N3

N2

N1

Additionally, we see that due to rotational alignments, θm = Kgθl, implyingthat:

ωm = Kgωl (1.7)

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

τeq = Jeqdωldt

+Beqωl + 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:

ηmηgktKg(Vm −Kgkmωl

Rm

) = Jeqdωldt

+Beqωl + 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 it’s 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 arm’s 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

)2

cos θ − sin θd2θ

dt2

and

4

d2

dtsin θ = −

(dθ

dt

)2

sin θ + 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:

ηmηgktKg(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 =ηmηgktkmK2

g+BeqRm

Rm

Then we have:

cα + dα + bθ = 0 (2.16)

ηmηgktKg

Rm

Vm −Gθ = aθ + bα (2.17)

solving, we get:

α = −adEα +

bG

Eθ − b

ηmηgktKg

RmEVm (2.18)

θ =bd

Eα− cG

Eθ + c

ηmηgktKg

RmEVm (2.19)

which consequently can be written in state space form:

θα

θα

=

0 0 1 00 0 0 1

0bd

E−cGE

0

0 −adE

+bG

E0

θα

θα

+

00

cηmηgktKg

RmE

−bηmηgktKg

RmE

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 arm’s 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

3mL2α−mgLα−mrLθ = 0 (3.4)

and

ηmηgktKg(Vm −Kgkmθ

Rm

) = mr2θ −mrLα + Jeqθ +Beqθ (3.5)

Doing the same substiutions as mentioned before, we have:

cα− dα− bθ = 0 (3.6)

ηmηgktKg

Rm

Vm −Gθ = aθ − bα (3.7)

solving, we get:

α =ad

Eα− bG

Eθ + b

ηmηgktKg

RmEVm (3.8)

θ =bd

Eα− cG

Eθ + c

ηmηgktKg

RmEVm (3.9)

which leads to the state space equation:

θα

θα

=

0 0 1 00 0 0 1

0bd

E−cGE

0

0ad

E−bGE

0

θα

θα

+

00

cηmηgktKg

RmE

bηmηgktKg

RmE

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:

EKgbηgηmkts

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

which can be rearranged as

EKgbηgηmkts

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

− EKgbηgηmkts

Rm(E(ads−Gcs2) − E2s3 −Gbd+Gacd)

which can be rearranged as

Kgbηgηmkts

Rm(E2s3 +Gcs2 − Eads+Gbd−Gacd)

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 MATLAB’s Simulink and subsequent data collection methods (Ap-

pendix, Figure (5), Appendix 2.3 ), we plot the experimental response, re-sponse using SRV02 user manual’s suggested state space value’s, and our ownmodel’s response.

9

From these charts, we see that the experimental results indicate far lessdamping than expected in the manual’s 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 system’s 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 as”zero”, 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 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 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− PBR−1BTP +Q = 0 (3.10)

for P, from which we can evaluate K, where K = R−1BTP . Then our transi-tion matrix (A−BK) 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 it’s 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 −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 ta g ∗ k t ∗K g /(R m∗E) ; −b∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ] ;

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 = C∗phi∗B;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 −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 ta g ∗ k t ∗K g /(R m∗E) ; b∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ] ;

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 = C∗phi∗B;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 current−torque

constant25 K g = 70 ; % High−gear 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) ; % High−gear equ iva l en t

moment w/o load29 B eq = 0 . 0 1 5 ; % High−gear 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 = 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 ta g ∗ k t ˆ2∗K gˆ2 + B eq∗R m) /R m;44

45 %Evaluat ion o f State Space46

47 A = [ 0 0 1 0 ; 0 0 0 1 ; 0 b∗d/E −c∗G/E 0 ; 0 −a∗d/Eb∗G/E 0 ] ;

48 B = [ 0 ; 0 ; c∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ; −b∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ] ;

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 <1.1) ) ; % d e f i n e input with

gain o f 557 y = ls im ( sys , v in , t ) ; % opta in y va lue s s tep (

pu l s e ) re sponse58

59 y converted = −y∗2/ p i + 2 . 0 5 ; % convert 0 to 2 .0560 % Conversion from rad ians to vo l tage61 % Need to conf i rm vo l tage s e t t i n g ( 2 . 0 5 )

i n d i v i d u a l l y62 % Negative s i gn 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 g63 f i g u r e (2 )64 p lo 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 , 1 .75 2 . 5 ] ) ;69 g r id on

27

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 <1.1) ) ;83 y manual = ls im ( sys 2 , v in , t ) ;84 y new = (−y manual ) ∗2/ p i + 2 . 0 5 ; % same conver s i on85 f i g u r e (3 )86 p lo 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 , 1 .75 2 . 5 ] ) ;91 g r id on

28

2.4 K-value Finder for Hanging Pendulum

1 %Ruisen ( Er ic ) Liu2 % K−value 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 = 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 ta g ∗ k t ˆ2∗K gˆ2 + B eq∗R m) /R m;27

28 %Evaluat ion o f State Space29

30 A = [ 0 0 1 0 ; 0 0 0 1 ; 0 b∗d/E −c∗G/E 0 ; 0 −a∗d/Eb∗G/E 0 ] ;

31 B = [ 0 ; 0 ; c∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ; −b∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ] ;

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 − B∗K;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 = 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 ta g ∗ k t ˆ2∗K gˆ2 + B eq∗R m) /R m;24

25 %Evaluat ion o f State Space26

27 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 ] ;

28 B = [ 0 ; 0 ; c∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ; b∗eta m∗ e ta g ∗ k t ∗K g /(R m∗E) ] ;

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 <

0 . 28 , alpha < 0 . 2 5 .47 % non−pena l i z ed / pena l i z ed s e t t l e t imes are

1 . 7 / 1 . 8 / 2 . 548

49 [K LQR, S , E] = l q r ( sys , Q , R) ;50

51 K LQR

32