简介这是一份面向优化算法学习者、科研人员与工程开发者的 MATLAB MPCC 经典代码合集围绕含互补约束的数学规划问题帮助读者理解模型构建、求解器调用、仿真验证与数值稳定性处理可用于课程设计、论文复现与算法原型验证。压缩包共 94 个文件约 14.99MB以 MATLAB 的 m 脚本和 C 的 cpp、h 源码为主辅以 jpg 示意图、json 配置、md 说明文档以及少量 mat 数据、sh 脚本等目录模块划分较清楚既便于阅读算法实现也方便对照工程接口与实验配置。已有 585 人学习下载具备一定的学习参考热度。内容覆盖互补约束建模、优化算法实现、求解器接口、轨迹与预测绘图、测试案例等环节读者可据此梳理从数学公式到代码落地的路径并借鉴数值稳定、调试分析与性能优化方面的处理思路。1. 为什么一段十年前的 MPCC 代码今天还值得翻出来跑从“跟线”到“贴边”的那点事如果你只做过纯跟踪或 LQR 跟线第一次翻开 matlab mpcc 经典代码大概率会愣一下明明已经有横向误差为什么还要把纵向进度单独拎出来答案在弯道。纯跟踪在直道上很稳一进连续弯就开始切内线速度一高就画龙MPCC 把“贴线”拆成轮廓误差和滞后误差再把“跑得快”写进目标函数里的进度项车就会自己找一条既贴边又不失速的线。这套代码适合已经能跑通自行车模型、想做路径跟踪闭环的读者也适合拿它当 MPC 课程的课程设计底稿。2. 把 MPCC 拆成三块轮廓误差、进度变量与自行车模型2.1 轮廓误差和滞后误差为什么不能只罚横向偏差MPCC 的全称是模型预测轮廓控制核心不是“让车走到某个时间点该到的位置”而是“让车贴着一条几何路径走同时尽量往前赶”。这两件事的差别落到代码里就是两个误差轮廓误差e_c和滞后误差e_l。轮廓误差是车辆到参考路径的垂直距离滞后误差是车辆沿参考路径切向的偏差。只罚横向偏差的控制器在弯道里会默认“晚一点到也没关系”而 MPCC 会把滞后误差也压住所以入弯更干脆。常见做法是给e_c较大的二次惩罚给e_l较小的二次惩罚再把进度变量θ单独奖励。function [e_c, e_l] contouring_lag_error(p, p_ref, t_ref, n_ref) % p: 车辆当前位置 [x; y] % p_ref: 参考路径上按 theta 参数化后的点 % t_ref: 参考点切向单位向量 % n_ref: 参考点法向单位向量左法向 d p - p_ref; e_c n_ref * d; % 轮廓误差垂直于路径的偏差带符号 e_l t_ref * d; % 滞后误差沿路径方向的偏差 end这段函数只做一件事把全局坐标下的位置误差投影到参考线的切向和法向。n_ref的方向决定e_c的正负号调试时不要一边用左法向、一边又按右法向画图否则你会看到误差曲线整体反号。t_ref和n_ref必须是单位向量参考线重采样后要重新算一遍不能直接拿原始离散点的差分当单位向量用。我一般会在函数入口加一句t_ref t_ref / norm(t_ref);代价很小但能省掉很多“为什么误差突然放大十倍”的排查。2.2 进度变量 θ把“跑得快”变成可优化的量MPCC 和普通轨迹跟踪最大的区别是它不要求车辆在固定时间到达固定点。参考路径用弧长参数θ表示车辆当前投影到路径上的位置也是θ控制器每步优化的是“未来 N 步内θ能推进多少”。所以目标函数里会出现-q_theta * θ这种负号奖励项θ越大代价越小车自然愿意往前跑。常见进度更新式是θ_dot v * cos(e_phi) / (1 - kappa * e_c)其中e_phi是航向误差kappa是参考点曲率。分母在急弯加大的轮廓误差下可能接近零甚至变负代码里必须做保护否则求解器会收到一个物理上不成立的进度速率。function theta_dot progress_rate(v, e_phi, kappa, e_c) % v: 纵向速度 % e_phi: 航向误差 phi - phi_ref % kappa: 参考点曲率 % e_c: 轮廓误差 denom 1 - kappa * e_c; if abs(denom) 1e-3 denom sign(denom) * 1e-3; % 防止除零和符号翻转 end theta_dot v * cos(e_phi) / denom; end这段代码的逻辑说明很短v * cos(e_phi)是车辆速度在参考线切向的投影分母是对曲率造成的弧长伸缩做修正。参数1e-3不是魔法数它只是防止数值爆炸如果你的赛道曲率很大、车辆又允许贴边很近可以把保护阈值调到5e-3但不要调得太大否则进度会被人为压慢。另一个容易翻车的地方是θ的初值它应该和起点在参考线上的弧长对应不能每步都从零开始重新搜索最近点否则目标函数里的进度奖励会变成锯齿波车一冲一停。2.3 自行车模型状态量和离散化经典 MPCC 代码大多用运动学自行车模型状态取[X; Y; phi; v]控制取[a; delta]也就是加速度和前轮转角。连续 dynamics 写成X_dot v*cos(phi)、Y_dot v*sin(phi)、phi_dot v*tan(delta)/L、v_dot a离散化用前向欧拉就够跑通第一版。之所以选运动学模型不是因为它更真实而是因为它把优化问题压得足够小状态 4 维、控制 2 维N20 的时候决策变量还在几十个量级fmincon或 CasADi 都能在几十毫秒内解完。车速超过 15 m/s 以后轮胎侧偏不能忽略这时候再往上加动态模型否则你会看到“仿真很稳、实车一进弯就推头”的经典错位。function x_next bicycle_step(x, u, dt, L) % x [X; Y; phi; v] % u [a; delta] X x(1); Y x(2); phi x(3); v x(4); a u(1); delta u(2); X_dot v * cos(phi); Y_dot v * sin(phi); phi_dot v * tan(delta) / L; v_dot a; x_next x dt * [X_dot; Y_dot; phi_dot; v_dot]; end这段bicycle_step只做一步前向欧拉参数L是轴距单位必须是米dt是采样时间单位必须是秒delta是前轮转角单位必须是弧度。常见错误是把方向盘角度直接当delta传进去中间少了转向传动比结果车在仿真里转得飞快一换实车参数就完全对不上。另一个边界是delta接近 ±90° 时tan会爆炸所以约束里必须给delta_max一般乘用车在 0.5 rad 左右赛车可以到 0.6 rad但不要开放到 1.0 rad 以上。离散化误差在dt0.05、v10 m/s时还能接受如果dt加到 0.2一个步长车已经走了 2 米欧拉积分会把弯道切成折线这时候要么减小dt要么换 RK4。3. 把经典代码跑起来从解压到第一张轨迹图3.1 先确认三件事MATLAB 版本、求解器、参考线数据拿到资源包之后不要急着点运行先确认三件事MATLAB 版本能不能跑、优化工具箱在不在、参考线数据是什么格式。经典 MPCC 代码常见两种求解路径一种是fmincon版依赖 Optimization Toolbox另一种是 CasADi 版需要额外安装 CasADi 并配置路径。两条路径的入口脚本不要混用我见过有人把 CasADi 版的mpcc_solve.m拷到fmincon版目录里结果报了一屏“未定义函数”其实只是依赖没对齐。下面这段检查代码不挑版本先把路径和许可证确认掉再决定跑哪个入口。% 把 mpcc 换成资源包解压后的实际目录名 addpath(genpath(fullfile(pwd, mpcc))); % 确认入口脚本存在实际名字以资源包为准 which mpcc_main % 检查优化工具箱CasADi 版则检查 casadi 是否在路径里 has_optim license(test, Optimization_Toolbox); has_casadi exist(casadi, file); fprintf(Optimization Toolbox: %d, CasADi: %d\n, has_optim, has_casadi);addpath(genpath(...))会把子目录一次性加进来适合资源包层级不深的场景如果目录里有大量数据文件genpath会拖慢启动这时候改成只加src和solver两个子目录。which mpcc_main只是为了确认入口脚本能被 MATLAB 找到实际函数名可能叫main_mpcc、run_mpcc或者demo_mpcc把名字替换掉即可。license(test, ...)返回 1 表示工具箱可用返回 0 就不要硬跑fmincon换 CasADi 版或者装工具箱省得在后面被exitflag误导。3.2 参考线预处理去重、按弧长重采样MPCC 对参考线的要求比纯跟踪高因为θ是按弧长推进的参考线里如果有重复点切向会变成零向量曲率会变成NaN求解器第一步就会给你脸色看。我一般先把原始点去重再按固定弧长重采样ds取 0.1 m 对大多数场地够了如果赛道特别小、弯特别急取 0.05 m代价是参考点数量翻倍求解时间也会涨。重采样之后要重新算切向和曲率不要沿用原始点的差分结果否则曲率在拼接处会跳变。下面这段代码只处理平面点假设资源包里的track.mat有x和y两个字段字段名不同就改读取那一行。raw load(track.mat); x_raw raw.x(:); y_raw raw.y(:); % 去掉重复点避免切向为零 d sqrt(diff(x_raw).^2 diff(y_raw).^2); keep [true; d 1e-6]; x_raw x_raw(keep); y_raw y_raw(keep); % 按弧长重采样 s [0; cumsum(sqrt(diff(x_raw).^2 diff(y_raw).^2))]; s_query 0:0.1:s(end); x_ref interp1(s, x_raw, s_query, linear, extrap); y_ref interp1(s, y_raw, s_query, linear, extrap);逻辑说明keep的第一个元素固定为true因为第一个点没有前向差分d 1e-6把距离过近的点删掉阈值不要设成 0.1否则会把正常的小步长也删掉。interp1的extrap是为了防止最后一个查询点刚好落在s(end)之外MATLAB 默认会返回NaN而NaN一旦进到优化问题里求解器会直接报“目标函数返回 NaN”。参数0.1是重采样步长单位米如果你的参考线本身只有几十个点重采样后点数是原来的几倍这是正常的不要为了“看起来简洁”又把点抽稀回去。3.3 跑第一圈闭环初始状态、仿真时长和日志入口脚本能跑之后先别急着改权重。用默认参数跑一圈 20 秒的闭环看看日志里有没有轨迹、轮廓误差和速度。初始状态一般取参考线起点航向用前两个参考点的atan2算出来速度给 5 到 8 m/s不要一上来就 30 m/s。仿真时长Tf取 20 秒足够看出直道和第一个弯的表现dt取 0.05 秒如果资源包默认dt是 0.1先按默认跑通再改成 0.05 对比。下面这段调用里的mpcc_sim是通用写法实际入口函数名以资源包为准参数顺序也可能不同把x0、x_ref、y_ref、Tf、dt对上即可。x0 [x_ref(1); y_ref(1); ... atan2(y_ref(2)-y_ref(1), x_ref(2)-x_ref(1)); 8]; Tf 20; dt 0.05; logs mpcc_sim(x0, x_ref, y_ref, Tf, dt);x0的第四个元素是初始速度单位 m/s航向用atan2算出来是弧度不要传角度。Tf是总仿真时间不是预测时域预测时域在参数结构体里的N和dt。logs里至少应该有t、x、y、v、e_c、u这几个字段如果资源包只存了轨迹没有存控制量就在仿真循环里补一行logs.u(k,:) u;后面排查抖振的时候没有控制量会很难受。3.4 三张图判断有没有跑对轨迹、轮廓误差、速度跑完不要只看“车有没有动”要看三张图轨迹图看车是否贴合参考线轮廓误差图看e_c是否收敛在边界内速度图看车是否在弯前减速、出弯加速。如果轨迹图看起来像一条直线先别调权重去看θ有没有更新如果e_c在 0 附近高频抖先看控制量有没有转向速率约束如果速度一直掉到 0先看目标函数里的进度奖励是不是被约束压死了。下面这段绘图代码不依赖工具箱直接拿日志画。figure(Name, MPCC 闭环日志); subplot(3,1,1); plot(x_ref, y_ref, k--); hold on; plot(logs.x, logs.y, b); axis equal; grid on; xlabel(X [m]); ylabel(Y [m]); legend(参考线, 车辆); subplot(3,1,2); plot(logs.t, logs.e_c); grid on; ylabel(e_c [m]); subplot(3,1,3); plot(logs.t, logs.v); grid on; ylabel(v [m/s]); xlabel(t [s]);第一张图里参考线用黑色虚线、车辆用蓝色实线axis equal必须加否则弯道会被拉扁看起来“贴得很好”实际横向误差已经很大。第二张图看e_c的峰值和收敛速度峰值超过道路半宽就说明约束没生效或者参考线有问题。第三张图看速度曲线是否平滑如果速度在 0 和 20 之间来回跳多半是进度奖励和速度约束打架先把q_theta降一半再试。还有一个小习惯把logs.solve_time也打印出来平均求解时间超过dt就说明这套参数在目标机器上跑不了实时后面调参之前先减N或者换求解器。4. 参数不是玄学权重、时域与约束的标定顺序4.1 先定物理约束再定权重参数标定最常见的翻车是一上来就调q_c和q_theta结果车要么画龙要么切弯回头发现delta_max填的是角度、a_max填的是g。正确的顺序是先定物理约束最大速度、最大加速度、最大减速度、最大前轮转角、单步转角增量这些量必须和车模对得上。权重是在可行域里决定“你更在意贴线还是更在意快”约束没定对权重调得再细也是白搭。下面这张表是我一般会先填的起步值单位都标清楚换车模的时候只改物理量权重先不动。参数含义起步值调整方向P.N预测步数20高速或长弯加大实时性不够就减小P.dt采样时间0.05 s抖振严重降到 0.02计算太慢升到 0.1P.L轴距2.7 m按实际车模填不要用方向盘转角代替P.v_max最大速度30 m/s先按 10 m/s 跑通再往上加P.a_max最大加速度5 m/s²电动小车可以更低赛车可以更高P.a_min最大减速度-8 m/s²注意负号约束里是下界P.delta_max最大前轮转角0.5 rad乘用车约 0.5赛车约 0.6P.ddelta_max单步转角增量0.05 rad抖振时先加这个约束P.q_c轮廓误差权重100切弯就加大画龙就减小P.q_l滞后误差权重10入弯拖沓就加大但不要超过q_cP.q_theta进度奖励权重50太慢就加大切内线就减小P.r_ddelta转角速率惩罚100抖振时加大响应变钝就减小P struct(); P.N 20; % 预测时域步数 P.dt 0.05; % 采样时间 [s] P.L 2.7; % 轴距 [m] P.v_max 30; % 最大速度 [m/s] P.a_max 5; % 最大加速度 [m/s^2] P.a_min -8; % 最大减速度 [m/s^2] P.delta_max 0.5; % 最大前轮转角 [rad] P.ddelta_max 0.05; % 单步转角增量上限 [rad] P.q_c 100; % 轮廓误差权重 P.q_l 10; % 滞后误差权重 P.q_theta 50; % 进度奖励权重 P.r_a 1; % 加速度惩罚 P.r_delta 10; % 转角惩罚 P.r_ddelta 100; % 转角速率惩罚这段结构体的作用是让参数集中管理后面调参只改字段不要把数字散落在cost.m、constraints.m和主脚本里。P.a_min是负值写到约束里的时候要用u_lb(1) P.a_min不要写成-P.a_min否则减速度会变成正的。P.ddelta_max是单步增量不是转角速率单位是 rad/step如果你按 rad/s 给要乘dt再传进去。P.r_ddelta是惩罚权重不是硬约束先加硬约束再加惩罚顺序反了会出现“求解器说可行但车还是抖”的怪现象。4.2 权重整定的顺序先轮廓再进度最后舒适性权重不要一起调一次只动一个不然你永远不知道是哪一项把车带偏的。我的顺序是先把q_theta设小只调q_c让车能贴住线再加q_l让车不要在弯前拖太久最后加q_theta让车愿意加速舒适性惩罚r_ddelta放在最后用来压抖振。每次调完看三个指标max(abs(e_c))、平均速度、最大单步转角增量。下面这段扫参代码只扫q_theta其他参数固定跑四组就能看出趋势不要一上来就网格搜索计算量吃不消。q_theta_list [10, 30, 50, 100]; for k 1:numel(q_theta_list) P.q_theta q_theta_list(k); logs mpcc_sim(x0, x_ref, y_ref, Tf, dt, P); fprintf(q_theta%4.1f, max|e_c|%.3f, mean v%.2f\n, ... P.q_theta, max(abs(logs.e_c)), mean(logs.v)); endmax(abs(logs.e_c))看的是最坏情况mean(logs.v)看的是整体进度两个指标要一起看。q_theta从 10 加到 100平均速度可能只涨 1 m/s但max|e_c|从 0.3 m 涨到 1.2 m说明进度奖励已经把车推到边界上了这时候再加就是切弯。反过来q_theta太小车在直道上慢吞吞e_c很漂亮但圈速没法看。我一般会选“max|e_c|不超过道路半宽的 60%、平均速度接近目标速度”的那组而不是只看圈速。4.3 时域和步长不是越长越好预测时域N*dt决定车辆能“看多远”。N20、dt0.05只有 1 秒预览在 30 m/s 时对应 30 米弯前重刹根本来不及但把N加到 60决策变量翻三倍求解时间可能从 20 ms 涨到 80 ms实时性直接没了。常见做法是先按目标速度估预览距离preview N * dt * v让预览距离至少覆盖 2 秒的行驶距离再在这个约束下选N和dt。下面这段代码只是把不同速度下的预览距离打出来方便你判断现在的N*dt够不够。v_list [5, 10, 20, 30]; for v v_list fprintf(v%2d m/s, preview%.1f m\n, v, P.N*P.dt*v); end如果目标车速是 30 m/sN20、dt0.05只有 30 米预览弯前重刹不够优先把N加到 30 到 40而不是把dt加到 0.1dt变大虽然预览距离也变大但离散误差会让弯道变成折线tan(delta)在大步长下更容易把车“甩”出去。N加大之后如果求解时间顶不住再考虑热启动把上一时刻的解往前平移一步作为初值能省掉不少迭代。热启动不是万能药如果参考线曲率突变上一时刻的解可能把求解器带进局部不可行这时候要保留一个零初值的回退分支。5. 避坑与排查五个把新手挡在门外的报错5.1 求解器与依赖类问题现象命令行输出exitflag 0或-2轨迹在第一个弯就停住日志里u全是零。原因初始猜测直接给零或者参考线在车辆后方约束边界太紧求解器找不到可行点。解决用上一时刻解平移做热启动把初始状态约束放宽检查x0是否在lb/ub内。下面这段热启动代码只做平移不改变控制量的物理含义适合第一版排查。% 热启动把上一次解向前平移一步 if ~isempty(u_prev) u0 [u_prev(2:end, :); u_prev(end, :)]; else u0 zeros(P.N, 2); endu_prev是上一时刻的最优控制序列列顺序要和当前u0一致常见是[a, delta]。如果资源包里的控制量顺序是[delta, a]平移之后要转置否则热启动会把转角和加速度对调求解器会给出一个看起来“可行”但完全错误的解。回退分支不要省热启动失败就退回零初值再失败就放宽约束三步排查比盯着exitflag猜要快。现象一运行就报“未定义函数或变量 casadi”或者fmincon提示许可证不可用。原因资源包分fmincon版和 CasADi 版依赖没装或者路径只加了主目录没加子目录。解决先确认入口脚本属于哪个版本再装对应依赖用addpath(genpath(...))加路径不要混用两个版本的求解函数。下面这段断言代码放在脚本开头能提前把问题挡掉。addpath(genpath(mpcc)); assert(license(test, Optimization_Toolbox) 1, ... 缺少 Optimization Toolbox请改用 CasADi 版或安装工具箱);assert的好处是报错信息直接告诉你缺什么而不是等到第 200 行才报一个看不懂的求解器错误。如果你用的是 CasADi 版把第二行换成assert(exist(casadi, file) 0, CasADi 未加入路径);。路径里如果有两个版本的mpcc_solve.mMATLAB 会调用路径靠前的那个which -all mpcc_solve可以看冲突别问我怎么知道的。5.2 模型与几何类问题现象轨迹图只有起点一个点或者车原地不动e_c一直是零。原因θ没有更新参考线点重复导致插值出NaN或者角度单位混用。解决检查θ是否单调递增参考线是否去重所有角度是否统一成弧度。下面这行断言放在仿真循环里能第一时间抓住进度不更新的问题。assert(all(diff(logs.theta) 0), theta 没有单调递增检查进度更新);logs.theta的差分出现零或负值常见原因是参考线在起点附近有回环或者θ每步都被重新初始化为零。如果是回环赛道θ本身可以绕圈但单圈内必须单调跑多圈的时候要把θ按总弧长累加不能直接取模。角度单位混用更隐蔽atan2输出弧度delta_max如果填了 30约束就形同虚设车会转得像陀螺。现象直道上很稳一进弯转角指令高频抖动e_c在 0 附近来回跳。原因没有转向速率约束q_c过大dt太大或者参考线曲率突变。解决加P.ddelta_max和P.r_ddelta把q_c降 30% 到 50%dt降到 0.02 到 0.05检查参考线曲率是否平滑。下面这段代码打印最大单步转角增量超过约束就说明求解器没把硬约束吃进去或者后处理又把控制量改了。max_ddelta max(abs(diff(logs.u(:,2)))); fprintf(最大单步转角增量 %.4f rad\n, max_ddelta);logs.u(:,2)假设第二列是前轮转角如果你的控制量顺序不同把列号换掉。max_ddelta大于P.ddelta_max时先看约束有没有写进Aineq或lb/ub再看求解器有没有返回不可行解。有些资源包为了“看起来平滑”会在求解后对u做低通滤波这会把硬约束破坏掉排查时要把滤波关掉先看原始解。现象同一套参数在 A 赛道能跑换到 B 赛道第一个弯就冲出去。原因参考线曲率不连续道路边界写成了全局坐标初始速度过高。解决对参考线做曲率平滑用 Frenet 系写道路边界初始速度先降到 5 m/s 再逐步加。曲率平滑不要用大窗口滑动平均会把弯道顶点磨圆常见做法是先用样条拟合再按弧长重采样最后检查kappa的最大值是否超过车辆最小转弯半径的倒数。道路边界如果写成“全局坐标下的左右护栏”换赛道就要重新标定用e_c的上下界写边界换赛道只需要改一个半宽参数代码复用性高很多。6. 从“能跑”到“能用”换车模、改参考线与闭环验证的一个小技巧换车模之后最容易被忽略的一步是验模型而不是调权重。我一般会拿日志里的x和u做一步预测残差用bicycle_step从当前状态推下一步和日志里的下一步状态比看残差在不在量级内。残差突然变大先查轴距、dt、转角符号再查参考线不要一上来就怀疑求解器。下面这段代码只做一步预测不依赖优化工具箱换车模之后跑一遍能省掉很多“参数越调越乱”的时间。res zeros(size(logs.x,1)-1, 4); for k 1:size(res,1) x_pred bicycle_step(logs.x(k,:), logs.u(k,:), P.dt, P.L); res(k,:) (logs.x(k1,:) - x_pred); end fprintf(一步预测最大残差: %.4f\n, max(abs(res(:))));res的列对应[X, Y, phi, v]位置残差单位是米航向残差单位是弧度速度残差单位是 m/s。最大残差如果超过 0.1 m 或 0.05 rad先别调 MPCC 权重去看模型轴距是不是填错了dt是不是和日志采样时间不一致转角是不是忘了乘传动比。我吃过一次亏换车模之后只改了L忘了改delta_max结果一步预测残差在直道上很小一进弯就飙到 0.5 rad排查了一下午才发现是转角限幅没跟着换。从那以后我每次换参考线或车模都先用 5 m/s 跑一圈开环把一步预测残差压在 0.02 以内再开闭环调权重。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/11/5 6:10:08 · 阅读 1.2k MPCC路径跟踪实战：MATLAB经典代码跑通、调参与避坑
简介这是一份面向优化算法学习者、科研人员与工程开发者的 MATLAB MPCC 经典代码合集围绕含互补约束的数学规划问题帮助读者理解模型构建、求解器调用、仿真验证与数值稳定性处理可用于课程设计、论文复现与算法原型验证。压缩包共 94 个文件约 14.99MB以 MATLAB 的 m 脚本和 C 的 cpp、h 源码为主辅以 jpg 示意图、json 配置、md 说明文档以及少量 mat 数据、sh 脚本等目录模块划分较清楚既便于阅读算法实现也方便对照工程接口与实验配置。已有 585 人学习下载具备一定的学习参考热度。内容覆盖互补约束建模、优化算法实现、求解器接口、轨迹与预测绘图、测试案例等环节读者可据此梳理从数学公式到代码落地的路径并借鉴数值稳定、调试分析与性能优化方面的处理思路。1. 为什么一段十年前的 MPCC 代码今天还值得翻出来跑从“跟线”到“贴边”的那点事如果你只做过纯跟踪或 LQR 跟线第一次翻开 matlab mpcc 经典代码大概率会愣一下明明已经有横向误差为什么还要把纵向进度单独拎出来答案在弯道。纯跟踪在直道上很稳一进连续弯就开始切内线速度一高就画龙MPCC 把“贴线”拆成轮廓误差和滞后误差再把“跑得快”写进目标函数里的进度项车就会自己找一条既贴边又不失速的线。这套代码适合已经能跑通自行车模型、想做路径跟踪闭环的读者也适合拿它当 MPC 课程的课程设计底稿。2. 把 MPCC 拆成三块轮廓误差、进度变量与自行车模型2.1 轮廓误差和滞后误差为什么不能只罚横向偏差MPCC 的全称是模型预测轮廓控制核心不是“让车走到某个时间点该到的位置”而是“让车贴着一条几何路径走同时尽量往前赶”。这两件事的差别落到代码里就是两个误差轮廓误差e_c和滞后误差e_l。轮廓误差是车辆到参考路径的垂直距离滞后误差是车辆沿参考路径切向的偏差。只罚横向偏差的控制器在弯道里会默认“晚一点到也没关系”而 MPCC 会把滞后误差也压住所以入弯更干脆。常见做法是给e_c较大的二次惩罚给e_l较小的二次惩罚再把进度变量θ单独奖励。function [e_c, e_l] contouring_lag_error(p, p_ref, t_ref, n_ref) % p: 车辆当前位置 [x; y] % p_ref: 参考路径上按 theta 参数化后的点 % t_ref: 参考点切向单位向量 % n_ref: 参考点法向单位向量左法向 d p - p_ref; e_c n_ref * d; % 轮廓误差垂直于路径的偏差带符号 e_l t_ref * d; % 滞后误差沿路径方向的偏差 end这段函数只做一件事把全局坐标下的位置误差投影到参考线的切向和法向。n_ref的方向决定e_c的正负号调试时不要一边用左法向、一边又按右法向画图否则你会看到误差曲线整体反号。t_ref和n_ref必须是单位向量参考线重采样后要重新算一遍不能直接拿原始离散点的差分当单位向量用。我一般会在函数入口加一句t_ref t_ref / norm(t_ref);代价很小但能省掉很多“为什么误差突然放大十倍”的排查。2.2 进度变量 θ把“跑得快”变成可优化的量MPCC 和普通轨迹跟踪最大的区别是它不要求车辆在固定时间到达固定点。参考路径用弧长参数θ表示车辆当前投影到路径上的位置也是θ控制器每步优化的是“未来 N 步内θ能推进多少”。所以目标函数里会出现-q_theta * θ这种负号奖励项θ越大代价越小车自然愿意往前跑。常见进度更新式是θ_dot v * cos(e_phi) / (1 - kappa * e_c)其中e_phi是航向误差kappa是参考点曲率。分母在急弯加大的轮廓误差下可能接近零甚至变负代码里必须做保护否则求解器会收到一个物理上不成立的进度速率。function theta_dot progress_rate(v, e_phi, kappa, e_c) % v: 纵向速度 % e_phi: 航向误差 phi - phi_ref % kappa: 参考点曲率 % e_c: 轮廓误差 denom 1 - kappa * e_c; if abs(denom) 1e-3 denom sign(denom) * 1e-3; % 防止除零和符号翻转 end theta_dot v * cos(e_phi) / denom; end这段代码的逻辑说明很短v * cos(e_phi)是车辆速度在参考线切向的投影分母是对曲率造成的弧长伸缩做修正。参数1e-3不是魔法数它只是防止数值爆炸如果你的赛道曲率很大、车辆又允许贴边很近可以把保护阈值调到5e-3但不要调得太大否则进度会被人为压慢。另一个容易翻车的地方是θ的初值它应该和起点在参考线上的弧长对应不能每步都从零开始重新搜索最近点否则目标函数里的进度奖励会变成锯齿波车一冲一停。2.3 自行车模型状态量和离散化经典 MPCC 代码大多用运动学自行车模型状态取[X; Y; phi; v]控制取[a; delta]也就是加速度和前轮转角。连续 dynamics 写成X_dot v*cos(phi)、Y_dot v*sin(phi)、phi_dot v*tan(delta)/L、v_dot a离散化用前向欧拉就够跑通第一版。之所以选运动学模型不是因为它更真实而是因为它把优化问题压得足够小状态 4 维、控制 2 维N20 的时候决策变量还在几十个量级fmincon或 CasADi 都能在几十毫秒内解完。车速超过 15 m/s 以后轮胎侧偏不能忽略这时候再往上加动态模型否则你会看到“仿真很稳、实车一进弯就推头”的经典错位。function x_next bicycle_step(x, u, dt, L) % x [X; Y; phi; v] % u [a; delta] X x(1); Y x(2); phi x(3); v x(4); a u(1); delta u(2); X_dot v * cos(phi); Y_dot v * sin(phi); phi_dot v * tan(delta) / L; v_dot a; x_next x dt * [X_dot; Y_dot; phi_dot; v_dot]; end这段bicycle_step只做一步前向欧拉参数L是轴距单位必须是米dt是采样时间单位必须是秒delta是前轮转角单位必须是弧度。常见错误是把方向盘角度直接当delta传进去中间少了转向传动比结果车在仿真里转得飞快一换实车参数就完全对不上。另一个边界是delta接近 ±90° 时tan会爆炸所以约束里必须给delta_max一般乘用车在 0.5 rad 左右赛车可以到 0.6 rad但不要开放到 1.0 rad 以上。离散化误差在dt0.05、v10 m/s时还能接受如果dt加到 0.2一个步长车已经走了 2 米欧拉积分会把弯道切成折线这时候要么减小dt要么换 RK4。3. 把经典代码跑起来从解压到第一张轨迹图3.1 先确认三件事MATLAB 版本、求解器、参考线数据拿到资源包之后不要急着点运行先确认三件事MATLAB 版本能不能跑、优化工具箱在不在、参考线数据是什么格式。经典 MPCC 代码常见两种求解路径一种是fmincon版依赖 Optimization Toolbox另一种是 CasADi 版需要额外安装 CasADi 并配置路径。两条路径的入口脚本不要混用我见过有人把 CasADi 版的mpcc_solve.m拷到fmincon版目录里结果报了一屏“未定义函数”其实只是依赖没对齐。下面这段检查代码不挑版本先把路径和许可证确认掉再决定跑哪个入口。% 把 mpcc 换成资源包解压后的实际目录名 addpath(genpath(fullfile(pwd, mpcc))); % 确认入口脚本存在实际名字以资源包为准 which mpcc_main % 检查优化工具箱CasADi 版则检查 casadi 是否在路径里 has_optim license(test, Optimization_Toolbox); has_casadi exist(casadi, file); fprintf(Optimization Toolbox: %d, CasADi: %d\n, has_optim, has_casadi);addpath(genpath(...))会把子目录一次性加进来适合资源包层级不深的场景如果目录里有大量数据文件genpath会拖慢启动这时候改成只加src和solver两个子目录。which mpcc_main只是为了确认入口脚本能被 MATLAB 找到实际函数名可能叫main_mpcc、run_mpcc或者demo_mpcc把名字替换掉即可。license(test, ...)返回 1 表示工具箱可用返回 0 就不要硬跑fmincon换 CasADi 版或者装工具箱省得在后面被exitflag误导。3.2 参考线预处理去重、按弧长重采样MPCC 对参考线的要求比纯跟踪高因为θ是按弧长推进的参考线里如果有重复点切向会变成零向量曲率会变成NaN求解器第一步就会给你脸色看。我一般先把原始点去重再按固定弧长重采样ds取 0.1 m 对大多数场地够了如果赛道特别小、弯特别急取 0.05 m代价是参考点数量翻倍求解时间也会涨。重采样之后要重新算切向和曲率不要沿用原始点的差分结果否则曲率在拼接处会跳变。下面这段代码只处理平面点假设资源包里的track.mat有x和y两个字段字段名不同就改读取那一行。raw load(track.mat); x_raw raw.x(:); y_raw raw.y(:); % 去掉重复点避免切向为零 d sqrt(diff(x_raw).^2 diff(y_raw).^2); keep [true; d 1e-6]; x_raw x_raw(keep); y_raw y_raw(keep); % 按弧长重采样 s [0; cumsum(sqrt(diff(x_raw).^2 diff(y_raw).^2))]; s_query 0:0.1:s(end); x_ref interp1(s, x_raw, s_query, linear, extrap); y_ref interp1(s, y_raw, s_query, linear, extrap);逻辑说明keep的第一个元素固定为true因为第一个点没有前向差分d 1e-6把距离过近的点删掉阈值不要设成 0.1否则会把正常的小步长也删掉。interp1的extrap是为了防止最后一个查询点刚好落在s(end)之外MATLAB 默认会返回NaN而NaN一旦进到优化问题里求解器会直接报“目标函数返回 NaN”。参数0.1是重采样步长单位米如果你的参考线本身只有几十个点重采样后点数是原来的几倍这是正常的不要为了“看起来简洁”又把点抽稀回去。3.3 跑第一圈闭环初始状态、仿真时长和日志入口脚本能跑之后先别急着改权重。用默认参数跑一圈 20 秒的闭环看看日志里有没有轨迹、轮廓误差和速度。初始状态一般取参考线起点航向用前两个参考点的atan2算出来速度给 5 到 8 m/s不要一上来就 30 m/s。仿真时长Tf取 20 秒足够看出直道和第一个弯的表现dt取 0.05 秒如果资源包默认dt是 0.1先按默认跑通再改成 0.05 对比。下面这段调用里的mpcc_sim是通用写法实际入口函数名以资源包为准参数顺序也可能不同把x0、x_ref、y_ref、Tf、dt对上即可。x0 [x_ref(1); y_ref(1); ... atan2(y_ref(2)-y_ref(1), x_ref(2)-x_ref(1)); 8]; Tf 20; dt 0.05; logs mpcc_sim(x0, x_ref, y_ref, Tf, dt);x0的第四个元素是初始速度单位 m/s航向用atan2算出来是弧度不要传角度。Tf是总仿真时间不是预测时域预测时域在参数结构体里的N和dt。logs里至少应该有t、x、y、v、e_c、u这几个字段如果资源包只存了轨迹没有存控制量就在仿真循环里补一行logs.u(k,:) u;后面排查抖振的时候没有控制量会很难受。3.4 三张图判断有没有跑对轨迹、轮廓误差、速度跑完不要只看“车有没有动”要看三张图轨迹图看车是否贴合参考线轮廓误差图看e_c是否收敛在边界内速度图看车是否在弯前减速、出弯加速。如果轨迹图看起来像一条直线先别调权重去看θ有没有更新如果e_c在 0 附近高频抖先看控制量有没有转向速率约束如果速度一直掉到 0先看目标函数里的进度奖励是不是被约束压死了。下面这段绘图代码不依赖工具箱直接拿日志画。figure(Name, MPCC 闭环日志); subplot(3,1,1); plot(x_ref, y_ref, k--); hold on; plot(logs.x, logs.y, b); axis equal; grid on; xlabel(X [m]); ylabel(Y [m]); legend(参考线, 车辆); subplot(3,1,2); plot(logs.t, logs.e_c); grid on; ylabel(e_c [m]); subplot(3,1,3); plot(logs.t, logs.v); grid on; ylabel(v [m/s]); xlabel(t [s]);第一张图里参考线用黑色虚线、车辆用蓝色实线axis equal必须加否则弯道会被拉扁看起来“贴得很好”实际横向误差已经很大。第二张图看e_c的峰值和收敛速度峰值超过道路半宽就说明约束没生效或者参考线有问题。第三张图看速度曲线是否平滑如果速度在 0 和 20 之间来回跳多半是进度奖励和速度约束打架先把q_theta降一半再试。还有一个小习惯把logs.solve_time也打印出来平均求解时间超过dt就说明这套参数在目标机器上跑不了实时后面调参之前先减N或者换求解器。4. 参数不是玄学权重、时域与约束的标定顺序4.1 先定物理约束再定权重参数标定最常见的翻车是一上来就调q_c和q_theta结果车要么画龙要么切弯回头发现delta_max填的是角度、a_max填的是g。正确的顺序是先定物理约束最大速度、最大加速度、最大减速度、最大前轮转角、单步转角增量这些量必须和车模对得上。权重是在可行域里决定“你更在意贴线还是更在意快”约束没定对权重调得再细也是白搭。下面这张表是我一般会先填的起步值单位都标清楚换车模的时候只改物理量权重先不动。参数含义起步值调整方向P.N预测步数20高速或长弯加大实时性不够就减小P.dt采样时间0.05 s抖振严重降到 0.02计算太慢升到 0.1P.L轴距2.7 m按实际车模填不要用方向盘转角代替P.v_max最大速度30 m/s先按 10 m/s 跑通再往上加P.a_max最大加速度5 m/s²电动小车可以更低赛车可以更高P.a_min最大减速度-8 m/s²注意负号约束里是下界P.delta_max最大前轮转角0.5 rad乘用车约 0.5赛车约 0.6P.ddelta_max单步转角增量0.05 rad抖振时先加这个约束P.q_c轮廓误差权重100切弯就加大画龙就减小P.q_l滞后误差权重10入弯拖沓就加大但不要超过q_cP.q_theta进度奖励权重50太慢就加大切内线就减小P.r_ddelta转角速率惩罚100抖振时加大响应变钝就减小P struct(); P.N 20; % 预测时域步数 P.dt 0.05; % 采样时间 [s] P.L 2.7; % 轴距 [m] P.v_max 30; % 最大速度 [m/s] P.a_max 5; % 最大加速度 [m/s^2] P.a_min -8; % 最大减速度 [m/s^2] P.delta_max 0.5; % 最大前轮转角 [rad] P.ddelta_max 0.05; % 单步转角增量上限 [rad] P.q_c 100; % 轮廓误差权重 P.q_l 10; % 滞后误差权重 P.q_theta 50; % 进度奖励权重 P.r_a 1; % 加速度惩罚 P.r_delta 10; % 转角惩罚 P.r_ddelta 100; % 转角速率惩罚这段结构体的作用是让参数集中管理后面调参只改字段不要把数字散落在cost.m、constraints.m和主脚本里。P.a_min是负值写到约束里的时候要用u_lb(1) P.a_min不要写成-P.a_min否则减速度会变成正的。P.ddelta_max是单步增量不是转角速率单位是 rad/step如果你按 rad/s 给要乘dt再传进去。P.r_ddelta是惩罚权重不是硬约束先加硬约束再加惩罚顺序反了会出现“求解器说可行但车还是抖”的怪现象。4.2 权重整定的顺序先轮廓再进度最后舒适性权重不要一起调一次只动一个不然你永远不知道是哪一项把车带偏的。我的顺序是先把q_theta设小只调q_c让车能贴住线再加q_l让车不要在弯前拖太久最后加q_theta让车愿意加速舒适性惩罚r_ddelta放在最后用来压抖振。每次调完看三个指标max(abs(e_c))、平均速度、最大单步转角增量。下面这段扫参代码只扫q_theta其他参数固定跑四组就能看出趋势不要一上来就网格搜索计算量吃不消。q_theta_list [10, 30, 50, 100]; for k 1:numel(q_theta_list) P.q_theta q_theta_list(k); logs mpcc_sim(x0, x_ref, y_ref, Tf, dt, P); fprintf(q_theta%4.1f, max|e_c|%.3f, mean v%.2f\n, ... P.q_theta, max(abs(logs.e_c)), mean(logs.v)); endmax(abs(logs.e_c))看的是最坏情况mean(logs.v)看的是整体进度两个指标要一起看。q_theta从 10 加到 100平均速度可能只涨 1 m/s但max|e_c|从 0.3 m 涨到 1.2 m说明进度奖励已经把车推到边界上了这时候再加就是切弯。反过来q_theta太小车在直道上慢吞吞e_c很漂亮但圈速没法看。我一般会选“max|e_c|不超过道路半宽的 60%、平均速度接近目标速度”的那组而不是只看圈速。4.3 时域和步长不是越长越好预测时域N*dt决定车辆能“看多远”。N20、dt0.05只有 1 秒预览在 30 m/s 时对应 30 米弯前重刹根本来不及但把N加到 60决策变量翻三倍求解时间可能从 20 ms 涨到 80 ms实时性直接没了。常见做法是先按目标速度估预览距离preview N * dt * v让预览距离至少覆盖 2 秒的行驶距离再在这个约束下选N和dt。下面这段代码只是把不同速度下的预览距离打出来方便你判断现在的N*dt够不够。v_list [5, 10, 20, 30]; for v v_list fprintf(v%2d m/s, preview%.1f m\n, v, P.N*P.dt*v); end如果目标车速是 30 m/sN20、dt0.05只有 30 米预览弯前重刹不够优先把N加到 30 到 40而不是把dt加到 0.1dt变大虽然预览距离也变大但离散误差会让弯道变成折线tan(delta)在大步长下更容易把车“甩”出去。N加大之后如果求解时间顶不住再考虑热启动把上一时刻的解往前平移一步作为初值能省掉不少迭代。热启动不是万能药如果参考线曲率突变上一时刻的解可能把求解器带进局部不可行这时候要保留一个零初值的回退分支。5. 避坑与排查五个把新手挡在门外的报错5.1 求解器与依赖类问题现象命令行输出exitflag 0或-2轨迹在第一个弯就停住日志里u全是零。原因初始猜测直接给零或者参考线在车辆后方约束边界太紧求解器找不到可行点。解决用上一时刻解平移做热启动把初始状态约束放宽检查x0是否在lb/ub内。下面这段热启动代码只做平移不改变控制量的物理含义适合第一版排查。% 热启动把上一次解向前平移一步 if ~isempty(u_prev) u0 [u_prev(2:end, :); u_prev(end, :)]; else u0 zeros(P.N, 2); endu_prev是上一时刻的最优控制序列列顺序要和当前u0一致常见是[a, delta]。如果资源包里的控制量顺序是[delta, a]平移之后要转置否则热启动会把转角和加速度对调求解器会给出一个看起来“可行”但完全错误的解。回退分支不要省热启动失败就退回零初值再失败就放宽约束三步排查比盯着exitflag猜要快。现象一运行就报“未定义函数或变量 casadi”或者fmincon提示许可证不可用。原因资源包分fmincon版和 CasADi 版依赖没装或者路径只加了主目录没加子目录。解决先确认入口脚本属于哪个版本再装对应依赖用addpath(genpath(...))加路径不要混用两个版本的求解函数。下面这段断言代码放在脚本开头能提前把问题挡掉。addpath(genpath(mpcc)); assert(license(test, Optimization_Toolbox) 1, ... 缺少 Optimization Toolbox请改用 CasADi 版或安装工具箱);assert的好处是报错信息直接告诉你缺什么而不是等到第 200 行才报一个看不懂的求解器错误。如果你用的是 CasADi 版把第二行换成assert(exist(casadi, file) 0, CasADi 未加入路径);。路径里如果有两个版本的mpcc_solve.mMATLAB 会调用路径靠前的那个which -all mpcc_solve可以看冲突别问我怎么知道的。5.2 模型与几何类问题现象轨迹图只有起点一个点或者车原地不动e_c一直是零。原因θ没有更新参考线点重复导致插值出NaN或者角度单位混用。解决检查θ是否单调递增参考线是否去重所有角度是否统一成弧度。下面这行断言放在仿真循环里能第一时间抓住进度不更新的问题。assert(all(diff(logs.theta) 0), theta 没有单调递增检查进度更新);logs.theta的差分出现零或负值常见原因是参考线在起点附近有回环或者θ每步都被重新初始化为零。如果是回环赛道θ本身可以绕圈但单圈内必须单调跑多圈的时候要把θ按总弧长累加不能直接取模。角度单位混用更隐蔽atan2输出弧度delta_max如果填了 30约束就形同虚设车会转得像陀螺。现象直道上很稳一进弯转角指令高频抖动e_c在 0 附近来回跳。原因没有转向速率约束q_c过大dt太大或者参考线曲率突变。解决加P.ddelta_max和P.r_ddelta把q_c降 30% 到 50%dt降到 0.02 到 0.05检查参考线曲率是否平滑。下面这段代码打印最大单步转角增量超过约束就说明求解器没把硬约束吃进去或者后处理又把控制量改了。max_ddelta max(abs(diff(logs.u(:,2)))); fprintf(最大单步转角增量 %.4f rad\n, max_ddelta);logs.u(:,2)假设第二列是前轮转角如果你的控制量顺序不同把列号换掉。max_ddelta大于P.ddelta_max时先看约束有没有写进Aineq或lb/ub再看求解器有没有返回不可行解。有些资源包为了“看起来平滑”会在求解后对u做低通滤波这会把硬约束破坏掉排查时要把滤波关掉先看原始解。现象同一套参数在 A 赛道能跑换到 B 赛道第一个弯就冲出去。原因参考线曲率不连续道路边界写成了全局坐标初始速度过高。解决对参考线做曲率平滑用 Frenet 系写道路边界初始速度先降到 5 m/s 再逐步加。曲率平滑不要用大窗口滑动平均会把弯道顶点磨圆常见做法是先用样条拟合再按弧长重采样最后检查kappa的最大值是否超过车辆最小转弯半径的倒数。道路边界如果写成“全局坐标下的左右护栏”换赛道就要重新标定用e_c的上下界写边界换赛道只需要改一个半宽参数代码复用性高很多。6. 从“能跑”到“能用”换车模、改参考线与闭环验证的一个小技巧换车模之后最容易被忽略的一步是验模型而不是调权重。我一般会拿日志里的x和u做一步预测残差用bicycle_step从当前状态推下一步和日志里的下一步状态比看残差在不在量级内。残差突然变大先查轴距、dt、转角符号再查参考线不要一上来就怀疑求解器。下面这段代码只做一步预测不依赖优化工具箱换车模之后跑一遍能省掉很多“参数越调越乱”的时间。res zeros(size(logs.x,1)-1, 4); for k 1:size(res,1) x_pred bicycle_step(logs.x(k,:), logs.u(k,:), P.dt, P.L); res(k,:) (logs.x(k1,:) - x_pred); end fprintf(一步预测最大残差: %.4f\n, max(abs(res(:))));res的列对应[X, Y, phi, v]位置残差单位是米航向残差单位是弧度速度残差单位是 m/s。最大残差如果超过 0.1 m 或 0.05 rad先别调 MPCC 权重去看模型轴距是不是填错了dt是不是和日志采样时间不一致转角是不是忘了乘传动比。我吃过一次亏换车模之后只改了L忘了改delta_max结果一步预测残差在直道上很小一进弯就飙到 0.5 rad排查了一下午才发现是转角限幅没跟着换。从那以后我每次换参考线或车模都先用 5 m/s 跑一圈开环把一步预测残差压在 0.02 以内再开闭环调权重。希望帮到你。本文还有配套的精品资源点击获取