Rotary Inverted Pendulum - Swarthmore...

Click here to load reader

  • date post

    08-Jul-2018
  • Category

    Documents

  • view

    214
  • download

    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