Python implementation of Cartpole modeling and PID, LQR, MPC control algorithms

The goal of the CartPole problem is to keep the pole vertical (angle = 0) and the cart at the center of the track (position = 0).

The tutorial does not cover formula derivations. If you want to understand the underlying principles of PID\LQR\MPC, you can study it on your own.

I. Environment and Basics Used

Windows
Matrix Theory and Control Theory
For environment installation, see requirments.txt

II. Cartpole Modeling

The code for the definition of the Cartploe official dynamics model is as follows: refer to http://incompleteideas.net/sutton/book/code/pole.c

For details, refer to the cartpole_env.py file in the code. During the implementation, in order to adapt to the linear controller, we made an approximation to the dynamics model, which will be introduced later.

python# temp = (force + self.polemass_length * theta_dot * theta_dot * sintheta) / self.total_mass
# thetaacc = (self.gravity * sintheta - costheta * temp) / (self.length * (4.0 / 3.0 - self.masspole * costheta * costheta / self.total_mass))
# xacc = temp - self.polemass_length * thetaacc * costheta / self.total_mass

The conversion into a formula can be expressed as:

\begin{cases} \ddot{x}=\frac{F+m_{p} l \dot{\theta}^{2} \sin \theta}{m_{p}+m_{c}}-\frac{m_{p} l \dot{\theta}^{2} \sin \theta}{m_{p}+m_{c}} \\ \ddot{\theta}=\frac{g \sin \theta-\cos \theta \cdot \frac{F+m_{p} l \dot{\theta}^{2} \sin \theta}{m_{p}+m_{c}}}{l\left(\frac{4}{3}-\frac{m_{p} \cos \theta^{2}}{m_{p}+m_{c}}\right)} \end{cases}

For the linear controller, since the angle of the inverted pendulum $\theta$ is very small, an approximation can be made. Therefore, $\theta$ is approximated to 0, and $\sin \theta$ and $\cos \theta$ are also approximated. As a result, $\sin \theta \approx \theta$ , $\cos \theta \approx 1$ can be obtained. After the approximation, the formula can be expressed as:

\begin{cases} \ddot{x}=\left(\frac{1}{m_{p}+m_{c}}+\frac{3 m_{p}}{m_{p}+4 m_{c}}\right) F-\frac{3 m_{p} g}{m_{p}+4 m_{c}} \theta \\ \ddot{\theta}=\frac{3 g\left(m_{p}+m_{c}\right) \theta}{l\left(m_{p}+4 m_{c}\right)}-\frac{3 F}{l\left(m_{p}+4 m_{c}\right)} \end{cases}

By organizing the above equation into a state space equation, we get:

\left[\begin{array}{c} \dot{x} \\ \ddot{x} \\ \dot{\theta} \\ \ddot{\theta} \end{array}\right]=\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{3 m_{p} g}{m_{p}+4 m_{c}} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{3 g\left(m_{p}+m_{c}\right)}{l\left(m_{p}+4 m_{c}\right)} & 0 \end{array}\right]\left[\begin{array}{c} x \\ \dot{x} \\ \theta \\ \dot{\theta} \end{array}\right]+\left[\begin{array}{c} 0 \\ \frac{1}{m_{p}+m_{c}}+\frac{3 m_{p}}{m_{p}+4 m_{c}} \\ 0 \\ -\frac{3}{l\left(m_{p}+4 m_{c}\right)} \end{array}\right] F

The state space equations are used in LQR and MPC code. The corresponding Python code is as follows:

pythonA = np.eye(4) + del_t * np.array([[0, 1, 0, 0],
                [0, 0, (-3*m*g)/(m+4*M), 0],
                [0, 0, 0, 1],
                [0, 0, (3*(M+m)*g)/(L*(m+4*M)), 0]])
B = del_t * np.array([[0],
                [(1/(M+m))+((3*m)/(m+4*M))],
                [0],
                [-(3)/(L*(m+4*M))]])

III. Code Explanation

1. PID Controller

In the code, only PD control is used. Specifically, the deviation between the position of cart and the angle of pole is calculated, and then PD control is applied. The core code for PD control is as follows:

pythondef pid_cart(self, position):
    bias = position
    d_bias = bias - self.bias_cart_1
    balance = self.kp_cart * bias + self.kd_cart * d_bias
    self.bias_cart_1 = bias
    return balance

def pid_pole(self, angle):
    bias = angle  #
    d_bias = bias - self.bias_pole_1
    balance = -self.kp_pole * bias - self.kd_pole * d_bias
    self.bias_pole_1 = bias
    return balance

2. LQR controller

Discrete LQR Controller Problem Description:

\begin{array}{l} \text { S.T. } \quad x(k+1)=A x(k)+B u(k),\\ J=\frac{1}{2} \sum_{k=k_{0}}^{\infty}\left[x^{T}(k) Q x(k)+u^{T}(k) R u(k) + 2x^{T}(k)Nu_{k}\right] \end{array}

The calculation steps for the discrete LQR controller are:

Solve for the S matrix:

A^{T} S A-S-\left(A^{T} S B+N\right)\left(B^{T} S B+R\right)^{-1}\left(B^{T} S A+N^{T}\right)+Q=0

Calculate the feedback matrix K:

K=R^{-1}B^{T}P

Calculate the control input u:

u=-K(x_{now}-x_{des})

During the actual use of Cartpole in the LQR controller, $N$ is the cross-weight matrix of state and control inputs. In the Cartpole problem, there is no need to specifically penalize this cross term, so it is set to a zero matrix.

The core code of the LQR controller is as follows:

pythondef control_output(self, state_des, state_now):
    S = np.matrix(linalg.solve_discrete_are(self.A, self.B, self.Q, self.R))
    K = np.matrix(linalg.inv(self.B.T * S * self.B + self.R) * (self.B.T * S * self.A))
    # print("state_now", state_now)
    # print("state_des", state_des)
    action_list  = -10*np.dot(K, state_now - state_des)
    action = action_list[0,0]
    return action

The Lirakhtia equation is calculated using the solve_discrete_are function from the linalg library.

3. MPC Controller

Core components of MPC:

Prediction model: Used to predict the future state trajectory of the system based on the current state and future control inputs.
Scroll optimization: Find a set of control inputs that make the defined performance metric (cost function) optimal within the prediction horizon.
Feedback correction: Adjust the model prediction based on measurements, and re-predict and optimize at the next time point according to the new state.

Description of the linear MPC problem:

\begin{array}{l} \text { S.T. } \quad x(k+j+1 \mid k)=A x(k+j \mid k)+B u(k+j \mid k), \quad j=0,1, \ldots, N-1,\\ J=\sum_{j=0}^{N-1}\left[x^{T}(k+j \mid k) Q x(k+j \mid k)+u^{T}(k+j \mid k) R u(k+j \mid k)\right]+x^{T}(k+N \mid k) F x(k+N \mid k) \end{array}

Among them:

$N$ is the prediction step size (prediction interval).
$x(k+j \mid k)$ is the prediction of the state at $k+j$ at the moment $k$ .
$u(k+j \mid k)$ is the control input scheduled at $k+j$ at the moment $k$ .
$Q$ is the state weight matrix.
$R$ is the control input weight matrix.
$F$ is the terminal state weight matrix.

The core code of the MPC controller is as follows:

Construct the matrix for the QP problem: The purpose is to calculate the matrices H and E in the QP formula.

pythondef get_QPMatrix(A, B, Q, R, F, N):
    M = np.vstack([np.eye(n), np.zeros((N*n, n))])
    C = np.zeros(((N+1)*n, N*p))
    temp = np.eye(n)
    for i in range(1,N+1):
        rows = i * n + np.arange(n)
        C[rows,:] = np.hstack([temp @ B, C[rows-n, :-p]])
        temp = A @ temp
        M[rows,:] = temp

    Q_ = np.kron(np.eye(N), Q)
    rows_Q, cols_Q = Q_.shape
    rows_F, cols_F = F.shape
    Q_bar = np.zeros((rows_Q+rows_F, cols_Q+cols_F))
    Q_bar[:rows_Q, :cols_Q] = Q_
    Q_bar[rows_Q:, cols_Q:] = F
    R_bar = np.kron(np.eye(N), R)

    # G = M.T @ Q_bar @ M
    E = C.T @ Q_bar @ M
    H = C.T @ Q_bar @ C + R_bar
    return E, H

Online MPC solution

CasADi: This is a powerful symbolic computation and automatic differentiation framework, commonly used to solve optimal control and nonlinear optimization problems.

pythondef mpc_prediction(x_k, E, H, N, p):
    # 定义优化变量
    U = ca.SX.sym('U', N * p)
    # 定义目标函数
    objective = 0.5 * ca.mtimes([U.T, H, U]) + ca.mtimes([U.T, E, x_k])
    qp = {'x': U, 'f': objective}
    opts = {'print_time': False, 'ipopt': {'print_level': 0}}
    solver = ca.nlpsol('solver', 'ipopt', qp, opts)

    # 求解问题
    sol = solver()
    # 提取最优解
    U_k = sol['x'].full().flatten()
    u_k = 10*U_k[:p]  # 取第一个结果

    return u_k