简介这份无人驾驶车辆模型预测控制程序代码面向学习自动驾驶控制算法的高校学生、科研人员与工程开发者聚焦MPC在路径跟踪、速度调节与转向控制中的落地实现可作为控制理论向工程代码过渡的参考样例。ZIP压缩包共16个文件、约38KB以13个.m脚本为主体辅以2个txt说明和1个.mdl仿真模型内容覆盖车辆运动学与动力学建模、预测时域内状态推演、带约束优化问题构造、控制序列滚动更新及仿真测试等环节并含Pacejka轮胎模型与C代码参考。已有1071人学习下载适合用于课程设计、算法复现和论文实验的对照阅读。读者可沿章节组织梳理从车辆模型到控制量更新的完整链路理解预测步长、时间间隔与优化参数对控制效果的影响也能借助脚本和Simulink模型搭建调试环境积累无人驾驶MPC的排错与验证思路。1. 一条 S 弯里画龙的车这套模型预测控制程序代码到底管什么去年帮人调一套轨迹跟踪的代码。双移线工况车速给到 36 km/h车身在第二个弯出口开始左右摆越摆越大。一开始按照直觉把预测时域从 20 步加到 40 步结果更糟求解时间翻了三倍摆动幅度反而变大。后来发现问题不在时域长短而是控制时域和转角增量权重 S 的比例失调求解器每拍都在“过度修正”。这几乎是我见过的无人驾驶车辆模型预测控制代码里最高频的一类翻车。这份无人驾驶车辆模型预测控制程序代码讲的就是把上面这条链路完整跑通运动学自行车模型怎么建、工作点处怎么线性化离散、预测方程怎么堆成 Psi 和 Theta 两个矩阵、代价函数和约束怎么组装成标准二次型、最后怎么丢给 QP 求解器并取第一拍执行。它面向的不是发论文是想把仿真跑起来、再往实车搬的人做毕设的学生、从 PID 转 MPC 的工程同学、需要一份能改参数就能看效果的基线代码的人。2. 代码结构拆解从 model 到 sim 的五层目录各管哪一段拿到一份控制类代码我第一件事不是跑 main而是先看目录怎么切的。目录切得清楚说明作者想过模块边界目录一团乱后面改一个权重可能要动五个文件。这套程序的目录是按“模型—预测—控制—仿真—配置”五层切的边界比较干净。2.1 目录结构与模块职责mpc_av/ ├── config/ │ └── params.m # 全部可调参数集中在这里 ├── model/ │ ├── kinematic_bicycle.m # 运动学自行车模型状态推进一步 │ ├── dynamic_bicycle.m # 二自由度动力学模型高速备用 │ └── linearize.m # 工作点处求雅可比 欧拉离散 ├── predictor/ │ ├── build_prediction.m # 堆 Psi / Theta 预测矩阵 │ └── reference.m # 参考轨迹生成与最近点投影 ├── controller/ │ ├── mpc_core.m # 组装 H、f调求解器 │ ├── constraints.m # 控制量、增量、道路边界约束 │ └── solver_wrapper.m # quadprog / OSQP 的统一封装 ├── sim/ │ ├── closed_loop.m # 闭环主循环 │ └── plot_result.m # 轨迹、误差、控制量三联图 └── README.md目录职责改动频率config采样周期、时域、权重、物理限幅极高调参都在这model状态转移与线性化低除非换模型predictor预测矩阵与参考轨迹中换参考生成方式才动controllerQP 组装与求解中加约束才动sim闭环驱动与可视化低这个划分最大的好处是调参不改代码逻辑。我见过太多把Q diag([10,10,1])直接写死在 mpc_core 里的版本换一个工况要全文搜索替换血泪经验。reference.m这一层经常被新手忽略。它做的不是简单的“给一条 x-y 点列”而是每一拍都要把当前车辆位置投影到参考路径上找到最近的路径点再往前取 Np 个点作为这一拍的参考序列。投影做不准后面权重调得再细也白搭。2.2 技术栈MATLAB 打样、Python 调参、C 上车这套代码给了三套实现不是为了炫技是三种使用场景决定的。MATLAB 版本是打样用的。quadprog开箱即用Active-set算法在几十个变量的小规模 QP 上表现稳定方便你在半小时内看到第一条跟踪曲线。缺点是跑批量仿真慢一次双移线扫 20 组参数要等很久。Python 版本是调参用的。核心是numpy拼矩阵osqp或cvxpy做求解。OSQP 是 ADMM 类的一阶算子分裂求解器最大的价值是支持 warm start——上一拍的解可以直接作为这一拍的初值在连续变化的工况下迭代次数能压到个位数。批量扫参数、画误差统计图、跑蒙特卡洛Python 比 MATLAB 顺手得多。C 版本是往实车搬的。Eigen做矩阵运算OSQP的 C 接口做求解整个控制回路按 20 Hz 跑也就是每拍 50 ms。这个周期下QP 求解本身一般要求压在 510 ms 以内剩下 40 ms 留给感知、定位、通信和调度抖动。如果你的求解器偶尔跑到 30 ms实车上就会看到控制量周期性地“迟一拍”那种抖动很难从曲线上反推原因。采样周期Ts的选择有个经验区间低速园区车 0.1 s 也够乘用车级别的跟踪一般用 0.05 s做高速紧急避障才往 0.02 s 走。周期越小同样的预测时域需要的步数越多QP 规模线性增长求解时间也跟着涨。这是个硬的取舍。2.3 一次控制循环的数据流从参考轨迹到方向盘转角中间走的是一条固定链路理解这条链路比背公式有用得多。% sim/closed_loop.m 主循环骨架 p params(); % 读参数 xi [0; 0; 0]; % 初始状态 [x; y; psi] u_prev [0; 0]; % 上一拍控制量 [v; delta] log struct(xi, [], u, [], t, []); for k 1:p.Nsim % 1) 参考轨迹投影找最近点往前取 Np 个参考点 ref reference(xi, p); % 2) MPC 求解返回最优控制序列只取第一拍 [u_opt, info] mpc_core(xi, ref, u_prev, p); % 3) 只执行第一拍这是滚动时域的核心 u u_opt(:, 1); % 4) 状态推进一步仿真里用模型实车上来自定位 xi kinematic_bicycle(xi, u, p); % 5) 记录供后面画图复盘 log.xi(:, k) xi; log.u(:, k) u; log.t(k) k * p.Ts; u_prev u; end逻辑上有三处值得展开。第一u_opt是一整个 Nc 列的矩阵但只有第一列被执行剩下的全部丢掉下一拍重新算——这就是滚动时域也是 MPC 区别于离线最优控制的地方。第二u_prev被传进下一拍用途有两个一是算控制增量约束Δu u_k - u_{k-1}二是做 warm start。第三仿真里状态来自模型推进实车上这个值来自定位模块所以xi的坐标系必须和参考轨迹严格一致坐标系搞错的表现是车永远往一边偏看起来像稳态误差实际是标定问题。3. 车辆模型与预测方程三个状态、两个控制量怎么推出 Psi 和 ThetaMPC 所有的“预测”能力都来自模型。模型选错后面的权重、约束、求解器调得再精细都是在错误的地基上盖楼。这一章把从模型到预测矩阵的推导和代码完整摊开。3.1 运动学自行车模型三个状态两个控制量低速场景下四轮车可以塌缩成前后两个轮子的自行车再进一步假设后轴中心点跟踪参考点就得到最常用的三状态模型。状态取[x, y, psi]控制取[v, delta]也就是速度和前轮转角。function xi_next kinematic_bicycle(xi, u, p) % 后轴参考点的运动学自行车模型欧拉法前向离散 % xi [x; y; psi] 位置与航向角u [v; delta] 速度与前轮转角 x xi(1); y xi(2); psi xi(3); v u(1); delta u(2); xi_next [ x v * cos(psi) * p.Ts; y v * sin(psi) * p.Ts; psi v * tan(delta) / p.L * p.Ts ]; % 航向角归一化防止长时间仿真后数值一直累加 xi_next(3) wrapToPi(xi_next(3)); end逐项说明。p.Ts是采样周期p.L是轴距紧凑型车取 2.7 m 左右比较合适。第三个式子是整个模型的关键航向角变化率等于v * tan(delta) / L它体现了“速度越快、同样转角下转得越快”这个几何事实。转角用tan而不是直接用delta是因为阿克曼转向几何下后轴中心的转弯半径是L / tan(delta)。wrapToPi这一行看起来可有可无实际是必需品。航向角在连续转弯工况下会一直累加跑几百秒后可能到 30 rad虽然数学上等价但后面算误差psi - psi_ref的时候会直接炸掉——两个物理上相同、数值上差 2π 的角度相减得到一个巨大的误差控制器会瞬间输出满舵。这个坑我在第一次做实车调试时踩过现场表现为车在直线段突然打死方向盘。模型的适用边界要说清楚这个模型假设轮胎没有侧偏也就是低速、小侧向加速度。经验分界线在 15 m/s 上下超过这个速度轮胎侧偏力带来的影响不能忽略必须换动力学模型。很多代码在仿真里跑得很漂亮一上实车高速就发散问题就出在这里。3.2 线性化与离散化雅可比矩阵怎么来的上面那个模型是非线性的cos(psi)和tan(delta)都不是线性项而 QP 求解器只吃线性约束和二次目标。所以要在线性化而且是每个控制周期都在当前工作点重新线性化一次——这叫线性时变 MPCLTV-MPC比在原点做一次全局线性化精度高得多。function [Ad, Bd] linearize(xi, u, p) % 在当前工作点 (xi, u) 处对运动学模型求雅可比并做欧拉离散 psi xi(3); v u(1); delta u(2); % 连续时间雅可比直接乘 Ts 完成前向欧拉离散 Ad [ 1, 0, -v * sin(psi) * p.Ts; 0, 1, v * cos(psi) * p.Ts; 0, 0, 1 ]; Bd [ cos(psi) * p.Ts, 0; sin(psi) * p.Ts, 0; tan(delta) / p.L * p.Ts, v / (p.L * cos(delta)^2) * p.Ts ]; endAd是 3×3 的状态转移矩阵Bd是 3×2 的控制矩阵。看Ad的第三列-v*sin(psi)*Ts和v*cos(psi)*Ts含义是“航向角偏一点位置会往哪个方向偏”偏的程度正比于速度。这就解释了为什么高速时航向误差的权重需要调大——同样的角度误差在高速下对应的横向偏移要大得多。Bd的第二列第二行v / (p.L * cos(delta)^2) * Ts是转角对航向角变化的增益它对delta敏感。当delta接近 ±π/2 时cos(delta)趋于零这一项会炸。所以转角上限一般卡在 0.50.6 rad一方面是机械限位另一方面也是避开这个数值奇点。欧拉离散在这个场景下够用因为Ts只有 0.05 s模型的变化很慢。要更准可以用矩阵指数或者二阶龙格库塔代价是每次线性化多算几次矩阵乘法。实车上我一般还是用欧拉省下来的时间留给求解器。3.3 预测方程堆叠Psi 和 Theta 两个矩阵线性化之后把未来 Np 步的状态用当前状态和控制序列表示出来得到一个形如Y Psi * xi0 Theta * U的矩阵等式。这一步是 MPC 代码里最“线性代数”的部分也是最容易写错维度的地方。function [Psi, Theta] build_prediction(Ad, Bd, Np, Nc) % Ad: n×nBd: n×mNp 预测步数Nc 控制步数Nc Np n size(Ad, 1); m size(Bd, 2); Psi zeros(n * Np, n); Theta zeros(n * Np, m * Nc); for i 1:Np rows (i-1)*n 1 : i*n; Psi(rows, :) Ad^i; % 自由响应初值的作用 for j 1:min(i, Nc) cols (j-1)*m 1 : j*m; % 第 j 拍控制量经过 i-j 步自由演化后到达第 i 拍 Theta(rows, cols) Ad^(i-j) * Bd; end end endPsi的维度是(n*Np) × n把初始状态映射到整段预测状态Theta的维度是(n*Np) × (m*Nc)把控制序列映射到预测状态。内层循环min(i, Nc)是控制时域短于预测时域时的标准处理超过 Nc 步之后控制量保持不变所以那一列不再填新值。举个具体数字n3、m2、Np20、Nc8 时Theta是 60×16 的矩阵。QP 的决策变量只有 16 个Hessian 是 16×16。这个规模对任何求解器都是小菜单次求解基本在 1 ms 量级。如果你的 Np 拉到 60、Nc 拉到 30决策变量变成 60 个Hessian 变成 60×60求解时间会明显上升——不是线性上升因为 QP 的复杂度对变量数是非线性的。Theta里Ad^(i-j)的幂次计算在循环里反复调Ad^是浪费。工程写法是先算一个Ad_pows cell(Np, 1)把Ad^1 ... Ad^Np预先存下来。Np 大的时候这个优化能省掉一半以上的组装时间。4. 代价函数与约束组装把跟踪问题塞进 QP 求解器的完整过程预测方程有了接下来要回答两个问题什么样的控制序列算“好”代价函数以及哪些控制序列根本不允许约束。这两块合起来才是一个标准的二次规划。4.1 目标函数Q、R、S 三个权重在调什么代价函数由三部分组成跟踪误差、控制量幅值、控制增量。用误差状态d_xi xi - xi_ref表达目标是最小化J Σ ||d_xi_k||²_Q Σ ||u_k - u_ref||²_R Σ ||u_k - u_{k-1}||²_SQ惩罚偏离参考轨迹R惩罚控制量本身省油、少打方向S惩罚控制量的变化率平顺性。三个权重不是独立的比例关系才是关键。function [H, f] build_cost(Psi, Theta, d_xi, u_ref, p) % 把 J 展开成 1/2 * U*H*U f*U 的标准形式 Np p.Np; Nc p.Nc; % 分块对角扩展每一步的 Q 拼成 Np 块R 拼成 Nc 块 Qbar kron(eye(Np), p.Q); % 3*Np 维 Rbar kron(eye(Nc), p.R); % 2*Nc 维 % 差分矩阵 D把控制序列 U 映射成增量序列 [u1-u0; u2-u1; ...] D build_diff_matrix(Nc, size(p.R, 1)); Sbar kron(eye(Nc), p.S); % 自由响应控制量为零时预测状态落在哪 e0 Psi * d_xi; u_prev_vec repmat(p.u_prev, Nc, 1); % 二次项系数注意 quadprog 的形式是 0.5*xHx fx H 2 * (Theta * Qbar * Theta Rbar D * Sbar * D); f 2 * (Theta * Qbar * e0 - Rbar * repmat(u_ref, Nc, 1) - D * Sbar * u_prev_vec); H (H H) / 2; % 强制对称消除浮点误差导致的求解器报错 endkron(eye(Np), p.Q)这个写法值得记一下它把 3×3 的小权重矩阵扩展成 60×60 的分块对角矩阵等价于对每一步的误差单独加权但避免了写循环。D是一个(2*Nc) × (2*Nc)的下三角差分矩阵作用是把U变成[u1-u0, u2-u1, ...]。最后那行H (H H)/2是血泪经验。理论上H一定对称但浮点运算下Theta*Qbar*Theta算出来的结果可能差个 1e-16某些求解器尤其是 C 接口的 OSQP 严格模式下会因为这个直接返回错误码报“Hessian not symmetric”。这个报错信息很难让人联想到是数值精度问题我第一次遇到查了大半天。权重怎么调给几条经验值。位置误差和航向误差的比一般从 10:1 开始试比如Q diag([10, 10, 1])。想跟得更紧就整体放大Q但放大到一定程度会因为约束不可行而求解失败。R的第二个元素转角通常要比第一个速度大因为速度是规划给的转角才是真正需要平滑的量。S控制抖动画龙工况下优先加S(2,2)从 20 往上加加到不抖为止。4.2 约束控制量、控制增量、道路走廊约束分三类全部要写成lb ≤ A*u ≤ ub的形式才能喂给求解器。function [A, lb, ub] build_constraints(Nc, m, p, road) % 三类约束控制量上下限、控制增量上下限、道路边界软约束 D build_diff_matrix(Nc, m); % 差分矩阵用于增量约束 % 1) 控制量幅值约束v ∈ [0, v_max]delta ∈ [-delta_max, delta_max] A_u eye(Nc * m); u_min repmat([0; -p.delta_max], Nc, 1); u_max repmat([p.v_max; p.delta_max], Nc, 1); % 2) 控制增量约束|u_k - u_{k-1}| ≤ d_max A_du D; du_min repmat([-p.dv_max; -p.d_delta_max], Nc, 1) ... repmat([0; p.u_prev(2)], Nc, 1) .* (1:Nc)*0 ... kron(ones(Nc,1), [0; 0]); % 首拍由 u_prev 决定见下文说明 du_max -du_min; % 3) 道路边界把横向偏移限制在可行驶走廊内用软约束 松弛因子 A_road road.C; % 由参考路径法向量构造 road_lb road.left; road_ub road.right; A [A_u; A_du; A_road]; lb [u_min; du_min; road_lb]; ub [u_max; du_max; road_ub]; end第一类是盒约束最简单很多求解器支持直接传上下界OSQP 的l和uquadprog 的lb/ub比塞进A矩阵效率高。第二类增量约束是抑制抖动的关键。D*u算出来是增量序列限制它在±d_max之内就等价于限制了每一拍的转角变化速率。这里有个细节第一拍的增量是u_0 - u_prevu_prev来自上一拍所以这个约束的右端项每一拍都在变不能预先算死。上面代码里我用了一个kron的写法把u_prev织进去实际实现时更清晰的做法是直接构造扩展决策变量U_tilde [u_prev; U]让差分矩阵作用在上面。第三类道路边界约束要注意A_road不是单位矩阵。横向偏移是相对于参考路径的法向距离需要用车体位置和路径点的法向量算出来通常写成n_x * (x - x_ref) n_y * (y - y_ref)的形式。这也是为什么第 2 章强调参考投影模块要在同一坐标系下——法向量算错走廊就会歪。道路边界一般做成软约束加松弛因子eps ≥ 0目标函数里再加一项rho * eps²。硬约束在窄路或者参考轨迹贴边的情况下极易不可行一不可行求解器就返回失败控制器拿不到解车就失控了。软约束的代价是允许越界但越界量会被罚工程上这是更安全的选择。4.3 求解器封装与异常处理求解器这一层代码量不大但决定了整套系统在异常情况下是“降级运行”还是“直接崩”。function [u_opt, info] solver_wrapper(H, f, A, lb, ub, u_last) % 统一封装超时、不可行、数值异常都要有兜底 opts optimoptions(quadprog, Algorithm, active-set, ... Display, off, MaxIterations, 200); [u_opt, ~, exitflag] quadprog(H, f, A, [], [], [], lb, ub, [], opts); switch exitflag case 1 % 正常收敛 info.status ok; case 0 % 达到最大迭代次数解可能可用 info.status maxiter; case -2 % 不可行 info.status infeasible; u_opt u_last; % 兜底沿用上一拍的控制序列 otherwise % 数值问题等 info.status error; u_opt u_last; end endexitflag的取值必须逐个处理不能只判断 1然后else报错。-2不可行在实车上一定会遇到典型场景是参考轨迹突然跳变或者定位跳点这时候最安全的动作是沿用上一拍的解——车按原方向继续走几十毫秒比输出一个随机解要安全得多。0达到迭代上限一般说明问题规模或者条件数出了问题解通常还能用但要打日志记下来回头查是哪段路况触发的。Python 版本用 OSQP 时要处理另一类问题setup之后可以反复solve中间只更新q、l、u甚至P的非零元素这就是 warm start 的实现方式。一次完整的setup开销大约是单次solve的十倍所以循环里千万不能每次都重新建 problem。注意兜底逻辑一定要在闭环仿真里主动触发验证一遍。把参考轨迹故意做一个 5 m 的跳变看控制器是不是平滑退化成“保持上一拍”而不是发散。没验证过的兜底代码等于没有兜底。5. 避坑与排查轨迹震荡、切弯、求解失败的五个现场检查点MPC 出问题的表现很有迷惑性代码不报错、仿真能跑完、曲线也在动就是跟不准。下面五条是这套代码交付过程中反复遇到的按“现象 → 原因 → 解决”记下来能省掉很多盯着屏幕发呆的时间。5.1 轨迹层面的异常一、高速 S 弯车身画龙越摆越大现象低速直线正常一进 S 弯开始小幅摆动频率大约 12 Hz速度越高越明显。原因控制增量权重S太小或者没做增量约束。每一拍求解器看到的是“当前误差大赶紧多打一点”下一拍误差反向又打回来。这不是求解器的问题是目标函数里缺少对变化率的惩罚。另一种可能是预测时域太短看不完一个完整的弯控制器“短视”。解决先把S(2,2)从 0.5 往上加试到 20、50观察摆动是否收敛。如果加到很大仍然摆检查Np * Ts是否覆盖了车辆的响应时间尺度——乘用车一般需要 0.81.5 s 的预测窗口Np20, Ts0.05正好是 1 s。别一上来就加长时域先调权重成本低得多。二、弯道切内线走不出参考路径的弧度现象直道跟得很准弯道明显往内侧切弯越急切得越多误差图上的峰值集中在弯心附近。原因运动学模型没有侧偏但更常见的原因是参考投影取错了点。如果每拍只找“最近的路径点”在弯道里最近点往往落在车身后方导致参考序列整体后移控制器一直在追一段已经走过的路。另一个原因是航向误差权重给得太小车头方向偏了但横向位置误差不大代价函数觉得“还行”。解决投影改成“在上一拍投影点附近的前向窗口内搜索最近点”窗口长度取 35 m保证参考点始终在车前方。同时把Q(3,3)从 1 提到 5 甚至 10让航向误差的权重大于位置误差。这两步基本能解决 90% 的切弯。三、控制量出现高频毛刺方向盘轻微抖动现象转角曲线放大看是一层细密的锯齿幅值不大但频率很高实车上能感觉到方向盘微振。原因参考轨迹本身不平滑。如果参考点是从离散路点插值来的点间距不均匀投影出来的参考航向角会有阶跃。控制器每一拍都在追一个跳变的参考输出自然是毛刺。也可能是Ts和求解精度不匹配QP 的容差设得太大。解决参考轨迹在投影之后做一次低通或者样条平滑尤其是航向角序列做完要再归一化一次。求解器容差从默认的 1e-8 调到 1e-6 可以省时间但不要再大。这个问题的排查技巧是把参考航向角单独画一条曲线如果它本身就有台阶那就不是控制器的问题。5.2 求解层面的异常四、求解器返回 infeasible控制器输出保持上一拍现象日志里status偶尔出现infeasible集中在急弯或者窄路路段出现频率不高但每次出现都伴随明显的跟踪滞后。原因道路边界用了硬约束参考轨迹贴边或者车已经越界时约束集合直接空了。另一个常见原因是增量约束和幅值约束冲突上一拍转角在 0.5这一拍幅值上限也是 0.5但增量约束只允许变化 0.05如果参考要求立刻往负方向打两组约束就打架。解决道路边界一律改软约束加松弛因子并在目标里罚。增量约束的上下界要留出余量别贴着幅值上限设置。还有一个容易忽略的点u_prev本身要经过限幅再传进去如果上一拍因为数值问题输出了一个 0.9 rad 的转角这一拍的增量约束就会把整个可行域锁死。五、航向角在 ±π 附近跳变控制量瞬间满舵现象车头方向接近正西或者正东时转角突然打满一下又恢复其他方向完全正常。原因psi和psi_ref一个在 3.14 附近一个在 -3.14 附近物理上只差 0.01 rad数值上差 6.28。误差状态没做归一化就直接进预测方程控制器看到的是“航向错了 360 度”。解决所有涉及航向角相减的地方统一走wrapToPi。包括误差状态计算、参考航向角生成、日志记录。这条我建议直接写成一个工具函数ang_diff(a, b)全工程只调这一个别在每个文件里手写a - b。这套代码里我在mpc_core的入口、reference的出口、plot_result的画图前各加了一次三处都不能少。6. 二次开发从运动学仿真走向能上车的动力学 MPC代码跑通只是起点。真要用起来第一件事是换模型。运动学模型在 15 m/s 以上就开始失真高速工况下必须换成二自由度动力学模型状态扩到[x, y, psi, vx, vy, r]控制量还是[delta, a]多出来的轮胎侧偏刚度Cf、Cr需要标定。换模型的时候不用重写整个工程只要保证新模型也提供linearize接口返回同样维度的Ad、Bdbuild_prediction和build_cost一行都不用改。这是分层的价值。第二件事是加速度前馈。光靠误差反馈纵向速度跟踪在坡道和加减速切换时会一直滞后。做法是在代价函数里把u_ref从常数改成随参考轨迹曲率算出来的前馈量v_ref来自规划的速度曲线delta_ref atan(L * kappa)由曲率反推。这样控制器只需要修正残差R权重可以调小跟踪精度和舒适性同时改善。第三件事是终端代价。有限时域的 MPC 在时域末端没有约束容易出现“末端放飞”表现为车在预测窗口末尾附近开始偏离。常见做法是加终端权重P由离散代数 Riccati 方程离线求解得到或者简单地给最后一步加一个更大的Q。验证方法我一般按三步走。先开环给定一组控制序列用模型推演和解析解对比确认Psi、Theta的维度与数值都对。再闭环标准工况双移线、正弦停滞、定半径圆周这三个覆盖了横向瞬态、稳态和持续转向。最后扫参数把Ts从 0.05 扫到 0.1Np从 10 扫到 40看跟踪误差和求解时间的帕累托前沿选一个在实车算力下稳定的点。改造项改动位置验证方式换动力学模型model/dynamic_bicycle.m开环推演对比解析解加速度前馈controller/mpc_core.m的u_ref坡道工况速度误差 0.3 m/s终端代价build_cost里追加P块双移线末端偏移量下降实车部署solver_wrapper换 C 接口单次求解稳定 10 ms最后说一个习惯。从那以后我每次改完权重或者约束都强制走一遍“故意触发兜底”的流程把参考轨迹做一次跳变把转角上限临时改小把u_prev塞一个超限值。三次都过了才敢把代码交出去。MPC 的优雅在于它能显式处理约束但它的脆弱也在这里——约束写错了一个符号表现出来的可能只是“跟踪精度差一点”而不是报错等你发现的时候已经在实车上跑了很久。希望帮到你。本文还有配套的精品资源点击获取
简介这份无人驾驶车辆模型预测控制程序代码面向学习自动驾驶控制算法的高校学生、科研人员与工程开发者聚焦MPC在路径跟踪、速度调节与转向控制中的落地实现可作为控制理论向工程代码过渡的参考样例。ZIP压缩包共16个文件、约38KB以13个.m脚本为主体辅以2个txt说明和1个.mdl仿真模型内容覆盖车辆运动学与动力学建模、预测时域内状态推演、带约束优化问题构造、控制序列滚动更新及仿真测试等环节并含Pacejka轮胎模型与C代码参考。已有1071人学习下载适合用于课程设计、算法复现和论文实验的对照阅读。读者可沿章节组织梳理从车辆模型到控制量更新的完整链路理解预测步长、时间间隔与优化参数对控制效果的影响也能借助脚本和Simulink模型搭建调试环境积累无人驾驶MPC的排错与验证思路。1. 一条 S 弯里画龙的车这套模型预测控制程序代码到底管什么去年帮人调一套轨迹跟踪的代码。双移线工况车速给到 36 km/h车身在第二个弯出口开始左右摆越摆越大。一开始按照直觉把预测时域从 20 步加到 40 步结果更糟求解时间翻了三倍摆动幅度反而变大。后来发现问题不在时域长短而是控制时域和转角增量权重 S 的比例失调求解器每拍都在“过度修正”。这几乎是我见过的无人驾驶车辆模型预测控制代码里最高频的一类翻车。这份无人驾驶车辆模型预测控制程序代码讲的就是把上面这条链路完整跑通运动学自行车模型怎么建、工作点处怎么线性化离散、预测方程怎么堆成 Psi 和 Theta 两个矩阵、代价函数和约束怎么组装成标准二次型、最后怎么丢给 QP 求解器并取第一拍执行。它面向的不是发论文是想把仿真跑起来、再往实车搬的人做毕设的学生、从 PID 转 MPC 的工程同学、需要一份能改参数就能看效果的基线代码的人。2. 代码结构拆解从 model 到 sim 的五层目录各管哪一段拿到一份控制类代码我第一件事不是跑 main而是先看目录怎么切的。目录切得清楚说明作者想过模块边界目录一团乱后面改一个权重可能要动五个文件。这套程序的目录是按“模型—预测—控制—仿真—配置”五层切的边界比较干净。2.1 目录结构与模块职责mpc_av/ ├── config/ │ └── params.m # 全部可调参数集中在这里 ├── model/ │ ├── kinematic_bicycle.m # 运动学自行车模型状态推进一步 │ ├── dynamic_bicycle.m # 二自由度动力学模型高速备用 │ └── linearize.m # 工作点处求雅可比 欧拉离散 ├── predictor/ │ ├── build_prediction.m # 堆 Psi / Theta 预测矩阵 │ └── reference.m # 参考轨迹生成与最近点投影 ├── controller/ │ ├── mpc_core.m # 组装 H、f调求解器 │ ├── constraints.m # 控制量、增量、道路边界约束 │ └── solver_wrapper.m # quadprog / OSQP 的统一封装 ├── sim/ │ ├── closed_loop.m # 闭环主循环 │ └── plot_result.m # 轨迹、误差、控制量三联图 └── README.md目录职责改动频率config采样周期、时域、权重、物理限幅极高调参都在这model状态转移与线性化低除非换模型predictor预测矩阵与参考轨迹中换参考生成方式才动controllerQP 组装与求解中加约束才动sim闭环驱动与可视化低这个划分最大的好处是调参不改代码逻辑。我见过太多把Q diag([10,10,1])直接写死在 mpc_core 里的版本换一个工况要全文搜索替换血泪经验。reference.m这一层经常被新手忽略。它做的不是简单的“给一条 x-y 点列”而是每一拍都要把当前车辆位置投影到参考路径上找到最近的路径点再往前取 Np 个点作为这一拍的参考序列。投影做不准后面权重调得再细也白搭。2.2 技术栈MATLAB 打样、Python 调参、C 上车这套代码给了三套实现不是为了炫技是三种使用场景决定的。MATLAB 版本是打样用的。quadprog开箱即用Active-set算法在几十个变量的小规模 QP 上表现稳定方便你在半小时内看到第一条跟踪曲线。缺点是跑批量仿真慢一次双移线扫 20 组参数要等很久。Python 版本是调参用的。核心是numpy拼矩阵osqp或cvxpy做求解。OSQP 是 ADMM 类的一阶算子分裂求解器最大的价值是支持 warm start——上一拍的解可以直接作为这一拍的初值在连续变化的工况下迭代次数能压到个位数。批量扫参数、画误差统计图、跑蒙特卡洛Python 比 MATLAB 顺手得多。C 版本是往实车搬的。Eigen做矩阵运算OSQP的 C 接口做求解整个控制回路按 20 Hz 跑也就是每拍 50 ms。这个周期下QP 求解本身一般要求压在 510 ms 以内剩下 40 ms 留给感知、定位、通信和调度抖动。如果你的求解器偶尔跑到 30 ms实车上就会看到控制量周期性地“迟一拍”那种抖动很难从曲线上反推原因。采样周期Ts的选择有个经验区间低速园区车 0.1 s 也够乘用车级别的跟踪一般用 0.05 s做高速紧急避障才往 0.02 s 走。周期越小同样的预测时域需要的步数越多QP 规模线性增长求解时间也跟着涨。这是个硬的取舍。2.3 一次控制循环的数据流从参考轨迹到方向盘转角中间走的是一条固定链路理解这条链路比背公式有用得多。% sim/closed_loop.m 主循环骨架 p params(); % 读参数 xi [0; 0; 0]; % 初始状态 [x; y; psi] u_prev [0; 0]; % 上一拍控制量 [v; delta] log struct(xi, [], u, [], t, []); for k 1:p.Nsim % 1) 参考轨迹投影找最近点往前取 Np 个参考点 ref reference(xi, p); % 2) MPC 求解返回最优控制序列只取第一拍 [u_opt, info] mpc_core(xi, ref, u_prev, p); % 3) 只执行第一拍这是滚动时域的核心 u u_opt(:, 1); % 4) 状态推进一步仿真里用模型实车上来自定位 xi kinematic_bicycle(xi, u, p); % 5) 记录供后面画图复盘 log.xi(:, k) xi; log.u(:, k) u; log.t(k) k * p.Ts; u_prev u; end逻辑上有三处值得展开。第一u_opt是一整个 Nc 列的矩阵但只有第一列被执行剩下的全部丢掉下一拍重新算——这就是滚动时域也是 MPC 区别于离线最优控制的地方。第二u_prev被传进下一拍用途有两个一是算控制增量约束Δu u_k - u_{k-1}二是做 warm start。第三仿真里状态来自模型推进实车上这个值来自定位模块所以xi的坐标系必须和参考轨迹严格一致坐标系搞错的表现是车永远往一边偏看起来像稳态误差实际是标定问题。3. 车辆模型与预测方程三个状态、两个控制量怎么推出 Psi 和 ThetaMPC 所有的“预测”能力都来自模型。模型选错后面的权重、约束、求解器调得再精细都是在错误的地基上盖楼。这一章把从模型到预测矩阵的推导和代码完整摊开。3.1 运动学自行车模型三个状态两个控制量低速场景下四轮车可以塌缩成前后两个轮子的自行车再进一步假设后轴中心点跟踪参考点就得到最常用的三状态模型。状态取[x, y, psi]控制取[v, delta]也就是速度和前轮转角。function xi_next kinematic_bicycle(xi, u, p) % 后轴参考点的运动学自行车模型欧拉法前向离散 % xi [x; y; psi] 位置与航向角u [v; delta] 速度与前轮转角 x xi(1); y xi(2); psi xi(3); v u(1); delta u(2); xi_next [ x v * cos(psi) * p.Ts; y v * sin(psi) * p.Ts; psi v * tan(delta) / p.L * p.Ts ]; % 航向角归一化防止长时间仿真后数值一直累加 xi_next(3) wrapToPi(xi_next(3)); end逐项说明。p.Ts是采样周期p.L是轴距紧凑型车取 2.7 m 左右比较合适。第三个式子是整个模型的关键航向角变化率等于v * tan(delta) / L它体现了“速度越快、同样转角下转得越快”这个几何事实。转角用tan而不是直接用delta是因为阿克曼转向几何下后轴中心的转弯半径是L / tan(delta)。wrapToPi这一行看起来可有可无实际是必需品。航向角在连续转弯工况下会一直累加跑几百秒后可能到 30 rad虽然数学上等价但后面算误差psi - psi_ref的时候会直接炸掉——两个物理上相同、数值上差 2π 的角度相减得到一个巨大的误差控制器会瞬间输出满舵。这个坑我在第一次做实车调试时踩过现场表现为车在直线段突然打死方向盘。模型的适用边界要说清楚这个模型假设轮胎没有侧偏也就是低速、小侧向加速度。经验分界线在 15 m/s 上下超过这个速度轮胎侧偏力带来的影响不能忽略必须换动力学模型。很多代码在仿真里跑得很漂亮一上实车高速就发散问题就出在这里。3.2 线性化与离散化雅可比矩阵怎么来的上面那个模型是非线性的cos(psi)和tan(delta)都不是线性项而 QP 求解器只吃线性约束和二次目标。所以要在线性化而且是每个控制周期都在当前工作点重新线性化一次——这叫线性时变 MPCLTV-MPC比在原点做一次全局线性化精度高得多。function [Ad, Bd] linearize(xi, u, p) % 在当前工作点 (xi, u) 处对运动学模型求雅可比并做欧拉离散 psi xi(3); v u(1); delta u(2); % 连续时间雅可比直接乘 Ts 完成前向欧拉离散 Ad [ 1, 0, -v * sin(psi) * p.Ts; 0, 1, v * cos(psi) * p.Ts; 0, 0, 1 ]; Bd [ cos(psi) * p.Ts, 0; sin(psi) * p.Ts, 0; tan(delta) / p.L * p.Ts, v / (p.L * cos(delta)^2) * p.Ts ]; endAd是 3×3 的状态转移矩阵Bd是 3×2 的控制矩阵。看Ad的第三列-v*sin(psi)*Ts和v*cos(psi)*Ts含义是“航向角偏一点位置会往哪个方向偏”偏的程度正比于速度。这就解释了为什么高速时航向误差的权重需要调大——同样的角度误差在高速下对应的横向偏移要大得多。Bd的第二列第二行v / (p.L * cos(delta)^2) * Ts是转角对航向角变化的增益它对delta敏感。当delta接近 ±π/2 时cos(delta)趋于零这一项会炸。所以转角上限一般卡在 0.50.6 rad一方面是机械限位另一方面也是避开这个数值奇点。欧拉离散在这个场景下够用因为Ts只有 0.05 s模型的变化很慢。要更准可以用矩阵指数或者二阶龙格库塔代价是每次线性化多算几次矩阵乘法。实车上我一般还是用欧拉省下来的时间留给求解器。3.3 预测方程堆叠Psi 和 Theta 两个矩阵线性化之后把未来 Np 步的状态用当前状态和控制序列表示出来得到一个形如Y Psi * xi0 Theta * U的矩阵等式。这一步是 MPC 代码里最“线性代数”的部分也是最容易写错维度的地方。function [Psi, Theta] build_prediction(Ad, Bd, Np, Nc) % Ad: n×nBd: n×mNp 预测步数Nc 控制步数Nc Np n size(Ad, 1); m size(Bd, 2); Psi zeros(n * Np, n); Theta zeros(n * Np, m * Nc); for i 1:Np rows (i-1)*n 1 : i*n; Psi(rows, :) Ad^i; % 自由响应初值的作用 for j 1:min(i, Nc) cols (j-1)*m 1 : j*m; % 第 j 拍控制量经过 i-j 步自由演化后到达第 i 拍 Theta(rows, cols) Ad^(i-j) * Bd; end end endPsi的维度是(n*Np) × n把初始状态映射到整段预测状态Theta的维度是(n*Np) × (m*Nc)把控制序列映射到预测状态。内层循环min(i, Nc)是控制时域短于预测时域时的标准处理超过 Nc 步之后控制量保持不变所以那一列不再填新值。举个具体数字n3、m2、Np20、Nc8 时Theta是 60×16 的矩阵。QP 的决策变量只有 16 个Hessian 是 16×16。这个规模对任何求解器都是小菜单次求解基本在 1 ms 量级。如果你的 Np 拉到 60、Nc 拉到 30决策变量变成 60 个Hessian 变成 60×60求解时间会明显上升——不是线性上升因为 QP 的复杂度对变量数是非线性的。Theta里Ad^(i-j)的幂次计算在循环里反复调Ad^是浪费。工程写法是先算一个Ad_pows cell(Np, 1)把Ad^1 ... Ad^Np预先存下来。Np 大的时候这个优化能省掉一半以上的组装时间。4. 代价函数与约束组装把跟踪问题塞进 QP 求解器的完整过程预测方程有了接下来要回答两个问题什么样的控制序列算“好”代价函数以及哪些控制序列根本不允许约束。这两块合起来才是一个标准的二次规划。4.1 目标函数Q、R、S 三个权重在调什么代价函数由三部分组成跟踪误差、控制量幅值、控制增量。用误差状态d_xi xi - xi_ref表达目标是最小化J Σ ||d_xi_k||²_Q Σ ||u_k - u_ref||²_R Σ ||u_k - u_{k-1}||²_SQ惩罚偏离参考轨迹R惩罚控制量本身省油、少打方向S惩罚控制量的变化率平顺性。三个权重不是独立的比例关系才是关键。function [H, f] build_cost(Psi, Theta, d_xi, u_ref, p) % 把 J 展开成 1/2 * U*H*U f*U 的标准形式 Np p.Np; Nc p.Nc; % 分块对角扩展每一步的 Q 拼成 Np 块R 拼成 Nc 块 Qbar kron(eye(Np), p.Q); % 3*Np 维 Rbar kron(eye(Nc), p.R); % 2*Nc 维 % 差分矩阵 D把控制序列 U 映射成增量序列 [u1-u0; u2-u1; ...] D build_diff_matrix(Nc, size(p.R, 1)); Sbar kron(eye(Nc), p.S); % 自由响应控制量为零时预测状态落在哪 e0 Psi * d_xi; u_prev_vec repmat(p.u_prev, Nc, 1); % 二次项系数注意 quadprog 的形式是 0.5*xHx fx H 2 * (Theta * Qbar * Theta Rbar D * Sbar * D); f 2 * (Theta * Qbar * e0 - Rbar * repmat(u_ref, Nc, 1) - D * Sbar * u_prev_vec); H (H H) / 2; % 强制对称消除浮点误差导致的求解器报错 endkron(eye(Np), p.Q)这个写法值得记一下它把 3×3 的小权重矩阵扩展成 60×60 的分块对角矩阵等价于对每一步的误差单独加权但避免了写循环。D是一个(2*Nc) × (2*Nc)的下三角差分矩阵作用是把U变成[u1-u0, u2-u1, ...]。最后那行H (H H)/2是血泪经验。理论上H一定对称但浮点运算下Theta*Qbar*Theta算出来的结果可能差个 1e-16某些求解器尤其是 C 接口的 OSQP 严格模式下会因为这个直接返回错误码报“Hessian not symmetric”。这个报错信息很难让人联想到是数值精度问题我第一次遇到查了大半天。权重怎么调给几条经验值。位置误差和航向误差的比一般从 10:1 开始试比如Q diag([10, 10, 1])。想跟得更紧就整体放大Q但放大到一定程度会因为约束不可行而求解失败。R的第二个元素转角通常要比第一个速度大因为速度是规划给的转角才是真正需要平滑的量。S控制抖动画龙工况下优先加S(2,2)从 20 往上加加到不抖为止。4.2 约束控制量、控制增量、道路走廊约束分三类全部要写成lb ≤ A*u ≤ ub的形式才能喂给求解器。function [A, lb, ub] build_constraints(Nc, m, p, road) % 三类约束控制量上下限、控制增量上下限、道路边界软约束 D build_diff_matrix(Nc, m); % 差分矩阵用于增量约束 % 1) 控制量幅值约束v ∈ [0, v_max]delta ∈ [-delta_max, delta_max] A_u eye(Nc * m); u_min repmat([0; -p.delta_max], Nc, 1); u_max repmat([p.v_max; p.delta_max], Nc, 1); % 2) 控制增量约束|u_k - u_{k-1}| ≤ d_max A_du D; du_min repmat([-p.dv_max; -p.d_delta_max], Nc, 1) ... repmat([0; p.u_prev(2)], Nc, 1) .* (1:Nc)*0 ... kron(ones(Nc,1), [0; 0]); % 首拍由 u_prev 决定见下文说明 du_max -du_min; % 3) 道路边界把横向偏移限制在可行驶走廊内用软约束 松弛因子 A_road road.C; % 由参考路径法向量构造 road_lb road.left; road_ub road.right; A [A_u; A_du; A_road]; lb [u_min; du_min; road_lb]; ub [u_max; du_max; road_ub]; end第一类是盒约束最简单很多求解器支持直接传上下界OSQP 的l和uquadprog 的lb/ub比塞进A矩阵效率高。第二类增量约束是抑制抖动的关键。D*u算出来是增量序列限制它在±d_max之内就等价于限制了每一拍的转角变化速率。这里有个细节第一拍的增量是u_0 - u_prevu_prev来自上一拍所以这个约束的右端项每一拍都在变不能预先算死。上面代码里我用了一个kron的写法把u_prev织进去实际实现时更清晰的做法是直接构造扩展决策变量U_tilde [u_prev; U]让差分矩阵作用在上面。第三类道路边界约束要注意A_road不是单位矩阵。横向偏移是相对于参考路径的法向距离需要用车体位置和路径点的法向量算出来通常写成n_x * (x - x_ref) n_y * (y - y_ref)的形式。这也是为什么第 2 章强调参考投影模块要在同一坐标系下——法向量算错走廊就会歪。道路边界一般做成软约束加松弛因子eps ≥ 0目标函数里再加一项rho * eps²。硬约束在窄路或者参考轨迹贴边的情况下极易不可行一不可行求解器就返回失败控制器拿不到解车就失控了。软约束的代价是允许越界但越界量会被罚工程上这是更安全的选择。4.3 求解器封装与异常处理求解器这一层代码量不大但决定了整套系统在异常情况下是“降级运行”还是“直接崩”。function [u_opt, info] solver_wrapper(H, f, A, lb, ub, u_last) % 统一封装超时、不可行、数值异常都要有兜底 opts optimoptions(quadprog, Algorithm, active-set, ... Display, off, MaxIterations, 200); [u_opt, ~, exitflag] quadprog(H, f, A, [], [], [], lb, ub, [], opts); switch exitflag case 1 % 正常收敛 info.status ok; case 0 % 达到最大迭代次数解可能可用 info.status maxiter; case -2 % 不可行 info.status infeasible; u_opt u_last; % 兜底沿用上一拍的控制序列 otherwise % 数值问题等 info.status error; u_opt u_last; end endexitflag的取值必须逐个处理不能只判断 1然后else报错。-2不可行在实车上一定会遇到典型场景是参考轨迹突然跳变或者定位跳点这时候最安全的动作是沿用上一拍的解——车按原方向继续走几十毫秒比输出一个随机解要安全得多。0达到迭代上限一般说明问题规模或者条件数出了问题解通常还能用但要打日志记下来回头查是哪段路况触发的。Python 版本用 OSQP 时要处理另一类问题setup之后可以反复solve中间只更新q、l、u甚至P的非零元素这就是 warm start 的实现方式。一次完整的setup开销大约是单次solve的十倍所以循环里千万不能每次都重新建 problem。注意兜底逻辑一定要在闭环仿真里主动触发验证一遍。把参考轨迹故意做一个 5 m 的跳变看控制器是不是平滑退化成“保持上一拍”而不是发散。没验证过的兜底代码等于没有兜底。5. 避坑与排查轨迹震荡、切弯、求解失败的五个现场检查点MPC 出问题的表现很有迷惑性代码不报错、仿真能跑完、曲线也在动就是跟不准。下面五条是这套代码交付过程中反复遇到的按“现象 → 原因 → 解决”记下来能省掉很多盯着屏幕发呆的时间。5.1 轨迹层面的异常一、高速 S 弯车身画龙越摆越大现象低速直线正常一进 S 弯开始小幅摆动频率大约 12 Hz速度越高越明显。原因控制增量权重S太小或者没做增量约束。每一拍求解器看到的是“当前误差大赶紧多打一点”下一拍误差反向又打回来。这不是求解器的问题是目标函数里缺少对变化率的惩罚。另一种可能是预测时域太短看不完一个完整的弯控制器“短视”。解决先把S(2,2)从 0.5 往上加试到 20、50观察摆动是否收敛。如果加到很大仍然摆检查Np * Ts是否覆盖了车辆的响应时间尺度——乘用车一般需要 0.81.5 s 的预测窗口Np20, Ts0.05正好是 1 s。别一上来就加长时域先调权重成本低得多。二、弯道切内线走不出参考路径的弧度现象直道跟得很准弯道明显往内侧切弯越急切得越多误差图上的峰值集中在弯心附近。原因运动学模型没有侧偏但更常见的原因是参考投影取错了点。如果每拍只找“最近的路径点”在弯道里最近点往往落在车身后方导致参考序列整体后移控制器一直在追一段已经走过的路。另一个原因是航向误差权重给得太小车头方向偏了但横向位置误差不大代价函数觉得“还行”。解决投影改成“在上一拍投影点附近的前向窗口内搜索最近点”窗口长度取 35 m保证参考点始终在车前方。同时把Q(3,3)从 1 提到 5 甚至 10让航向误差的权重大于位置误差。这两步基本能解决 90% 的切弯。三、控制量出现高频毛刺方向盘轻微抖动现象转角曲线放大看是一层细密的锯齿幅值不大但频率很高实车上能感觉到方向盘微振。原因参考轨迹本身不平滑。如果参考点是从离散路点插值来的点间距不均匀投影出来的参考航向角会有阶跃。控制器每一拍都在追一个跳变的参考输出自然是毛刺。也可能是Ts和求解精度不匹配QP 的容差设得太大。解决参考轨迹在投影之后做一次低通或者样条平滑尤其是航向角序列做完要再归一化一次。求解器容差从默认的 1e-8 调到 1e-6 可以省时间但不要再大。这个问题的排查技巧是把参考航向角单独画一条曲线如果它本身就有台阶那就不是控制器的问题。5.2 求解层面的异常四、求解器返回 infeasible控制器输出保持上一拍现象日志里status偶尔出现infeasible集中在急弯或者窄路路段出现频率不高但每次出现都伴随明显的跟踪滞后。原因道路边界用了硬约束参考轨迹贴边或者车已经越界时约束集合直接空了。另一个常见原因是增量约束和幅值约束冲突上一拍转角在 0.5这一拍幅值上限也是 0.5但增量约束只允许变化 0.05如果参考要求立刻往负方向打两组约束就打架。解决道路边界一律改软约束加松弛因子并在目标里罚。增量约束的上下界要留出余量别贴着幅值上限设置。还有一个容易忽略的点u_prev本身要经过限幅再传进去如果上一拍因为数值问题输出了一个 0.9 rad 的转角这一拍的增量约束就会把整个可行域锁死。五、航向角在 ±π 附近跳变控制量瞬间满舵现象车头方向接近正西或者正东时转角突然打满一下又恢复其他方向完全正常。原因psi和psi_ref一个在 3.14 附近一个在 -3.14 附近物理上只差 0.01 rad数值上差 6.28。误差状态没做归一化就直接进预测方程控制器看到的是“航向错了 360 度”。解决所有涉及航向角相减的地方统一走wrapToPi。包括误差状态计算、参考航向角生成、日志记录。这条我建议直接写成一个工具函数ang_diff(a, b)全工程只调这一个别在每个文件里手写a - b。这套代码里我在mpc_core的入口、reference的出口、plot_result的画图前各加了一次三处都不能少。6. 二次开发从运动学仿真走向能上车的动力学 MPC代码跑通只是起点。真要用起来第一件事是换模型。运动学模型在 15 m/s 以上就开始失真高速工况下必须换成二自由度动力学模型状态扩到[x, y, psi, vx, vy, r]控制量还是[delta, a]多出来的轮胎侧偏刚度Cf、Cr需要标定。换模型的时候不用重写整个工程只要保证新模型也提供linearize接口返回同样维度的Ad、Bdbuild_prediction和build_cost一行都不用改。这是分层的价值。第二件事是加速度前馈。光靠误差反馈纵向速度跟踪在坡道和加减速切换时会一直滞后。做法是在代价函数里把u_ref从常数改成随参考轨迹曲率算出来的前馈量v_ref来自规划的速度曲线delta_ref atan(L * kappa)由曲率反推。这样控制器只需要修正残差R权重可以调小跟踪精度和舒适性同时改善。第三件事是终端代价。有限时域的 MPC 在时域末端没有约束容易出现“末端放飞”表现为车在预测窗口末尾附近开始偏离。常见做法是加终端权重P由离散代数 Riccati 方程离线求解得到或者简单地给最后一步加一个更大的Q。验证方法我一般按三步走。先开环给定一组控制序列用模型推演和解析解对比确认Psi、Theta的维度与数值都对。再闭环标准工况双移线、正弦停滞、定半径圆周这三个覆盖了横向瞬态、稳态和持续转向。最后扫参数把Ts从 0.05 扫到 0.1Np从 10 扫到 40看跟踪误差和求解时间的帕累托前沿选一个在实车算力下稳定的点。改造项改动位置验证方式换动力学模型model/dynamic_bicycle.m开环推演对比解析解加速度前馈controller/mpc_core.m的u_ref坡道工况速度误差 0.3 m/s终端代价build_cost里追加P块双移线末端偏移量下降实车部署solver_wrapper换 C 接口单次求解稳定 10 ms最后说一个习惯。从那以后我每次改完权重或者约束都强制走一遍“故意触发兜底”的流程把参考轨迹做一次跳变把转角上限临时改小把u_prev塞一个超限值。三次都过了才敢把代码交出去。MPC 的优雅在于它能显式处理约束但它的脆弱也在这里——约束写错了一个符号表现出来的可能只是“跟踪精度差一点”而不是报错等你发现的时候已经在实车上跑了很久。希望帮到你。本文还有配套的精品资源点击获取
