简介：一套MPCC经典代码资料，面向正在学习MATLAB优化、模型预测控制及互补约束问题的研究人员和工程师，适合用于高校课程设计、科研复现和工程参考。资源包含完整的MPC-MPCC仿真框架，覆盖目标函数设计、互补约束建模、车辆与障碍物状态处理、边界调整、离散线性化等关键环节，并封装了Hpipm、CVX、Yalmip、QuadProg等常用求解器接口，便于对比不同求解方案。压缩包共94个文件，以28个MATLAB脚本和20组C++源文件为主，辅以JSON配置、Markdown说明、MAT数据、效果图及安装脚本，整体约15MB，目录结构清晰，按模型、约束、代价、求解器、仿真等模块划分。目前已有581人学习下载。通过运行仿真与阅读源码，读者可以掌握MPCC从建模到求解的完整链路，理解数值稳定性处理、预测时域滚动、障碍物避让逻辑和轨迹调试的实用技巧，并可将代码迁移到自己的控制项目中。
1. 拿到一份 matlab mpcc 经典代码，先别急着跑：先确认控制器在做什么
做轨迹跟踪的工程师大概都有过这类体验：PID 在直线路段表现良好，一进弯道就开始抖，加了低通滤波又变得迟钝；换个参考轨迹，参数又要从头调一遍。这不是参数玄学，而是控制器结构本身缺乏对未来的预判能力。matlab mpcc 经典代码解决的就是这个问题——模型预测控制（MPC）会在每个采样时刻往前看一段预测时域，把控制量序列当成一个优化问题来求解，把输入限幅、状态边界、安全距离这类约束显式写进优化里，让控制器在动作前就知道前方的约束是什么。适合正在做车辆轨迹跟踪、机器人路径规划和过程控制的开发者，一条路是从这份代码里拆出滚动优化的骨架，换成自己的模型。
2. MPCC 的数学框架与 Matlab 实现选型：目标函数、三条约束和一个求解器
2.1 预测模型：把连续动力学写成离散状态方程
MPC 的一切都建立在预测模型上。模型不准，后面算得再漂亮也是空转。我拿到一段 MPCC 代码，第一步永远先找模型函数，看它到底把什么当作状态、什么当作输入。
最常见的运动学小车模型有四个状态：位置 px、py，航向角 psi，线速度 v，两个输入：加速度 a 和方向盘转角 delta。连续形式是：
px_dot = v * cos(psi) py_dot = v * sin(psi) psi_dot = v / L * tan(delta) v_dot = a
在 MATLAB 里做离散化，工程上最常用的是零阶保持加欧拉法，采样时间够小时误差可接受。代码写出来是这样：
function x_next = model_step(x, u, T_s, L) % 简化自行车运动学模型，欧拉法离散 % x = [px; py; psi; v]，u = [a; delta] px = x(1); py = x(2); psi = x(3); v = x(4); a = u(1); delta = u(2); x_next = zeros(4, 1); x_next(1) = px + T_s * v * cos(psi); x_next(2) = py + T_s * v * sin(psi); x_next(3) = psi + T_s * v / L * tan(delta); x_next(4) = v + T_s * a; % 这里暂时没加加速度变化率 end这段代码的逻辑就一件事：给定当前状态和输入，算出下一个采样时刻的状态。参数 T_s 是采样时间，L 是车辆轴距。新手容易在这里犯一个错误：把 T_s 取得很大（比如 0.5 秒）来减少预测步数，结果欧拉法离散误差在高速场景下直接把预测轨迹带偏。我一般建议低速场景 T_s 取 0.05 到 0.1 秒，高速场景还要换更精细的离散方法，比如四阶龙格库塔。
2.2 代价函数与约束：权重矩阵、输入限幅和终端约束的矩阵化
预测模型定了，下一步是把「控制得好不好」翻译成一个可微的代价函数。经典写法是每一预测步累加状态偏差的二次型和输入的二次型：
J = sum(e' * Q * e + u' * R * u)
其中 e = x_k - ref_k 是状态对参考轨迹的偏差，Q 惩罚状态偏差，R 惩罚控制量消耗。Q 和 R 都是对角矩阵时，物理意义最直观：Q 里某个状态对应的权值越大，控制器就越优先压平那个方向的偏差；R 里的权值越大，控制器动作越保守。
约束分三类。第一类是输入限幅，比如加速度在 [-3, 3] 之间，方向盘转角在 [-0.5, 0.5] 之间，这类约束直接写成决策变量的上下界，交给 fmincon 的 lb 和 ub 参数。第二类是状态路径约束，比如不许越过车道边界，这类要写成非线性不等式约束 c(x) <= 0。第三类是终端约束，为了让闭环稳定，一般要求预测终点落在参考点附近的某个邻域内，但这类约束最容易导致优化问题不可行，后面避坑章节细说。
在 MATLAB 里把这三类约束组织起来的常见做法是写一个独立的约束函数：
function [c, ceq] = mpc_constraints(z, x0, model_params) % 非线性不等式约束与等式约束 % z 是决策变量向量，包含 N_c 步输入序列 N_c = model_params.N_c; n_u = model_params.n_u; U = reshape(z, n_u, N_c); c = []; ceq = []; % 状态边界约束：假设预测轨迹由单独函数推进 x_pred = predict_trajectory(z, x0, model_params); lane_limit = 3.0; % 路径约束：横向偏移不超过车道限界 c = [c; abs(x_pred(2, :))' - lane_limit]; % 终端等式约束：终点强行贴到参考点（谨慎使用，容易不可行） % ceq = x_pred(:, end) - ref_final; % 工程上更推荐松弛 end这段代码里predict_trajectory根据输入序列向前推一段状态轨迹，c是逐点的不等式约束，ceq是等式约束。路径约束写成abs(x_pred(2,:))' - lane_limit，含义是任意预测时刻的横向位置绝对值不能超过 3 米。终端约束我注释掉了，实际项目里硬等式约束几乎必然导致求解失败，后面换成松弛变量才是常见做法。
2.3 求解器选型：fmincon、quadprog 与 CasADi 的取舍
MPCC 经典代码里最容易让新手看不懂的地方是求解器封装。同样的优化问题，选不同的求解器，代码量差一个量级。
如果模型是线性的、代价函数是二次型、约束全是线性的，那就是标准的 QP 问题，MATLAB 里用quadprog最合适，速度快、没有初始点敏感性。如果模型带非线性（比如预测模型里有 cos/sin 项），就得用fmincon配 SQP 算法。更工程化的选择是 CasADi 配合 IPOPT，处理非线性最优控制问题更专业，但需要额外安装工具包。
我把三者的取舍整理成一张表：
| 求解器 | 适用问题 | 优点 | 主要坑 |
|---|---|---|---|
| quadprog | 线性模型 + 线性约束 + 二次代价 | 求解快，适合实时 | 必须把模型线性化写成矩阵 |
| fmincon | 非线性模型，中等规模 | 无需额外安装，调试直观 | 局部最优，实时性不稳 |
| CasADi + IPOPT | 非线性强、规模大的 NMPC | 符号求导，求解稳定，支持 AD | 安装配置有门槛 |
对大多数教学用 MPCC 代码，fmincon 是主流选择。你拿到的资源如果是基于 MATLAB 优化工具箱写的，核心就是重复调用 fmincon 求解一个滚动优化问题，不存在黑匣子。
3. 经典 MPCC 代码逐段拆解：从主循环到闭环节点的调用链
3.1 主循环：初始化、求解、施加、推进
MPC 的控制律是一段循环：每个采样时刻求解一次优化问题，得到最优输入序列，只把第一步施加到被控对象，然后状态推进到下一时刻，用新状态重新求解。这就是滚动优化，也叫闭环预测控制。
经典代码的主循环一般长这样：
% 参数初始化 T_s = 0.1; % 采样时间 N_p = 10; % 预测时域 N_c = 3; % 控制时域 L = 2.5; % 轴距 N_sim = 200; % 仿真步数 x0 = [0; 0; 0; 2.0]; % 初始状态 ref = build_reference(N_p + N_sim, T_s); % 预生成参考轨迹 u_prev = zeros(2, 1); % 上一时刻控制量，用于热启动 history.x = zeros(4, N_sim); history.u = zeros(2, N_sim); for k = 1:N_sim ref_k = ref(:, k:k+N_p); % 当前窗口的参考轨迹 % 求解当前时刻的最优控制序列 [u_opt, exitflag] = solve_mpc(x0, ref_k, u_prev, model_params); if exitflag <= 0 warning('第 %d 步求解失败，沿用上一时刻控制量', k); u_opt = [u_prev, u_prev]; end u_applied = u_opt(:, 1); % 只施加第一步 x0 = model_step(x0, u_applied, T_s, L); % 状态推进 history.x(:, k) = x0; history.u(:, k) = u_applied; u_prev = u_applied; end注意这里的三个关键点。第一，solve_mpc返回的不只是当前控制量，而是一整段最优序列，但闭环只取第一列u_opt(:,1)，这是 MPC 区别于一次离线优化的地方。第二，ref_k是滑动的参考窗口，窗口随主循环一起移动。第三，exitflag这个返回值非常重要——求解失败时如果还硬用结果，闭环直接发散，这里做的是降级处理：沿用上一时刻控制量，至少保住系统不崩。
build_reference是预生成参考轨迹的函数，常见做法是把期望路径和期望速度拆开存储，比如参考列向量为 [px_ref; py_ref; psi_ref; v_ref]，方便在代价函数里直接计算偏差。
3.2 代价函数与约束的匿名函数封装
fmincon 要求把代价函数、约束函数都写成只依赖决策变量的句柄，所以代码里到处都是匿名函数。这一步封装是新手最容易绕晕的地方：为什么明明代价函数需要 x0 和 ref，却只把 z 传给 fmincon？
答案在嵌套函数或匿名函数的变量捕获。solve_mpc 内部实现通常是这样：
function [u_opt, exitflag] = solve_mpc(x0, ref_k, u_prev, model_params) N_c = model_params.N_c; n_u = model_params.n_u; z0 = build_initial_guess(u_prev, N_c); % 热启动初值 % 代价函数：只暴露决策变量 z，其余参数由匿名函数捕获 cost_fun = @(z) mpc_cost(z, x0, ref_k, model_params); % 约束函数 con_fun = @(z) mpc_constraints(z, x0, model_params); lb = repmat(model_params.u_min, N_c, 1); ub = repmat(model_params.u_max, N_c, 1); options = optimoptions('fmincon', ... 'Algorithm', 'sqp', ... 'MaxIterations', 400, ... 'OptimalityTolerance', 1e-4, ... 'Display', 'off'); [z_opt, ~, exitflag] = fmincon(cost_fun, z0, ... [], [], [], [], lb, ub, con_fun, options); % 把决策变量重新解构为输入矩阵 [n_u, N_c] u_opt = reshape(z_opt, n_u, N_c); end这段封装的逻辑是：外部把 x0、ref_k、model_params 传进来，内部用@(z)把它们冻结成一个只关于决策变量 z 的函数。build_initial_guess的作用是把上一时刻的最优输入序列平移一位、末尾复制一组，作为本次迭代的初始点，这能让求解快很多，也是热启动的核心。reshape(z_opt, n_u, N_c)把一维决策变量还原成输入矩阵，第 i 列就是第 i 步的控制量。
有几个容易踩的点。第一，fmincon 的 lb 和 ub 长度必须与 z0 完全一致，否则直接报错；很多代码从 3 步控制时域改成 5 步时，只改了 N_c 忘了同步修 lb、ub 的维度。第二，reshape默认按列填充，所以决策变量的排列顺序必须和代价函数里解构的顺序一致，否则控制量序列会错位，表现就是参考轨迹跟踪正常但输入乱跳。
3.3 求解器调用与结果解码
代价函数内部的实现决定了优化的质量。我在拆代码时最关注的是它如何处理控制时域和预测时域的关系：控制时域 N_c 之外的预测步，输入保持为最后一个控制量，这是教科书标准做法，但很多魔改代码会把这时段输入置零，得到的结果就是轨迹末端控制力不足。
function J = mpc_cost(z, x0, ref_k, model_params) N_p = model_params.N_p; N_c = model_params.N_c; n_u = model_params.n_u; T_s = model_params.T_s; L = model_params.L; Q = model_params.Q; % 状态权重，4x4 对角 R = model_params.R; % 输入权重，2x2 对角 U = reshape(z, n_u, N_c); x = x0; J = 0; for i = 1:N_p if i <= N_c u = U(:, i); else u = U(:, N_c); % 预测域剩余步保持最后输入 end x = model_step(x, u, T_s, L); e = x - ref_k(:, i); J = J + e' * Q * e + u' * R * u; end end这段代码逐步推进模型、累加代价。Q 和 R 直接以矩阵形式从 model_params 中取出，便于修改。这里我还要强调一个细节：参考轨迹的列顺序必须与预测步对应，ref_k(:, i)的第 i 列对应第 i 步预测。如果参考轨迹生成时索引对不上，控制器会在跟踪一个时间偏移过的轨迹，表现出来就是曲线的相位滞后。
4. 参数配置与调试：预测时域、权重矩阵与求解器选项的真实调法
4.1 采样时间与预测时域：先定 T_s，再定 N_p
参数配置的顺序是有讲究的。我见过太多人上来就调 Q 和 R，结果问题根本不在权重上。标准的做法是先定采样时间 T_s，再定预测时域 N_p，最后才碰权重矩阵。
采样时间 T_s 由被控对象的动态特性和执行机构决定。对于车辆的横向控制，T_s 在 50ms 到 100ms 之间是常见区间；T_s 取得太小，求解时间跟不上，实时性直接翻车；取得太大，两次控制之间的状态变化过大，线性化模型失真。
预测时域 N_p 的意义是「往前看多远」。N_p 太大，一方面 fmincon 的求解时间线性增长，另一方面预测模型本身的误差被逐级放大，远期预测基本是猜的，反而干扰近期控制。一个经验值是让 T_s * N_p 覆盖被控对象主导时间常数的 1 到 2 倍。比如车辆横摆响应时间常数约 1.5 秒，T_s = 0.1 时 N_p 取 10 到 15 就很合理。
控制时域 N_c 可以比 N_p 小得多，常见取 N_p 的 1/3 到 1/2。N_c 越短，决策变量越少，求解越快，但控制自由度也越低。我的习惯是先设 N_c = 3 跑通，再逐步增加观察求解时间和闭环表现。
4.2 权重矩阵：先调量纲，再调比例
Q 和 R 的调参不是纯粹的经验玄学，里面有一个基本功：量纲归一化。状态里的位置偏差是米，速度偏差是米每秒，航向偏差是弧度，三者数值范围差一个量级。如果直接用 Q = diag([10 10 10 10])，航向偏差的弧度值天然比位置偏差的米值小很多，相当于被无视了。
先做归一化，再定权重比例。操作是把 Q 的每个对角元取为典型偏差最大值的倒数，比如位置允许偏差 0.5m、航向允许偏差 0.1rad、速度允许偏差 0.5m/s，则 Q 的初始值可以取：
Q = diag([1/0.5^2, 1/0.5^2, 1/0.1^2, 1/0.5^2]);这样每个状态项的惩罚量级在同一水平，然后在此基础上调整比例。横向位置权重如果还是要加强，就在对角线前乘一个系数，比如横向权重是纵向的 2 到 4 倍。R 的初始值可以取输入允许最大值的倒数平方，再根据响应速度微调。
4.3 求解器选项与热启动：控制实时性的关键
fmincon 的默认选项直接用于实时控制很多时候会超时。我一般会收紧三个选项：
options = optimoptions('fmincon', ... 'Algorithm', 'sqp', ... 'MaxIterations', 300, ... 'MaxFunctionEvaluations', 3000, ... 'StepTolerance', 1e-6, ... 'OptimalityTolerance', 1e-3, ... 'ConstraintTolerance', 1e-4, ... 'Display', 'off', ... 'SpecifyConstraintGradient', false);MaxIterations和MaxFunctionEvaluations是硬性上限，防止某个时刻求解器卡死在迭代里，占完 CPU 时间才返回。OptimalityTolerance从 1e-6 放宽到 1e-3，求解会快很多，而闭环控制对最优解的精度要求没那么苛刻——次优解一样能控住，真正的难点是每次都能在固定时间内返回一个可行解。
热启动是这条策略里最值钱的一环。上一节提到的build_initial_guess就是热启动入口。它的常见做法是把上一时刻最优输入序列左移一位，新的一位沿用最后的值：
function z0 = build_initial_guess(u_prev_seq, N_c, n_u) % u_prev_seq 是上一时刻最优输入矩阵 [n_u, N_c] if isempty(u_prev_seq) z0 = zeros(n_u * N_c, 1); return; end % 左移并复制最后一项 shifted = [u_prev_seq(:, 2:end), u_prev_seq(:, end)]; z0 = reshape(shifted, [], 1); end这样 fmincon 的起点离上一时刻的最优解很近，在轨迹平滑变化时通常只需要少量迭代就能收敛。注意如果模型跳变或者参考轨迹突变，热启动反而可能把求解器带进局部最优的陷阱，这时需要在求解器外部检测 exitflag，一旦失败就退化为冷启动（z0 全零或取一个保守可行输入）。
5. 避坑指南：MPCC 调试中五个常见问题与排查路径
5.1 fmincon 求解超时，实时性无法保证
现象：闭环仿真偶尔卡顿，一个控制周期要几十毫秒甚至上百毫秒，在硬件上直接导致控制频率不达标。
原因：你以为是求解器不行，其实是约束函数里做了太重的计算。预测轨迹在约束函数里推了一遍，在代价函数里又推了一遍，双倍计算量；另外 MaxIterations 没有设上限，求解器在复杂约束下可以迭代上千次。
解决：把代价函数和约束函数里的模型推进公共部分抽出来，在同一个函数内同时返回代价、约束、梯度（如果有解析梯度），避免重复计算。同时设 MaxIterations = 300，Display设为 off。若仍然超时，缩短 N_c，把 N_c 从 5 降到 3，决策变量维度从 10 降到 6，求解时间通常能降一半以上。
5.2 参考轨迹跟踪出现固定相位滞后，拐弯处外抛
现象：直线段跟踪良好，弯道处轨迹明显落后于参考线，而且滞后量与车速成正比。
原因：不是权重不够，而是代价函数里只惩罚了当前预测点的位置偏差，没有把参考轨迹的航向和曲率信息吃进去。车速越高，一个预测步内车辆走过的弧长越长，位置偏差惩罚的等效目标点其实在参考线内侧，控制器看到的是一根扭曲的期望路径。
解决：把航向偏差权重提高，或者在参考轨迹里加入前馈偏置。也可以把位置偏差投影到参考线的法线与切线上，分别给不同的权重——这就是轮廓控制（Contouring Control）的雏形。MPCC（Model Predictive Contouring Control）一类方法正是围绕这个思想展开的，核心是把跟踪偏差分解为轮廓误差（法向）与弧长误差（切向），各自独立加权。
% 把偏差分解到参考系下的轮廓误差与弧长误差 theta_r = ref_k(3, i); % 参考航向 ex = x(1) - ref_k(1, i); ey = x(2) - ref_k(2, i); e_contour = ex * sin(theta_r) - ey * cos(theta_r); % 法向 e_lag = ex * cos(theta_r) + ey * sin(theta_r); % 切向 J = J + q_contour * e_contour^2 + q_lag * e_lag^2;5.3 控制量输出高频抖动，执行器寿命堪忧
现象：控制量不是平滑曲线，而是 bang-bang 式的锯齿波，方向盘转角在限幅边界来回跳动。
原因：R 矩阵权重太小，控制器认为猛打方向盘不费成本；同时预测时域偏短，远处的惩罚看不见，于是只用当前几步的激进动作来压偏差。另一个隐蔽因素是代价函数里没有惩罚输入变化率，相邻两步的控制量可以任意跳变。
解决：在代价函数里加入 Δu 惩罚项，也就是相邻控制步之间的差值二次型：
% 在 mpc_cost 内部，代价累加时加入变化率惩罚 if i > 1 du = u - u_prev_step; J = J + du' * Rd * du; endRd是变化率权重矩阵，初始取 R 的 0.1 到 0.5 倍。这个惩罚会让控制量序列变得平滑，代价是响应变慢一点。优先调 Rd，不要一来就把 R 提高十倍，那样轨迹跟踪精度会明显下降。
5.4 终端约束开硬脑洞，问题直接不可行
现象：加上终端等式约束后，fmincon 返回 infeasible，闭环第一步就没法进行。
原因：终端等式约束要求预测终点精确落在参考点，这在非线性模型下几乎不可能满足，尤其是预测时域有限时。强行要求等于，相当于在优化问题里加了一个大概率不满足的苛刻条件。
解决：把终端等式约束换成终端不等式约束或软约束。常见做法是保留终端代价：预测终点偏差乘一个很大的权重，而不是硬约束：
% 把终端约束写进代价函数，而不是 ceq e_terminal = x_pred(:, end) - ref_k(:, end); J = J + e_terminal' * P * e_terminal;P 取 Q 的 5 到 10 倍。这个做法的含义是：不强制终点必须一致，但偏离终点会产生巨大代价，优化器会自动在可行域内找一个折中。
5.5 仿真与实物行为不一致，离线跑得好好的，上机就发散
现象：同一份代码，在 Simulink 或脚本仿真里跟踪完美，搬到硬件上第一圈就震荡甚至跑飞。
原因：最常见的是初始状态不一致与测量噪声被忽略。仿真里 x0 是精确的给定值，硬件上状态来自估计器，初始偏差大；代价函数和约束没有对状态噪声留余量，约束边界被噪声顶破。
解决：在仿真阶段就注入状态噪声做蒙特卡洛验证，约束边界留出 10% 到 20% 的裕量。例如车道边界是 3 米，约束里写成 2.5 米；速度上限是 5m/s，约束里写成 4.5m/s。另外检查状态反馈回路的延时——如果执行机构和传感器各有一个采样周期的延迟，主循环里需要对预测模型做一个一拍延迟补偿，常见做法是在预测前用 model_step 先推一步，把估计状态对齐到当前控制时刻。
6. 进阶验证：闭环仿真三步检查法与后续要补的功课
代码能跑通只是第一步，判断一份 MPCC 代码能不能扛事，我习惯在交付前强制走一遍三步检查法。
第一步，开环一致性检查。取一段固定的参考轨迹和一组固定的输入序列，把 MPC 代价函数里的预测轨迹单独导出来，再用同样的初始状态和输入序列调用 model_step 逐步推进，两条轨迹必须完全重合。这一步排查的是代价函数内部模型推进与外部被控对象模型是否同步，很多人调了半天权重，最后发现内部预测和外部位移不一致，整个优化只是在追一条错误的轨迹。
% 开环一致性检查脚本片段 [~, x_pred_mpc] = mpc_cost_and_traj(z_test, x0, ref_k, params); x_pred_manual = zeros(4, N_p + 1); x_pred_manual(:, 1) = x0; for i = 1:N_p u = decode_input(z_test, i, params); x_pred_manual(:, i+1) = model_step(x_pred_manual(:, i), u, T_s, L); end assert(norm(x_pred_mpc - x_pred_manual(:, 2:end), 'fro') < 1e-8);第二步，闭环鲁棒性检查。在闭环仿真里给初始状态加 5% 的随机偏差，连续跑 20 组，统计最大跟踪误差和控制量最大变化率都必须在设计范围内。这条能暴露那些只在理想初始条件下才成立的脆弱控制器。
第三步，约束余量检查。把参考轨迹改成比设计更激进的情况，比如参考横摆角速度加大 20%，观察 fmincon 的 exitflag 是否持续为正，约束是否被频繁激活。如果 exitflag 频繁为 0 或负值，说明约束配置没有留余量，需要回过去调约束边界或加松弛。
从那以后我每次拿到一套新的 MPCC 代码，都会在修改任何权重之前先跑开环一致性检查，再决定往哪调。这个习惯帮我省掉了太多排查时间——很多所谓调参玄学，数据一测就现原形。希望帮到你。
本文还有配套的精品资源，点击获取
