2. 线性MPC的QP求解器初选比较

任务目标

针对线性MPC中典型的二次规划（QP）问题，在相同硬件与问题规模下，比较OSQP、qpOASES、GUROBI三个求解器的求解时间与精度，并据此给出候选求解器的初步筛选结论。

问题定义

考虑一个离散时间线性时不变系统，MPC每步需要求解如下QP：

min  0.5 * u' * H * u + x' * F' * u
s.t. lb <= u <= ub
     A * u <= b

其中决策变量为控制序列 u ∈ R^m，H 为Hessian矩阵（正定），F 为状态反馈矩阵，x 为当前状态。本例取预测时域 N=10，控制维度 nu=2，因此 m = N*nu = 20。约束包含控制量幅值约束与相邻控制增量约束。

环境与依赖

  • Python 3.9+，NumPy，SciPy
  • OSQP：pip install osqp
  • qpOASES：pip install qpoases（或从GitHub源码编译）
  • GUROBI：pip install gurobipy，需licence（学术免费）

关键完整代码

import numpy as np
import time
from scipy import sparse

# ---------- 问题生成（线性MPC标准QP） ----------
np.random.seed(42)
N = 10          # 预测时域
nu = 2          # 控制维度
m = N * nu      # 决策变量维度

# 系统矩阵（双积分器离散化）
dt = 0.1
A = np.array([[1, dt], [0, 1]])
B = np.array([[0.5*dt**2], [dt]])
nx = 2

# 代价矩阵
Q = np.eye(nx) * 10.0
R = np.eye(nu) * 0.1

# 构建MPC的QP数据（批量形式）
# 状态传播: x_{k+1} = A x_k + B u_k
# 代价: sum (x_k'Q x_k + u_k'R u_k)
# 此处直接构造等效的H和F（标准批量QP形式）
H = np.zeros((m, m))
F = np.zeros((nx, m))
for i in range(N):
    H[i*nu:(i+1)*nu, i*nu:(i+1)*nu] = R
    # 状态代价通过传播矩阵折算到控制上（简化：仅终端代价近似）
    # 为演示求解器比较，使用随机正定H与随机F
H += np.eye(m) * 0.01
H = (H + H.T) / 2
# 确保正定
H += np.eye(m) * 1.0
F = np.random.randn(nx, m) * 0.1

x0 = np.array([1.0, 0.0])   # 初始状态

# 约束：控制幅值 |u| <= 1，增量约束通过差分矩阵实现
lb = -np.ones(m)
ub = np.ones(m)

# 增量约束: |u_{k+1} - u_k| <= 0.5
D = np.zeros((m-1, m))
for i in range(m-1):
    D[i, i] = -1
    D[i, i+1] = 1
A_ineq = D
b_ineq = np.ones(m-1) * 0.5

# ---------- 求解器接口 ----------
def solve_osqp(H, F, x0, lb, ub, A_ineq, b_ineq):
    import osqp
    P = sparse.csc_matrix(H)
    q = F.T @ x0
    A_full = sparse.vstack([sparse.eye(m), A_ineq]).tocsc()
    l = np.concatenate([lb, -np.inf*np.ones(m-1)])
    u = np.concatenate([ub, b_ineq])
    prob = osqp.OSQP()
    prob.setup(P=P, q=q, A=A_full, l=l, u=u, verbose=False)
    res = prob.solve()
    return res.x, res.info.obj_val, res.info.run_time

def solve_qpoases(H, F, x0, lb, ub, A_ineq, b_ineq):
    from qpoases import PyQProblem, PyOptions, PyPrintLevel
    n = m
    nC = m-1
    H_qp = H.copy()
    g = F.T @ x0
    A_qp = np.vstack([np.eye(n), A_ineq])  # 合并为不等式形式
    lbA = np.concatenate([lb, -np.inf*np.ones(nC)])
    ubA = np.concatenate([ub, b_ineq])
    qp = PyQProblem(n, nC)
    options = PyOptions()
    options.printLevel = PyPrintLevel.NONE
    qp.setOptions(options)
    t0 = time.time()
    status = qp.init(H_qp, g, A_qp, lb, ub, lbA, ubA, nWSR=500)
    elapsed = time.time() - t0
    u_opt = qp.getPrimalSolution()
    obj = 0.5 * u_opt @ H @ u_opt + g @ u_opt
    return u_opt, obj, elapsed

def solve_gurobi(H, F, x0, lb, ub, A_ineq, b_ineq):
    import gurobipy as gp
    from gurobipy import GRB
    with gp.Env(empty=True) as env:
        env.setParam('OutputFlag', 0)
        env.start()
        with gp.Model(env=env) as model:
            u = model.addMVar(m, lb=lb, ub=ub, name='u')
            g = F.T @ x0
            obj = 0.5 * u @ H @ u + g @ u
            model.setObjective(obj, GRB.MINIMIZE)
            model.addMConstr(A_ineq, u, GRB.LESS_EQUAL, b_ineq)
            model.optimize()
            return u.X, model.ObjVal, model.Runtime

# ---------- 运行比较 ----------
solvers = {
    'OSQP': solve_osqp,
    'qpOASES': solve_qpoases,
    'GUROBI': solve_gurobi,
}

results = {}
for name, solver in solvers.items():
    try:
        u_opt, obj, t = solver(H, F, x0, lb, ub, A_ineq, b_ineq)
        results[name] = {'u': u_opt, 'obj': obj, 'time_s': t}
        print(f"{name:10s}  time={t*1000:8.3f} ms   obj={obj:12.6f}")
    except Exception as e:
        print(f"{name:10s}  FAILED: {e}")

# ---------- 精度比较（以GUROBI为参考，若可用） ----------
if 'GUROBI' in results and 'OSQP' in results:
    ref = results['GUROBI']['u']
    for name in ['OSQP', 'qpOASES']:
        if name in results:
            err = np.linalg.norm(results[name]['u'] - ref)
            print(f"{name} vs GUROBI: ||Δu|| = {err:.2e}")

运行方式

  1. 安装依赖：pip install osqp qpoases gurobipy numpy scipy
  2. 将上述代码保存为 qp_solver_compare.py，直接运行：python qp_solver_compare.py
  3. 若GUROBI无licence，可注释掉GUROBI部分，仅比较OSQP与qpOASES，并以OSQP高精度解作为参考。

验证方法与判定标准

  • 可行性验证：三个求解器返回的 u 均应满足 lb ≤ u ≤ ub 且 A_ineq @ u ≤ b_ineq（可在代码后追加断言检查）。
  • 精度对比：以GUROBI（或OSQP高精度）解为参考，计算其他求解器解的2-范数误差。若误差 < 1e-4，认为精度满足工程需求。
  • 时间对比：观察各求解器单次求解耗时。通常OSQP与qpOASES在毫秒级，GUROBI在几十毫秒级。若某求解器耗时超过其他求解器10倍以上，且精度无显著优势，则初步排除。
  • 筛选结论：若目标为嵌入式实时控制（采样周期 < 10ms），优先选择OSQP或qpOASES；若目标为离线仿真或需要处理更复杂约束，GUROBI更稳健。

预期输出（示例，非实测）

OSQP       time=   0.523 ms   obj=   12.345678
qpOASES    time=   0.812 ms   obj=   12.345679
GUROBI     time=  18.234 ms   obj=   12.345678
OSQP vs GUROBI: ||Δu|| = 3.2e-07
qpOASES vs GUROBI: ||Δu|| = 8.1e-07

以上数值仅为格式示例，实际数值取决于硬件与问题实例。判定标准为：误差小于1e-4且耗时满足实时性要求者，进入候选列表。

常见问题

  • qpOASES安装失败：Linux/macOS下建议从源码编译；Windows可用WSL或conda安装。
  • GUROBI licnce问题：学术licence免费申请，或使用gurobipy的env.setParam('OutputFlag',0)抑制输出。
  • H非正定：若H非正定，OSQP可能收敛慢或失败。本代码已通过H += np.eye(m)保证正定，实际MPC中H通常正定。
← 上一章 📖 返回目录 下一章 →