前几天我把手头这套无人驾驶车辆轨迹跟踪与模型预测控制第二版配套程序完整整理了一遍。整理之前我以为只是把文件分类，真正动手才发现，这个“配套程序”牵扯的东西远比想象中多：车辆模型用运动学还是动力学、预测时域取多少步、约束怎么设、求解器怎么接、仿真和实车参数怎么对齐，每一个决定都会影响最终的跟踪效果。
这篇文章不是官方教程，只是我个人在整理、跑通、调参过程中的一套经验总结。我默认你对控制理论有一点概念，至少知道状态方程和目标函数的基本含义，但如果你完全没接触过模型预测控制（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(k+1) = x(k) + v(k)\cos(\psi(k)) dt ] [ y(k+1) = y(k) + v(k)\sin(\psi(k)) dt ] [ \psi(k+1) = \psi(k) + \frac{v(k)\tan(\delta(k))}{L} dt ] [ v(k+1) = v(k) + a(k) dt ]
其中 (L) 是轴距。这个模型简单、参数少，在低速场景下精度足够，是配套程序的首选。但要注意它的使用前提是轮胎侧偏可以忽略，也就是说车速不能太高。如果你要跑 60 km/h 以上的场景，运动学模型就不太够了，得切到动力学模型，引入侧偏刚度、横摆转动惯量等参数，状态量里通常还要加入横摆角速度 (r) 和侧向速度。
整理时我强烈建议你保留两套模型接口：一套运动学，一套动力学。它们的输入输出格式一致，只是内部计算不同。这样你可以先在低速下用运动学模型跑通整个 MPC，再切换动力学模型对比效果，不需要改控制器代码。
2.2 预测时域和控制时域怎么定
很多新手直接把参考代码里的 (N_p) 和 (N_c) 抄过来，结果换了个场景就出问题。这两个参数不是随便填的，它们背后代表的是“你要看多远、你要决定多自由”。
预测时域 (N_p) 决定 MPC 每次计算时向前看多少步。假设仿真步长 (dt=0.1s)，(N_p=30) 意味着当前控制器能看到未来 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，而在“你根本不知道这套参数什么时候会失效”。
如果你也要整理一套类似的配套程序，我的建议是每一次改参数都做记录，哪怕只是微调一个权重，也要在结果曲线上留下对比数据。第一次这样做你会觉得麻烦，但当你遇到“上一版还好好的，今天怎么就不对”的时候，这些记录会直接告诉你答案。
