Rotary Inverted Pendulum

The objective of this project is to design a controller which is capable of driving the pendulum from its “hanging-down” position to upright position and then holding it there.. Design,

1 Objectives

Control of an inverted pendulum is one of the most interesting and classical problems for control engineering The objective of this project is to design a controller which is capable of driving the pendulum from its “hanging-down” position to upright position and then holding it there The experiment system, as shown in Fig 1, consists of a vertical pendulum, a horizontal arm, a gear chain, and a servomotor which drives the pendulum through the gear transmission system The rotating arm is mounted on the output gear

of the gear chain An encoder is attached to the arm shaft to measure the rotating angle of the arm At the end of the rotating arm there is a hinge instrumented with an encoder The pendulum is attached to the hinge

Fig 1 Rotary inverted pendulum system

On the project, you are asked to do the following:

Derive a nonlinear model of the rotary inverted pendulum system

Derive a linearized model of the same system in the neighborhood of the upright position

Verify the linear and non-linear models to see if they are indeed true models of the physical system over a specific operating range

Design, implement, and simulate a stabilizing controller which maintains the pendulum in the upright position with some robustness for small disturbance to the pendulum

Design and implement a mode controller which activates the stabilizing controller when the pendulum is placed in a small neighborhood of the upright position

Design and implement a destabilizing controller which swings up the pendulum to near upright position, so that the stabilizing controller can be activated to stabilize the pendulum

2 System requirements

To complete this project, the following hardware and software are required:

1) A Quanser UPM 1503 universal power module

2) A Quanser Q4 data acquisition card

3) A Quanser terminal card

3) A Quanser SRV02-ET servomotor and the geared driving unit

4) A Quanser rotary inverted pendulum

5) A PC

6) Quanser’s WinCon 5.0 software

7) Matlab/Simulink and control system toolbox

All of the above-mentioned hardware and software are provided in the Robotics lab (JH608)

3 Modeling of the inverted pendulum system

The inverted pendulum (mechanical part only) is sketched in Fig 2, α and θ are employed as the generalized coordinates to describe the inverted pendulum system The pendulum is displaced with a given

α while the arm rotates an angle of θ In this project, we assume that θ ≡θl where the latter has been used in Project I We assume the pendulum to be a lump mass at point B which is located at the geometric center of the pendulum The xyz frame is fixed to the arm at pointA For a complete listing of the symbols used in the math formulation, please refer to Appendix A

z y

Fig 2 Simplified model of the rotary inverted pendulum system

3.1 Using Free Body Diagram method

The Free Body Diagram of the inverted pendulum (mechanical part only) system is shown in Fig 3 Note that the arm rotates in the horizontal plane (xz plane) only and the pendulum rotates in the vertical plane (xy plane) only, we can draw the forces in these two planes only to simplify the drawing

Referring back to Fig 2, we notice that the velocity of pointB on the pendulum relative to that of point

A on the arm is

αα

αα

)cos(

Ly

Lx

ααθ

)cos(

Ly

Lrx

A x

y B

α

ααα

αθ

)cos(

)cos(

)sin(

2 2

LL

y

LL

rx

=

(3)

Applying Newton’s 2nd Law to the pendulum in x direction, we obtain

x x

B

AmL

mLmg

mgAmL

mLF

ym

α

ααα

)cos(

)sin(

)cos(

)cos(

3

1

)sin(

)cos(

)2(121

2 2

αα

α

αα

αα

LAL

AmL

LAL

ALmM

J

y x

y x

B B

JOθ&=∑ O ⇒ eqθ&= l − eqθ&− x (7) Substituting (4) and (5) into (6), we are left

0)sin(

3

4)cos(

))(sin)

cos(

)sin(

)sin(

(

))(cos)

cos(

)sin(

)cos(

(3

1

)sin(

))sin(

)cos(

(

)cos(

))cos(

)sin(

(3

1

2

2 2 2 2

2 2 2 2

2

2

2 2

=

−+

−

⇒

−

−+

−+

=

⇒

−

−+

−+

=

αα

θα

ααα

ααα

ααα

ααθ

αα

αααα

α

αααα

αθ

α

mgLmL

mLr

mLmL

mgL

mLmL

mLrmL

LmL

mLmg

LmL

mLmrmL

αα

αθ

ααα

αθ

θθ

ααα

αθ

θθ

eq l eq

eq l eq

BTmLr

mLrmr

J

mLrmLr

mrB

TJ

rmL

mLmrBTJ

−

=+

−+

⇒

−+

−

−

=

2 2

2 2

)sin(

)cos(

)(

))cos(

)sin(

(

))cos(

)sin(

(

(9)

Combining (8) and (9), we obtain the motion of equation of the system

0)sin(

3

4)cos(

)sin(

)cos(

)(

2

2 2

=

−+

−

−

=+

−+

αα

θα

θα

αα

αθ

mgLmL

mLr

BTmLr

mLrmr

3.2 Using Lagranian Formulation

The kinetic energy of the mechanical system arising from the rotating arm and pendulum is

αθαα

θ

αα

αα

αθ

θ

αθ

3

2)(

2

1

2

1]))sin(

())cos(

[(

2

12

1

2

1)(

2

12

1

2 2 2 2

2 2

2 2

2 2

2 2

mLrmL

mrJ

JL

LrmJ

JyxmJ

T

eq

B eq

B B

B eq

−+

+

=

+

−+

−+

=

+++

1

mLL

m

JB = = is the moment of inertia of the pendulum about its center of mass Taking the horizontal plane where the arm lies as the datum plane, the only potential energy in the mechanical system is gravity, i.e.,

)cos(α

mgL

V = (12) Since we have two generalized coordinates, θ and α, we therefore have two equations according to Lagrangian Formulation

0)

(

)(

=

∂

∂+

θθ

θθ

VTTdtd

BTVTTdt

d

eq l

ηη

η

θη

θη

η

θη

η

θη

m g t g m m m

g t g m

m g g m

g m m g t g m

g m m t m g g

m m m g g l

JKR

KKKV

R

KK

JKR

KKVKK

KJIKK

JTKT

2 2

2

)(

)(

)cos(

)sin(

)

=

−+

−

=++

−

αα

θα

θααα

αθ

dcb

fVeb

g t g m

m

m g t g m eq

m g g eq

R

KKf

R

KKKB

e

mgLd

mLc

mLrb

JKmrJa

ηη

ηηη

=

2

2

34

−

=+

−

ααθ

θαθ

dcb

fVeb

))cos(

)cos(

)cos(

)sin(

)sin(

()(cos

1

)sin(

)cos(

)sin(

)(cos1

))

cos(

)sin(

)sin(

()(cos

1

)sin(

)cos(

)sin(

)(cos1

2 2

2 2

2 2

2

2 2

2

2 2

2

m m

m m

Vbfbe

bad

bac

db

fVeb

ab

ac

cfVcebd

bcb

ac

cd

bfVeb

bac

αθ

αα

ααα

α

αα

θααα

α

θααα

αα

α

αθ

ααα

−

−

=

−+

11

)(

11

2 2

2 2

m m

m m

bfVbeadbacd

b

fVeabac

cfVcebdbaccd

bfVebac

−

−

=

θαα

θα

θαα

θθ

)()()()(

2 2

2 2

=Α

−Α+Θ

−

=Θ+Α

−Θ

sdsscssb

sfVssessbss

bfss

Vs

Α

2 3 4 2

2

)()()(

(21) From the above transfer function, it can be seen that there is a pole and a zero at the origin They can be canceled from each other and the resulting transfer function becomes

deadscessbac

bfss

Vs

Α

2 3

2)()()(

(22) This is our final model of the inverted pendulum system for designing the controller Note that this transfer function represents the linearized model only

4 Verification of the mathematical model

4.1 Verification of the linear model against the nonlinear model

In order to get some sense about how well the linearized model represents the original nonlinear system,

we are going to simulate the dynamics of the system using both the linear and non-linear models and then compare their simulation outputs The simulation will not only verify the linear model, but also establish a threshold for us to know the threshold (on α ) of the linear model

The main Simulink diagram of comparing the linear model with the nonlinear model is shown in Fig 4 The nonlinear and linear models are shown in Figs 5 and 6, respectively α is given an initial condition (0.00001(rad)) and thus the pendulum is allowed to fall As one can see from the simulation results shown

in Fig 7, the linear model correctly depicts the motion of the pendulum for the first 1.4 seconds and then begin to break down Zooming in the plots around 1.4 seconds, we saw that the linear model quite accurately described the system for the first 15 degrees and then began to diverge from the actual motion

Fig 4 Main diagram of verifying the linear model

Fig 5 Diagram of the nonlinear model

Fig 6 Diagram of the linear model

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8-20

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.80

Fig 7 Overlapped plots of the simulation outputs from the linear and nonlinear models

4.2 Verification of both linear and nonlinear models against hardware

In order to get some sense about how well the dynamics models represent the real hardware system, we are going to compare the outputs of both the linear model and the nonlinear model with hardware data To simplify the verification, we do the experiment near the “hanging-down” position Replacing α with

π

α+ in (18), we obtain the nonlinear model of the pendulum system at the “hanging-down” position:

))cos(

)cos(

)cos(

)sin(

)sin(

()(cos1

))

cos(

)sin(

)sin(

()(cos1

2 2

2 2

2 2

2

m

m

Vbfbe

bad

bac

cfVcebd

bcb

ac

αθ

αα

ααα

αα

θααα

αα

θ

−+

1

)(

1

2 2

m

m

bfVbeadbac

cfVcebdbac

−+

θαθ

Fig 8 Main diagram of verifying both the linear model and the nonlinear model

Fig 9 Interface to the inverted pendulum system

Fig 10 Liner model of the inverted pendulum system

Fig 11 Nonliner model of the inverted pendulum system

In the experiment, a ramp signal with a slope of 2, as shown in Fig 12, is used as the input voltage of the system From the plots of θ and α, as shown in Fig 13, we can see that the responses of the models are similar to these of the physical system They have almost the same shape The responses of the physical system have some delay due to the friction in the physical system as explained in project I

0 0.2 0.4 0.6 0.8 1 1.2 0

Fig 13 Plots of the verifying both the linear model and the nonlinear model with the physical system

5 Destabilizing controller

The controller of the whole system consists of three parts: destabilizing controller, stabilizing controller, and mode controller The destabilizing controller, as the name implies, oscillates the arm until it has built up enough energy to break the initial stable (hanging-down) state and get the pendulum into an almost upright but unstable state Then the stabilizing controller is turned on to stabilize the pendulum in its

upright sate The mode controller determines when to switch between the destabilizing controller and stabilizing controller We will discuss the design and implementation of the destabilizing controller in this section The mode controller and the stabilizing controller will be discussed in the next two sections Destabilizing controller will essentially drive the position of the arm in order to get away from the stable “hanging-down” position of the pendulum It simply makes sense that, by moving the arm back and forth strongly enough, it can eventually swing up the pendulum Hence, the first thing we need to do is to design a position controller which can swing the arm to achieve the destabilizing goal

5.1 Position controller

5.1.1 Design of the position controller

The pendulum in the system has a length of2L=0.335(m)and its center of mass is located at its geometric center Thus the natural frequency for small oscillations of the pendulum is given by

)(rad/

628.64

3

sL

gI

0.780(rad/s),

512.26

or

%2

%OS ,

where %OS is the maximum overshoot of the response for a step input

For the arm to track the desired position, we design a PD control law

θθ

v d

p

V = ( − )− (27) This is a position control loop that controls the voltage applied to the motor so that θ tracks θ with zero ddesired velocity Now we need to determine KpandKv according the above defined specifications (26) The closed-loop transfer function of the input and output is

p t g m g v t g m g g t m m g m eq m

eq

p t g m g

KKK

ηηη

ηη

η

ηηθ

θ

++

++

eq

p t g m g

n m

eq

v t g m g g t m m g m eq

RJ

KKK

RJ

KKKK

KKR

B

ωη

η

ζωη

ηη

η

=

=+

+

(29)

Solving (29), we obtain

585.02

m eq n p

t g m g

g t m m g m eq m eq n v

KK

RJK

KK

KKKR

BRJK

ηηω

ηη

ηηζ

ω

(30)

With these values of the control grains, we expect the arm tracks the desired position and velocity with the required specifications

5.1.2 Simulation of the position controller

The main Simulink diagram of the simulation of the position controller is shown in Fig 14 and the Simulink diagram of the servomotor and gear transmission system is shown in Fig 15 Given the required specificationsωn =26.512(rad/s),%OS=2%, the step response of the closed-loop system is shown in Fig

17 We can see that the response has a maximum overshoot of 2% and the first peak at 0.189 second So the position controller meets the required specifications

Fig 14 Main diagram for simulation of the position controller

Fig 15 Model of the servomotor and gear transmission system

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 -2

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0

Fig 17 System response due to a step input 5.1.3 Implementation of the position controller

The main Simulink diagram of the implementation of the position controller is shown in Fig 18 and the interface to the servomotor and gear transmission system is shown in Fig 19 The model of the servomotor and gear transmission system is shown in Fig 15 An example step response of the system is shown in Fig

21 In this example, the desired angular position of the arm is set to 20 degrees

Fig 18 Main diagram for implementation of the position controller

Fig 19 Interface to the servomotor and gear transmission system

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -2

Fig 21 Simulated and real dynamic responses of the system to a step input

5.2 Destabilizing controller

Many schemes can be devised to gradually swing up the pendulum In this project we will design a positive feedback controller to destabilize the pendulum and eventually swing up it Notice that we have a useful application of positive feedback here Assume the arm position can be commanded via θ Then the dfeedback

αα

θd =P +D& (31) can be made to destabilize the system with the proper choice of the gains PandD This means that we want to command the arm based on the position and velocity of the pendulum Moreover, by limiting θ , d

we can ensure that the arm does not reach a position that will cause a collision with the nearby hardware (e.g., the table) The gains PandDare crucial in bring up the pendulum smoothly Based on the experiment, we choose P=0.5(deg/deg)andD=0.00001deg/(deg/s) One can tune the value of D to adjust the “damping” in the system

The main Simulink diagram of the destabilizing controller using positive feedback is shown in Fig 22 The diagram of the rotary inverted pendulum is shown in Fig 23 From the plot of alpha in Fig 24, we can see that the pendulum is brought up (the curve passing through the α=0 line) in about 1.25 seconds

Fig 22 Main diagram of the Destabilizing controller using positive feedback

Fig 23 Interface to the rotary inverted pendulum system

6 Mode controller

The purpose of the mode controller is to track the pendulum angle α and facilitate switching between the destabilizing controller and stabilizing controller This controller is to be enabled when α is in the neighborhood of zero, within the threshold of α (currently set to 10 degrees)

The Simulink diagram of the simulation of the mode controller is shown in Fig 26 From the simulation results shown in Fig 27 we can see that the mode controller works very well It output 1 when degrees

10

≤

α and 0 whenα >10degrees

Fig 26 Simulink diagram of the mode controller

Fig 27 Plots of the input signal and the mode signal

7 Stabilizing controller by feeding back both θ and α

If we can feed back both θ and α angles, we can calculate the control signal using both of them Assuming the pendulum is almost upright, two PD controllers can be implemented to maintain it at the upright position (capable of rejecting disturbances up to a certain extent) The PD controller for θ is

θθ

Kthetautheta_ = _ ( − )+ _ (32) where θ is the desired position of the rotating arm after balancing The PD controller for α is d

αα

Kalphaualpha_ = _ ( − )+ _ (33) where αd ≡0 The control signal (i.e., the input voltage of the motor) is then given by

uthetaualpha

u= _ − _ (34) Based on the experiment, we choose