从零实现模型预测控制：QP求解器选型与轨迹跟踪实战

模型预测控制这几年在工业界和学术界的讨论热度一直居高不下，尤其是做自动驾驶轨迹跟踪、机器人运动控制、化工过程优化的朋友，几乎绕不开MPC这个词。但很多刚入门的朋友跟我一样，最初看论文时被那一堆预测时域、控制时域、滚动优化、二次规划搞得云里雾里，公式能看懂，但真到写代码实现的时候又不知道从哪下手。这篇内容就是把我自己从零实现MPC的完整过程拆开来讲，从核心思想到QP求解器的选型，再到一个完整的轨迹跟踪案例，尽量把每个环节的“为什么”说清楚。适合有一定控制理论基础、想动手实现MPC的工程师和学生，也适合已经用过MPC但对其内部机制一知半解的从业者。

1. 模型预测控制的核心思想与方案选型

1.1 为什么是“预测”而不是“反馈”

传统PID控制的核心逻辑是“误差出现后再修正”，它不关心系统未来会怎样，只看当前误差。这在很多场景下够用，但遇到有约束、有滞后、多变量耦合的系统时，PID就显得力不从心。MPC的出发点完全不同：它在每个控制周期内，利用系统的数学模型去预测未来一段时间内系统的行为，然后在这个预测的基础上求解一个优化问题，找到一串最优的控制输入序列，但只把第一个控制量施加给系统，下一个周期再重新预测、重新优化。

这个“滚动时域”的机制是MPC最本质的特征。你可以把它理解成开车时不断看前方路况调整方向盘，而不是等到车偏离车道了才猛打方向。预测时域就是你“看多远”，控制时域就是你“规划多远的动作”。看多远和规划多远这两个参数直接决定了MPC的性能和计算量，后面我会详细讲怎么选。

1.2 MPC相比LQR和PID的优势在哪

LQR也是一种基于模型的最优控制方法，它通过求解Riccati方程得到一个全局最优的静态反馈增益矩阵。但LQR有两个硬伤：一是无法显式处理约束，二是它优化的是无限时域的二次型代价，对时变参考轨迹的跟踪不够灵活。MPC则天然支持约束，你可以把执行器的物理限制、状态的安全边界都写进优化问题里，求解器会在满足这些约束的前提下找最优解。

PID的优势在于实现简单、不需要模型，但它的参数整定依赖经验，面对多输入多输出系统时耦合严重，调参难度指数级上升。MPC虽然需要模型，但一旦模型建立起来，约束和代价函数的调整非常直观，而且能统一处理多变量系统。

1.3 线性MPC与非线性MPC的取舍

MPC按模型类型分为线性MPC和非线性MPC。线性MPC假设系统动态可以用线性状态空间方程描述，优化问题转化为二次规划（QP），求解速度快，有成熟的求解器可用，实时性有保障。非线性MPC直接用非线性模型做预测，优化问题是非线性规划（NLP），求解慢且可能陷入局部最优，但对强非线性系统的控制效果更好。

我的建议是：如果你的系统在工作点附近可以用线性模型较好地近似，优先选线性MPC。绝大多数轨迹跟踪、过程控制场景，线性MPC已经足够。只有当系统非线性非常强、线性化误差不可接受时，才考虑非线性MPC。这篇内容主要围绕线性MPC展开，因为它是入门和工程落地的主力。

1.4 优化问题的数学形式

线性MPC的标准优化问题可以写成如下形式。假设系统模型为：

x(k+1) = A x(k) + B u(k)

其中x是状态向量，u是控制输入。在每个时刻k，我们求解：

min J = Σ [x(k+i|k)^T Q x(k+i|k) + u(k+i|k)^T R u(k+i|k)]

约束条件包括：

• 系统动态约束：x(k+i+1|k) = A x(k+i|k) + B u(k+i|k)
• 状态约束：x_min ≤ x(k+i|k) ≤ x_max
• 输入约束：u_min ≤ u(k+i|k) ≤ u_max

Q和R是权重矩阵，Q越大越注重状态收敛，R越大越注重控制量平滑。这个优化问题在每一时刻求解一次，得到最优控制序列后只取第一个元素施加。

2. 二次规划求解器的选型与实操要点

2.1 为什么MPC最终变成QP问题

线性MPC的代价函数是二次型，约束是线性的，这正好是二次规划（QP）的标准形式。QP问题的数学表达是：

min (1/2) z^T H z + f^T z s.t. A_eq z = b_eq A_ineq z ≤ b_ineq

其中z是决策变量，在MPC里就是未来N步的控制输入序列。H是Hessian矩阵，由Q、R和系统矩阵A、B组合而成。把MPC问题转化成QP标准形式是代码实现的关键一步，转化得好不好直接影响求解效率和数值稳定性。

2.2 常用QP求解器对比

我个人的选择习惯是：MATLAB环境下用quadprog做验证，Python环境下用OSQP做部署，C++环境下用qpOASES。OSQP的稀疏矩阵支持非常好，MPC问题天然稀疏，用OSQP能获得很好的性能。

2.3 把MPC问题转成QP标准形式的详细步骤

这一步是很多初学者卡住的地方。我以预测时域N=10、状态维度n=2、输入维度m=1为例，把转化过程拆开讲。

首先，把预测方程展开。给定当前状态x0，未来N步的状态可以写成：

X = Φ x0 + Γ U

其中X = [x1, x2, ..., xN]^T是堆叠的状态向量，U = [u0, u1, ..., u_{N-1}]^T是堆叠的输入向量。Φ和Γ是由A、B递推得到的矩阵：

Φ = [A; A^2; ...; A^N] Γ = [B, 0, ..., 0; AB, B, ..., 0; ...; A^{N-1}B, A^{N-2}B, ..., B]

然后代价函数J = X^T Q_bar X + U^T R_bar U，把X的表达式代入，整理成U的二次型：

J = U^T (Γ^T Q_bar Γ + R_bar) U + 2 x0^T Φ^T Q_bar Γ U + const

这样H = 2(Γ^T Q_bar Γ + R_bar)，f = 2 Γ^T Q_bar^T Φ x0。约束条件也类似地写成U的线性不等式。

注意：Q_bar和R_bar是块对角矩阵，由单步的Q和R扩展而来。构造这两个矩阵时要注意维度对齐，状态和输入的排列顺序要和Φ、Γ一致。

2.4 稀疏性与求解效率的优化

MPC问题的H矩阵通常是稀疏的，因为每个时刻的状态只和相邻时刻有关。OSQP利用这个稀疏性可以把求解速度提升一个数量级。构造H时用稀疏矩阵格式（如scipy.sparse.csc_matrix）而不是稠密矩阵，能显著减少内存占用和计算时间。

另一个优化点是热启动。相邻两个控制周期的QP问题非常相似，把上一周期的解作为本周期的初始猜测，能大幅减少迭代次数。OSQP和qpOASES都支持热启动，实测下来在轨迹跟踪场景中能减少30%到50%的求解时间。

3. 完整案例：二维轨迹跟踪的MPC实现

3.1 问题描述与系统建模

我选一个经典的二维轨迹跟踪案例：一个质点模型，状态是位置(x, y)和速度(vx, vy)，控制输入是加速度(ax, ay)。这个模型虽然简单，但足够说明MPC的完整流程，而且和自动驾驶、无人机轨迹跟踪的本质是一样的。

系统方程写成状态空间形式：

x(k+1) = A x(k) + B u(k)

其中：

• 状态向量 x = [px, py, vx, vy]^T
• 输入向量 u = [ax, ay]^T
• A = [[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]
• B = [[0.5dt^2, 0], [0, 0.5dt^2], [dt, 0], [0, dt]]

dt是采样时间，我取0.1秒。这个模型假设加速度在两个采样点之间保持不变，是零阶保持器的离散化结果。

3.2 参数选择与权重整定

参数选择是MPC调参的核心。预测时域N选多大？控制时域选多长？Q和R怎么定？这些问题没有标准答案，但有规律可循。

预测时域N决定了MPC“看多远”。N太小，系统看不到远处的约束和参考轨迹变化，控制会短视；N太大，计算量线性增长，而且远处的预测精度下降。经验法则是N * dt应该覆盖系统的主要动态响应时间。对于这个质点模型，我选N=20，对应2秒的预测窗口，足够覆盖从静止加速到目标速度的过程。

控制时域通常小于等于预测时域。为了简化，我让控制时域等于预测时域，即每一步都优化控制量。如果计算资源紧张，可以只优化前M步，后面M到N步的控制量保持不变。

Q和R的整定：Q是4x4矩阵，对应位置和速度的权重。位置权重设大一些（比如100），速度权重小一些（比如10），因为我们更关心位置跟踪精度。R是2x2矩阵，对应两个加速度输入的权重，设为单位矩阵的0.1倍，表示对控制量变化有一定惩罚但不苛刻。

实操心得：Q和R的比例比绝对值更重要。如果发现控制量抖动厉害，增大R；如果发现跟踪滞后，增大Q。我通常先把R设得很小，调Q到跟踪效果满意，再逐步增大R来平滑控制量。

3.3 Python代码实现

下面是完整的Python实现，用OSQP求解QP问题。代码可以直接运行，依赖numpy、scipy和osqp。

import numpy as np import scipy.sparse as sp import osqp # 系统参数 dt = 0.1 A = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) B = np.array([[0.5*dt**2, 0], [0, 0.5*dt**2], [dt, 0], [0, dt]]) n = 4 # 状态维度 m = 2 # 输入维度 N = 20 # 预测时域 # 权重矩阵 Q = np.diag([100, 100, 10, 10]) R = np.diag([0.1, 0.1]) # 构造预测矩阵 Phi = np.zeros((N*n, n)) Gamma = np.zeros((N*n, N*m)) A_pow = np.eye(n) for i in range(N): A_pow = A_pow @ A if i > 0 else A Phi[i*n:(i+1)*n, :] = A_pow for j in range(i+1): A_pow_j = np.linalg.matrix_power(A, i-j) Gamma[i*n:(i+1)*n, j*m:(j+1)*m] = A_pow_j @ B # 构造Q_bar和R_bar Q_bar = sp.kron(sp.eye(N), Q, format='csc') R_bar = sp.kron(sp.eye(N), R, format='csc') # 构造H和f的矩阵部分 H = 2 * (Gamma.T @ Q_bar.toarray() @ Gamma + R_bar.toarray()) H_sparse = sp.csc_matrix(H) # 约束：加速度范围 [-1, 1] u_min = -1.0 u_max = 1.0 A_ineq = sp.vstack([sp.eye(N*m), -sp.eye(N*m)], format='csc') l_ineq = np.full(N*m, -np.inf) l_ineq = np.concatenate([np.full(N*m, u_min), np.full(N*m, -u_max)]) u_ineq = np.concatenate([np.full(N*m, u_max), np.full(N*m, -u_min)]) # 创建OSQP求解器 prob = osqp.OSQP() prob.setup(P=H_sparse, q=np.zeros(N*m), A=A_ineq, l=l_ineq, u=u_ineq, verbose=False, warm_start=True) # 仿真参数 T_sim = 10.0 steps = int(T_sim / dt) x = np.array([0, 0, 0, 0], dtype=float) # 初始状态 trajectory = [] for k in range(steps): # 参考轨迹：圆形 t = k * dt ref_x = 5 * np.cos(0.5 * t) ref_y = 5 * np.sin(0.5 * t) ref_vx = -2.5 * np.sin(0.5 * t) ref_vy = 2.5 * np.cos(0.5 * t) x_ref = np.tile(np.array([ref_x, ref_y, ref_vx, ref_vy]), N) # 更新f向量 f = 2 * Gamma.T @ Q_bar.toarray() @ (Phi @ x - x_ref) prob.update(q=f) # 求解 res = prob.solve() u_opt = res.x[:m] # 施加第一个控制量 x = A @ x + B @ u_opt trajectory.append(x.copy()) trajectory = np.array(trajectory)

这段代码的核心逻辑是：每个控制周期更新f向量（因为x0变了），然后调用OSQP求解，取第一个控制量施加给系统。热启动通过warm_start=True开启，OSQP会自动利用上一次的解。

3.4 仿真结果分析

跑完这段代码，你会看到质点从原点出发，逐渐跟踪上圆形参考轨迹。前几秒会有明显的跟踪误差，因为初始状态和参考轨迹差距大，MPC在约束范围内全力加速。大约2到3秒后，跟踪误差收敛到很小的范围。

如果发现跟踪效果不理想，可以从这几个方向排查：预测时域N是否足够覆盖动态过程；Q矩阵中位置权重是否够大；加速度约束是否太紧导致无法及时跟踪。我实测下来，N=20、Q位置权重100、加速度限制±1这个配置，跟踪半径5米、角速度0.5rad/s的圆形轨迹，稳态误差在0.05米以内。

4. 常见问题与排查技巧实录

4.1 QP求解失败或无解怎么办

这是MPC落地时最常见的问题。求解失败通常有几个原因：约束之间互相矛盾，导致可行域为空；H矩阵不是正定的，导致QP非凸；数值精度问题，矩阵条件数太大。

排查步骤：先检查约束是否合理。比如加速度上下限是否写反了，状态约束是否和初始状态冲突。然后检查H矩阵的正定性，H = 2(Γ^T Q_bar Γ + R_bar)，只要Q和R是正定的，H就是正定的。如果Q或R有零特征值，加一个小正则项（如1e-6 * I）保证正定。

如果约束确实可能导致无解，可以引入软约束：在代价函数里加一个松弛变量，允许约束被轻微违反，但违反量会被惩罚。这样即使原问题无解，也能得到一个次优但可用的解。

4.2 控制量抖动严重怎么调

控制量抖动通常是因为R太小，或者预测时域太短。R太小意味着对控制量变化的惩罚不够，求解器会倾向于用剧烈的控制动作来快速消除误差。增大R能平滑控制量，但会牺牲跟踪速度。

另一个原因是模型和实际系统不匹配。如果模型预测的状态和实际状态偏差大，MPC会不断修正，导致控制量振荡。这时候需要重新辨识模型参数，或者降低模型精度要求，增大R来容忍模型误差。

我踩过的一个坑是：采样时间dt选得太小，导致离散化后的B矩阵数值很小，控制量对状态的影响被削弱，求解器为了达到同样的控制效果会输出很大的控制量，进而引发抖动。后来把dt从0.01调到0.1，问题就解决了。

4.3 预测时域和控制时域怎么选

这个问题没有万能答案，但有几个经验规则。预测时域N * dt应该至少覆盖系统阶跃响应的上升时间。对于一阶系统，上升时间约3倍时间常数；对于二阶系统，约4到5倍。控制时域M通常取N的10%到20%，如果计算资源充足，取M=N效果最好。

如果发现系统响应慢、跟踪滞后，先增大N。如果发现计算时间太长，先减小M。N和M的调整会相互影响，建议固定一个调另一个，观察效果变化。

4.4 常见问题速查表

4.5 几个容易被忽略的实操细节

第一个细节是状态约束的处理。很多人只加输入约束，不加状态约束，结果系统状态跑到了物理上不可能的区域。状态约束在QP里体现为对X的线性不等式，而X = Φ x0 + Γ U，所以约束要转化成对U的约束。转化过程中要注意Φ x0是已知量，移到不等式右边。

第二个细节是参考轨迹的预处理。如果参考轨迹有跳变，MPC会试图用有限的控制量去跟踪一个不可能跟踪的信号，导致求解器输出饱和。实际使用中要对参考轨迹做平滑滤波，或者用参考轨迹的变化率作为前馈。

第三个细节是求解器的终止条件。OSQP默认的终止精度是1e-3，对于大多数控制场景够用。如果发现求解结果精度不够，可以调到1e-4或1e-5，但求解时间会增加。我通常先用默认精度跑通，再根据效果微调。

4.6 从仿真到实机的迁移经验

仿真跑通只是第一步，实机上还有几个坑要填。首先是传感器噪声，仿真里状态是精确已知的，实机上要用状态估计器（如卡尔曼滤波）从带噪声的测量中估计状态。状态估计的滞后和误差会直接影响MPC的性能，需要在Q矩阵里适当降低对速度状态的权重，因为速度通常估计得不如位置准。

其次是执行器延迟，仿真里假设控制量立即生效，实机上从计算完到执行器响应有延迟。补偿方法是在预测模型里把延迟建模进去，或者用Smith预估器。我通常先在模型里加一个纯延迟环节，看MPC能否补偿，如果不行再考虑更复杂的方案。

最后是计算平台的算力。仿真在PC上跑，实机可能在嵌入式平台上跑，算力差几十倍。迁移前要评估QP求解时间是否满足控制周期要求。如果不够，可以减小N和M，或者用C语言重写求解器，或者换用更高效的求解器如qpOASES。

5. MPC的扩展方向与进阶思路

5.1 从线性MPC到自适应MPC

线性MPC假设模型参数固定，但实际系统参数可能随时间变化。自适应MPC在线辨识模型参数，并实时更新预测模型。实现方式有两种：一是递推最小二乘法在线辨识A、B矩阵，二是用多个模型切换。自适应MPC的难点在于辨识的稳定性和计算量，参数变化太快会导致辨识发散，变化太慢又跟不上。

5.2 从确定性MPC到鲁棒MPC

实际系统有扰动和模型不确定性，确定性MPC在这些情况下可能违反约束。鲁棒MPC考虑最坏情况下的扰动，保证约束在所有可能情况下都满足。常见方法有管状MPC（Tube MPC）和min-max MPC。管状MPC把实际状态约束在一个以标称轨迹为中心的管子里，管子半径由扰动上界决定。鲁棒MPC的代价是保守性增加，控制性能下降。

5.3 MPC与学习方法的结合

最近几年学习MPC是个热点，用神经网络学习MPC的控制律，或者用强化学习调MPC的权重。学习MPC的优势是推理速度快，训练好后不需要在线求解QP，适合算力受限的场景。但缺点是泛化能力有限，训练分布外的场景可能失效。我的看法是，学习MPC适合特定场景的定制化部署，通用性不如传统MPC。

5.4 显式MPC的思路

显式MPC把QP的求解过程离线化，预先计算所有可能状态下的最优控制律，在线时只需要查表。显式MPC适合状态维度低、约束简单的场景，因为状态维度一高，离线计算的复杂度指数增长。对于状态维度小于等于3的系统，显式MPC非常实用，在线计算时间可以做到微秒级。

6. 工程落地中的性能优化技巧

6.1 代码层面的优化

QP求解是MPC的计算瓶颈，优化求解器调用是关键。第一，用稀疏矩阵格式构造H和A_ineq，OSQP对稀疏矩阵的处理效率远高于稠密矩阵。第二，开启热启动，相邻周期的解非常接近，热启动能减少迭代次数。第三，预分配所有数组，避免在控制循环里动态分配内存。第四，如果用的是Python，把求解器调用封装成C扩展或者用Cython加速。

6.2 问题规模的缩减

如果计算资源实在紧张，可以从这几个方向缩减问题规模。降低预测时域N，但要注意N太小会影响稳定性。增大采样时间dt，但dt太大会降低控制精度。只优化控制时域M步，后面N-M步的控制量保持不变。对状态进行降维，去掉对控制目标影响小的状态。

6.3 多速率MPC的设计

有些系统不同状态的动态时间尺度差异很大，比如位置变化慢、电流变化快。这时候可以用多速率MPC：慢状态用大采样时间，快状态用小采样时间。实现上可以用两个不同频率的MPC级联，慢MPC输出参考给快MPC。这种设计能兼顾计算效率和控