2. 二阶系统MPC调节器设计
任务目标
为如下二阶振荡系统设计MPC调节器，使状态从非零初值回归零点，并比较预测时域 N=5 与 N=20 时的控制效果差异：
x(k+1) = A x(k) + B u(k)
A = [1.0 0.1; -0.5 1.0], B = [0; 0.1]
系统矩阵A的特征值模为1（临界稳定），需通过MPC施加约束控制使其镇定。状态约束 |x|≤2，控制约束 |u|≤1。
实现方案
采用显式MPC（离线求解QP）与仿真验证结合的方式。核心步骤：
- 将约束MPC问题转化为标准二次规划（QP），使用
cvxopt求解器在线求解； - 在每个采样时刻，基于当前状态求解最优控制序列，仅施加第一个控制量；
- 分别以预测时域 N=5 和 N=20 进行闭环仿真，对比状态轨迹与控制输入。
完整代码
import numpy as np
import matplotlib.pyplot as plt
from cvxopt import matrix, solvers
# 系统模型
A = np.array([[1.0, 0.1], [-0.5, 1.0]])
B = np.array([[0.0], [0.1]])
nx, nu = 2, 1
# 权重矩阵
Q = np.eye(nx)
R = np.eye(nu) * 0.1
# 约束: |x| <= 2, |u| <= 1
x_max = 2.0
u_max = 1.0
def mpc_controller(x0, N):
"""求解MPC问题，返回最优控制序列的第一个控制量"""
# 构建预测矩阵
n_total = N * nx + N * nu
P = np.zeros((n_total, n_total))
# 状态权重
for i in range(N):
P[i*nx:(i+1)*nx, i*nx:(i+1)*nx] = Q
# 控制权重
for i in range(N):
idx = N*nx + i*nu
P[idx:idx+nu, idx:idx+nu] = R
P = 2 * P # cvxopt 最小化 0.5*x'Px
# 构建等式约束: x(k+1) = A x(k) + B u(k)
A_eq = np.zeros((N*nx, n_total))
b_eq = np.zeros(N*nx)
# 初始状态
A_eq[0:nx, 0:nx] = -A
A_eq[0:nx, N*nx:N*nx+nu] = B
b_eq[0:nx] = -x0
for k in range(1, N):
A_eq[k*nx:(k+1)*nx, k*nx:(k+1)*nx] = -A
A_eq[k*nx:(k+1)*nx, (k-1)*nx:k*nx] = np.eye(nx)
A_eq[k*nx:(k+1)*nx, N*nx+k*nu:N*nx+(k+1)*nu] = B
# 不等式约束: |x| <= x_max, |u| <= u_max
n_ineq = 2 * (N*nx + N*nu)
G = np.zeros((n_ineq, n_total))
h = np.zeros(n_ineq)
row = 0
for i in range(N):
for j in range(nx):
G[row, i*nx+j] = 1
h[row] = x_max
row += 1
G[row, i*nx+j] = -1
h[row] = x_max
row += 1
for i in range(N):
idx = N*nx + i*nu
G[row, idx] = 1
h[row] = u_max
row += 1
G[row, idx] = -1
h[row] = u_max
row += 1
# 求解QP
P_qp = matrix(P)
q_qp = matrix(np.zeros(n_total))
G_qp = matrix(G)
h_qp = matrix(h)
A_qp = matrix(A_eq)
b_qp = matrix(b_eq)
solvers.options['show_progress'] = False
sol = solvers.qp(P_qp, q_qp, G_qp, h_qp, A_qp, b_qp)
if sol['status'] != 'optimal':
raise RuntimeError('QP求解失败')
u_opt = np.array(sol['x'][N*nx:N*nx+nu]).flatten()
return u_opt[0]
def simulate(N, x0, steps=50):
"""闭环仿真"""
x = x0.copy()
x_traj = [x.copy()]
u_traj = []
for _ in range(steps):
u = mpc_controller(x, N)
u = np.clip(u, -u_max, u_max)
x = A @ x + B.flatten() * u
x_traj.append(x.copy())
u_traj.append(u)
return np.array(x_traj), np.array(u_traj)
# 仿真比较
x0 = np.array([1.5, -1.0])
steps = 50
x5, u5 = simulate(5, x0, steps)
x20, u20 = simulate(20, x0, steps)
# 绘图
t = np.arange(steps+1)
fig, axes = plt.subplots(2, 2, figsize=(10, 8))
axes[0,0].plot(t, x5[:,0], 'b-', label='x1 (N=5)')
axes[0,0].plot(t, x5[:,1], 'r-', label='x2 (N=5)')
axes[0,0].set_title('状态轨迹 (N=5)')
axes[0,0].legend(); axes[0,0].grid(True)
axes[0,1].plot(t, x20[:,0], 'b-', label='x1 (N=20)')
axes[0,1].plot(t, x20[:,1], 'r-', label='x2 (N=20)')
axes[0,1].set_title('状态轨迹 (N=20)')
axes[0,1].legend(); axes[0,1].grid(True)
axes[1,0].plot(np.arange(steps), u5, 'g-', label='u (N=5)')
axes[1,0].set_title('控制输入 (N=5)')
axes[1,0].legend(); axes[1,0].grid(True)
axes[1,1].plot(np.arange(steps), u20, 'g-', label='u (N=20)')
axes[1,1].set_title('控制输入 (N=20)')
axes[1,1].legend(); axes[1,1].grid(True)
plt.tight_layout()
plt.savefig('mpc_comparison.png', dpi=150)
plt.show()
# 打印最终状态
print(f'N=5 最终状态: {x5[-1]}')
print(f'N=20 最终状态: {x20[-1]}')
运行方式
pip install numpy matplotlib cvxopt
python mpc_second_order.py
验证方法
- 程序运行后生成
mpc_comparison.png，包含四幅子图； - 观察状态轨迹：两种预测时域下状态均从 [1.5, -1.0] 收敛至零点附近；
- 对比控制输入：N=5 时控制量变化更剧烈，N=20 时控制更平滑；
- 终端输出最终状态，其范数应小于 0.05（50步后基本收敛）。
预期结果说明
由于系统临界稳定且存在状态约束，MPC需在约束边界内寻找可行控制。N=5 的预测视野较短，控制动作更激进；N=20 能预见更远未来，控制更平缓。两种情况下状态均能收敛至零点，但收敛速度与控制能耗存在差异。