This is the part 4 in a series of blog posts devoted to the classic control problem of an inverted pendulum on a driven cart. This series is still in production (Nov. 2020) but when finished they will be structured as follows.

1. Derivation of Simple Pendulum (Python Simulation) - link
2. Building a Physical Inverted Pendulum
3. Controlling Inverted Pendulum with PID controller
4. State Space Model of Inverted Pendulum - link
5. Designing the Optimal Controller for an Inverted Pendulum (LQR control) - link
6. Simulating an Inverted Pendulum with an LQR Controller (Python) - link
7. Deploying an LQR Controller on a Physical Inverted Pendulum

The associated code can be found on GitHub and a video series on YouTube (not currently released as of Nov. 2020). While this project aims to be encompassing and self contained, it does assume familiarity with classical mechanics, linear algebra, differential equations, some control theory, and programming (Python).

## Introduction

A sketch of the inverted pendulum system is shown in the figure below. From this we will derive the state space model (and eventually find the optimal controller to balance the inverted pendulum). The overall process for finding the state space model will be as follows: find the Lagrangian, evaluate the Euler-Lagrange equations, rearrange the coupled differential equations in state space form, and then linearize by evaluating the system at $\theta=\pi$ (pendulum vertical position).

## Part 1: Finding the Lagrangian

The Lagrangian is defined as $\mathcal{L}=T-V$, where $T$ and $V$ are the kinetic and potential energies, respectively. The terms in the sketch, above, are largely self evident, however I will choose to define $l=L/2$. I also make the assumption that the rod is made of a uniform mass, thus making $l$ the centre of mass of the rod. Using this information we can establish:

$$\dot{\vec{v}}_{cart} = \dot{x}\hat{i}$$

$$\dot{\vec{v}}_{rod} = \frac{d}{dt}\Big( x+l\sin(\theta) \Big)\hat{i} + \frac{d}{dt}\Big( l\cos(\theta) \Big)\hat{j} = (\dot{x} + l\dot{\theta}\cos(\theta))\hat{i} + (l\dot{\theta}\sin(\theta))(-\hat{j})$$

$$T = \frac{1}{2}m_1v_{cart}^2 + \frac{1}{2}m_2v_{rod}^2 + \frac{1}{2}I_{rod}\omega^2$$

$$V = m_2gl(1-\cos(\theta)$$

where $I_{rod}$ and $\omega$ are the moment of inertia and angular velocity of the rod, respectively. Substituting the above four equations into the Lagrangian, $\mathcal{L}=T-V$.

$$\mathcal{L}=\frac{1}{2}m_1\dot{x}^2 + \frac{1}{2}m_2(\dot{x}^2+\dot{x}l\dot{\theta}\cos(\theta) + l^2\dot{\theta}^2) + \frac{1}{2}I_{rod}\dot{\theta}^2-m_2gl(1-\cos(\theta)$$

## Part 2: Evaluate Euler-Lagrange Equations

From the Lagrangian by using the Euler-Lagrange equations, below, the equations of motion of the system can be found.

$$\frac{d}{dt}\Big( \frac{\partial\mathcal{L}}{\partial \dot{q_i}} \Big) - \frac{\partial\mathcal{L}}{\partial q_i} = Q_i \quad \quad \quad i =x,\theta$$

Where $Q_{\theta}, Q_x$ are the external forces applied to the system in the $\theta$ and $x$ axes. In the $x$-axis there is a force applied to the cart (that we will later use to stabilize the inverted pendulum) and some dampening force proportional to the velocity. As such $Q_x = F-\beta \dot{x}$. In the $\theta$-axis there is also some friction at the pivot point however this resistance is negligible. As such I will define: $Q_{\theta}=0$. These quantities can be substituted into the Euler Lagrange equations.

For the sake of brevity (and my own sanity), I'm opting out of transcribing the partial derivatives in LaTeX. I will leave it as an exercise to verify that when the Euler-Lagrange equations are evaluated for $i =x,\theta$ that the equations of motion come out to be:

$$m_2\ddot{x}l\cos(\theta)+(m_2l^2-I)\ddot{\theta}= -m_2gl\sin(\theta)$$
$$(m_1+m_2)\ddot{x} + m_2l\ddot{\theta}\cos(\theta) = m_2l\dot{\theta}^2\sin(\theta) + F - \beta\dot{x}$$

Going forward I will rename the force, $F$, as $u$ which is the convention in control theory for a 'control input'. In subsequent blog posts I will clarify this convention and explain how $F$ can be expressed in terms of the motor torque and how in the real world the torque is controlled by the electrical current to the motor. But for the moment we will live in mathematical bliss and leave the control input as simply $u$.

## Part 3: Some Algebra

At this point we have found the coupled equations of motion of the system. However $\ddot{x}$ and $\ddot{\theta}$ appear in both equations. In this section I will rearrange these equations in matrix form and through some tedious manipulations separate the $\ddot{x}$ and $\ddot{\theta}$ terms.

In matrix form, the coupled equations of motion can be expressed in matrix form as:

$$\begin{bmatrix} m_2l\cos(\theta) & m_2l^2 +I \\ m_1+m_2 & m_2l\cos(\theta)\end{bmatrix} \begin{bmatrix} \ddot{x} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 0\\ -\beta & m_2l\dot{\theta}\sin(\theta) \end{bmatrix} \begin{bmatrix} \dot{x}\\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} x \\ \theta \end{bmatrix} + \begin{bmatrix} 0 & -m_2gl\sin(\theta) \\ 0 & 0 \end{bmatrix} \begin{bmatrix} 1 \\1 \end{bmatrix} +\begin{bmatrix} 0 \\ 1 \end{bmatrix} u$$

Refactoring the terms a more concise form is:

$$E\ddot{\vec{x}} = F\dot{\vec{x}} + G \vec{x} + H + Iu$$

And by taking the inverse of $E$:

$$\ddot{\vec{x}} = E^{-1}F\dot{\vec{x}} + E^{-1}G \vec{x} + E^{-1}H + E^{-1}Iu$$

$$E^{-1} = \frac{1}{m_2^2l^2 \cos^2(\theta) - (m_1 + m_2)(m_2l^2+I) } \begin{bmatrix} m_2l\cos(\theta) & -m_2l^2 -I \\ -m_1-m_2 & m_2l\cos(\theta)\end{bmatrix}$$

By doing some tedious matrix multiplication (exercise for the reader) the expression for vector $\ddot{\vec{x}}$ can be expanded to:

$$\ddot{x} = \frac{1}{D} \Big[ \beta(m_2l^2 +I)\dot{x} - m_2l\dot{\theta}^2\sin(\theta)(m_2l^2 +I) - m_2^2gl^2\cos(\theta)\sin(\theta) - (m_2l^2 +I)u\Big]$$

$$\ddot{\theta} = \frac{1}{D} \Big[ -\beta m_2l\cos(\theta)\dot{x} + m_2^2l^2\dot{\theta}^2\cos(\theta)\sin(\theta) +(m_1 + m_2)m_2gl\sin(\theta) + m_2l\cos(\theta)u \Big]$$

$$D = m_2^2l^2 \cos^2(\theta) - (m_1 + m_2)(m_2l^2+I)$$

## Part 4: Linearization

To recap, so far we have found the equations of motion of the system and expressed them as separate equations of $\ddot{x}$ and $\ddot{\theta}$. These non-linear equations will be useful in the blog post regarding simulating the physical system. However, to find the optimal LQR controller these equations must be linearized around the unstable equilibrium point $\theta = \pi$. Since the LQR controller is only valid for near the equilibrium point we can assume that the system remains within $\phi$ of $\pi$, where $\phi$ is a small angle. Therefore the nonlinear terms of interest become:

$$\cos(\theta) = \cos(\pi + \phi) = \cos(\pi) - \sin(\pi\phi) + O^2 \approx -1$$

$$\sin(\theta) = \sin(\pi + \phi) = \sin(\pi) + \cos(\pi\phi) + O^2 \approx -\phi$$

$$\dot{\theta}^2 = \dot{\phi}^2 \approx 0$$

And so, using this information $\ddot{x}$ and $\ddot{\theta}$ become:

$$\ddot{x} = \frac{1}{D} \Big[ \beta(m_2l^2 +I)\dot{x} - m_2^2gl^2\theta - (m_2l^2 +I)u\Big]$$

$$\ddot{\theta} = \frac{1}{D} \Big[ \beta m_2l\dot{x} -(m_1 + m_2)m_2gl\theta - m_2lu \Big]$$

$$D = -m_1m_2^2l^2 - (m_1 + m_2)I$$

## Part 5: State Space Model

To review, the state space model representation is a system of two equations (below). Where A, B, C, and D are matrices that describe the state, input, output, and feedthrough, respectively. In most cases, including the inverted pendulum, D is a zero matrix.

$$\dot{\vec{x}} = A\vec{x}+Bu$$

$$\vec{y} = C\vec{x}+Du$$

Looking at the pair of equations at the end of part 4, we can see that by converting the linear system back into matrix form this will yield the general state space form:

$$\begin{bmatrix} \dot{x} \\ \dot{\theta} \\ \ddot{x} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & \frac{m_2^2gl^2}{m_1m_2l^2 + I(m_1 + m_2)} & \frac{- \beta(m_2 l^2+I)}{m_1m_2l^2 + I(m_1 + m_2)} & 0 \\ 0 & \frac{m_2gl(m_1+m_2)}{m_1m_2l^2 + I(m_1 + m_2)} & \frac{-m_2l \beta}{m_1m_2l^2 + I(m_1 + m_2)} & 0 \end{bmatrix} \begin{bmatrix} x \\ \theta \\ \dot{x} \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0\\0\\ \frac{m_2l^2+I}{m_1m_2l^2 + I(m_1 + m_2)} \\ \frac{m_2l}{m_1m_2l^2 + I(m_1 + m_2)} \end{bmatrix} u$$

$$y=\begin{bmatrix} 1 & 0&0&0\\0&1&0&0\end{bmatrix}\begin{bmatrix} x \\ \theta \\ \dot{x} \\ \dot{\theta} \end{bmatrix} +\begin{bmatrix} 0 \\ 0\end{bmatrix}u$$