前几天我把手头这套无人驾驶车辆轨迹跟踪与模型预测控制第二版配套程序完整整理了一遍。整理之前我以为只是把文件分类真正动手才发现这个“配套程序”牵扯的东西远比想象中多车辆模型用运动学还是动力学、预测时域取多少步、约束怎么设、求解器怎么接、仿真和实车参数怎么对齐每一个决定都会影响最终的跟踪效果。这篇文章不是官方教程只是我个人在整理、跑通、调参过程中的一套经验总结。我默认你对控制理论有一点概念至少知道状态方程和目标函数的基本含义但如果你完全没接触过模型预测控制MPC也没关系文中遇到专业名词我会用大白话解释。文章会覆盖程序整理的思路、MPC实现的核心细节、完整跑通流程、常见问题排查最后聊聊怎么把它改造成你自己的控制器。适合正在啃这本书、抄过参考代码但没跑通或者在无人驾驶路径跟踪方向需要快速入门的同学。1. 整理程序之前先想清楚这三件事1.1 配套程序不整理就是一堆碎片我先说一个很现实的情况这类书里的配套程序通常按章节组织每个示例文件独立可跑但彼此之间的变量命名、坐标系定义、单位制都不一样。有的文件用全局变量传参有的文件把参数写死在函数内部有的模型用国际单位有的用厘米和度。你直接把某一个示例拿过来换个地图、换个车辆参数大概率跑不出理想结果甚至直接报错。所以整理的第一步不是改代码而是搞清楚这些碎片之间的依赖关系。我当时先做了一次“静态扫描”把每个源码文件里出现的函数名、类名、依赖文件列出来画一张简单的调用关系图。你不需要用工具Excel 就能干这件事关键是给自己建立一张索引表这个文件是什么模块、被谁调用、输入输出是什么。有了这张表后面重构代码才不会改坏原逻辑。第二步是统一接口。我见过最典型的混乱是明明同一个仿真步长有的文件写dt 0.1有的写Ts 0.05还有的写step 1。整理时我把所有时间参数统一成dt所有车辆参数统一放在一个配置字典里所有参考轨迹统一输出成(x, y, yaw, v)四元组。这样改完后不同章节的示例之间可以直接互相调用不用再每次去猜单位。最后一步是给代码补注释。不是说每行都要写而是每个函数顶上写清楚“输入是什么、输出是什么、依赖什么”。这一条看起来简单但真做过的都知道原版程序里很多注释写的是“此处为控制器”几乎没有信息量。我补注释时特别标注了每个变量在控制中的物理意义比如e_y是横向误差、e_yaw是航向误差这比写在正文里有用得多。1.2 第二版相对第一版变化点在哪里如果你手头同时有第一版和第二版整理时会明显感觉到第二版的程序更“工程化”。我这里说的是普遍情况不针对特定作者或出版社。第一版程序往往是单文件演示把车辆模型、MPC、可视化全塞在一个脚本里跑起来方便但想改模型就得整个文件动刀。第二版的配套程序更像一套小型软件模型、控制器、仿真器、可视化开始分层这给整理工作省了不少力气。另一个典型变化是约束处理更完整。第一版的示例里很多时候只做了控制量限幅比如前轮转角直接砍到最大角度这种做法实现简单但会给求解器带来“突变输入”表现在曲线上就是转角突然跳一下车辆轨迹出现抖动。第二版程序普遍把约束写进优化问题里让求解器在约束范围内寻找最优解而不是解完再限幅效果平滑很多。还有一个变化是参数配置从“散落”走向“集中”。第一版里改一个预测时域可能要翻三个文件第二版通常提供统一的配置文件把 (N_p)、(N_c)、权重矩阵、约束上下限都集中在同一个地方。整理时我建议你把配置文件作为唯一修改入口代码里不允许出现散落的魔法数字。跑实验时只需要改配置文件不需要动核心代码这个习惯会给你后续调参省下大量时间。1.3 目录结构是第一份“代码文档”整理这类程序我最推荐的分层结构是这样的tracking_mpc/ ├── config/ # 全局参数、车辆参数、权重配置 │ └── vehicle_params.yaml ├── src/ │ ├── models/ # 车辆运动学/动力学模型 │ ├── controller/ # MPC主控制器、QP求解 │ ├── planning/ # 参考轨迹生成、路径点插值 │ ├── simulation/ # 仿真器、状态更新 │ └── utils/ # 可视化、日志、数据记录 ├── examples/ # 可直接运行的示例脚本 │ ├── run_tracking.py │ └── run_compare.py ├── tests/ # 单元测试、模型一致性校验 ├── results/ # 仿真输出、曲线图 └── docs/ # 整理笔记、公式推导为什么要把models和simulation分开因为模型是“你希望车辆怎么动”仿真器是“车辆实际怎么动”。MPC 内部用预测模型算下一步状态仿真器用真实模型或更精细的模型计算反馈给控制器的状态。如果不分开你无法验证“模型失配”对控制的影响而这种失配在实车上是躲不掉的。planning模块单独拎出来也很重要。配套程序里的参考轨迹多为生成式曲线比如圆形、双移线、直线加圆弧但实际应用时参考轨迹来自全局路径规划数据结构可能是离散点列。我把参考点插值、坐标转换、轨迹重采样都放进这个模块目的就是让控制器只面对统一的轨迹接口不管轨迹来源是哪里。2. 轨迹跟踪与MPC核心细节逐个拆2.1 状态量和控制量怎么选这部分是整套程序的地基。大多数配套程序选择的是自行车运动学模型车辆用一个质点加一个航向角表示。状态量取 ([x, y, \psi, v])分别代表横坐标、纵坐标、航向角、纵向速度控制量取 ([ \delta, a ])分别是前轮转角和纵向加速度。离散后的运动学模型常见写法如下[ x(k1) x(k) v(k)\cos(\psi(k)) dt ] [ y(k1) y(k) v(k)\sin(\psi(k)) dt ] [ \psi(k1) \psi(k) \frac{v(k)\tan(\delta(k))}{L} dt ] [ v(k1) v(k) a(k) dt ]其中 (L) 是轴距。这个模型简单、参数少在低速场景下精度足够是配套程序的首选。但要注意它的使用前提是轮胎侧偏可以忽略也就是说车速不能太高。如果你要跑 60 km/h 以上的场景运动学模型就不太够了得切到动力学模型引入侧偏刚度、横摆转动惯量等参数状态量里通常还要加入横摆角速度 (r) 和侧向速度。整理时我强烈建议你保留两套模型接口一套运动学一套动力学。它们的输入输出格式一致只是内部计算不同。这样你可以先在低速下用运动学模型跑通整个 MPC再切换动力学模型对比效果不需要改控制器代码。2.2 预测时域和控制时域怎么定很多新手直接把参考代码里的 (N_p) 和 (N_c) 抄过来结果换了个场景就出问题。这两个参数不是随便填的它们背后代表的是“你要看多远、你要决定多自由”。预测时域 (N_p) 决定 MPC 每次计算时向前看多少步。假设仿真步长 (dt0.1s)(N_p30) 意味着当前控制器能看到未来 3 秒的轨迹。预测时域太短控制器“近视”看不到前方弯道等弯道到了跟前才开始转横向误差必然大预测时域太长远处的参考轨迹对当前决策没有太多意义反而增加优化问题规模计算变慢。我的经验是预测时域对应的物理时间 (N_p \cdot dt) 至少需要覆盖车辆完成一个“机动”的时间。低速园区工况车辆 2 秒内能改变明显朝向所以 (N_p) 取 20 到 30 就够高速匝道场景车辆需要 3 到 5 秒完成变道这时候 (N_p) 可能要取到 50 步甚至更多。控制时域 (N_c) 代表“未来有多少步控制量可以由你自由决定”。通常取 (N_c \le N_p)。(N_c) 越小求解越快但控制越保守(N_c) 越大控制越激进求解越慢。配套程序里常见默认值是 (N_c 8) 到 (10)在低速跟踪场景下表现已经不错。我的建议是先固定 (N_p)从 (N_c 5) 开始往上加观察跟踪误差和求解时间的变化找到一个拐点再加控制量提升就不明显了那就说明这个值够了。2.3 约束处理硬约束与软约束约束是 MPC 区别于普通 LQR 的核心也是配套程序里最容易出问题的地方。约束一般分几类约束类型典型例子作用控制量幅值约束前轮转角 (\delta \in [-30°, 30°])防止给转向执行器发超限指令控制增量约束转角变化率 (\Delta\delta \in [-5°, 5°])限制转向速度保护执行器状态量约束横向偏差 (e_y \in [-0.5m, 0.5m])保证车辆不偏离车道太远速度/加速度约束(a \in [-3, 3] m/s^2)保证乘坐舒适性和制动能力硬约束的意思是“绝对不能违反”但实际求解时如果参考轨迹突然出现一个极窄的弯道硬约束可能导致优化问题无解也就是常说的infeasible。这是 MPC 工程落地中的一个常见坑配套程序里如果处理得不好你会看到求解器直接退出控制量停在上一帧车辆瞬间失控。我的做法是引入软约束把状态量约束从“必须满足”改成“尽量满足”用松弛变量 (s) 来刻画偏离程度目标函数里额外加上惩罚项比如 (w_s \cdot s^2)。这样约束被突破时不会直接无解而是让车辆以最小代价偏离约束优先保证求解连续性。整理程序时我会在配置文件中专门设一个开关use_soft_constraint: true方便随时切换硬软约束对比效果。还有一个经验约束数值不要卡在物理极限上。前轮转角真实极限是 30 度不要直接约束在 ±30 度建议留出 3% 到 5% 的余量比如 ±28.5 度。因为执行器响应有延迟控制器算出 30 度指令等执行器到位时可能已经晚了指令反而被限幅影响效果。2.4 求解器选型与接口封装MPC 最终要解一个带约束的二次规划问题QP形式可以写成[ \min \frac{1}{2} \mathbf{x}^T H \mathbf{x} f^T \mathbf{x} ] [ s.t. \quad Ax \le b ]其中 (\mathbf{x}) 是包含预测状态和控制增量的决策变量向量。整理配套程序时我见过有人在代码里手动写求解逻辑也有直接用现成求解器的。我的建议是直接用开源 QP 求解器比如 OSQP、qpOASES或者更上层的优化框架 CasADi、CVXPY。它们本身是工具库不涉及商业授权问题文档也完善适合做二次开发。不过求解器选型有一个容易忽略的细节H 矩阵和 f 向量的构造。H 由误差权重和控制权重组合而成f 里带有参考轨迹的交叉项这一块构造错了求解器不会报错但跟踪效果就是差。整理程序时我会单独写一个build_qp.py模块专门负责从预测模型、参考轨迹、权重矩阵生成 H、f、A、b这样控制器主体只负责调用求解器数学细节全部隔离在独立模块里排查问题也方便。3. 实操过程从源码到第一条跟踪轨迹3.1 环境准备我用的基础环境是 Python理由是配套程序里很多示例跑在 Python 下且数据处理和可视化方便。如果你的环境是 C思路一样只是语法不同。先装齐依赖pip install numpy scipy matplotlib pyyaml # 如果使用 CVXPY 作为 QP 层 pip install cvxpy osqp装完后建立一个项目根目录把配置文件、源码、示例分开。我第一次跑这套程序时直接把所有文件平铺在一个文件夹里结果修改完一个参数忘了备份后面想对比都非常痛苦。现在我会在config下维护多份参数文件比如vehicle_fast.yaml、vehicle_slow.yaml不同工况直接切换配置文件。这里要特别提醒一个版本兼容问题较新版本的 numpy 对一些旧代码里的线性代数接口有变动比如np.matrix相关用法。配套程序如果是早期写好的直接运行可能报AttributeError。处理办法是把所有np.matrix改成np.array或者在项目里固定依赖版本用虚拟环境隔离。我在整理时选择了前者一次性把源码里的矩阵运算统一成numpy.ndarray后续运行再没出过这类问题。3.2 主循环的核心逻辑跑通示例的第一步不是运行而是弄懂主循环在干什么。我把它拆成七个步骤读取参考轨迹重采样成固定步长离散点。获取当前车辆状态计算相对参考点的横向误差和航向误差。在参考点附近对预测模型做线性化得到线性时变模型。构造 QP 问题的 H、f、A、b。调用求解器求解控制增量序列。取序列第一个控制量应用到车辆模型。记录状态更新可视化进入下一循环。下面这段是主循环的简化伪代码对应“配置、模型、控制、仿真”四层from controller.mpc import MPCController from simulation.vehicle import VehicleSimulator # 初始化控制器和车辆 mpc MPCController(cfg) sim VehicleSimulator(cfg) while not sim.is_finished(): # 1. 当前状态 x_cur sim.get_state() # 2. 计算参考点索引 ref_idx find_nearest_point(ref_traj, x_cur) # 3. 模型线性化 构建QP mpc.set_reference(ref_traj[ref_idx:ref_idx cfg.Np]) # 4. 求解 action mpc.solve(x_cur) # 5. 将第一个控制量应用到车辆 sim.step(action)配套程序里你可能会看到一些判断逻辑比如参考点索引不是只取最近点而是要带上“前瞻距离”也就是在最近点前方再偏一点。这是因为纯取最近点会导致车辆总在追赶目标点控制滞后实际表现就是过弯时车辆总贴着弯道内侧走。前瞻距离可以设置为当前车速乘以一个常数比如 (L_{lookahead} v \cdot 1.2s)。跑通示例后第一步先看车辆能不能走出一个圆滑轨迹不要急着调权重。如果车辆根本没动优先检查参考点索引是否在每次循环里正常前移很多时候问题出在“车辆原地绕圈”而不是控制算法。3.3 参数调优手记调参是这套程序里最花时间的一步。先说一个典型初始参数表后续在这个基础上微调参数初始取值说明仿真步长 (dt)0.1 s控制周期也是离散间隔预测时域 (N_p)30约为 3 秒水平控制时域 (N_c)10自由控制量个数横向误差权重 (q_y)10越大越贴近轨迹航向误差权重 (q_\psi)5影响朝向对齐速度控制增量权重 (r_{\Delta\delta})0.1越大转向越平滑加速度权重 (r_a)0.5限制纵向加减速调参顺序也有讲究。我的经验是先调 Q误差权重再调 R控制权重最后动约束。为什么因为误差权重直接决定跟踪精度是最直观的指标控制权重影响的是控制的平滑性和能耗两者需要权衡约束则是保底项约束太松可能导致控制量频繁触边太紧又容易无解。实际操作中你会发现一个反直觉现象横向误差权重不是越大越好。(q_y) 加得太大控制器为了贴线会把转角打得很大反而引起震荡。这时候需要同步加大控制增量权重 (r_{\Delta\delta})让转向动作更温和。简单说误差权重和控制权重是一对“油门和刹车”只踩油门不踩刹车速度上去了但一定会失控。4. 常见问题与排查实录4.1 求解失败infeasible 出现时从哪里入手求解失败的直接表现是控制量输出为 NaN 或者控制器直接抛出无解异常。很多人第一反应是求解器坏了实际大概率是约束和参考轨迹不匹配。我举一个真实遇到过的例子某条参考轨迹包含一个半径很小的回头弯而我设的前轮转角约束是 ±25 度车辆在当前速度下根本转不过这个弯于是硬约束下优化问题无解。排查思路分几步。先检查参考轨迹的曲率可以用三个相邻点计算曲率半径对比当前速度下车辆的转向能力判断是不是轨迹本身太极限。再把硬约束改成软约束暂时跳过无解问题观察控制量是否在边界处饱和。最后测减小当前速度降低转向需求。很多时候不是代码问题而是工况超出车辆能力MPC 只是诚实地告诉你“这里转不过去”。提示遇到 infeasible先别改代码先用示意图把参考轨迹和车辆转向半径画出来一眼就能看出是不是轨迹曲率超过了车辆极限。4.2 轨迹震荡权重匹配和模型失配的博弈轨迹震荡表现为车辆沿着参考轨迹左右摆动幅度时大时小。这种情况常见于横向误差权重过大、控制增量权重过小或者参考点索引每次都跳变导致误差计算不稳定。我先讲权重匹配的调整逻辑。震荡意味着控制器对误差反应过度有点像开车时看到路线偏了立刻猛打方向盘打完之后又发现打多了再反打回来。解决思路是把控制增量的权重 (r_{\Delta\delta}) 调大限制每一次决策里的转角变化幅度同时可以适当降低 (q_y)让控制器不那么敏感。如果调权重之后震荡依旧就要怀疑预测模型和仿真模型是不是一致。配套程序里预测模型通常用运动学模型仿真模型如果用了更精细的模型两者在高速下差异会放大。验证方法是把预测模型和仿真模型完全一致化跑一次如果震荡消失说明问题出在模型失配而不是权重。这时候要么换更准的预测模型要么减少仿真速度降低失配的影响。4.3 过弯切弯严重误差定义有问题切弯是指车辆在入弯时没有及时朝弯道内侧走而是切出一个比参考轨迹更直的路径。从数据上看横向误差不大但轨迹形态明显和参考不一样。很多人会把原因归结为预测时域不够长这确实是一个可能但我遇到更多的问题在于误差定义。MPC 的目标函数里如果只使用全局坐标偏差也就是用车辆坐标直接减去参考坐标那么车辆在弯道上会同时出现纵向和横向的偏差控制器无法区分“我落后了”和“我偏了”于是它选择同时修正结果就是在弯道里切弯。正确做法是沿参考轨迹建立局部坐标系把误差拆成沿参考方向的分量纵向误差和垂直于参考方向的分量横向误差目标函数里主要惩罚横向误差。整理程序时我会单独写一个函数完成坐标转换并且在输出里分别记录两种误差方便对比。看到切弯问题时先确认误差定义是否用了局部坐标这一步经常被忽略。4.4 运行速度慢矩阵构造和热启动配套程序跑起来之后你可能会发现仿真几秒钟的数据要计算很久。MPC 的求解时间主要由两个因素决定优化问题的规模以及求解器是否有良好的初值。规模上(N_p) 和 (N_c) 越大决策变量越多求解越慢。如果只是低速跟踪没必要把 (N_p) 拉满到 50 步30 步通常足够。矩阵构造方面Python 循环逐元素填充 H 矩阵非常慢应该用向量化运算一次生成块矩阵。我整理时会把 H、f 的计算改成 numpy 批量构造运行速度提升明显。求解器还有一个省时间的窍门叫热启动意思是把上一帧的最优解作为当前帧的初始值。仿真场景里相邻两个控制周期车辆状态差别不大解也不会突变热启动能让求解器在几步内收敛。OSQP 这类求解器天然支持这个能力只要在接口里传入上一次的解就行。下面我把这几类问题汇总成速查表实际调试时可以对照使用现象优先排查点处理方式infeasible轨迹曲率 vs 转向能力减小速度、放宽状态约束、改软约束轨迹震荡权重匹配/模型失配增大 (r_{\Delta\delta})、检查预测模型一致性过弯切弯误差坐标系定义改用局部坐标化误差惩罚横向偏差求解速度慢QP规模/初值降低 (N_p)、向量化矩阵构造、热启动5. 从配套程序到自己的控制器5.1 三个值得扩展的方向配套程序跑通之后不要急着关掉页面我建议从三个方向往下扩展每个方向都能让你对 MPC 的理解加深不少。第一个方向是加障碍物约束。把静态障碍物抽象成车辆位置必须避开的多边形或圆都能用“避距”刻画。如果想引入动态障碍物就需要把障碍物的未来轨迹也加入预测变成用一辆时间和空间同时耦合的控制器。这个方向对应无人驾驶里最常用的场景之一局部避障。第二个方向是换车辆模型。运动学模型跑通后把模块换成带侧偏刚度、转动惯量的动力学模型。对比两种模型在同一参考轨迹、同一速度下的跟踪差异你能直观感受到高速下运动学模型为什么“不够用”这是理论书里读不到的经验。第三个方向是往硬件上搬。很多配套程序只输出仿真曲线而实车或小车平台还需要考虑执行器延迟、传感器延迟、坐标对齐。可以先在仿真里加入固定延迟比如控制量输出后延迟 0.1 秒再作用到车辆模型观察控制效果劣化再考虑在预测模型里做延迟补偿。每个方向的工作量不算小但核心框架已经分层了改模型不会碰到控制器改控制约束不会碰到仿真器这也是前面花时间整理目录结构的回报。5.2 整理完之后的一点体会整理完这套程序我最大的感受是MPC 最后比拼的往往不是数学有多漂亮而是参数、约束和工况有没有和你的车匹配。同样的控制器换一个轴距、换一个轮胎特性、换一个参考轨迹速度效果可能天差地别。很多同学拿着标准模型跑了半天发现效果不好但其实问题不在 MPC而在“你根本不知道这套参数什么时候会失效”。如果你也要整理一套类似的配套程序我的建议是每一次改参数都做记录哪怕只是微调一个权重也要在结果曲线上留下对比数据。第一次这样做你会觉得麻烦但当你遇到“上一版还好好的今天怎么就不对”的时候这些记录会直接告诉你答案。