3. 基于OSQP的MPC求解器配置与验证

任务目标

本案例将完成一个标准线性MPC问题的OSQP求解闭环：构建二次规划(QP)形式的MPC优化问题，配置OSQP求解器参数（容差、迭代上限），验证求解结果满足约束且代价函数达到最优，并对比不同参数配置对求解精度与迭代次数的影响。

环境依赖

需要 osqp 与 numpy。安装命令：pip install osqp numpy。本案例使用OSQP原生Python接口，不依赖CasADi等高级封装，以便直接观察求解器参数配置。

MPC问题定义

考虑离散时间线性系统 x_{k+1} = A x_k + B u_k，预测时域 N=10，状态维度 n=2，控制维度 m=1。目标函数为 Σ(x^T Q x + u^T R u)，约束为控制输入限幅 |u| ≤ 1 和状态约束 |x| ≤ 2。将MPC滚动优化转化为标准QP：

min 0.5 z^T P z + q^T z,  s.t. l ≤ A_ineq z ≤ u_ineq

其中 z = [u_0, x_1, u_1, x_2, ..., u_{N-1}, x_N] 为决策变量。

关键完整代码

import numpy as np
import osqp
from scipy import sparse

# 系统模型
A = np.array([[1.0, 0.1], [0.0, 1.0]])
B = np.array([[0.0], [1.0]])
n, m = 2, 1
N = 10  # 预测时域

# 权重矩阵
Q = np.diag([1.0, 1.0])
R = np.array([[0.1]])

# 初始状态与目标
x0 = np.array([1.5, 0.5])
x_ref = np.zeros(n)

# 构建QP矩阵
nx = n * (N + 1)  # x0..xN
nu = m * N        # u0..uN-1
nz = nx + nu

# 等式约束: x_{k+1} = A x_k + B u_k
Aeq_rows, Aeq_cols, Aeq_vals = [], [], []
leq, ueq = [], []
# 初始状态约束 x0 = x_init
for i in range(n):
    Aeq_rows.append(i); Aeq_cols.append(i); Aeq_vals.append(1.0)
    leq.append(x0[i]); ueq.append(x0[i])

# 动态约束
for k in range(N):
    row_offset = n * (k + 1)
    u_offset = nx + m * k
    for i in range(n):
        row = row_offset + i
        # x_{k+1} - A x_k - B u_k = 0
        for j in range(n):
            Aeq_rows.append(row); Aeq_cols.append(n*k + j); Aeq_vals.append(-A[i, j])
        Aeq_rows.append(row); Aeq_cols.append(row_offset + i); Aeq_vals.append(1.0)
        for j in range(m):
            Aeq_rows.append(row); Aeq_cols.append(u_offset + j); Aeq_vals.append(-B[i, j])
        leq.append(0.0); ueq.append(0.0)

# 不等式约束: |u_k| ≤ 1, |x_k| ≤ 2 (x0除外)
Aineq_rows, Aineq_cols, Aineq_vals = [], [], []
lineq, uineq = [], []
for k in range(N):
    u_idx = nx + m * k
    # u_k ≤ 1, -u_k ≤ 1
    Aineq_rows.append(len(lineq)); Aineq_cols.append(u_idx); Aineq_vals.append(1.0)
    lineq.append(-np.inf); uineq.append(1.0)
    Aineq_rows.append(len(lineq)); Aineq_cols.append(u_idx); Aineq_vals.append(-1.0)
    lineq.append(-np.inf); uineq.append(1.0)

for k in range(1, N+1):
    for i in range(n):
        x_idx = n*k + i
        Aineq_rows.append(len(lineq)); Aineq_cols.append(x_idx); Aineq_vals.append(1.0)
        lineq.append(-np.inf); uineq.append(2.0)
        Aineq_rows.append(len(lineq)); Aineq_cols.append(x_idx); Aineq_vals.append(-1.0)
        lineq.append(-np.inf); uineq.append(2.0)

# 目标函数: 0.5 z^T P z + q^T z
P = sparse.csc_matrix((nz, nz))
q = np.zeros(nz)
# 状态代价 (x_k - x_ref)^T Q (x_k - x_ref), k=1..N
for k in range(1, N+1):
    for i in range(n):
        idx = n*k + i
        for j in range(n):
            P[idx, idx] += Q[i, j]  # 对角近似，非对角项忽略（Q为对角阵）
# 控制代价 u^T R u
for k in range(N):
    u_idx = nx + m*k
    P[u_idx, u_idx] += R[0, 0]

P = sparse.csc_matrix(P)
# 组装约束矩阵
A_total = sparse.vstack([
    sparse.csc_matrix((np.array(Aeq_rows), (np.array(Aeq_cols), np.array(Aeq_vals))), shape=(len(leq), nz)),
    sparse.csc_matrix((np.array(Aineq_rows), (np.array(Aineq_cols), np.array(Aineq_vals))), shape=(len(lineq), nz))
], format='csc')
l_total = np.concatenate([leq, lineq])
u_total = np.concatenate([ueq, uineq])

# 求解器配置与求解
solver = osqp.OSQP()
solver.setup(P=P, q=q, A=A_total, l=l_total, u=u_total,
             eps_abs=1e-5, eps_rel=1e-5, max_iter=5000, verbose=False)
result = solver.solve()

# 提取控制序列
u_opt = result.x[nx:nx+m*N].reshape(N, m)
x_traj = result.x[:nx].reshape(N+1, n)

# 验证：约束满足性
u_max_viol = np.max(np.abs(u_opt)) - 1.0
x_max_viol = np.max(np.abs(x_traj[1:])) - 2.0
print(f"求解状态: {result.info.status}")
print(f"迭代次数: {result.info.iter}")
print(f"目标函数值: {result.info.obj_val:.6f}")
print(f"控制约束最大违反: {u_max_viol:.2e}")
print(f"状态约束最大违反: {x_max_viol:.2e}")
print(f"最优控制序列 u*: {u_opt.flatten()}")

运行与验证方法

直接运行上述代码。判定通过条件：

• 求解状态：result.info.status 输出 solved，表示OSQP在迭代上限内收敛。
• 约束满足：u_max_viol 与 x_max_viol 均为负值或接近0（1e-4量级），说明控制与状态均未超出限幅。
• 最优性验证：目标函数值 obj_val 为有限正数，且与手动计算代价一致（可自行用 u_opt 和 x_traj 代入代价函数验证）。

参数调优对比

将 eps_abs 和 eps_rel 从 1e-5 改为 1e-3，重新运行，观察迭代次数减少但目标函数精度下降（obj_val 偏差增大）。将 max_iter 改为 100，观察状态变为 maximum iterations reached，此时结果不可用——这验证了迭代上限配置对求解成功与否的决定性影响。

预期输出（示例）

求解状态: solved
迭代次数: 45
目标函数值: 1.234567
控制约束最大违反: -3.21e-06
状态约束最大违反: -1.08e-05
最优控制序列 u*: [-0.823  -0.412  -0.198  ...]

注意：具体数值由OSQP内部算法决定，不同版本可能有微小差异，但状态必须为 solved 且违反量在 1e-4 以下。