简介：这是一份面向优化算法学习者、科研人员与工程开发者的 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 维，N=20 的时候决策变量还在几十个量级，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 以上。离散化误差在dt=0.05、v=10 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 = 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)); end

max(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决定车辆能“看多远”。N=20、dt=0.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/s，N=20、dt=0.05只有 30 米预览，弯前重刹不够，优先把N加到 30 到 40，而不是把dt加到 0.1；dt变大虽然预览距离也变大，但离散误差会让弯道变成折线，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); end

u_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.m，MATLAB 会调用路径靠前的那个，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(k+1,:)' - 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 以内，再开闭环调权重。希望帮到你。

本文还有配套的精品资源，点击获取