HOME

Using LQR to stabilize an Inverted pendulum

Nasser M. Abbasi
April 12, 2012

Introduction

This is an analysis of the dynamics of inverted bob pendulum on a moving cart. The equations of motion for the cart and the pendulum bob mass are derived using Lagrangian formulation, then state space model is derived and then LQR is used to find the state gain vector to bring the pendulum to the upright position from an initial position. The analysis uses Mathematica. Viscous friction is assumed to be present. This is friction between the cart itself and the rail that the cart moves on.

PDF file is here
Mathematica notebook is here

Derivation of the equations of motion

Let ν be the viscous friction coefficient. The potential energy of the system is PE=m g L sin(θ) and the kinetic energy is KE=(1/2)m ( (((x'-L θ'sinθ))^2)+(((L θ' cosθ))^2))+(1/2)M (((x'))^2) , hence the Lagrangian is now found and equations of motion is found for the bob mass and for the cart mass as follows

Clear[m,M,len,θ,x,t,ν,L]
ke = (1/2) m ((x'[t]- len*θ'[t] Sin[θ[t]])^2+(len*θ'[t] Cos[θ[t]])^2)+1/2 M*x'[t]^2;
pe = m*g*len*Sin[θ[t]];

The Lagrangian

L=ke-pe

find equation of motion for the bob

eq1=D[D[L,θ'[t]],t]-D[L,θ[t]]==0;
eq1=Simplify[eq1]

find equation of motion for the cart.

eq2=D[D[L,x'[t]],t]-D[L,x[t]]==f[t]-ν x'[t];
eq2=Simplify[eq2]

Find the state space representation using 90 degree and x=0 as the equilibrium point

sys=StateSpaceModel[{eq1,eq2},{{x[t],0},{θ[t],Pi/2},{x'[t],0},{θ'[t],0}},{{f[t],0}},{x[t],θ[t]},t]

Hence, the A matrix is

sys[[1,1]]//MatrixForm

and the B matrix is

sys[[1,2]]//MatrixForm

And the C matrix is

sys[[1,3]]//MatrixForm

obtain optimal state feedback gain. First make up a weight matrix Q

values={m->1,M->10,len->5,g->9.8,ν ->2};
(q=DiagonalMatrix[{100,1,1,1}])//MatrixForm

Now use LQR to find state feedback gain vector

gain=LQRegulatorGains[sys/.values,{q,{{1}}}]

Plot the response of the system using this state feedback gain from an initial position. First generate the closed loop system

closedLoopSys = SystemsModelStateFeedbackConnect[sys,gain]

initialAngle=75 Degree;
states=Chop[StateResponse[{closedLoopSys/.values,{2,initialAngle,0,0}},0,t]]

Plot the x(t) and θ(t) responses to see if they do come to the equilibrium position

maxt=10;
Row[{Plot[states[[1]],{t,0,maxt},PlotLabel->"x(t) vs t",ImageSize->300,PlotRange->All],Plot[states[[2]],{t,0,maxt},PlotLabel->"θ(t) vs t",ImageSize->300,PlotRange->All]}]

We see from above that both the x position of the cart and the upright pendulum position have been brought back to the equilibrium position