简介这是一套面向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 从状态空间到 QPF、G 矩阵的构造与 MATLAB 实现预测方程是 MPCC 代码的第一根梁。假设离散状态方程是 x_{k1} 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)*n1 : 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)*n1 : i*n, (j-1)*m1 : 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 太大控制器会过度考虑远期表现为系统的响应变慢而且在约束解除后还有一个远期决策的尾巴收敛过程看起来迟钝。实际调试时我会先用 N10 跑一次然后翻倍到 N20、N30观察两个指标约束满足情况和收敛时间。如果 N 从 10 增加到 20 时约束越界幅度明显改善而收敛时间几乎不变说明系统还在看得不够远的阶段如果 N 从 20 增加到 40收敛时间明显变长控制量也变得平缓说明已经过了最佳点。选一个再增加 N 收益递减的拐点那就是你的工作点。还有个容易忽略的影响N 直接决定 QP 变量数量进而影响求解耗时。N12 的时候 quadprog 基本是瞬间返回N50 的时候单步可能到几毫秒如果采样周期只有 5ms这就会实时性告急。所以 N 不是越大越好它是在优化质量和实时预算之间做权衡。4.2 Q / R 权重的调整节奏与常见误区Q 和 R 是目标函数里真正表达你想要什么的参数。Q 大、R 小控制器就舍得用大控制量换取快速收敛Q 小、R 大控制器会温柔很多但收敛变慢、状态轨线更平滑。这个关系本身不难难的是先调哪个、按什么节奏调。我见过不少从业者在 Q 和 R 上反复横跳几小时都是空耗绕不出局部最优。我的调试顺序是先把 R 固定为 1Q 设为对角阵且只调第一个对角元对应最重要的状态如位置误差直到系统在无约束情况下有可接受的收敛速度接着把速度状态的权重调大直到速度曲线没有明显超调最后再把 R 降下来观察控制量是否出现抖动。这样一轮下来通常只需要改 5~10 个数字而且每个数字的作用都很清晰。这里有三个高频误区。误区一Q 和 R 都设成同一数量级导致控制量权重过高整个系统软绵绵的。误区二直接给 Q 设置大数值却忘了 R 也需要同步调整结果控制量疯狂顶到边界状态轨线出现剧烈摆动。误区三以为 Q 必须是对角阵其实完全可以用非对角权重描述状态之间的耦合需求但除非你明确需要否则别引入这种复杂性。记住一个原则MPCC 的重量不在权重上而在约束和预测的配合上权重只是让你往约束边界靠的速度可控而已。4.3 dt、约束矩阵、求解器容差的联动关系采样周期 dt 不是一个孤立参数它直接影响模型的离散化精度、预测时域 N 的实际物理长度以及约束在时间轴上的分布。把一个连续模型用 c2d 离散时dt 越大离散模型和连续行为偏差越大MPCC 的预测越不准会出现约束在连续系统中满足、但在离散仿真中越界的情况。dt 也不宜太小因为同样的时间跨度下需要更大 NQP 规模直接膨胀。约束矩阵的构建精度则取决于你对自己系统边界的认知。状态约束如果太紧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_{j1}^{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 这条路上少走几步弯路。本文还有配套的精品资源点击获取
简介这是一套面向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 从状态空间到 QPF、G 矩阵的构造与 MATLAB 实现预测方程是 MPCC 代码的第一根梁。假设离散状态方程是 x_{k1} 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)*n1 : 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)*n1 : i*n, (j-1)*m1 : 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 太大控制器会过度考虑远期表现为系统的响应变慢而且在约束解除后还有一个远期决策的尾巴收敛过程看起来迟钝。实际调试时我会先用 N10 跑一次然后翻倍到 N20、N30观察两个指标约束满足情况和收敛时间。如果 N 从 10 增加到 20 时约束越界幅度明显改善而收敛时间几乎不变说明系统还在看得不够远的阶段如果 N 从 20 增加到 40收敛时间明显变长控制量也变得平缓说明已经过了最佳点。选一个再增加 N 收益递减的拐点那就是你的工作点。还有个容易忽略的影响N 直接决定 QP 变量数量进而影响求解耗时。N12 的时候 quadprog 基本是瞬间返回N50 的时候单步可能到几毫秒如果采样周期只有 5ms这就会实时性告急。所以 N 不是越大越好它是在优化质量和实时预算之间做权衡。4.2 Q / R 权重的调整节奏与常见误区Q 和 R 是目标函数里真正表达你想要什么的参数。Q 大、R 小控制器就舍得用大控制量换取快速收敛Q 小、R 大控制器会温柔很多但收敛变慢、状态轨线更平滑。这个关系本身不难难的是先调哪个、按什么节奏调。我见过不少从业者在 Q 和 R 上反复横跳几小时都是空耗绕不出局部最优。我的调试顺序是先把 R 固定为 1Q 设为对角阵且只调第一个对角元对应最重要的状态如位置误差直到系统在无约束情况下有可接受的收敛速度接着把速度状态的权重调大直到速度曲线没有明显超调最后再把 R 降下来观察控制量是否出现抖动。这样一轮下来通常只需要改 5~10 个数字而且每个数字的作用都很清晰。这里有三个高频误区。误区一Q 和 R 都设成同一数量级导致控制量权重过高整个系统软绵绵的。误区二直接给 Q 设置大数值却忘了 R 也需要同步调整结果控制量疯狂顶到边界状态轨线出现剧烈摆动。误区三以为 Q 必须是对角阵其实完全可以用非对角权重描述状态之间的耦合需求但除非你明确需要否则别引入这种复杂性。记住一个原则MPCC 的重量不在权重上而在约束和预测的配合上权重只是让你往约束边界靠的速度可控而已。4.3 dt、约束矩阵、求解器容差的联动关系采样周期 dt 不是一个孤立参数它直接影响模型的离散化精度、预测时域 N 的实际物理长度以及约束在时间轴上的分布。把一个连续模型用 c2d 离散时dt 越大离散模型和连续行为偏差越大MPCC 的预测越不准会出现约束在连续系统中满足、但在离散仿真中越界的情况。dt 也不宜太小因为同样的时间跨度下需要更大 NQP 规模直接膨胀。约束矩阵的构建精度则取决于你对自己系统边界的认知。状态约束如果太紧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_{j1}^{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 这条路上少走几步弯路。本文还有配套的精品资源点击获取
