LQR Control for an Inverted Pendulum on a Cart

The purpose of this page is to show the derivation of the linearized dynamics for the highly non-linear cart-pole system and to propose an algorithm to calculate the optimal balancing input using a LQR controller. This system is classically studied in non-linear controls.

A LQR controller is used to stabilize the cart-pole system around its unstable equilibrium with the cart at the origin and the pole in its upright position.The cart-pole is described by the following dynamics:

$(m_c+m_p)\ddot{x}+m_{p}l\ddot{\theta }\cos \theta -m_{p}l{\dot{\theta }}^2\sin \theta =u$

$m_{p}l\ddot{x}cos\theta +m_p{l}^2\ddot{\theta }+m_{p}gl\sin \theta =0$

This may be put into standard form with $q=[x,\theta {]}^T$:

$𝐇(𝐪)\ddot{𝐪}+𝐂(𝐪,\dot{𝐪})\dot{𝐪}+𝐆(𝐪)=𝐁𝐮$

Where the above matrices are defined as:

File:Matrix def.jpg

The linearized dynamics are found by performing a Taylor expansion around the fixed point of interest:

File:Linear matrix.jpg

With the linearized dynamics a stable LQR controller may be calculated using the cost function:

$g(x,u)=(x^T\mathbf{\text{Q}}x+u^T\mathbf{\text{R}}u)$

$\sum _{k=0}^{\mathrm{\infty }}{\gamma }^{k}g(x_k,u_k)$

The optimal solution to this cost function may be found using techniques in dynamic programming and is of the form:

${𝐊}_{i+1}=-(𝐑+\gamma {𝐁}^T{𝐏}_i𝐁{)}^{-1}{𝐁}^T{𝐏}_i𝐀$

${𝐏}_{i+1}=𝐐+{𝐊}_{i+1}^T𝐑{𝐊}_{i+1}+\gamma (𝐀+𝐁𝐊{)}^T{𝐏}_i(𝐀+𝐁{𝐊}_{i+1})$

The following code may be used in MATLAB to converge on the optimal gain matrix:

File:Lqr code.jpg

For the given system, the optimal gain matrix can be found to be:

$𝐊=\left[\begin{array}{cccc}21.13& -320.74& 30.23& -70.18\end{array}\right]$

With the Qand R weights as follows:

$𝐐=\left[\begin{array}{cccc}500& 0& 0& 0\\ 0& 100& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right],R=1$

And the optimal control input can be calculated.

$u(t)=𝐊x(t)$

This policy can be empirically shown to balance the pendulum at its upright position:

File:Plot2.jpg