MPC 里的约束与 QP 求解器实战——从原理、例子到 C++ 落地

本文介绍了模型预测控制（MPC）的基本原理及其在二次规划（QP）求解器中的实现。MPC通过预测未来状态并优化控制输入来跟踪参考轨迹，同时满足系统约束。文章详细讲解了MPC的标准问题公式、约束类型及QP求解器原理，并通过双积分器系统的实例展示了从问题构建到C++落地的完整流程。使用开源工具OSQP求解器，结合Eigen库实现高效实时控制，适用于自动驾驶、机器人等领域。

likuolei

159人浏览 · 2026-02-06 16:30:27

likuolei · 2026-02-06 16:30:27 发布

MPC 中的约束与 QP 求解器实战——从原理、例子到 C++ 落地

模型预测控制（MPC）是现代控制理论中的核心技术，尤其在自动驾驶、机器人、化工过程等领域广泛应用。它通过预测未来状态并优化控制输入来实现系统跟踪参考轨迹，同时严格满足约束。MPC 的核心是实时求解一个有限时域优化问题，通常转化为二次规划（QP）问题来求解。

本文从原理（MPC 公式与约束）、例子（简单线性系统）、C++ 落地（使用 OSQP 求解器）一步步讲解。基于主流开源工具如 OSQP（高效 QP 求解器），适用于实时控制场景。假设你有线性系统基础，如果需要非线性 MPC 可以进一步讨论。

一、MPC 原理与 QP 求解器概述

MPC 的本质：在每个采样时刻，基于当前状态预测未来 N 步（预测视窗），求解一个优化问题，得到控制序列，但只应用第一个控制输入，然后滚动优化。

MPC 标准问题公式（线性时不变系统，带约束的线性二次型）：

[
\min_{x_k, u_k} \quad (x_N - x_r)^T Q_N (x_N - x_r) + \sum_{k=0}^{N-1} (x_k - x_r)^T Q (x_k - x_r) + u_k^T R u_k
]

( x_k \in \mathbb{R}^{n_x} ): 第 k 步预测状态
( u_k \in \mathbb{R}^{n_u} ): 第 k 步控制输入
( x_r ): 参考状态（目标轨迹）
( Q, Q_N \succ 0 ): 状态代价矩阵（Q_N 是终端代价）
( R \succ 0 ): 输入代价矩阵
N: 预测视窗长度

约束（MPC 的关键特色，使其优于 PID/LQR 等无约束控制器）：

约束类型	数学形式	物理含义	QP 中的体现
动态约束	( x_{k+1} = A x_k + B u_k )	系统模型（必须满足）	等式约束（Aeq, leq = ueq）
状态约束	( x_{\min} \leq x_k \leq x_{\max} )	安全界限（如位置/速度限）	不等式约束（lineq, uineq）
输入约束	( u_{\min} \leq u_k \leq u_{\max} )	执行器限幅（如电机扭矩限）	不等式约束（lineq, uineq）
初始约束	( x_0 = \bar{x} )	当前测量状态（滚动起点）	等式约束（更新 leq/ueq）
终端约束（可选）	( x_N \in \mathcal{X}_f )	保证稳定性（如终端集）	额外不等式或等式约束

为什么用 QP？ MPC 问题是凸优化（二次目标 + 线性约束），QP 是其标准形式。QP 问题一般形式：
[
\min_x \frac{1}{2} x^T P x + q^T x \quad s.t. \quad A x = b, \quad l \leq C x \leq u
]
MPC 通过向量栈（stack x = [x0, x1, …, xN, u0, …, u_{N-1}]）转化为这个形式。

QP 求解器原理：

活跃集法（Active-Set）：迭代激活/去激活约束，适合小规模问题（如 qpOASES）。
内点法（Interior-Point）：从可行域内部逼近最优，适合大规模（如 OSQP 的 ADMM 变体）。OSQP 使用操作符分裂（Operator Splitting）实现 ADMM，高效处理稀疏矩阵，支持热启动（warm-starting），实时性强。

求解流程：

构建 QP 矩阵（P, q, A, l, u）。
初始化求解器。
求解，提取第一个 u0。
更新 x0，重复。

二、简单例子：双积分器系统（车辆位置控制）

考虑一个简单线性系统：双积分器（位置-速度模型），如无人车纵向控制。

系统：( \dot{x} = \begin{bmatrix} 0 & 1 \ 0 & 0 \end{bmatrix} x + \begin{bmatrix} 0 \ 1 \end{bmatrix} u )（离散化后 A, B）。
状态 x = [位置, 速度]，输入 u = 加速度。
参考：位置跟踪到 10m。
约束：速度 |v| ≤ 5 m/s，|u| ≤ 2 m/s²。
N=10, Q=diag(1,0.1), R=0.01。

QP 构建（基于 Python 示例，作为 C++ 蓝图）：

import osqp
import numpy as np
from scipy import sparse

# 系统参数（离散化 A, B）
nx, nu = 2, 1
Ad = np.array([[1., 1.], [0., 1.]])  # 采样时间 Ts=1
Bd = np.array([[0.5], [1.]])

N = 10
Q = sparse.diags([1., 0.1])
QN = Q
R = sparse.diags([0.01])

xr = np.array([10., 0.])  # 参考状态
x0 = np.array([0., 0.])   # 初始状态

xmin, xmax = np.array([-np.inf, -5.]), np.array([np.inf, 5.])
umin, umax = -2., 2.

# 代价矩阵 P, q
P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
                       sparse.kron(sparse.eye(N), R)], format='csc')
q = np.hstack([np.kron(np.ones(N), -Q@xr), -QN@xr, np.zeros(N*nu)])

# 等式约束（动态 + 初始）
Ax = sparse.kron(sparse.eye(N+1), -sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-x0, np.zeros(N*nx)])
ueq = leq

# 不等式约束（状态 + 输入界限）
Aineq = sparse.eye((N+1)*nx + N*nu)
lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])

# 组合约束
A = sparse.vstack([Aeq, Aineq], format='csc')
l = np.hstack([leq, lineq])
u = np.hstack([ueq, uineq])

# 设置 OSQP
prob = osqp.OSQP()
prob.setup(P, q, A, l, u, warm_start=True)

# 求解
res = prob.solve()
if res.info.status != 'solved':
    print("求解失败！")

# 提取第一个 u
ctrl = res.x[-N*nu:-(N-1)*nu]
print("第一个控制输入:", ctrl)

模拟滚动：应用 ctrl 后，更新 x0 = A x0 + B ctrl，修改 l[:nx] = -x0, u[:nx] = -x0，然后 prob.update(l=l, u=u) 重新求解。
约束作用：如果无约束，速度可能超限；带约束后，优化会自动饱和到边界。

三、C++ 落地实战（使用 OSQP-MPC 库）

OSQP 是高效的 QP 求解器，结合 Eigen（线性代数库），可以轻松实现实时 MPC。这里推荐 OSQP-MPC 单头文件库：轻量级、易集成，支持约束。

准备：

下载 OSQP-MPC.hpp（GitHub 单文件）。
依赖：Eigen 3.x, OSQP。
编译：g++ -std=c++11 main.cpp -losqp（需安装 OSQP）。

完整 C++ 示例（基于双积分器系统）：

#include "osqp_mpc.hpp"  // 下载自 GitHub
#include <Eigen/Dense>
#include <iostream>
#include <osqp/osqp.h>  // OSQP 头文件

int main() {
    // 1. 定义视窗和维度
    std::size_t N = 10;  // 预测视窗
    std::size_t nx = 2;  // 状态维度
    std::size_t nu = 1;  // 输入维度
    MPCProblem mpc(N);

    // 2. 设置系统矩阵（离散化）
    Eigen::MatrixXd A(nx, nx);
    A << 1.0, 1.0, 0.0, 1.0;  // Ad (Ts=1)
    Eigen::MatrixXd B(nx, nu);
    B << 0.5, 1.0;  // Bd

    // 3. 设置代价矩阵
    Eigen::MatrixXd Q = Eigen::DiagonalMatrix<double, 2>(Eigen::Vector2d(1.0, 0.1));
    Eigen::MatrixXd R = Eigen::DiagonalMatrix<double, 1>(Eigen::Vector1d(0.01));
    Eigen::MatrixXd QN = Q;  // 终端代价

    // 4. 设置参考和初始状态
    Eigen::VectorXd xr(nx);
    xr << 10.0, 0.0;  // 参考位置 10m, 速度 0
    Eigen::VectorXd x0(nx);
    x0 << 0.0, 0.0;  // 初始

    // 5. 填充 MPCProblem
    mpc.x0 = x0;
    for (std::size_t i = 0; i < N; ++i) {
        mpc.xref[i] = xr;
        mpc.Q[i] = Q;
        mpc.C[i] = Eigen::MatrixXd::Identity(nx, nx);  // 状态约束矩阵 (默认 box)
        mpc.lbx[i] = Eigen::VectorXd(nx); mpc.lbx[i] << -INFINITY, -5.0;  // x_min
        mpc.ubx[i] = Eigen::VectorXd(nx); mpc.ubx[i] << INFINITY, 5.0;   // x_max
        if (i < N - 1) {
            mpc.A[i] = A;
            mpc.B[i] = B;
            mpc.R[i] = R;
            mpc.D[i] = Eigen::MatrixXd::Identity(nu, nu);  // 输入约束矩阵
            mpc.lbu[i] = Eigen::VectorXd(nu); mpc.lbu[i] << -2.0;  // u_min
            mpc.ubu[i] = Eigen::VectorXd(nu); mpc.ubu[i] << 2.0;   // u_max
        }
    }
    mpc.Q[N-1] = QN;  // 终端

    // 6. OSQP 设置
    OSQPSettings settings;
    osqp_set_default_settings(&settings);
    settings.verbose = true;  // 打印求解信息

    // 7. 求解 MPC
    MPCSolution sol = solveMPC(mpc, &settings);

    // 8. 提取第一个控制输入
    OSQPVector u0 = sol.ustar[0];
    std::cout << "第一个控制输入 u0: " << u0.transpose() << std::endl;

    // 9. 模拟下一步（更新 x0）
    x0 = A * x0 + B * u0;
    mpc.x0 = x0;
    // 重复求解...

    return 0;
}