带有终端约束的模型预测控制 代码

以下是一个简单的带有终端约束的模型预测控制（MPC）代码样例：

import numpy as np
from scipy.optimize import minimize
import matplotlib.pyplot as plt

# 系统模型
def model(x, u):
    A = np.array([[1.01, 0.01], [0.01, 1.01]])
    B = np.array([0.01, 0.03])
    x_next = np.dot(A, x) + np.dot(B, u)
    return x_next

# 预测模型
def predict(x0, u, N):
    x = np.zeros((N+1, 2))
    x[0] = x0
    for i in range(N):
        x[i+1] = model(x[i], u[i])
    return x

# 代价函数
def cost(u, x0, x_ref, Q, R, N):
    J = 0
    x = predict(x0, u, N)
    for i in range(N):
        J += np.dot(np.dot(x[i] - x_ref, Q), x[i] - x_ref) + np.dot(np.dot(u[i], R), u[i])
    return J

# 约束条件
def constraint(u, x0, N):
    x = predict(x0, u, N)
    g = np.zeros(N)
    for i in range(N):
        g[i] = x[i+1, 0] - 1.0
    return g

# MPC控制器
def mpc_controller(x0, x_ref, Q, R, N, umax, umin):
    u0 = np.zeros(N)
    bounds = [(umin, umax) for i in range(N)]
    cons = [{'type': 'ineq', 'fun': constraint, 'args': (x0, N)}]
    res = minimize(cost, u0, args=(x0, x_ref, Q, R, N), bounds=bounds, constraints=cons)
    return res.x[0]

# 模拟系统响应
def simulate_system(x0, x_ref, Q, R, N, umax, umin, T):
    x = np.zeros((T+1, 2))
    u = np.zeros(T)
    x[0] = x0
    for i in range(T):
        u[i] = mpc_controller(x[i], x_ref, Q, R, N, umax, umin)
        x[i+1] = model(x[i], u[i])
    return x, u

# 设置模拟参数
T = 20
N = 5
x0 = np.array([1.0, 1.0])
x_ref = np.array([0.0, 0.0])
Q = np.diag([1.0, 1.0])
R = np.diag([0.1])
umax = 1.0
umin = -1.0

# 模拟系统响应
x, u = simulate_system(x0, x_ref, Q, R, N, umax, umin, T)

# 绘制系统响应
plt.subplot(2, 1, 1)
plt.plot(x[:,0], 'r')
plt.plot(np.ones(T+1), 'k--')
plt.plot(np.ones(T+1) * x_ref[0], 'g--')
plt.ylabel('x1')

plt.subplot(2, 1, 2)
plt.plot(u, 'b')
plt.plot(np.ones(T) * umax, 'k--')
plt.plot(np.ones(T) * umin, 'k--')
plt.ylabel('u')
plt.xlabel('Time')

plt.show()

这个代码演示了一个简单的MPC控制器，其中系统模型是一个二阶系统，代价函数是二次型形式，约束条件是终端约束。在模拟中，我们可以看到控制器成功地将系统从初始状态控制到了参考状态，并且在控制过程中保持了输入的约束条件。