Rotary Inverted Pendulum KCC 2012 Inverted Pendulum Dynamic System Investigation K. Craig 8 Physical...

Rotary Inverted PendulumDynamic System Investigation

K. Craig 1

Mechatronic System Case

Study:Rotary Inverted

PendulumDynamic System

Investigation

Dr. Kevin CraigGreenheck Chair in Engineering Design &

Professor of Mechanical EngineeringMarquette University


K. Craig 2


K. Craig 3


K. Craig 4


K. Craig 5


K. Craig 6


K. Craig 7

Mechatronic System Case

Study:Rotary Inverted

PendulumDynamic System

Investigation

Physical System


K. Craig 8

Physical System Components

• Two Links– Motor-driven horizontal link– Un-actuated vertical pendulum link

• Permanent-magnet, brushed DC motor actuator• 24-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: 2000 cpr (horizontal link) and 5000 cpr (pendulum)


K. Craig 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 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 11

• Motor operates in the torque mode with VinKA = i and T = KTi where:– KT is the motor torque constant (N-m/A)– KA is the amplifier constant (A/V)– Vin 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-s2/rad = kg-m2)– Bf is the viscous friction coefficient (N-m-s/rad)– Tf is the Coulomb friction torque (N-m)


K. Craig 12

Physical & Mathematical Modeling

x

y

x1

y1

O

y1

z1

y2

z2

B

Top View

Front View

PendulumLink 2

ArmLink 1

Reference Frames:R: ground xyzR1: arm x1y1z1R2: pendulum x2y2z2

1

1

1

2 1

2 1

2 1

ˆ ˆi icos sin 0ˆ ˆj sin cos 0 jˆ ˆ0 0 1k k

ˆ ˆi i1 0 0ˆ ˆj 0 cos sin jˆ ˆ0 sin cosk k


K. Craig 13

• Angular Velocities of Links

• Velocities of CG’s of Links– Point A is CG of Link 1– Point C is CG of Link 2

2RR

1 1

2 2 2

ˆ ˆ ˆcos i sin j kˆ ˆi kˆ ˆ ˆi sin j cos k

R A11 11

R C1 21 21

1 21 21

21

ˆ ˆv sin i cos j

ˆv sin cos cos sin sin i

ˆcos cos sin sin cos j

ˆcos k

1RR1

ˆ ˆk k


K. Craig 14

• Definitions:

1 11 12

11

12

length of link 1 = distance from pivot O to CG of link 1 distance from CG of link 1 to end of link 1

2 21 22

21

22

length of link 2 = distance from pivot B to CG of link 2 distance from CG of link 2 to end of link 2

2R A 2 211

2R C 2 2 2 2 2 2 221 1 1 21 21

v

v 2 sin cos


K. Craig 15

Lagrange’s Equations

• Lagrange’s Equations

• Generalized Coordinates

• Kinetic Energy T of System

ii i i

d T T V Qdt q q q

1

2

qq

z1

x y z x y2 2 2 2 2

2 2R A 2 R C1 1 2

2 2 2 2 22 2 2 2

1 1 1T m v I m v2 2 2

1 I I sin I cos I sin2


K. Craig 16

• Potential Energy V of the System

• Generalized Forces

• Equations of Motion

2 21V m g 1 sin

f

f

Q T B T sgn

Q B T sgn

f

f

T motor torqueB viscous damping constant jointT Coulomb friction constant jointB viscous damping constant jointT Coulomb friction constant joint

d T T V Qdtd T T V Qdt


K. Craig 17

Nonlinear Equations of Motion

z z y1 2 2

x y x y2 2 2 2

y z2 2

2 2 2 2 2 21 11 1 2 1 2 21 2 2

22 2 1 21 2 2 1 21

22 2 21 2 f

m I m m cos I cos I sin

I m sin I m cos

I m I 2cos sin T B T sgn

[1]

x x y2 2 2

y z2 2

22 21 2 2 2 1 21

2 22 21 2 2 2 21 f

m I I m sin

m I I cos sin m g cos B T sgn

[2]


K. Craig 18

z z y1 2 2

x y x y2 2 2 2

z y2 2

2 2 2 2 2 21 11 1 2 1 2 21 2 2

22 2 1 21 2 2 1 21

22 2 21 2 f

m I m m sin I sin I cos

I m cos I m sin

I m I 2cos sin T B T sgn

[1A]

x x y2 2 2

y z2 2

22 21 2 2 2 1 21

2 22 21 2 2

2 21 f

m I I m cos

m I I cos sin

m g sin B T sgn

[2A]

Define:2


K. Craig 19

z y x y1 2 2 2

2 21 11 1 2 1 2 2 2 1 21m I m I I m T B

x x y2 2 2

22 21 2 2 2 1 21 2 21m I I m m g B

[3]

[4]

Definitions:

1 2C C T B [5]

3 2 4C C C B [6]

z y2 2

x y2 2

x2

2 21 1 11 1 2 1 2

2 2 1 21 2

23 2 21 2

4 2 21

C m I m I

C m I

C m I

C m g

Linearization: 00

Operating Point


K. Craig 20

Transfer Functions (neglect damping terms):

23 4

2 2 21 3 2 1 4

22

2 2 21 3 2 1 4

C s CT s C C C s C C

C sT s C C C s C C

State-Space Equations(neglect damping terms):

1 1 32 4

2 21 3 2 1 3 22 2

3 3

4 41 4 22 2

1 3 2 1 3 2

00 1 0 0q q CC C0 0 0

C C C C C Cq qT

q q0 0 0 1 0q qC C C0 0 0

C C C C C C

1

2

3

4

q

qqq


K. Craig 21

Model Parameter Identification• Motor Parameters• Masses of Links 1 and 2• Location of CG’s of Links 1 and 2• Moment of Inertia for Link 1:• Inertia Matrix for Link 2:

• System Friction: Coulomb and Viscous

z11I

x x y x z2 2 2 2 2

y x y y z2 2 2 2 2

z x z y z2 2 2 2 2

2 2 2

2 2 2

2 2 2

I I I

I I I

I I I


K. Craig 22

• Motor Parameters: Given by Manufacturer– Kb = 0.0822; back-emf constant (V-s/rad)– Kt = 0.0833; torque constant (N-m/A)– R = 1.0435; resistance (ohms)– L = 3.3E-3; inductance (H)– Bm = 0; viscous damping constant (N-m-s/rad)– Tf = 0.0124; Coulomb friction (N-m)– J = 4.1E-5; inertia (kg-m2)


K. Craig 23

• Link Masses (experimental)– Link 1 (horizontal link): 0.911 kg– Link 2 (pendulum link): 0.133 kg

• Location of CG’s for Links 1 and 2 (experimental)– Link 1 (horizontal link): – Link 2 (pendulum link): r 0.188 m

r 0.01 m


K. Craig 24

• Pendulum Link (Link 2): Friction and Inertia– Initially Assume Viscous Damping (Coefficient B)

o

o

o o

2n n n

o o

2 2 2o o2

n

1

n 1

J B mgr sin 0 sin

J B mgr 0B mgr 0J J

B mgr2 5.48 rad/s r .188 m m 0.133 kgJ J

mgrJ 0.0082 kg-m J J mr 0.00347 kg-m

1 lnn

2 2

n o

0.02 0.0032

B 2 J 0.00027 N-m-s/rad


K. Craig 25

0 10 20 30 40 50 60 70 80 90 100-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

time (sec)

thet

a (ra

d)

Experimental Results: Free Oscillation of the Pendulum Link

Exponential Envelope

Frequency of oscillation = 0.87 cycles/sec

Friction is a combination of viscous and Coulomb


K. Craig 26


K. Craig 27

0 10 20 30 40 50 60 70 80 90 100-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

time (sec)

thet

a (ra

d)

Simulation Results: Free Oscillation of the Pendulum Link

f

N-mB 2.0E 4 rad/s

T 1.8E 4 N-m


K. Craig 28

– Horizontal Link: Inertia• Test horizontal link on a frictionless pivot• Test Result: 10 cycles in 27.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

o

o

2n n

o

2 2 2o o2

n

J mgr sin 0 sin

J mgr 0mgr 2.29 rad/s r .01 m m 0.911 kgJ

mgrJ 0.017 kg-m J J mr 0.0169 kg-m


K. Craig 29

– Horizontal Link Motor and Slip-Ring Friction: Coulomb Friction

o f

o f

fn n 1

f n 1 n

2o

n 1 n

J mgr sin T 0 sin

J mgr T 04Tmgr

mgrT4

J 0.017 0.0036 0.0017 0.0223 kg-mm (0.911 0.3415 0.3415) kg = 1.594 kg

0.219 rad(0.911)(0.01) (0.342)(0.102) (0r

f

.342)(0.070) 0.043 m(0.911 0.3415 0.3415)

T 0.0368 N-m

System was modified, i.e., masses were added to produce oscillations.

Does this change friction value determined? How

can we check this?


K. Craig 30

0 1 2 3 4 5 6 7 8 9 10-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

time (sec)

thet

a (ra

d)

Experimental Results: Free Oscillation of the Horizontal Link

Straight-Line Envelope

1 = 0.637

2 = 0.409

3 = 0.199


K. Craig 31


K. Craig 32

0 1 2 3 4 5 6 7 8 9 10-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

time (sec)

thet

a (ra

d)

Simulation Results: Free Oscillation of the Modified Horizontal Link


K. Craig 33

• Pendulum Inertia Matrix: Computational Results

• Experimental Result

x x y x z2 2 2 2 2

y x y y z2 2 2 2 2

z x z y z2 2 2 2 2

2 2 2

2 2 2

2 2 2

2

I I I

I I I

I I I

3.5374E 3 6.5383E 5 06.5383E 5 2.8457E 5 0 kg-m

0 0 3.5430E 3

x2

22I 0.00347 kg-m


K. Craig 34

Horizontal Arm Parametersg=9.81; %m/s^2

L11=-0.01; %meters

L1=0.210; %meters

M1=0.911; %kg

I_bar_1_z1=0.0169; %kg-m^2 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 ParametersKb=0.0822; %back-emf constant (V-s/rad)

Kt=0.0833; %torque constant (N-m/A)

R_motor=1.0435; %resistance (ohms)

L=3.3E-3; %inductance (H)

B_motor=0; %viscous damping constant (N-m-s/rad)

Tf_motor=0.0124; %Coulomb friction (N-m)

J=4.1E-5; %inertia (kg-m^2)

Pendulum ParametersM2=0.1326; %kg

L21=0.1885; %meters

I_bar_2_y2=2.8457E-5; % kg-m^2

I_bar_2_x2=3.5374E-3; %kg-m^2

I_bar_2_z2=3.5430E-3; %kg-m^2

I_bar_2_x2y2=-6.5383E-5; kg-m^2

Tf_phi=1.8E-4; % friction torque (N-m)

B_phi=2.0E-4; %viscous friction (N-m-s/rad)

MatLabModel Parameter

File


K. Craig 35


K. Craig 36

0 10 20 30 40 50 60-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

time (sec)

thet

a (ra

d)

Experimental Results: Total System Response - Link 1


K. Craig 37

0 10 20 30 40 50 60-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

time (sec)

thet

a (ra

d)

Simulation Results: Total System Response - Link 1


K. Craig 38

0 10 20 30 40 50 60-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

time (sec)

thet

a (ra

d)

Simulation vs. Experimental Results: Total System Response - Link 1

Simulation

Experimental


K. Craig 39

0 10 20 30 40 50 600

1

2

3

4

5

6

time (sec)

alph

a (ra

d)

Experimental Results: Total System Response - Link 2


K. Craig 40

0 10 20 30 40 50 60-5

-4

-3

-2

-1

0

1

2

time (sec)

phi (

rad)

Simulation Results: Total System Response - Link 2


K. Craig 41

0 10 20 30 40 50 600

1

2

3

4

5

6

time (sec)

alph

a (ra

d)

Simulation vs. Experimental Results: Total System Response - Link 2

Experimental

Simulation


K. Craig 42MatLab / Simulink Control Block Diagram


K. Craig 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 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:

– 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.

A 0dV K E E sgn cosdt


K. Craig 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 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 47

– The switching between the controllers has a dead-band of 5. When the pendulum is within ± 25 of vertical, the swing up controller will turn off. If the pendulum coasts to within ± 20 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 48

Control Selection: Swing-up vs. Balance

1Control Torque

0

Zero

? 0.43633-0.43632

Limit Switch+ or - 25 degrees

? 0.34907-0.34907

Limit Switch+ or - 20 degrees

3Balancing Control

2Swing-UpControl

1Pendulum Angle

Normalized

Control Selection: Swing-Up vs. Balance


K. Craig 49

Simulation Results

State-Space Control

Classical Control


K. Craig 50

Simulation Results

State-Space Control

Classical Control

Rotary Inverted Pendulum KCC 2012 Inverted Pendulum Dynamic System Investigation K. Craig 8 Physical...

Documents

Transcript of Rotary Inverted Pendulum KCC 2012 Inverted Pendulum Dynamic System Investigation K. Craig 8 Physical...