Mechatronic System Case Study: Rotary Inverted Pendulum Dynamic System Investigation

Transcription

1 Mechatronic System Case Study: Rotary Inverted Pendulum Dynamic System Investigation Dr. Kevin Craig Greenheck Chair in Engineering Design & Professor of Mechanical Engineering Marquette University K. Craig 1

2 K. Craig

3 K. Craig 3

4 K. Craig 4

5 K. Craig 5

6 K. Craig 6

7 Mechatronic System Case Study: Rotary Inverted Pendulum Dynamic System Investigation Physical System K. Craig 7

8 Physical System Components Two Links Motor-driven horizontal link Un-actuated vertical pendulum link Permanent-magnet, brushed DC motor actuator 4-volt, 5-amp, DC power supply Pulse-width-modulated servo-amplifier Two rotary incremental optical encoders The resolution with quadrature decoding of each encoder is: 000 cpr (horizontal link) and 5000 cpr (pendulum) K. Craig 8

9 One encoder measures pendulum angle One encoder measures horizontal link angle Velocity data is derived for each link from the encoder data Slip-ring assembly, mounted between the housing and the motor shaft, used to connect power to the pendulum optical encoder and read the signal from the three channels of the encoder Counter-weight on the horizontal arm Leveling screws on the housing base The command to the amplifier is provided by a MatLab-based real-time control system. K. Craig 9

10 Physical Model: Simplifying Assumptions Links are rigid Two-degree-of-freedom system; generalized coordinates are: - horizontal link angle - pendulum arm angle Assume both Coulomb and viscous friction in the motor, in the slip-ring assembly, and at the pendulum revolute joint and perform tests to identify those parameters Dynamic response of the encoders is sufficiently fast that it can be considered instantaneous Dynamic response of the servo-amplifier is sufficiently fast that it can be considered instantaneous K. Craig 10

11 Motor operates in the torque mode with V in K A = i and T = K T i where: K T is the motor torque constant (N-m/A) K A is the amplifier constant (A/V) V in is the command voltage (V) T is the electromagnetic torque applied to the motor rotor (N-m) i is the motor current (A) Motor is modeled in a lumped parameter way where: J is the rotor inertia (N-m-s /rad = kg-m ) B f is the viscous friction coefficient (N-m-s/rad) T f is the Coulomb friction torque (N-m) K. Craig 11

12 Physical & Mathematical Modeling Reference Frames: R: ground xyz R 1 : arm x 1 y 1 z 1 R : pendulum x y z ˆi ˆ 1 cos sin 0 i ˆj ˆ 1 sin cos 0 j kˆ ˆ 1 k ˆi ˆ i 1 ˆj ˆ 0 cos sin j 1 kˆ 0 sin cos ˆ k 1 y 1 Top View z O B Front View y z 1 x 1 y Arm Link 1 x Pendulum Link y 1 K. Craig 1

13 Angular Velocities of Links R ˆ kˆ R 1 k Velocities of CG s of Links Point A is CG of Link 1 Point C is CG of Link R A v ˆ ˆ 11 sin i 11cos j R C v sin cos cos sin sin ˆi R cos ˆi sin ˆj kˆ ˆi ˆ 1 k1 ˆi sinˆj coskˆ R cos cos sin sin cos ˆj 1 cos kˆ K. Craig 13

14 R A v 11 R C v sin 1 cos Definitions: length of link 1 = distance from pivot O to CG of link 1 distance from CG of link 1 to end of link 1 length of link = 1 1 distance from pivot B to CG of link distance from CG of link to end of link K. Craig 14

15 Lagrange s Equations Lagrange s Equations Generalized Coordinates Kinetic Energy T of System d T T V Q dt q i q i q i q q T m v I m v R A R C 1 1 z 1 1 I I sin x I cos I sin y z xy i K. Craig 15

16 Potential Energy V of the System Generalized Forces Q TB T sgn f Q B T sgn f Equations of Motion V m g 1sin 1 B T f f T motor torque viscous damping constant joint Coulomb friction constant joint B viscous damping constant joint T Coulomb friction constant joint d T T V Q dt d T T V Q dt K. Craig 16

17 Nonlinear Equations of Motion m1 11 I1 m z 1 m 1cos I cos I 1 z sin y I m sin I m cos xy 1 1 xy 1 1 I m y 1 I cos sin T B T z f sgn m I I m sin 1 x xy 1 1 [1] m1 I I y cossin m z g 1cos B Tf sgn [] K. Craig 17

18 Define: m1 11 I1 m z 1 m 1sin I sin I 1 z cos y I m cos I m sin xy 1 1 xy 1 1 I m z 1 I cos sin T B T y f sgn m I I m cos 1 x xy 1 1 m 1 I I y cossin z mg 1sinB Tf sgn [1A] [A] K. Craig 18

19 Linearization: 0 0 Operating Point m I m I I m T B z 1 1 y xy 1 1 x xy m I I m m g B [3] [4] Definitions: [5] C1 C T B [6] C3 C C4 B C m I m I C m I 1 1 C m I C 3 1 m g 4 1 z y x xy K. Craig 19

20 Transfer Functions Cs 3 C 4 T s CC 1 3 C s CC 1 4 Cs T s CC 1 3 C s CC 1 4 (neglect damping terms): State-Space Equations (neglect damping terms): q q q q q 1 CC 4 q1 C q CC 1 3 C q CC 1 3 C q q3 0 q 4 CC 1 4 q4 C CC 1 3 C CC 1 3 C T K. Craig 0

21 Model Parameter Identification Motor Parameters Masses of Links 1 and Location of CG s of Links 1 and Moment of Inertia for Link 1: I 1z1 Inertia Matrix for Link : I I I I I I I I I x xy xz yx y yz zx zy z System Friction: Coulomb and Viscous K. Craig 1

22 Motor Parameters: Given by Manufacturer K b = 0.08; back-emf constant (V-s/rad) K t = ; torque constant (N-m/A) R = ; resistance (ohms) L = 3.3E-3; inductance (H) B m = 0; viscous damping constant (N-m-s/rad) T f = 0.014; Coulomb friction (N-m) J = 4.1E-5; inertia (kg-m ) K. Craig

23 Link Masses (experimental) Link 1 (horizontal link): kg Link (pendulum link): kg Location of CG s for Links 1 and (experimental) Link 1 (horizontal link): r 0.01 m Link (pendulum link): r m K. Craig 3

24 Pendulum Link (Link ): Friction and Inertia Initially Assume Viscous Damping (Coefficient B) J ob mgr sin 0 sin J ob mgr0 B mgr 0 Jo Jo B mgr 5.48 rad/s r.188 m m kg n n n Jo Jo mgr Jo kg-m J J mr kg-m 1 ln n n o 1 n1 B J N-m-s/rad n o K. Craig 4

25 0.6 Experimental Results: Free Oscillation of the Pendulum Link 0.4 Exponential Envelope 0. theta (rad) Frequency of oscillation = 0.87 cycles/sec Friction is a combination of viscous and Coulomb time (sec) K. Craig 5

26 K. Craig 6

27 0.6 Simulation Results: Free Oscillation of the Pendulum Link theta (rad) N-m B.0E 4 rad/s T 1.8E 4 N-m f time (sec) K. Craig 7

28 Horizontal Link: Inertia Test horizontal link on a frictionless pivot Test Result: 10 cycles in 7.4 seconds Computed inertia does not include motor and slip-ring inertia; however, these are small compared to the computed inertia of the horizontal link and will be neglected J omgr sin 0 sin J omgr0 mgr n n.9 rad/s r.01 m m kg Jo mgr Jo kg-m J Jo mr kg-m n K. Craig 8

29 Horizontal Link Motor and Slip-Ring Friction: Coulomb Friction J omgr sin Tf 0 sin System was modified, J i.e., masses were added omgrtf 0 to produce oscillations. 4Tf n n 1 Does this change friction mgr value determined? How mgr can we check this? Tf n1 n 4 J kg-m o m ( ) kg = kg n 1n0.19 rad (0.911)(0.01) (0.34)(0.10) (0.34)(0.070) r ( ) T N-m f m K. Craig 9

30 theta (rad) Experimental Results: Free Oscillation of the Horizontal Link 1 = Straight-Line Envelope = = time (sec) K. Craig 30

31 K. Craig 31

32 1 Simulation Results: Free Oscillation of the Modified Horizontal Link theta (rad) time (sec) K. Craig 3

33 Pendulum Inertia Matrix: Computational Results I I I I I I I I I x xy xz yx y yz zx zy z E E E E 5 0 kg-m E 3 Experimental Result I kg-m x K. Craig 33

34 Horizontal Arm Parameters g=9.81; %m/s^ L11=-0.01; %meters L1=0.10; %meters M1=0.911; %kg I_bar_1_z1=0.0169; %kg-m^ mass moment of inertia of arm about z axis (rotation axis) through its own CG Tf_theta=0.0368; %friction torque (N-m) B_theta=0; %viscous friction Motor Parameters Pendulum Parameters MatLab Model Parameter File Kb=0.08; %back-emf constant (V-s/rad) M=0.136; %kg Kt=0.0833; %torque constant (N-m/A) L1=0.1885; %meters R_motor=1.0435; %resistance (ohms) I_bar y=.8457e-5; % kg-m^ L=3.3E-3; %inductance (H) I_bar x=3.5374e-3; %kg-m^ B_motor=0; %viscous damping constant (N-m-s/rad) I_bar z=3.5430e-3; %kg-m^ Tf_motor=0.014; %Coulomb friction (N-m) I_bar xy= e-5; kg-m^ J=4.1E-5; %inertia (kg-m^) Tf_phi=1.8E-4; % friction torque (N-m) B_phi=.0E-4; %viscous friction (N-m-s/rad) K. Craig 34

35 K. Craig 35

36 0.8 Experimental Results: Total System Response - Link theta (rad) time (sec) K. Craig 36

37 0.6 Simulation Results: Total System Response - Link theta (rad) time (sec) K. Craig 37

38 0.8 Simulation vs. Experimental Results: Total System Response - Link Experimental 0.5 theta (rad) Simulation time (sec) K. Craig 38

39 6 Experimental Results: Total System Response - Link 5 4 alpha (rad) time (sec) K. Craig 39

40 Simulation Results: Total System Response - Link 1 0 phi (rad) time (sec) K. Craig 40

41 6 Simulation vs. Experimental Results: Total System Response - Link 5 Experimental 4 alpha (rad) 3 Simulation time (sec) K. Craig 41

42 MatLab / Simulink Control Block Diagram K. Craig 4

43 Balancing Controllers Full-State Feedback Regulator Classical Control Design Swing-Up Controller Calculates the total system energy based on the kinetic energy of both links and the potential energy of the pendulum. The calculated total system energy is compared to a defined quantity of energy when the pendulum is balanced (i.e., zero energy when balanced). The difference between the desired energy and the actual energy is multiplied by an aggressivity gain and applied to the motor. K. Craig 43

44 The objective of the swing-up control exercise is to move the system from the stable equilibrium position to the unstable equilibrium position. Energy must be added to the system to achieve this swing-up action. The manipulated input to achieve this is given by the control law: d V KAEE0sgn cos dt The first two terms in the above control law are the "aggressivity" gain and the difference between actual and desired system energy. These two terms provide the magnitude of energy that has to be added to the system at any given time. K. Craig 44

45 The "aggressivity" gain determines what proportion of the available input will be used to increase or decrease the system energy. This gain could be the difference in swinging the pendulum up in 3 or 10 oscillations. The second half of the energy swing up equation determines the direction the input should be applied to increase the energy of the system. The velocity term causes the input to change directions when the pendulum stops and begins to swing in the opposite direction. The cosine term is negative when the pendulum is below horizontal and positive above horizontal. This helps the driven link to get under the pendulum and catch it. K. Craig 45

46 By controlling on energy feedback, the system automatically stops inputting excess energy and allows the system to coast to a balanced position. When the remaining potential energy required is equal to the kinetic energy, the feedback will become very small and the pendulum will coast to vertical position. By setting the desired energy to a value greater than zero, unmodeled energy dissipation effects can be overcome as the pendulum is approaching its balanced point. If this is too much, the pendulum will overshoot and the driven link will not be able to catch it. K. Craig 46

47 The switching between the controllers has a deadband of 5. When the pendulum is within ± 5 of vertical, the swing up controller will turn off. If the pendulum coasts to within ± 0 of vertical, the balance controller will be activated and the driven link will attempt to catch the pendulum. If the balance controller is not successful, the pendulum will fall and the swing up algorithm will automatically engage. K. Craig 47

48 1 Pendulum Angle Normalized 0? ? Control Torque Zero Swing-Up Control Limit Switch + or - 5 degrees 3 Balancing Control Limit Switch + or - 0 degrees Control Selection: Swing-up vs. Balance Control Selection: Swing-Up vs. Balance K. Craig 48

49 Simulation Results State-Space Control Classical Control K. Craig 49

50 Simulation Results State-Space Control Classical Control K. Craig 50