Model Predictive Control Python Implementation

Table of Contents

Model Predictive Control (MPC) is increasingly finding more applications in the world of control engineering, and Python is one the languages that makes it easy to implement.

This technique (MPC) is based on optimization.

There are many libraries in Python that explain how MPC works and assist you in the test. However, there’s little freedom to implement the algorithm in a way that can be easily ported on a microcontroller.

In this post I want to show how to implement Model Predictive Control in Python without using a specific library.

On top of that, we will test how MPC reacts to variations of the plant (i.e. robustness).

The code used to generate this example is available at https://github.com/simorxb/MPC-Pendulum-Python-2.

Understanding the Basics of Model Predictive Control

The idea of MPC is to find the optimal control sequence over a control horizon of $N$ steps that minimises a cost function, for example:

$J = \sum_{i=1}^{N-1} Q (r_i - y_i)^2 + R \Delta u_i^2$

where:

$r$ : setpoint
$y$ : controlled variable
$u$ : command
$Q$ : weighting coefficient related to $y$
$R$ : weighting coefficient related to $u$

Plant: The Pendulum

The plant considered is the pendulum model described in The Pendulum Model: Differential Equations into Simulations.

Let’s recap:

The rod is rigid, massless, and non-extensible.
The object at the end can be considered a point mass.

The equation of motion is:

$\tau - k \dot{\theta} - m g l \sin{\theta} = J \ddot{\theta}, ~ J = m l^2$

The values used are:

$m = 0.5 ~ kg$
$l = 1 ~ m$
$k = 0.5 ~ N m s$
$g = 9.81 ~ m/s^2$

Implementing Model Predictive Control in Python Code

Discretized Plant

To formulate the MPC algorithm, we need the system in the discrete-time form:

$\ddot{\theta}(i+1) = \frac{\tau - k \dot{\theta}(i) - m g l sin(\theta(i))}{m l^2}$
$\dot{\theta}(i+1) = \dot{\theta}(i) + dt ~ \ddot{\theta}(i+1)$
$\theta(i+1) = \theta(i) + dt ~ \dot{\theta}(i+1)$

where $dt$ is our time step.

The Python function pendulum_step implements the discretized pendulum step.

# Step of the pendulum system
def pendulum_step(theta, dtheta, tau, l, k, m, g, dt):

    # Calculate the change in angular acceleration
    ddtheta = (tau - k * dtheta - m * g * l * math.sin(theta)) / (m * l * l)

    # Update the angular velocity and angle
    dtheta_next = dtheta + ddtheta * dt
    theta_next = theta + dtheta_next * dt
    
    return (theta_next, dtheta_next)

Cost Function

This is the cost function used for the algorithm, including a term that considers the angular speed:

$J = \sum_{i=1}^{N-1} Q_{11} \dot \theta_i^2 + Q_{22} (\theta_{{ref}_i} - \theta_i)^2 + R \Delta \tau_i^2$

Also, I thought of two logical constraints for the torque control sequence:

Max torque: $-\tau_{max} < \tau_i < \tau_{max}$
Torque rate of change: $-\Delta \tau_{max} < \tau_{i+1} - \tau_i < \Delta \tau_{max}$

Have a look at the Python code to see the implementation of the second constraint, I think it’s interesting!

This cost function is implemented in the code using the function mpc_cost.

# Cost function to be minimized
def mpc_cost(tau, tau_ini, theta_ref, theta0, dtheta0, l, k, m, g, dt, Q11, Q22, R, N):

    # Initialise cost = 0 and states to current measured states
    cost = 0
    theta = theta0
    dtheta = dtheta0

    # Simulate the next N steps of the system
    for idx in range(N):

        # Calculate the change in angular acceleration
        ddtheta = (tau[idx] - k * dtheta - m * g * l * math.sin(theta)) / (m * l * l)

        # Update the angular velocity and angle
        dtheta = dtheta + ddtheta * dt
        theta = theta + dtheta * dt

        # Update cost
        cost += Q11 * dtheta**2 + Q22 * (theta_ref - theta)**2 + R * (tau[idx] - tau_ini[idx])**2

    return cost

Model Predictive Control Algorithm

The function mpc_cost computes J for the next N (20) steps, starting from the current state.
The function solve_mpc sets the constraints and bounds and uses scipy.optimize.minimize to minimise the cost computed by mpc_cost and then returns the optimal control input sequence.

The code implements a simulation of the loop for 10 seconds, where the setpoint is 180 deg for the first 5 seconds and 90 deg from 5s to 10s.

The prediction horizon of 20 steps corresponds to 2 seconds with the dt = 0.1s that I used.

Here’s the Python code for solve_mpc:

# Solve MPC optimization problem
def solve_mpc(theta_ref, theta, dtheta, tau_ini, l, k, m, g, dt, Q11, Q22, R, N, tau_max, delta_tau_max):

    # Linear constraints on the rate of change of tau: -delta_tau_max <= tau[i+1] - tau[i] <= delta_tau_max for all i in the N - 1
    # Implemented using LinearConstraint as -delta_tau_max <= delta_tau_matrix * tau <= delta_tau_max
    delta_tau_matrix = np.eye(N) - np.eye(N, k=1)
    constraint1 = LinearConstraint(delta_tau_matrix, -delta_tau_max, delta_tau_max)

    # We need a constraint on the rate of change of tau[0] respect to its previous value, which is tau_ini[0]
    first_element_matrix = np.zeros([N, N])
    first_element_matrix[0, 0] = 1
    constraint2 = LinearConstraint(first_element_matrix, tau_ini[0]-delta_tau_max, tau_ini[0]+delta_tau_max)
    
    # Add constraints
    delta_tau_constraint = [constraint1, constraint2]

    # Bounds --> -tau_max <= tau[idx] <= tau_max for idx = 0 to N-1
    bounds = [(-tau_max, tau_max) for idx in range(N)]

    # Starting optimisation point for theta and dtheta are the current measurements
    theta0 = theta
    dtheta0 = dtheta

    # Minimization
    result = minimize(mpc_cost, tau_ini, args=(tau_ini, theta_ref, theta0, dtheta0, l, k, m, g, dt, Q11, Q22, R, N), bounds=bounds, constraints=delta_tau_constraint)

    # Extract the optimal control sequence
    tau_mpc = result.x

    return tau_mpc

Simulation

The following plot shows the same simulation run with 3 values of the pendulum mass (0.3, 0.5, and 0.7 kg).

This is the Python code used to run the simulation:

# ---------- SIMULATION INITIALISATION ----------

# Plant parameters
l = 1  # length of the pendulum
k = 0.5  # coefficient of friction
m_V = [0.3, 0.5, 0.7]  # mass of the pendulum
g = 9.81  # acceleration due to gravity

# Time step
dt = 0.1

# Simulation time
time_range = 10

# Simulation steps
L = round(time_range/dt)

# Init time
time = 0

# Initial state
theta0 = 0
dtheta0 = 0

# Arrays for logging
theta = np.zeros(L + 1)
dtheta = np.zeros(L + 1)
tau = np.zeros(L)
theta_ref = np.zeros(L)

# Init arrays to initial state
theta[0] = theta0
dtheta[0] = dtheta0

# ---------- CONTROL SYSTEM CALIBRATION ----------

# Model predictive control horizon
N = 20

# Cost weights
Q11 = 0.1 # angular speed weight
Q22 = 1 # angular position weight
R = 0.1 # control input weight

# Max torque allowed
tau_max = 7

# Max delta torque between two steps
delta_tau_max = 1

# Plant parameters - estimated
l_est = 1  # length of the pendulum
k_est = 0.5  # coefficient of friction
m_est = 0.5  # mass of the pendulum
g_est = 9.81  # acceleration due to gravity

# Array for initial solution for MPC
tau_ini = np.zeros(N)

# Array for values of theta and tau for each simulation
theta_V = []
tau_V = []

# ---------- SIMULATION ----------

for idx_m in range(len(m_V)):

    # Init time
    time = 0

    # Arrays for logging
    theta = np.zeros(L + 1)
    dtheta = np.zeros(L + 1)
    tau = np.zeros(L)
    theta_ref = np.zeros(L)

    # Init arrays to initial state
    theta[0] = theta0
    dtheta[0] = dtheta0

    for idx in range(L):

        # ---------- SIMULATE USER INPUT ----------

        # Generate reference setpoint
        if time < 5:
            theta_ref[idx] = math.pi # 180 deg
        else:
            theta_ref[idx] = math.pi*0.5 # 90 deg
        
        # Increment time
        time += dt

        # ---------- CONTROL SYSTEM LOOP ----------
        
        # Find optimal sequence for the next N steps
        tau_mpc = solve_mpc(theta_ref[idx], theta[idx], dtheta[idx], tau_ini, l_est, k_est, m_est, g_est, dt, Q11, Q22, R, N, tau_max, delta_tau_max)
        
        # Use first element of control input optimal solution
        tau[idx] = tau_mpc[0]

        # Initial solution for next step = current solution
        tau_ini = tau_mpc

        # ---------- SIMULATION LOOP  ----------

        # Run dynamic system - pendulum
        (theta[idx+1], dtheta[idx+1]) = pendulum_step(theta[idx], dtheta[idx], tau[idx], l, k, m_V[idx_m], g, dt)

    # Append result for this simulation
    theta_V.append(theta)
    tau_V.append(tau)

And the code used to plot the results:

# Plot results

plt.subplot(2, 1, 1)
for idx_m in range(len(m_V)):
    plt.plot(np.arange(L+1)*dt, theta_V[idx_m][:]*180/math.pi, label=f"Response - Mass = {m_V[idx_m]} kg")
#plt.plot(np.arange(L+1)*dt, dtheta[:]*180/math.pi, label="Angular speed [deg/s]")
plt.plot(np.arange(L)*dt, theta_ref*180/math.pi, '--', label="Setpoint")
plt.ylabel(r" [deg]")
plt.legend()
plt.grid()

plt.subplot(2, 1, 2)
for idx_m in range(len(m_V)):
    plt.plot(np.arange(L)*dt, tau_V[idx_m], label=f"Control command - Mass = {m_V[idx_m]} kg")
plt.ylabel(r" [Nm]")
plt.xlabel("Time")
plt.legend()
plt.grid()
plt.show()

Conclusion

As we can see the control shows steady-state error when the mass of the pendulum is not identical to the one used for the internal model.

There are ways to avoid this in MPC. Stay tuned to find out how.

Model Predictive Control Python Implementation

Understanding the Basics of Model Predictive Control

Plant: The Pendulum

Implementing Model Predictive Control in Python Code

Discretized Plant

Cost Function

Model Predictive Control Algorithm

Practical Model Predictive Control Course

PID Control – Things I Wish I Knew When I Graduated

Simulation

Conclusion

Leave a Comment Cancel Reply

Practical Model Predictive Control Course

PID Control - Things I Wish I Knew When I Graduated

Model Predictive Control Python Implementation

Understanding the Basics of Model Predictive Control

Plant: The Pendulum

Implementing Model Predictive Control in Python Code

Discretized Plant

Cost Function

Model Predictive Control Algorithm

Practical Model Predictive Control Course

PID Control – Things I Wish I Knew When I Graduated

Simulation

Conclusion

Related Posts

Leave a Comment Cancel Reply