Introduction¶
The inverted pendulum is a classic controllability example. Most likely, you have performed this kind of control yourself by balancing a broom or stick on your palm. In that case the pendulum was a physical pendulum and your arm the external driving force. In this notebook we will try to balance an inverted mathematical pendulum on a cart subjected by an external force acting on the cart's center of mass.
Figure 1: MIT Professor Alan V. Oppenheim in his lecture The Inverted Pendulum from the course Signals and Systems. Source: Oppenheim's lecture notes [1].
The figure above shows a common experimental setup for controlling an inverted pendulum. The pendulum is moved by pulling a suspended wire between the cart and an electrical motor. In this particular setup, the controller will balance the pendulum independently of the mass of the pendulum "bead", explaining the confidence when pouring more liquid into the glass.
Initially, we will model our pendulum system using Lagrangian mechanics and later control the pendulum using proportional control and then construct an algorithm to optimise the regulation based on quadratic weighing factors (linear quadratic regulator [LQR])
import numpy as np
import progressbar
import matplotlib.pyplot as plt
from matplotlib import animation
from IPython.display import display, Image
from scipy import signal
import control
%matplotlib inline
Theory Part I¶
Figure 2 below shows a schematic of the system used in this example. The inverted pendulum is fixed in one end on the top of a cart with a mass $M$. The rotation around the fixed point is assumed frictionless. The bead in the end of the pendulum has a mass $m$, while the stiff rod connecting the bead to the cart is massless. This makes the inverted pendulum a mathematical pendulum. The cart is able to move relative to the surface, i.e. only in the $x$-direction. Wheels are only included for illustration. The forces acting on the bead is the conservative gravitational force $\vec{G}$ in the negative $y$-direction, while the force acting on the cart is the external applied force $\vec{u}$ in the $x$-direction. The force $\vec{F}$ is an approximation of the friction on the entire system, also acting in the $x$-direction. The length of the massless rod is assigned $l$, and the angular displacement of the pendulum relative to $\vec{y}$ is given by $\theta$.
Figure 2: A schematic on the inverted pendulum. The bead angle $\theta$ is zero at the unstable (upright) equilibrium. The gravitational force, $\vec{G}$, acting on the bead with mass $m$, the friction on the system $\vec{F}$ and external force $\vec{u}$ is illustrated.
figSizeX = 12
figSizeY = 9
# System parameters
m = 0.1 # mass of bead [kg]
M = 1 # mass of cart [kg]
l = 0.2 # length of rod [m]
g = 9.81 # gravitational acceleration [m s^-2]
mu = 10 # friction coefficient [kg s^-1]
# Initial conditions of generalised coordinates
x0_0 = 0 # cart position [m]
x1_0 = 0 # rod angle [radians]
x2_0 = 0 # cart velocity [m s^-1]
x3_0 = 0 # rod angular velocity [radians s^-1]
u_0 = 0 # initial cart force [kg m s^-2]
The system parameters above can be altered to get different motion and response of the system. As example, a more massive bead would require more external force to be balanced, and a longer rod would reduce the pendulum swing period.
Equations of motion¶
It is possible to derive the equations of motion using Newtonian forces, but the Lagrangian formulation is more elegant. A full derivation of the equations from the Lagrangian $$ L = T - U $$ is found in appendix A.
The system sketched in figure 2 has two holonomic constraints: The rod length $l$ is fixed, and the $y$-coordinate of the cart is always zero. This results in two generalised coordinates: The bead angle $\theta$, the cart position $x$ and their time derivatives $\dot{\theta}$ and $\dot{x}$. The corresponding equations of motion evaluates to
\begin{equation} \ddot{x} = \lambda(\theta)\big[mgl^2\dot{\theta}^2\sin\theta - mgl \sin\theta \cos\theta - l\mu \dot{x} + ul \big] \label{cartacc} \end{equation}\begin{equation} \ddot{\theta} = \lambda(\theta)\big[(m + M)g \sin\theta - mgl\dot{\theta}^2 \sin\theta \cos\theta + \mu\dot{x}\cos\theta - u\cos\theta \big] \label{angelacc} \end{equation}where
\begin{equation} \lambda(\theta) = \frac{1}{l(M + m\sin^2\theta)} \label{lambda} \text{.} \end{equation}The friction coefficient $\mu$ originates from the friction force $\vec{F}$, which is set to be proportional to the cart velocity, $F = -\mu\dot{x}$.
By renaming the generalized coordinates we get the state vector:
\begin{equation} \textbf{x} = \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ x_3 \end{bmatrix} = \begin{bmatrix} x \\ \theta \\ \dot{x} \\ \dot{\theta} \end{bmatrix} \text{.} \end{equation}Then, by realising that $\dot{x_0} = x_2$ and $\dot{x_1} = x_3$, we get the state equation:
\begin{equation} \dot{\textbf{x}} = \frac{d}{dt} \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ x_3 \end{bmatrix} = \begin{bmatrix} x_2 \\ x_3 \\ \lambda(x_1)\big[ml^2{x_3}^2\sin x_1 - mgl \sin x_1 \cos x_1 - l\mu x_2 + ul \big] \\ \lambda(x_1)\big[(m + M)g \sin x_1 - ml{x_3}^2 \sin x_1 \cos x_1 + \mu x_2\cos x_1 - u\cos x_1 \big] \end{bmatrix} \text{.} \label{stateEquation} \end{equation}The last two equations in the state vector are non-linear. Which means that the motion of the system is very hard to regulate. Consequently, we will have to linearise them. This is done in part II of the notebook.
Implementation - Part I¶
Simulating the system¶
To simulate the inverted pendulum on a cart we have to solve the state equation iteratively. In this notebook we will use the numerical method Runge-Kutta fourth order (RK4) to integrate the system of non-linear differential equations, \eqref{systemEquation}. If you have never heard of this method, or want a recap, check out this notebook for a practical introduction.
In addition to defining the RK4 step function, we define a function that returns the right hand side of \eqref{stateEquation} given a vector $\textbf{x}$ and the system parameters, and a function to calculate the systems potential and kinetic energy.
def RHS(z, RHSargs):
"""Given a specified state vector and system parameters,
z and RHSargs respectively, this function returns the
state vector element's time derivative, i.e. the right
hand side of the state equations.
Parameters:
z 4x1 array
RHSargs 6x1 array
Returns:
dxdt 4x1 array
"""
m, M, l, g, mu, u = RHSargs
x0, x1, x2, x3 = z
dx0dt = x2
dx1dt = x3
lambdax1 = 1/(l*(M + m*np.sin(x1)**2))
dx2dt = lambdax1*(m*l*l*x3**2*np.sin(x1) - m*g*l*np.sin(x1)*np.cos(x1)
- l*mu*x2 + u*l)
dx3dt = lambdax1*((m + M)*g*np.sin(x1) - m*l*x3*x3*np.sin(x1)*np.cos(x1)
+ mu*x2*np.cos(x1) - u*np.cos(x1))
dxdt = np.array([dx0dt, dx1dt, dx2dt, dx3dt])
return dxdt
def RK4Step(z, dt, RHS, RHSargs):
""" Performs one step of the 4th order Runge-Kutta method
z is the current state vector, dt is the numerical time
step, RHS is a function for the right hand side of the
system of differential equations, and RHSargs is the input
parameters for that function. RK4step returns the next
state vector, i.e. the values of z after a time dt.
Parameters:
z 4x1 array
dt float
RHS function
RHSargs 6x1 array
Returns:
The state vactor, z, after a time, dt.
"""
k1 = np.array(RHS(z, RHSargs))
k2 = np.array(RHS(z + k1*dt/2, RHSargs))
k3 = np.array(RHS(z + k2*dt/2, RHSargs))
k4 = np.array(RHS(z + k3*dt, RHSargs))
return z + dt/6*(k1 + 2*k2 + 2*k3 + k4)
def addEnergy(z):
""" Given the the state vector, z, calculates and returns the kinetic
and potential energy of the system in an arrray. See appendix B for the
energy term formulas.
"""
x0, x1, x2, x3 = z
U = m*g*l*np.cos(x1)
T = 0.5*x2**2*(m + M) + 0.5*m*x3**2*l**2 + m*x2*x3*l*np.cos(x1)
return np.array([T, U])
Now, we can integrate the state equation after defining a step size, $\Delta t$, and the integration duration $t_{max}$. The matrix Z is initialised to contain the state vector at any time $t$. While the initial angle of the pendulum is zero, it will never fall down because the x-component to the gravitational force is excactly zero. So, to make the situation more realistic, a random noise is added as external force, $u$, in every iteration. The noise could correspond to e.g. a wind gust (isolated on the cart).
duration = 10 # Integration duration [sec]
timeStep = 1e-2 # Time step
n = int(duration/timeStep) # Number of datapoints
# Initialising matrices
RHSargs = np.array([m, M, l, g, mu, u_0]) # System parameters
Z = np.zeros((4, n)) # Z = [[x0_0, ... x0_n], [x1_0, ... x1_n], [x2,...,], [x3,...,]
# Inserting initial state vector into Z
Z[:, 0] = np.array([x0_0, x1_0, x2_0, x3_0])
# Initialising matrix containing the noise and potential force on the cart
extForce = np.zeros((2, n))
extForce[0, 0] = u_0
# Initialising matrix containing the kinetic and potential energy of the system
energy = np.zeros((2, n)) # energy = [[T_0 ... T_n], [U_0, ... U_n]]
energy[:, 0] = addEnergy([x0_0, x1_0, x2_0, x3_0])
bar = progressbar.ProgressBar()
for i in bar(range(n - 1)):
# Compute and insert the next state vector using RK4
Z[:, i + 1] = RK4Step(Z[:, i], timeStep, RHS, RHSargs)
# Generating a random noise value
noise = np.random.uniform(-0.001, 0.001)
extForce[1, i +1] = noise
# Updating the system parameter u
RHSargs[5] = noise
# Add the energy terms
energy[:, i + 1] = addEnergy(Z[:, i + 1])
100% (999 of 999) |######################| Elapsed Time: 0:00:00 Time: 0:00:00
Next, we make a phase-space plot of the generalised coordinates in addition to the evolution to the state vector. The definition of the plotting function is found in Appendix B.
plotData(Z, extForce, energy, appliedForce=False)