LQR Controller for an Inverted Pendulum on a Cart - Maple Help
For the best experience, we recommend viewing online help using Google Chrome or Microsoft Edge.

Online Help

All Products    Maple    MapleSim


Home : Support : Online Help : Applications and Example Worksheets : Science and Engineering : LQR Controller for an Inverted Pendulum on a Cart

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 y⁡t=ⅆⅆt⁢x⁡t and φt=ⅆⅆt⁢theta⁡t.

> 

EQ1≔x.t=yt:EQ2≔θ.t=φt:

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

> 

r≔xt−L⋅ sinθ⁡t,L⋅cosθ⁡t:

 

The acceleration of the center of mass is then:

> 

acc≔ⅆ2ⅆt2r

acc≔x..⁡t−L⁢θ..⁡t⁢cos⁡θ⁡t+L⁢θ.⁡t2⁢sin⁡θ⁡t,−L⁢θ..⁡t⁢sin⁡θ⁡t−L⁢θ.⁡t2⁢cos⁡θ⁡t

(2.1)

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

> 

rpp≔subsEQ1,EQ2,acc

rpp≔y.⁡t−L⁢φ.⁡t⁢cos⁡θ⁡t+L⁢φ⁡t2⁢sin⁡θ⁡t,−L⁢φ.⁡t⁢sin⁡θ⁡t−L⁢φ⁡t2⁢cos⁡θ⁡t

(2.2)

 

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

> 

eq1≔N=m ⋅rpp1

eq1≔N=m⁢y.⁡t−L⁢φ.⁡t⁢cos⁡θ⁡t+L⁢φ⁡t2⁢sin⁡θ⁡t

 

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

> 

eq2≔P+g⁢sin⁡θ⁡t−N⁢cos⁡θ⁡t=m⁢L⁢φ.⁡t−m⁢y.⁡t

eq2≔P+g⁢sin⁡θ⁡t−N⁢cos⁡θ⁡t=m⁢L⁢φ.⁡t−m⁢y.⁡t

 

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

> 

eq3≔u⁡t−N=M⁢y.⁡t

eq3≔u⁡t−N=M⁢y.⁡t

 

Apply M=I⁢alpha 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.

> 

inertia≔13⁢m⁢L2:eq4≔P⁢L⁢sinθ⁡t−N⁢L⁢cosθ⁡t=inertia⋅ φ.t

eq4≔P⁢L⁢sin⁡θ⁡t−N⁢L⁢cos⁡θ⁡t=m⁢L2⁢φ.⁡t3

 

> 

eqns≔solveeq1,eq2,eq3,eq4,N,P,φ.t,y.t

eqns≔N=2⁢L⁢M⁢φ⁡t2⁢sin⁡θ⁡t⁢m−3⁢M⁢cos⁡θ⁡t⁢sin⁡θ⁡t⁢g−3⁢u⁡t⁢cos⁡θ⁡t⁢m+2⁢u⁡t⁢m−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m,P=2⁢L⁢M⁢cos⁡θ⁡t⁢φ⁡t2⁢sin⁡θ⁡t⁢m−L⁢φ⁡t2⁢sin⁡θ⁡t⁢m2−3⁢M⁢cos⁡θ⁡t2⁢sin⁡θ⁡t⁢g−3⁢u⁡t⁢cos⁡θ⁡t2⁢m+sin⁡θ⁡t⁢g⁢M+2⁢u⁡t⁢cos⁡θ⁡t⁢m+sin⁡θ⁡t⁢g⁢m+u⁡t⁢m−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m⁢sin⁡θ⁡t,φ.⁡t=−3⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m2−sin⁡θ⁡t⁢g⁢M−sin⁡θ⁡t⁢g⁢m−u⁡t⁢m−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m⁢m⁢L,y.⁡t=−2⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m−3⁢cos⁡θ⁡t⁢sin⁡θ⁡t⁢g−2⁢u⁡t−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m

> 

EQ3≔y.t=evaly.⁡t,eqns

EQ3≔y.⁡t=−2⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m−3⁢cos⁡θ⁡t⁢sin⁡θ⁡t⁢g−2⁢u⁡t−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m

(2.3)

> 

EQ4≔φ.t=evalφ.⁡t,eqns

EQ4≔φ.⁡t=−3⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m2−sin⁡θ⁡t⁢g⁢M−sin⁡θ⁡t⁢g⁢m−u⁡t⁢m−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m⁢m⁢L

 

The final nonlinear model is:

> 

sysEqs≔VectorEQ1,EQ2,EQ3,EQ4

sysEqs≔x.⁡t=y⁡tθ.⁡t=φ⁡ty.⁡t=−2⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m−3⁢cos⁡θ⁡t⁢sin⁡θ⁡t⁢g−2⁢u⁡t−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢mφ.⁡t=−3⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m2−sin⁡θ⁡t⁢g⁢M−sin⁡θ⁡t⁢g⁢m−u⁡t⁢m−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m⁢m⁢L

Linearization and State Space Model

Assign values to the parameters.

> 

param_values≔g=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.

> 

sysLin≔LinearizeconvertsysEqs,list, ut,  xt,  thetat, lin_point

sysLin≔State Spacecontinuous2 output(s); 1 input(s); 4 state(s)inputvariable=u1⁡toutputvariable=y1⁡t,y2⁡tstatevariable=x1⁡t,x2⁡t,x3⁡t,x4⁡t,x1⁡t=φ⁡t,x2⁡t=θ⁡t,x3⁡t=x⁡t,x4⁡t=y⁡t,u1⁡t=u⁡t,y1⁡t=θ⁡t,y2⁡t=x⁡t

(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:

> 

A≔sysLin1:-a

> 

B≔sysLin1:-b

The transfer function of the system is:

> 

sysTF≔TransferFunctionsysLin1;tf≔sysTF:-tf

sysTF≔Transfer Functioncontinuous2 output(s); 1 input(s)inputvariable=u1⁡soutputvariable=y1⁡s,y2⁡s

tf≔0.0555555555599999973s2−3.266666666999999970.1111111111⁢s2−0.2722222222s4−3.266666667⁢s2

(3.2)

Controllability and Observability Matrices

> 

CC≔ControllabilityMatrixsysLin1

> 

OO≔ObservabilityMatrixsysLin1

OO≔010000101.0.0.0.0.0.0.1.0.3.266666666999999970.0.0.1.633333332999999940.0.3.266666666999999970.0.0.1.633333332999999940.0.0.

(4.1)

 

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

> 

RankCC;Rank⁡OO

4

4

 

The eigenvalues of A:

> 

fnormal⁡Eigenvalues⁡A

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

> 

Q≔DiagonalMatrix1,0,1,0:R≔IdentityMatrix1:


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,RK≔84.4997540652218147.768218117330−1.−6.02607321694750:


Hence, the controlled input is:

> 

controller≔ut=−K·z1

controller≔u⁡t=−84.4997540652218⁢φ⁡t−147.768218117330⁢θ⁡t+1.⁢x⁡t+6.02607321694750⁢y⁡t

Simulate the Controlled System

Substitute the controller into the system equations.

> 

sysEqsController≔evalsysEqs,controller

sysEqsController≔x.⁡t=y⁡tθ.⁡t=φ⁡ty.⁡t=−2⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m−3⁢cos⁡θ⁡t⁢sin⁡θ⁡t⁢g+168.9995081⁢φ⁡t+295.5364362⁢θ⁡t−2.⁢x⁡t−12.05214643⁢y⁡t−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢mφ.⁡t=−3⁢L⁢φ⁡t2⁢sin⁡θ⁡t⁢m2−sin⁡θ⁡t⁢g⁢M−sin⁡θ⁡t⁢g⁢m−−84.4997540652218⁢φ⁡t−147.768218117330⁢θ⁡t+1.⁢x⁡t+6.02607321694750⁢y⁡t⁢m−3⁢cos⁡θ⁡t⁢m+2⁢M+2⁢m⁢m⁢L

 

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

> 

ics≔x0=0,θ0=0.25 π,y0=0,φ0=0:

> 

res≔dsolvesysEqsController1,sysEqsController2,sysEqsController3,sysEqsController4,ics,φ⁡t,θ⁡t,x⁡t,y⁡t,type=numeric,output=listprocedure:

> 

res_x≔evalxt,res;res_theta≔evalθt,res

res_x≔proct...end proc

res_theta≔proct...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,x⁡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 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

> 

pendplot≔procx,t        uses plottools,plots,ColorTools:local pt1,pt2,pt3,pt4,pendulum,wheel1,wheel2,cart,w,r1,r2,y0;cart ≔ rectanglex − 1,1,x+1,0,color=ColorRGB,0/255,79/255,121/255,style=surface;wheel1 ≔ diskx − 0.6,0,0.25,color=black;wheel2 ≔ diskx+0.6,0,0.25,color=black;w ≔ 0.1;r1 ≔ 3.8;r2 ≔ 0.2;y0 ≔ 0.7;pt1 ≔ x − r1⋅sint+w⋅ cost,y0+r1⋅cost+ w⋅sint;pt2 ≔ x − r1⋅sint − w⋅ cost,y0+r1⋅cost− w⋅sint;pt3 ≔ x − r2⋅sint − w⋅ cost, y0 − r2⋅cost − w⋅sint;pt4 ≔ x − r2⋅sint+w⋅ cost, y0 − r2⋅cost+ w⋅sint;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

>