简介：这是一套面向MATLAB与C++混合编程的MPCC（含互补约束的数学规划）优化求解代码包，适合研究自动驾驶轨迹规划、模型预测控制及非线性优化的工程师与研究生使用。包内围绕MPC控制问题提供完整实现，包含MATLAB脚本、C++头文件与源文件、仿真动态图、参数配置文件、说明文档及安装脚本等，共94个文件，压缩包约15MB，目录按模型、约束、代价、求解器接口、测试等模块划分，便于定位与二次开发。已有581人在线学习。代码涵盖从问题建模、离散线性化、边界处理到不同求解器接口（如hpipm、CVX、Yalmip、QuadProg）的完整链路，并附有车辆模型、障碍物状态、仿真日志与绘图脚本，可直接运行simulation.m观察MPCC在变道、避障场景下的效果，是理解MPCC数值稳定性与收敛性处理的经典参考。
1. MPCC 经典代码到底解决什么问题
搜索 MPCC 经典代码的人，大概率是这两类：一类刚啃完状态空间和 LQR，想把模型预测控制从一堆连加式从纸面变成能跑的循环；另一类是手里已经有一套控制方案在跑，但系统加了约束之后，控制量天天顶到饱和，甚至因为超出执行器极限直接翻车。这里的 MPCC 我按工程社区里最常见的理解来处理：带约束的模型预测控制，也就是 MPC with Constraints。它和教科书里那个互补约束数学规划（Mathematical Program with Complementarity Constraints）是两回事，检索这个词的绝大多数人找的都是前者——滚动时域里反复求解一个带状态约束和控制约束的二次规划问题。
这套东西解决的核心问题就一句话：当执行器有极限、状态有安全边界时，怎么在保证不越界的前提下，把性能指标压到最低。PID 做不到，LQR 本身也做不到，因为它们是"算出一个增益然后一直用"，约束一旦起作用，控制量该怎么退让、该提前多远退让，只有预测时域里的优化能回答。经典代码的骨架就是模型预测、约束表达、QP 求解、滚动执行这四件事。对新手来说，找到一份结构清晰、能跑通、能改参的 MATLAB 代码，比去啃那些动辄几百行的商业工具箱源码要友好得多；对熟手来说，自己维护一份经典代码意味着随时能改预测时域、改约束矩阵、加干扰项，工具箱反而成了一个验证对错的参照物。
下面我按一个一线工程师复盘自己方案的思路，从数学结构讲到可运行的完整脚本，再把参数调整和四个高频翻车点拆开讲，最后给一个从 u 约束升级到 Δu 约束的进阶方向。你拿到的不只是一份代码，而是一套能往自己模型上移植的框架。
2. 约束模型预测控制的数学结构与 MATLAB 里的标准写法
2.1 MPCC 的完整形状：预测、约束、滚动
MPCC 的内核其实是三个动作的重复。第一步，用系统的状态空间方程从当前状态出发，预测未来 N 步的状态轨迹；第二步，把控制序列当成优化变量，构造一个包含状态偏差惩罚和控制量惩罚的二次型目标函数，同时把执行器约束、状态安全边界写成不等式约束；第三步，求解这个 QP，只用解出来的第一步控制量，然后系统往前走一个采样周期，再重新预测、重新求解。这就是滚动时域。
这跟"一次性算完所有控制量"的开环最优控制有一个本质差别：每走一步，模型误差和外部扰动都会被重新测量到，然后被新一轮优化吸收。换句话说，MPCC 是拿计算量换鲁棒性。代价也很明显——每个采样周期都得解一个 QP，所以代码里真正决定你能不能实时跑起来的部分，是 QP 的规模和数据怎么构造，而不是什么高深的数学。
经典代码里几乎都会用同一个套路：把未来 N 步的控制输入 U 当作变量，状态 x 通过预测方程表达成 x0 和 U 的线性组合，然后把目标函数和约束全部写成 U 的函数。这样原本"状态和控制耦合在一起"的问题，就变成了一个标准的二次规划。MATLAB 里这个 QP 直接交给 quadprog 就行，这也是为什么经典代码都不需要额外装任何工具箱。
2.2 从状态空间到 QP：F、G 矩阵的构造与 MATLAB 实现
预测方程是 MPCC 代码的第一根梁。假设离散状态方程是 x_{k+1} = A x_k + B u_k，那么从 x0 出发，未来几步的状态可以写成：
x1 = A x0 + B u0 x2 = A^2 x0 + A B u0 + B u1 x3 = A^3 x0 + A^2 B u0 + A B u1 + B u2
把它整理成矩阵形式 X = F x0 + G U，其中 X 是未来 N 步状态堆成的列向量，U 是未来 N 步控制输入堆成的列向量。F 是状态传播矩阵，描述"如果没有控制输入，状态会怎么自由演化"；G 是输入响应矩阵，描述"每一步控制输入对后续每一步状态的影响"。这两个矩阵是整个 MPCC 代码里最值得亲手写一遍的部分，因为你一旦看懂了它，后面加约束、改预测时域都不会懵。
下面是这段代码的常见写法，按 MATLAB 习惯我用循环来填矩阵，不用 kron 那种让新手读不懂的写法：
function [F, G] = build_prediction_matrices(A, B, N) % 构建预测方程 X = F * x0 + G * U % A: 离散状态矩阵 n x n % B: 离散输入矩阵 n x m % N: 预测时域 n = size(A, 1); % 状态维度 m = size(B, 2); % 输入维度 F = zeros(N * n, n); G = zeros(N * n, N * m); % 填充 F: 第 i 块对应 A^i for i = 1:N F((i-1)*n+1 : i*n, :) = A^i; end % 填充 G: 第 i 块对应 [A^(i-1)*B, A^(i-2)*B, ..., B, 0, ...] for i = 1:N for j = 1:i G((i-1)*n+1 : i*n, (j-1)*m+1 : j*m) = A^(i-j) * B; end end end这段代码有两个参数需要你特别注意。一个是 N，预测时域，它直接决定 F 和 G 的行数，也就是 QP 的变量规模；另一个是 A 的幂次计算，默认用 A^i，在 MATLAB 里对离散矩阵做整数幂没什么问题，但如果你把连续模型直接拿来用，这一步就会得出完全错误的结果。经典代码里一般会约定调用方先做离散化，最常见的做法是用 c2d 把连续状态空间转换成离散状态空间，再把离散 A、B 传进来。
F 和 G 的构造方式还有另一种常见写法，用分块矩阵加 kron 函数一步生成，速度快但可读性差。我个人的习惯是：框架验证阶段用循环版本，方便打断点看每个分块；真要压到实时仿真时，再把这段换成 kron 版本或把它编译成 MEX。因为当 N 超过 50，循环版本在仿真里会明显拖慢耗时。
2.3 终端代价和滚动时域：为什么经典代码都带 dlqr
经典 MPCC 代码里还有一个高频出现的细节：目标函数里除了状态偏差惩罚和控制量惩罚，还会带一个终端代价项，而且要取 x_N' P x_N。这个 P 一般直接由 dlqr 求解黎卡提方程得到。为什么需要它？因为预测时域 N 是有限的，优化只关心未来 N 步内的事情，最后一步之后的状态被一刀切断，失去了约束和控制。如果不加终端代价，控制器会"只顾眼前"，末段状态很容易失控，表现为仿真末尾状态偏离目标点、甚至发散。
终端代价的数学含义是"从当前终端状态出发，继续用最优 LQR 控制下去，还需要付出的代价"。用 dlqr 求出来的 P 就是稳态黎卡提解，把它加进目标函数，相当于把有限的预测窗口在末端无缝接上一个无限时域的最优控制器。这是经典代码能稳定工作的原因之一。
目标函数的完整构造可以直接用矩阵乘法来实现，把未来 N 步的状态惩罚矩阵 Q 堆成一个块对角大矩阵 Q_bar，把控制惩罚矩阵 R 堆成 R_bar，然后再叠加上终端代价 P。写成 MATLAB 代码，核心只有三行：
Q_bar = kron(eye(N), Q); Q_bar(N*n - n + 1 : N*n, N*n - n + 1 : N*n) = P; % 最后一块换成终端代价 P R_bar = kron(eye(N), R); H = G' * Q_bar * G + R_bar; f = x0' * F' * Q_bar * G;这里的 H 和 f 是 quadprog 需要的目标函数参数：标准 QP 写成 0.5 * U' * H * U + f' * U，前面这个 0.5 的系数要注意——quadprog 内部带 0.5，所以不能用单纯按 1/2 系数去构造 H，否则控制量会翻倍，这是新手最容易犯的错误。f 这一行也不难理解：把目标函数展开之后，x0' * F' * Q_bar * G 这一项就是交叉项，它把当前状态的影响带到优化里。
3. 在 MATLAB 里跑通最小可用的 MPCC 仿真
3.1 一个完整可复现的脚本：二阶系统约束控制
理论可以讲得天花乱坠，但真正让从业者信服的还是"我复制这段代码能立刻看到曲线"。我用一个二阶系统来演示最简可用的 MPCC：质量-弹簧-阻尼系统，状态是位移和速度，控制量是外力。工程上这是最常见的验证模型，因为它的 Dynamics 直观，而且能自然地体现出"速度约束"和"控制量饱和"这两类典型约束。
下面这段代码是一个完整的单文件脚本，从系统定义、离散化、构建预测矩阵，到滚动仿真、约束处理、打印控制量，全都在里面。我按"直接可以复制保存并运行"的要求来写。
%% MPCC 最简可运行示例：质量-弹簧-阻尼系统 % 状态: x1 位移, x2 速度; 控制: 外力 u % 约束: u 在 [-2, 2] 内; x2 在 [-1.5, 1.5] 内 clear; clc; close all; % 1. 连续模型参数 m = 1.0; % 质量 kg k = 0.5; % 弹簧系数 N/m c = 0.2; % 阻尼系数 N*s/m dt = 0.05; % 采样周期 s N = 12; % 预测时域 A_c = [0 1; -k/m -c/m]; B_c = [0; 1/m]; % 2. 离散化 sys_c = ss(A_c, B_c, [], []); sys_d = c2d(sys_c, dt); A = sys_d.A; B = sys_d.B; % 3. 权重矩阵与终端代价（用 dlqr 近似无限时域） Q = diag([10, 1]); % 位移偏差权重大 R = 0.5; % 控制量权重 [~, P, ~] = dlqr(A, B, Q, R); % 4. 构造预测矩阵 [F, G] = build_prediction_matrices(A, B, N); n = size(A, 1); m = size(B, 2); % 5. 目标函数矩阵 Q_bar = kron(eye(N), Q); Q_bar(N*n - n + 1 : N*n, N*n - n + 1 : N*n) = P; R_bar = kron(eye(N), R); H = G' * Q_bar * G + R_bar; H = (H + H') / 2; % 强制对称，quadprog 要求严格对称 % 6. 约束：控制量边界 lb = -2 * ones(N * m, 1); ub = 2 * ones(N * m, 1); % 状态约束: 速度 |x2| <= 1.5 % x = F*x0 + G*U => x2_ik <= 1.5 与 >= -1.5 M_vel = zeros(N, size(G, 2)); idx = 2 : n : N*n; % 每隔 n 取一个，对应每个时刻的速度分量 for i = 1:N M_vel(i, :) = G(idx(i), :); end Aineq = [ M_vel; -M_vel ]; bineq = [ 1.5 - F(idx, :) * x0; 1.5 + F(idx, :) * x0 ]; % 7. 滚动仿真 x0 = [2.0; 0]; % 初始位移 2m，初始速度 0 x_ref = 0; % 目标位移 0 steps = 120; x_history = zeros(n, steps); u_history = zeros(1, steps); u0 = zeros(N * m, 1); for t = 1:steps % 重新计算 f（依赖当前状态） f = (F * x0 - x_ref)' * Q_bar * G; % 重新计算状态约束的 bineq（依赖当前状态） bineq = [1.5 - F(idx, :) * x0; 1.5 + F(idx, :) * x0]; % 调用 quadprog 求解 options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex'); [u_opt, ~] = quadprog(H, f, Aineq, bineq, [], [], lb, ub, u0, options); % 只取第一步 u_apply = u_opt(1); x_history(:, t) = x0; u_history(t) = u_apply; % 系统更新 x0 = A * x0 + B * u_apply; end % 8. 绘图 figure(1); subplot(2,1,1); plot(x_history(1,:), 'LineWidth', 1.5); hold on; plot(x_history(2,:), 'LineWidth', 1.5); yline(1.5, 'r--', '速度上界'); yline(-1.5, 'r--', '速度下界'); legend('位移', '速度', 'Location', 'best'); title('MPCC 状态轨迹'); subplot(2,1,2); plot(u_history, 'LineWidth', 1.5); yline(2, 'r--', '控制上界'); yline(-2, 'r--', '控制下界'); title('MPCC 控制量');这段代码有几个容易被忽略的细节，值得一一点出。首先是H = (H + H') / 2，这一行在几乎所有教科书代码里都看不到，但实际运行中非常关键：因为数值误差，矩阵乘法得到的 H 可能有极微小的非对称成分，quadprog 的 interior-point-convex 算法遇到非对称矩阵会直接报错或者给出莫名其妙的结果，所以做一次对称化处理是低成本高收益的习惯。
其次是f的符号：这里我写的是F * x0 - x_ref，即"当前预测轨迹相对于目标值的偏差"，再乘上 Q_bar 和 G。这个方向的细节直接决定控制量是推着系统去目标还是推着系统远离目标。如果符号反了，你会看到控制量一路加到底，系统越推越偏，最后撞到约束边界上不动了。经验做法是第一步先用无约束 LQR 对比效果，确认控制方向正确后再加约束。
最后是u0参数，它是 quadprog 的初始点。每次滚动求解时，把上一次求出的完整控制序列作为这一次的初值，可以明显加速收敛。对于小规模问题影响不大，但当你把 N 加到 30 以上、且约束数量较多时，这个习惯能减少 20%~30% 的求解时间。
3.2 把仿真结果拆开看：时域、控制量、约束边界
运行完上面的脚本，第一件事不是看性能指标，而是看三条曲线对应的时间关系。先看状态轨迹：位移应该从 2 衰减到接近 0，速度在初期快速上升，然后减速接近 0。关键验证点是速度是否始终在 ±1.5 的红色虚线以内。如果速度约束生效太晚，你可能看到速度先顶破 1.5，而后面的优化周期才把速度"拉回来"——这不是代码 bug，这是约束预测限制的必然结果：第一帧优化时，速度还没有越界，所以优化器不会预见性地大幅减速，只有速度真的接近边界时，约束不等式变为活跃，梯度方向才突然反转。
这就是 MPCC 与无约束 LQR 的最大差异，也是刚接触的人最容易误解的地方：约束不是"绝对保护"而是"预测性地限制"。如果约束越界的幅度超过 5%，才需要排查代码问题，例如 Aineq 和 bineq 里 F 矩阵的索引是否对上了状态维度。
再看控制量曲线：初始阶段 u 应该在一两拍内达到上限 2，这是典型的时间最优行为——预测控制看到初始误差很大，所以尽量用满控制力。随着状态接近目标，控制量会快速回落。如果控制量曲线出现持续的锯齿状高频切换（比如每个采样周期从 +2 跳到 -2），说明你的 Q/R 权重设置的惩罚不合理，控制量权重 R 太小，控制器在用不必要的暴力方式换状态收敛。
约束边界本身也需要验证：在 plot 里把u_history与yline(2)对齐，理论上 u 永远不越界。如果 quadprog 返回的控制量正好等于上限，这就是"约束活跃"的正常现象，代表系统在此刻确实是受限的。你会观察到在收敛过程的中段，控制量会长时间贴在上边界，然后逐渐脱离，这是经典代码期望看到的形态——先全力加速，再适度制动。
3.3 从仿真到实验：接口函数怎么留
仿真跑通后，很多人第一步就跳到接真实设备，这是最容易翻车的地方。我一般会先在仿真脚本里把核心部分抽成一个独立函数mpcc_step(x0, A, B, Q, R, N, lb, ub, u0)，输入当前状态和约束参数，输出这一步的控制量。这样做有两个好处：第一，仿真和实验共用同一个函数，避免"仿真代码和实验代码各写一份、改来改去对不上"；第二，实验环境里你会用实时内核或外部设备读状态，只要每次给它当前状态，它返回下一步控制量，这个函数就是纯净的控制算法，不掺任何硬件相关代码。
接口函数的典型签名大概长这样：
function u = mpcc_step(x_now, ~, x_ref, A, B, Q, R, P, N, lb, ub, u_prev) % 输入当前状态，输出这一步控制量 % 内部重新构建 f 和 bineq，调用 quadprog % u_prev 是上一步的完整控制序列，作为求解初始点注意这里的参数里我把 A、B、Q、R、P 全作为外部传入，而不是放在函数内部硬编码。原因是调参时你只需要改仿真脚本里的权重矩阵，不用去翻控制函数内部。这个习惯在模型变化时特别省事：比如从二阶系统换成三阶系统，只要传入新的 A、B、P、N，其他逻辑一行不用动。
然后你再考虑给这个函数加一个"约束软化"扩展口。最常见的做法是在目标函数里加一个松弛变量，把硬约束变成软约束：
% 把状态约束放宽为 x <= x_max + slack，松弛变量加到目标函数 % 目标: U'*H*U + f'*U + rho * norm(slack) % 其中 slack 也作为优化变量加入这段扩展在初期可以先不做，但接口位置要留好。因为真实系统中，模型误差和测量噪声几乎是必然存在的，硬约束在某些采样周期里不可能满足（例如速度测量噪声导致当前状态已经在约束外），如果强行求硬约束，QP 可能直接变成不可行，quadprog 会返回空解或错误信息。而加上松弛变量后，优化器会在"满足约束"和"最小化代价"之间权衡，系统不会瞬间崩溃。这是从仿真到现场必经的一步，几乎每个做运动控制的工程师都会遇到。
4. MPCC 的 5 个关键参数怎么调才不翻车
4.1 预测时域 N：太小失控，太大迟钝
预测时域 N 是 MPCC 里唯一一个直接决定"看得多远"的参数。它不是一个可以随便拍脑袋的数字，而是跟系统的动态速度、采样周期强相关。经验法则是用系统的主导时间常数去除采样周期，然后再除以 2~3。比如一个系统响应大概 3 秒到稳态，采样周期 0.05 秒，那 N 应该在 20~30 之间。N 太小，控制器看不到约束逼近的趋势，表现为状态缓缓逼近约束边界直到越界才反应；N 太大，控制器会过度考虑远期，表现为系统的响应变慢，而且在约束解除后还有一个"远期决策"的尾巴，收敛过程看起来迟钝。
实际调试时，我会先用 N=10 跑一次，然后翻倍到 N=20、N=30，观察两个指标：约束满足情况和收敛时间。如果 N 从 10 增加到 20 时，约束越界幅度明显改善，而收敛时间几乎不变，说明系统还在"看得不够远"的阶段；如果 N 从 20 增加到 40，收敛时间明显变长，控制量也变得平缓，说明已经过了最佳点。选一个"再增加 N 收益递减"的拐点，那就是你的工作点。
还有个容易忽略的影响：N 直接决定 QP 变量数量，进而影响求解耗时。N=12 的时候 quadprog 基本是瞬间返回，N=50 的时候单步可能到几毫秒，如果采样周期只有 5ms，这就会实时性告急。所以 N 不是越大越好，它是在"优化质量"和"实时预算"之间做权衡。
4.2 Q / R 权重的调整节奏与常见误区
Q 和 R 是目标函数里真正表达"你想要什么"的参数。Q 大、R 小，控制器就舍得用大控制量换取快速收敛；Q 小、R 大，控制器会温柔很多，但收敛变慢、状态轨线更平滑。这个关系本身不难，难的是"先调哪个、按什么节奏调"。我见过不少从业者在 Q 和 R 上反复横跳，几小时都是空耗，绕不出局部最优。
我的调试顺序是：先把 R 固定为 1，Q 设为对角阵，且只调第一个对角元（对应最重要的状态，如位置误差），直到系统在无约束情况下有可接受的收敛速度；接着把速度状态的权重调大，直到速度曲线没有明显超调；最后再把 R 降下来，观察控制量是否出现抖动。这样一轮下来，通常只需要改 5~10 个数字，而且每个数字的作用都很清晰。
这里有三个高频误区。误区一：Q 和 R 都设成同一数量级，导致控制量权重过高，整个系统软绵绵的。误区二：直接给 Q 设置大数值却忘了 R 也需要同步调整，结果控制量疯狂顶到边界，状态轨线出现剧烈摆动。误区三：以为 Q 必须是对角阵，其实完全可以用非对角权重描述状态之间的耦合需求，但除非你明确需要，否则别引入这种复杂性。记住一个原则：MPCC 的重量不在权重上，而在约束和预测的配合上，权重只是让你往约束边界靠的速度可控而已。
4.3 dt、约束矩阵、求解器容差的联动关系
采样周期 dt 不是一个孤立参数，它直接影响模型的离散化精度、预测时域 N 的实际物理长度，以及约束在时间轴上的分布。把一个连续模型用 c2d 离散时，dt 越大，离散模型和连续行为偏差越大，MPCC 的预测越不准，会出现约束在连续系统中满足、但在离散仿真中越界的情况。dt 也不宜太小，因为同样的时间跨度下需要更大 N，QP 规模直接膨胀。
约束矩阵的构建精度则取决于你对自己系统边界的认知。状态约束如果太紧，QP 会经常陷入不可行状态；太松则约束形同虚设。一个在工程上很实用的做法是把约束边界的外侧加一层 5% 左右的缓冲带，在模型里用缓冲带做约束，避免真实设备刚好顶着边界跑。这不影响理论正确性，但能极大提高现场容错性。
求解器容差（constraint tolerance）是 quadprog 的参数，默认值一般可用，但当你发现约束轻微越界、或者 QP 在边界上来回切换时，试着把ConstraintTolerance从 1e-8 放宽到 1e-5。这不是降低要求，而是给求解器一点数值上的呼吸空间，让它更容易收敛。同理OptimalityTolerance也可以适度放宽，换取求解速度。
4.4 参数调整的顺序与快速验证方法
把上面的参数串起来，我的调参顺序是固定的，每次都不变：先根据系统时间常数定 dt，再根据 dt 和时间尺度定 N，再调 Q/R 确定基本响应，最后把约束和容差加上，微调轮次控制在三轮以内。快速验证方法是这样：每次改动只改一个参数，跑一次同样的初始化场景（同一初始状态、同一目标点），把三条曲线（位移、速度、控制量）打印出来对比，而不是用一堆随机场景去看平均效果。因为 MPCC 的非线性行为集中在约束活跃的边界上，平均指标掩盖了局部的瞬态行为，单个场景的可视化才更容易暴露问题。
如果一轮调完还不理想，大概率不是参数问题，而是你的模型或约束描述出了问题。这时候我会把无约束 LQR 跑一遍，如果无约束 LQR 的响应都不对，那就根本不是 MPCC 的锅，是你 A、B 矩阵或者离散化错了。这个交叉验证的办法能帮你快速定位问题在哪一层，省下大量在参数里打转的时间。
5. MPCC 避坑指南：四个高频踩坑点
5.1 现象：状态约束经常轻微越界，幅值 2%~5%
原因：quadprog 内部的约束容差默认是一个相对值，它会允许约束在数值上略微违反；此外，如果你的 Aineq 和 bineq 是从 F、G 矩阵算出来的，任何一步的计算误差（尤其是矩阵乘法累积误差）都会直接传递到约束上。解决：检查你构建 Aineq 和 bineq 时用的索引是否对应正确的状态分量，然后尝试减小ConstraintTolerance，通常设到 1e-6 或 1e-7 能解决 80% 的情况。另一个有效手段是给约束边界加 3%~5% 的缓冲，比如把 1.5 的约束在代码里写为 1.45，确保真机设备不顶着边界跑。
5.2 现象：控制量在一段时间内剧烈抖动，单步从 +2 跳到 -2
原因：Q/R 权重设置有问题，R 太小导致优化器认为"大幅翻转控制量"的代价很低；另一个可能是预测时域 N 太小，控制器看不到剧烈控制动作带来的远期后果。解决：先调大 R 试试，把 R 从 0.5 提高到 2 或 5，抖动通常会立刻减弱。如果抖动仍然存在，加长 N，并检查你的离散 A、B 矩阵是否有不稳定的零极点。还有一个容易被忽略的细节：如果你的目标函数里没有终端代价 P，末端状态的不确定性也容易导致控制序列尾部抖动，加上 P 会显著改善。
5.3 现象：第一次加约束后系统直接不稳定，甚至 QP 无解
原因：约束设置过紧，或者你加了相互矛盾的约束。比如既要求速度在 5 秒内降到 0，又要求控制量小于某个极小值，优化器找不到满足所有条件的控制序列，返回空解。另一个常见原因是初始状态本身就违反了状态约束——比如初始速度已经是 2，你设置约束上限为 1.5，那第一帧优化就不可行。解决：检查初始状态是否在可行域内；把约束放宽 20% 验证可行性；然后给目标函数加松弛变量，这是工程上几乎必须的一步。加上松弛变量后，QP 永远不会因为约束矛盾而无解，它会在最小化松弛量的前提下尽可能满足约束。
5.4 现象：在线跑的耗时比离线仿真高一个数量级
原因：离线仿真时 MATLAB 的 JIT 加速和内存复用让循环看起来很快，但你在 Simulink 里跑或接实时环境时，每个采样周期都重新解析、重新分配矩阵，quadprog 每次都要重新初始化，这个开销会急剧放大。解决：把核心预测矩阵 F、G、H、约束矩阵在仿真开始前一次性算好存入 structure 变量，循环里只更新 f 和 bineq 这两个依赖当前状态的向量，不要每次重建整个 QP 数据。然后把优化选项也提前建好，传进来复用它。实测这个优化能把单步耗时降到原来的三分之一以下。还有一个技巧：如果模型固定，H 和 Aineq 在运行过程中完全不改变，应该把它们存成 persistent 变量或通过匿名函数配置好，不要重复构建。
6. 进阶：用增量形式 MPCC 提高鲁棒性
前面所有代码约束的都是控制量 u 本身，这在原理上没有问题，但在实际系统中往往不够用。因为真实执行器关心的不只是当前控制量的幅度，还有控制量的变化速度——电机有最大加速度、液压阀有最大开闭速度、发动机有动态响应限制。直接约束 u 无法表达这种"变化率"限制，这就引出了增量形式：把优化变量从 u 换成 Δu，也就是每一步控制量的增量，然后在目标函数里加上对 Δu 的惩罚，同时把 u 的上下界写成一个累积约束。
改造的核心是引入一个累加矩阵。假设未来每一步的控制量 u_k = u_{k-1} + sum_{j=1}^{k} Δu_j，那么把所有步合成向量后，u = u_{prev} + L * ΔU，这里的 L 是一个下三角的全 1 分块矩阵。于是 u 的上下界约束变成 u_lb = u_{prev} + L * ΔU，整理成不等式约束后，quadprog 的 Aineq、bineq 要做相应调整。
% 增量形式：优化变量是 Δu，控制量 u 累加得到 L = tril(ones(N)); U_seq = u_prev + L * delta_U; % u_prev 是上一步实际作用的控制量 % 目标函数中把 u 用 delta_U 表示，会多出一个二次项 H_delta = G' * Q_bar * G * L^2 + R_bar * L^2 + R_delta * I; f_delta = (F * x0 - x_ref)' * Q_bar * G * L + u_prev' * R_bar * L; % 约束 lb/ub 也在 delta_U 空间中表达 lb_d = (lb - u_prev) / L; ub_d = (ub - u_prev) / L;增量形式带来的好处非常明显：它天然包含了对控制量变化率的惩罚，控制器输出的变化平缓，不会出现前面说的那种单步从 +2 跳到 -2 的抖动；同时，即使系统的模型参数有偏差，控制量每一步的变化仍然受到严格的增量边界限制，系统的行为会温和很多。代价是 QP 的变量数量不变，但约束矩阵的构造变得更复杂，而且你需要额外引入一个增量的权重矩阵 R_delta，又多了一个调参的自由度。
验证增量形式是否值得投入，我建议做一个对比实验：同一个系统、同一组初始状态，分别跑"约束 u"和"约束 Δu"两个版本，记录控制量的总变差（相邻两步差的绝对值求和）和收敛时间。如果总变差显著下降且收敛时间没有明显恶化，说明增量形式适合你的场景；如果收敛时间翻倍，就要权衡是换个权重还是放弃增量形式。我个人的经验是，凡是真实执行器存在延迟或带宽限制的系统，增量形式几乎是必须的；而纯仿真、模型极度精确的系统，u 约束版本更简单、更容易调试。
我自己维护 MPCC 代码时，常年在文件开头留一段注释，记录当前工程下 N、Q、R、P 的取值依据和上一次调参的结论。这个习惯帮我避免了许多次"改了 R 之后忘记为什么改"的尴尬。把代码当作活文档对待，比任何花哨的技巧都重要。希望这些写法能帮你在 MPCC 这条路上少走几步弯路。
本文还有配套的精品资源，点击获取
