# Inverted Pendulum Control

An inverted pendulum on a cart consists of a mass $$m$$ at the top of a pole of length $$l$$ pivoted on a horizontally moving base as shown in the adjacent.

The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to.

## Modeling

• $$M$$: mass of the cart

• $$m$$: mass of the load on the top of the rod

• $$l$$: length of the rod

• $$u$$: force applied to the cart

• $$x$$: cart position coordinate

• $$\theta$$: pendulum angle from vertical

Using Lagrange’s equations:

$\begin{split}& (M + m)\ddot{x} - ml\ddot{\theta}cos{\theta} + ml\dot{\theta^2}\sin{\theta} = u \\ & l\ddot{\theta} - g\sin{\theta} = \ddot{x}\cos{\theta}\end{split}$

See this link for more details.

So

$\begin{split}& \ddot{x} = \frac{m(gcos{\theta} - \dot{\theta}^2l)sin{\theta} + u}{M + m - mcos^2{\theta}} \\ & \ddot{\theta} = \frac{g(M + m)sin{\theta} - \dot{\theta}^2lmsin{\theta}cos{\theta} + ucos{\theta}}{l(M + m - mcos^2{\theta})}\end{split}$

Linearized model when $$\theta$$ small, $$cos{\theta} \approx 1$$, $$sin{\theta} \approx \theta$$, $$\dot{\theta}^2 \approx 0$$.

$\begin{split}& \ddot{x} = \frac{gm}{M}\theta + \frac{1}{M}u\\ & \ddot{\theta} = \frac{g(M + m)}{Ml}\theta + \frac{1}{Ml}u\end{split}$

State space:

$\begin{split}& \dot{x} = Ax + Bu \\ & y = Cx + Du\end{split}$

where

$\begin{split}& x = [x, \dot{x}, \theta,\dot{\theta}]\\ & A = \begin{bmatrix} 0 & 1 & 0 & 0\\0 & 0 & \frac{gm}{M} & 0\\0 & 0 & 0 & 1\\0 & 0 & \frac{g(M + m)}{Ml} & 0 \end{bmatrix}\\ & B = \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ \frac{1}{Ml} \end{bmatrix}\end{split}$

If control only theta

$\begin{split}& C = \begin{bmatrix} 0 & 0 & 1 & 0 \end{bmatrix}\\ & D = [0]\end{split}$

If control x and theta

$\begin{split}& C = \begin{bmatrix} 1 & 0 & 0 & 0\\0 & 0 & 1 & 0 \end{bmatrix}\\ & D = \begin{bmatrix} 0 \\ 0 \end{bmatrix}\end{split}$

## LQR control

The LQR controller minimize this cost function defined as:

$J = x^T Q x + u^T R u$

the feedback control law that minimizes the value of the cost is:

$u = -K x$

where:

$K = (B^T P B + R)^{-1} B^T P A$

and $$P$$ is the unique positive definite solution to the discrete time algebraic Riccati equation (DARE):

$P = A^T P A - A^T P B ( R + B^T P B )^{-1} B^T P A + Q$

## MPC control

The MPC controller minimize this cost function defined as:

$J = x^T Q x + u^T R u$

subject to:

• Linearized Inverted Pendulum model

• Initial state