LQR Controller for an Inverted Pendulum on a Cart - Maple Programming Help

Online Help

All Products    Maple    MapleSim


Home : Support : Online Help : Applications and Example Worksheets : Science and Engineering : applications/InvertedPendulum

LQR Controller for an Inverted Pendulum on a Cart

Introduction

This worksheet derives the equations that describe the dynamics of an inverted pendulum on a cart, creates a linear quadratic state (LQR) controller that stabilizes the position of the pendulum, and animates the motion of the controlled cart.

 

 

 

Note: This application uses the LQR command from optional Control Design Toolbox to calculate the parameters of an LQR controller. The parameters generated by this command are copied into this application. This allows you to run the application without the toolbox.

restart:withDynamicSystems:withLinearAlgebra:

Model Of Inverted Pendulum on Cart

We will use the following variables and constants in the analysis.

 

• 

xt is the position of the cart

• 

θt is the counter-clockwise angular displacement of the pendulum from the upright position

• 

φt is the angular velocity of the pendulum

• 

u(t) is horizontal force applied to the cart

• 

L is the half-length of the pendulum

• 

m is the mass of the pendulum

• 

M is the mass of the cart

• 

P is the downward force exerted by the pendulum on the cart

• 

N is the horizontal force exerted by the pendulum on the cart

• 

g is the gravitational constant

 

Let yt=ⅆⅆtxt and φt=ⅆⅆtthetat.

EQ1x.t=yt:EQ2θ.t=φt:

 r is the position of the center of mass of the pendulum.

rxtL sinθt,Lcosθt:

 

The acceleration of the center of mass is then:

accⅆ2ⅆt2r

accx..tLθ..tcosθt+Lθ.t2sinθt,Lθ..tsinθtLθ.t2cosθt

(2.1)

Rewrite the acceleration in terms of the cart velocity and angular velocity of pendulum.

rppsubsEQ1,EQ2,acc

rppy.tLφ.tcosθt+Lφt2sinθt,Lφ.tsinθtLφt2cosθt

(2.2)

 

Apply F=ma in the horizontal (x-direction) to the pendulum.

eq1N=m rpp1

eq1N=my.tLφ.tcosθt+Lφt2sinθt

 

Apply F=ma in the direction perpendicular to the pendulum.

eq2P+gsinθtNcosθt=mLφ.tmy.t

eq2P+gsinθtNcosθt=mLφ.tmy.t

 

Apply F=ma in the horizontal direction to the cart.

eq3utN=My.t

eq3utN=My.t

 

Apply M=Ialpha to the pendulum, where M is the sum of all moments about the pendulum's center of mass, I is the pendulum's moment of inertia, and alpha is its angular acceleration.

inertia13mL2:eq4PLsinθtNLcosθt=inertia φ.t

eq4PLsinθtNLcosθt=mL2φ.t3

 

eqnssolveeq1,eq2,eq3,eq4,N,P,φ.t,y.t

eqnsN=2LMφt2sinθtm3Mcosθtsinθtg3utcosθtm+2utm3cosθtm+2M+2m,P=2LMcosθtφt2sinθtmLφt2sinθtm23Mcosθt2sinθtg3utcosθt2m+sinθtgM+2utcosθtm+sinθtgm+utm3cosθtm+2M+2msinθt,φ.t=3Lφt2sinθtm2sinθtgMsinθtgmutm3cosθtm+2M+2mmL,y.t=2Lφt2sinθtm3cosθtsinθtg2ut3cosθtm+2M+2m

EQ3y.t=evaly.t,eqns

EQ3y.t=2Lφt2sinθtm3cosθtsinθtg2ut3cosθtm+2M+2m

(2.3)

EQ4φ.t=evalφ.t,eqns

EQ4φ.t=3Lφt2sinθtm2sinθtgMsinθtgmutm3cosθtm+2M+2mmL

 

The final nonlinear model is:

sysEqsVectorEQ1,EQ2,EQ3,EQ4

sysEqsx.t=ytθ.t=φty.t=2Lφt2sinθtm3cosθtsinθtg2ut3cosθtm+2M+2mφ.t=3Lφt2sinθtm2sinθtgMsinθtgmutm3cosθtm+2M+2mmL

Linearization and State Space Model

Assign values to the parameters.

param_valuesg=9.8,M=10.,m=2.,L=3.0:assignparam_values

The linearization point:

lin_point  xt=0, yt=0, thetat=0, ut = 0,phit=0 :


Linearize the system and create a state-space model.

sysLinLinearizeconvertsysEqs,list, ut,  xt,  thetat, lin_point

sysLinState Spacecontinuous2 output(s); 1 input(s); 4 state(s)inputvariable=u1toutputvariable=y1t,y2tstatevariable=x1t,x2t,x3t,x4t,x1t=φt,x2t=θt,x3t=xt,x4t=yt,u1t=ut,y1t=θt,y2t=xt

(3.1)


Note that the states are in the order given in z.

zφ(t),θ(t),x(t),y(t):


Hence, the A and B matrices are:

AsysLin1:-a

A0.03.2666666670.00.01.00.00.00.00.00.00.01.00.01.6333333330.00.0

(3.2)

BsysLin1:-b

B0.055555555560.00.00.1111111111

(3.3)

The transfer function of the system is:

sysTFTransferFunctionsysLin1;tfsysTF:-tf

sysTFTransfer Functioncontinuous2 output(s); 1 input(s)inputvariable=u1soutputvariable=y1s,y2s

tf0.0555555555599999973s23.266666666999999970.1111111111s20.2722222222s43.266666667s2

(3.4)

Controllability and Observability Matrices

CCControllabilityMatrixsysLin1

CC0.055555555560.00.18148148151451850.00.00.055555555560.00.18148148151451850.00.11111111110.00.090740740729481480.11111111110.00.090740740729481480.0

(4.1)

OOObservabilityMatrixsysLin1

OO010000101.0.0.0.0.0.0.1.0.3.266666666999999970.0.0.1.633333332999999940.0.3.266666666999999970.0.0.1.633333332999999940.0.0.

(4.2)

 

The controllability and observability matrices have full rank; hence, the system is controllable and observable.

RankCC;RankOO

4

4

 

The eigenvalues of A:

fnormalEigenvaluesA

−1.807392228+0.I1.807392228+0.I0.I0.I

 

The poles of the transfer function are:

fsolvedenomtf2,1,s;

−1.807392228,0.,0.,1.807392228

 

The eigenvalues are equal to the poles of the transfer function since the system is controllable and observable. Since there is an eigenvalue in the left half plane, the system is unstable.

LQR Controller Design

Specify the weights on the input variables.

QDiagonalMatrix1,0,1,0:RIdentityMatrix1:


Note: The following command uses the optional Control Design Toolbox. The output of this command is copied below so that you can use this application without the toolbox.

#K≔ControlDesign:-LQRsysLin1,Q,RK84.4997540652218147.7682181173301.6.02607321694750:


Hence, the controlled input is:

controllerut=K·z1

controllerut=84.4997540652218φt147.768218117330θt+1.xt+6.02607321694750yt

Simulate the Controlled System

Substitute the controller into the system equations.

sysEqsControllerevalsysEqs,controller

sysEqsControllerx.t=ytθ.t=φty.t=2Lφt2sinθtm3cosθtsinθtg+168.9995081φt+295.5364362θt2.xt12.05214643yt3cosθtm+2M+2mφ.t=3Lφt2sinθtm2sinθtgMsinθtgm84.4997540652218φt147.768218117330θt+1.xt+6.02607321694750ytm3cosθtm+2M+2mmL

 

The initial conditions (note that the angle of the pendulum is not vertical):

icsx0=0,θ0=0.25 π,y0=0,φ0=0:

resdsolvesysEqsController1,sysEqsController2,sysEqsController3,sysEqsController4,ics,φt,θt,xt,yt,type=numeric,output=listprocedure:

res_xevalxt,res;res_thetaevalθt,res

res_xproct...end proc

res_thetaproct...end proc

The angular displacement of the pendulum with respect to time.

plots:-odeplotres,t,θt,0..20,background=ColorTools:-ColorRGB,218/255,223/255,225/255,axis=gridlines=color=RGB1,1,1,size=800,400,axesfont=Calibri,labelfont=Calibri

 

The cart displacement with respect to time.

plots:-odeplotres,t,xt,0..20,background=ColorTools:-ColorRGB,218/255,223/255,225/255,axis=gridlines=color=RGB1,1,1,size=800,400,axesfont=Calibri,labelfont=Calibri

 

The pendulum returns to its upright position and the cart returns to its initial position, as intended by the design of the controller.

Animation of Controlled Cart

pendplotprocx,t        uses plottools,plots,ColorTools:local pt1,pt2,pt3,pt4,pendulum,wheel1,wheel2,cart,w,r1,r2,y0;cart  rectanglex1,1,x+1,0,color=ColorRGB,0/255,79/255,121/255,style=surface;wheel1  diskx0.6,0,0.25,color=black;wheel2  diskx+0.6,0,0.25,color=black;w0.1;r13.8;r20.2;y00.7;pt1x  r1sint+w cost,y0+r1cost+ wsint;pt2x  r1sint  w cost,y0+r1cost wsint;pt3x  r2sint  w cost, y0  r2cost  wsint;pt4x  r2sint+w cost, y0  r2cost+ wsint;pendulum  polygonpt1,pt2,pt3,pt4,color=ColorRGB,150/255,40/255,27/255,style=surface;displaypendulum,wheel1,wheel2,cartend proc:

plots:-animatependplot,res_xt,res_thetat,t=0..25,frames=100, scaling=constrained,axes=none