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
K. Craig
K. Craig 3
K. Craig 4
K. Craig 5
K. Craig 6
Mechatronic System Case Study: Rotary Inverted Pendulum Dynamic System Investigation Physical System K. Craig 7
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
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
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
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
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ˆ 0 0 1 ˆ 1 k ˆi ˆ 1 0 0 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
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 1 1 1 1 1 1 1 cos cos sin sin cos ˆj 1 cos kˆ K. Craig 13
R A v 11 R C v 1 1 11 sin 1 cos Definitions: length of link 1 = 11 1 1 11 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
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 1 1 1 1 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
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
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
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
Linearization: 0 0 Operating Point m I m I I m T B 1 11 1z 1 1 y xy 1 1 x xy m I I m m g B 1 1 1 1 [3] [4] Definitions: [5] C1 C T B [6] C3 C C4 B C m I m I 1 1 11 1 1 C m I 1 1 C m I C 3 1 m g 4 1 z y x xy K. Craig 19
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 1 3 4 0 1 0 0 0 q 1 CC 4 q1 C 3 0 0 0 q CC 1 3 C q CC 1 3 C q 3 0 0 0 1 q3 0 q 4 CC 1 4 q4 C 0 0 0 CC 1 3 C CC 1 3 C T K. Craig 0
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
Motor Parameters: Given by Manufacturer K b = 0.08; back-emf constant (V-s/rad) K t = 0.0833; torque constant (N-m/A) R = 1.0435; 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
Link Masses (experimental) Link 1 (horizontal link): 0.911 kg Link (pendulum link): 0.133 kg Location of CG s for Links 1 and (experimental) Link 1 (horizontal link): r 0.01 m Link (pendulum link): r 0.188 m K. Craig 3
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 0.133 kg n n n Jo Jo mgr Jo 0.008 kg-m J J mr 0.00347 kg-m 1 ln n n o 1 n1 B J 0.0007 N-m-s/rad n o 0.0 0.003 K. Craig 4
0.6 Experimental Results: Free Oscillation of the Pendulum Link 0.4 Exponential Envelope 0. theta (rad) 0-0. -0.4-0.6 Frequency of oscillation = 0.87 cycles/sec Friction is a combination of viscous and Coulomb -0.8 0 10 0 30 40 50 60 70 80 90 100 time (sec) K. Craig 5
K. Craig 6
0.6 Simulation Results: Free Oscillation of the Pendulum Link 0.4 0. theta (rad) 0-0. -0.4-0.6 N-m B.0E 4 rad/s T 1.8E 4 N-m f -0.8 0 10 0 30 40 50 60 70 80 90 100 time (sec) K. Craig 7
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 0.911 kg Jo mgr Jo 0.017 kg-m J Jo mr 0.0169 kg-m n K. Craig 8
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 0.017 0.0036 0.0017 0.03 kg-m o m (0.9110.3415 0.3415) kg = 1.594 kg n 1n0.19 rad (0.911)(0.01) (0.34)(0.10) (0.34)(0.070) r (0.9110.3415 0.3415) T 0.0368 N-m f 0.043 m K. Craig 9
theta (rad) 1 0.8 0.6 0.4 0. 0-0. Experimental Results: Free Oscillation of the Horizontal Link 1 = 0.637 Straight-Line Envelope = 0.409 3 = 0.199-0.4-0.6-0.8-1 0 1 3 4 5 6 7 8 9 10 time (sec) K. Craig 30
K. Craig 31
1 Simulation Results: Free Oscillation of the Modified Horizontal Link 0.8 0.6 0.4 theta (rad) 0. 0-0. -0.4-0.6-0.8 0 1 3 4 5 6 7 8 9 10 time (sec) K. Craig 3
Pendulum Inertia Matrix: Computational Results I I I I I I I I I x xy xz yx y yz zx zy z 3.5374E 3 6.5383E 5 0 6.5383E 5.8457E 5 0 kg-m 0 0 3.5430E 3 Experimental Result I 0.00347 kg-m x K. Craig 33
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=-6.5383e-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
K. Craig 35
0.8 Experimental Results: Total System Response - Link 1 0.7 0.6 0.5 theta (rad) 0.4 0.3 0. 0.1 0-0.1 0 10 0 30 40 50 60 time (sec) K. Craig 36
0.6 Simulation Results: Total System Response - Link 1 0.5 0.4 theta (rad) 0.3 0. 0.1 0-0.1 0 10 0 30 40 50 60 time (sec) K. Craig 37
0.8 Simulation vs. Experimental Results: Total System Response - Link 1 0.7 0.6 Experimental 0.5 theta (rad) 0.4 0.3 0. 0.1 Simulation 0-0.1 0 10 0 30 40 50 60 time (sec) K. Craig 38
6 Experimental Results: Total System Response - Link 5 4 alpha (rad) 3 1 0 0 10 0 30 40 50 60 time (sec) K. Craig 39
Simulation Results: Total System Response - Link 1 0 phi (rad) -1 - -3-4 -5 0 10 0 30 40 50 60 time (sec) K. Craig 40
6 Simulation vs. Experimental Results: Total System Response - Link 5 Experimental 4 alpha (rad) 3 Simulation 1 0 0 10 0 30 40 50 60 time (sec) K. Craig 41
MatLab / Simulink Control Block Diagram K. Craig 4
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
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
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
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
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
1 Pendulum Angle Normalized 0? 0.43633-0.4363? 0.34907-0.34907 1 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
Simulation Results State-Space Control Classical Control K. Craig 49
Simulation Results State-Space Control Classical Control K. Craig 50