📚 课程目录第2章二阶系统MPC无约束控制

2. 二阶系统MPC无约束控制

任务目标

对如下二阶线性时不变系统设计无约束模型预测控制器（MPC），实现状态从非零初值调节至原点，并对比不同权重矩阵对闭环性能的影响：

x(k+1) = A x(k) + B u(k)
A = [1.0  0.1; -0.2  1.0]
B = [0.0; 0.1]

系统采样周期取1s，状态约束与输入约束均不设置（无约束MPC）。预测时域 N=10，控制时域与预测时域相同。通过修改状态权重矩阵 Q 与输入权重 R，观察状态轨迹与输入序列的变化。

核心原理

无约束MPC在每个采样时刻求解如下有限时域最优控制问题：

min  Σ_{k=0}^{N-1} ( x_k^T Q x_k + u_k^T R u_k ) + x_N^T P x_N
s.t. x_{k+1} = A x_k + B u_k

由于无约束且目标函数为二次型，该问题可转化为线性二次调节器（LQR）问题的有限时域形式，通过求解离散Riccati差分方程获得最优反馈增益序列。实际实现中，可直接使用矩阵运算求解该二次规划（QP），或利用LQR终端代价矩阵 P 近似无限时域解。本案例采用直接求解QP的方式，在每个采样时刻基于当前状态重新优化。

环境依赖

需要 Python 3.8+ 及以下库：

numpy, scipy, matplotlib

安装命令：pip install numpy scipy matplotlib

完整实现代码

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import solve_discrete_are

# 系统矩阵
A = np.array([[1.0, 0.1], [-0.2, 1.0]])
B = np.array([[0.0], [0.1]])
nx = A.shape[0]
nu = B.shape[1]

# 预测时域
N = 10

def mpc_control(x0, Q, R):
    """求解无约束MPC，返回最优输入序列和状态轨迹"""
    # 构建QP矩阵（无约束时可直接求解）
    # 使用LQR终端代价矩阵P（由离散Riccati方程求得）
    P = solve_discrete_are(A, B, Q, R)
    
    # 构建预测模型矩阵（批量形式）
    # 状态预测: X = Phi*x0 + Gamma*U
    Phi = np.zeros((N*nx, nx))
    Gamma = np.zeros((N*nx, N*nu))
    
    # 构建Phi
    A_pow = np.eye(nx)
    for i in range(N):
        Phi[i*nx:(i+1)*nx, :] = A_pow
        A_pow = A_pow @ A
    
    # 构建Gamma
    for i in range(N):
        A_pow = np.eye(nx)
        for j in range(i+1):
            if i-j > 0:
                A_pow = A_pow @ A
            elif i-j == 0:
                A_pow = np.eye(nx)
            else:
                break
        # 实际计算: Gamma[i] = A^(i)*B + A^(i-1)*B + ... + B
        # 简化计算方式
        Gamma_block = np.zeros((nx, N*nu))
        for j in range(i+1):
            A_j = np.linalg.matrix_power(A, i-j)
            Gamma_block[:, j*nu:(j+1)*nu] = A_j @ B
        Gamma[i*nx:(i+1)*nx, :] = Gamma_block
    
    # 构建代价矩阵
    Q_bar = np.kron(np.eye(N), Q)
    # 终端代价加入最后一步
    Q_bar[(N-1)*nx:, (N-1)*nx:] += P
    R_bar = np.kron(np.eye(N), R)
    
    # 无约束最优解: U* = - (Gamma^T Q_bar Gamma + R_bar)^(-1) Gamma^T Q_bar Phi x0
    H = Gamma.T @ Q_bar @ Gamma + R_bar
    F = Gamma.T @ Q_bar @ Phi
    
    # 最优输入序列
    U_opt = -np.linalg.solve(H, F @ x0)
    
    # 计算状态轨迹
    X_pred = Phi @ x0 + Gamma @ U_opt
    
    return U_opt, X_pred

def simulate_closed_loop(x0, Q, R, steps=20):
    """闭环仿真"""
    x = x0.copy()
    x_history = [x.copy()]
    u_history = []
    
    for _ in range(steps):
        U_opt, _ = mpc_control(x, Q, R)
        u = U_opt[0:nu]  # 只取第一步
        u_history.append(u.copy())
        x = A @ x + B @ u
        x_history.append(x.copy())
    
    return np.array(x_history), np.array(u_history)

# ============ 仿真与对比 ============
x0 = np.array([2.0, -1.0])
steps = 30

# 权重矩阵设置
configs = [
    {"name": "Q=I, R=1", "Q": np.eye(nx), "R": np.array([[1.0]])},
    {"name": "Q=10I, R=1", "Q": 10*np.eye(nx), "R": np.array([[1.0]])},
    {"name": "Q=I, R=0.1", "Q": np.eye(nx), "R": np.array([[0.1]])},
]

plt.figure(figsize=(12, 8))
colors = ['b', 'r', 'g']

for idx, cfg in enumerate(configs):
    x_hist, u_hist = simulate_closed_loop(x0, cfg["Q"], cfg["R"], steps)
    
    plt.subplot(2, 2, 1)
    plt.plot(x_hist[:, 0], label=cfg["name"], color=colors[idx])
    plt.xlabel("Time step")
    plt.ylabel("x1")
    plt.legend()
    plt.grid(True)
    
    plt.subplot(2, 2, 2)
    plt.plot(x_hist[:, 1], label=cfg["name"], color=colors[idx])
    plt.xlabel("Time step")
    plt.ylabel("x2")
    plt.legend()
    plt.grid(True)
    
    plt.subplot(2, 2, 3)
    plt.plot(u_hist[:, 0], label=cfg["name"], color=colors[idx])
    plt.xlabel("Time step")
    plt.ylabel("u")
    plt.legend()
    plt.grid(True)

plt.subplot(2, 2, 4)
# 绘制相轨迹
for idx, cfg in enumerate(configs):
    x_hist, _ = simulate_closed_loop(x0, cfg["Q"], cfg["R"], steps)
    plt.plot(x_hist[:, 0], x_hist[:, 1], label=cfg["name"], color=colors[idx])
plt.xlabel("x1")
plt.ylabel("x2")
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.show()

# 打印稳态误差（最终状态范数）
print("=== 闭环性能对比（30步后） ===")
for idx, cfg in enumerate(configs):
    x_hist, _ = simulate_closed_loop(x0, cfg["Q"], cfg["R"], steps)
    final_norm = np.linalg.norm(x_hist[-1])
    print(f"{cfg['name']}: 最终状态范数 = {final_norm:.4f}")

运行与验证

运行方式：将上述代码保存为 mpc_second_order.py，在终端执行 python mpc_second_order.py。

预期输出：程序将显示4个子图：状态 x1、x2 的时间响应曲线，控制输入 u 的序列，以及状态相平面轨迹。控制台输出三种权重配置下30步后的状态范数。

通过条件：

所有状态轨迹均收敛至原点附近（最终范数 < 0.05）
增大 Q（状态权重）时，状态收敛更快但输入幅值更大
减小 R（输入权重）时，控制作用更强，状态调节更迅速
相轨迹平滑趋近原点，无振荡发散

关键说明

本实现采用批量QP求解方式，在每个采样时刻基于当前状态重新计算最优输入序列，并仅施加第一步控制量（滚动时域控制）。终端代价矩阵 P 通过求解离散代数Riccati方程获得，用于近似无限时域最优性，保证闭环稳定性。代码中 mpc_control 函数返回完整预测序列，simulate_closed_loop 执行闭环仿真并记录状态与输入历史。