Matlab MPCC经典代码拆解：模型预测控制轨迹跟踪避坑指南

简介一套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:kN_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 300Display设为 off。若仍然超时缩短 N_c把 N_c 从 5 降到 3决策变量维度从 10 降到 6求解时间通常能降一半以上。5.2 参考轨迹跟踪出现固定相位滞后拐弯处外抛现象直线段跟踪良好弯道处轨迹明显落后于参考线而且滞后量与车速成正比。原因不是权重不够而是代价函数里只惩罚了当前预测点的位置偏差没有把参考轨迹的航向和曲率信息吃进去。车速越高一个预测步内车辆走过的弧长越长位置偏差惩罚的等效目标点其实在参考线内侧控制器看到的是一根扭曲的期望路径。解决把航向偏差权重提高或者在参考轨迹里加入前馈偏置。也可以把位置偏差投影到参考线的法线与切线上分别给不同的权重——这就是轮廓控制Contouring Control的雏形。MPCCModel 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(:, i1) 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 代码都会在修改任何权重之前先跑开环一致性检查再决定往哪调。这个习惯帮我省掉了太多排查时间——很多所谓调参玄学数据一测就现原形。希望帮到你。本文还有配套的精品资源点击获取