简介：《决策规划控制讲义》是一份面向自动驾驶入门与进阶学习者的课程配套资料，聚焦决策规划与控制算法这一核心模块，帮助读者建立从环境感知到车辆执行的整体认知。内容围绕路径规划、行为选择与轨迹生成展开，并延伸到PID、LQR、模型预测控制等控制方法，同时涉及卡尔曼滤波、粒子滤波在多传感器融合中的应用，以及故障检测、应急策略和实时性等工程考量。压缩包内为1个PDF文件，共69.2MB，以扫描讲义形式呈现，便于按章节阅读和批注。已有973人学习，说明该资料在相关课程学习者中具有一定参考价值。讲义还结合老王的手写笔记与实践案例，适合对照课堂内容梳理算法脉络、补齐关键概念，并为后续实验与项目实践提供思路。
1. 决策规划控制讲义翻完三遍，为什么还是不会调那一个参数
一份两百多页的决策规划控制讲义，最常见的结局是：目录漂亮、公式齐全，翻开第一天就停在第七页；真到要写横向控制、要定采样密度、要解释车在弯里画龙的时候，翻回去依然找不到那一页。问题不出在讲义写得差，而在于讲义本身是「结论的集合」——它告诉你 Frenet 坐标系能把二维问题拆成两个一维问题，却不会告诉你参考线曲率超过多少之后这种拆法开始失真。这篇笔记把一份决策规划控制讲义按「骨架、算法链、控制层、踩坑、复现路线」重新排一遍：新手能照着搭出第一条跑得起来的链路，熟手能看到每个参数背后的边界在哪。适合正在做低速园区车、记忆泊车、AGV 或移动机器人导航，手里攥着讲义却落不了地的人。
2. 讲义的骨架：决策、规划、控制三层怎么切、接口怎么定
2.1 三层的输出物不同，别用同一套指标考核
决策层回答的是「走哪条路、让不让、超不超」，输出必须是离散量：目标车道编号、行为原语（跟车、换道、借道绕行、路口停车让行）、期望速度区间。规划层回答「沿着这个意图，走一条什么形状的曲线」，输出是带时间戳的轨迹点序列，常见时域 3～8 秒、点间隔 0.1 秒。控制层回答「这一拍油门给多少、方向盘打多少」，输出加速度和曲率（或直接是前轮转角）。
三层混在一起是讲义里最普遍的毛病。判断某个算法归哪一层，我一般只看一件事：优化变量里有没有控制量 u。只优化轨迹形状、把动力学当约束校核的，归规划（比如带曲率约束的二次规划、采样加打分）；把 u 直接作为决策变量、把状态 x 当约束滚着走的，归控制（MPC 就卡在这条线上，讲义必须明确表态，否则学生会把同一个 MPC 在两个章节里学两遍且互相矛盾）。
接口契约比算法更值得在讲义里单开一节。规划交给控制的东西，缺一项就等着翻车：
| 要素 | 交付要求 | 不满足的后果 |
|---|---|---|
| 时间戳 | 与控制器时钟同源，抖动小于 10 ms | 控制按错拍跟踪，高速时等效于加了纯延迟 |
| 速度剖面 | 每点带 v 与 a，首点与当前车速差小于 0.5 m/s | 起步一顿、跟车点头 |
| 曲率序列 | 相邻点曲率变化率限幅，不允许阶跃 | 方向盘高频抖动，执行器过热 |
| 坐标系声明 | 显式写清全局系还是车辆系、单位是度还是弧度 | 符号错误，直线看不出来，进弯反向打轮 |
频率也要按层定，别一律写 100 Hz。决策 1～2 Hz 就够，人的换道意图本来就是秒级；规划 5～10 Hz 重规划一次；控制 50～100 Hz，低速园区车 20～50 Hz 也能跑，但低于 20 Hz 时纯跟踪的预瞄点会跳，横向误差曲线会出现明显的锯齿。这三个数字在讲义里写清楚，后面所有关于延迟、抖动的讨论才有基准。
2.2 先把坐标系和状态量钉死，再讲算法
讲义的第一章如果直接从采样规划开始讲，后面基本会崩。状态量必须先钉死：x = [x, y, θ, v, a, κ]，θ 取 x 轴逆时针为正、单位弧度，κ 是曲率、单位 1/m。然后定义 Frenet 系：s 是沿参考线的弧长，d 是相对参考线的横向偏移，正方向定左还是定右，讲义自己说了算，但全书必须一致。
这里有个血泪经验：符号搞反，纯跟踪在直线上完全看不出来，一进弯就往弯外打方向，很多人第一反应是控制器参数不对，实际是 d 的正方向跟转角的正方向对不上。定完符号，还要定参考线本身怎么来——是全局路径一次性给，还是每帧从感知/地图重取一段。重取就会带来 s 的跳变，规划器输出的 s 序列突然少了两米，控制端看到的是速度阶跃。
还要约定参考线曲率 κ_r(s) 的获取方式：解析式还是离散差分。离散差分在点距 0.5 m 以上时噪声很大，建议先做滑动平均再送进转换公式，否则横向加速度会算出一个离谱的值。
2.3 一份能照着排的讲义目录模板
把上面的东西落成目录，大概是这样一张表。它的价值在于：每一章都必须产出一个可验证的东西，不产出东西的章节直接砍掉，讲义就不会变成数学手册。
| 章节 | 内容 | 必须交付的产物 |
|---|---|---|
| 第 1 章 | 坐标系、状态量、符号、单位、频率 | 一张约定表，全员签字 |
| 第 2 章 | 参考线与 Frenet 转换 | 一段能把全局路径转成 s-d 的脚本 |
| 第 3 章 | 行为决策与状态机 | 一张状态迁移图 + 触发阈值表 |
| 第 4 章 | 路径与速度规划 | 一条可在仿真里复现的轨迹 |
| 第 5 章 | 横向控制 | 一组横向误差曲线 |
| 第 6 章 | 纵向控制 | 一组速度跟踪曲线 + 标定记录 |
| 第 7 章 | 联调与故障排查 | 一份翻车清单 |
注意第 7 章。绝大多数讲义把它省了，而实车上 70% 的时间花在这一章。翻车清单不是「注意事项」，是把「现象、原因、解决」写成三列，出了事直接查表，比翻公式快得多。
3. 把讲义读成一条能跑的算法链：Frenet、采样与 MPC 的取舍
3.1 Frenet 解耦的前提，以及它在什么曲率下失效
Frenet 的核心诱惑是把二维轨迹规划拆成 s 方向和 d 方向两个一维问题：s 方向管速度、d 方向管绕障，各自做动态规划或采样，最后乘起来。这个拆法在直道和缓弯上极其好用，代价是把曲率转换的近似当成了恒等式。
严格的关系是 κ_l = κ_r / (1 - κ_r · d) 这一族形式（不同讲义写法略有差异，符号约定不同结果差个负号），分母在 d 接近 1/κ_r 时归零——物理含义是车辆正好落在参考线的曲率圆心上，此时横向偏移的方向定义本身就崩了。工程上不会开到那个位置，但曲率大的时候误差会先悄悄变大：参考线曲率 0.2 /m（半径 5 m）、横向偏移 2 m 时，近似带来的曲率误差已经能到百分之十几，控制器会明显感觉到「规划给的曲率比实际需要的偏小」。
我一般会给讲义补一张适用范围表，比空谈「近似有效」有用得多：
| 参考线曲率 κ_r (1/m) | 允许横向偏移 |d| | 处理方式 |
|---|---|---|
| < 0.05 | < 4 m | 直接用近似，误差可忽略 |
| 0.05 ～ 0.15 | < 2 m | 用近似但要限幅，横向加速度按精确式校核 |
| > 0.15 | < 1 m | 放弃全局 Frenet，改局部参考线或直接笛卡尔系优化 |
低速泊车场景尤其要注意：车位附近的参考线曲率经常超过 0.3 /m，这时候还硬套全局 Frenet，规划出来的轨迹在 s-d 图里看着很顺，回代到笛卡尔系就是一串自我交叉的点。常见做法是分段：直道和长弯用 Frenet，接近车位切成局部圆弧或直接做笛卡尔系的 Hybrid A*。
3.2 采样规划、二次规划与 MPC 各吃哪一段场景
讲义里这三种方法经常并列出现却不给选择依据，学生看完只知道三个名词。实际选型按三个维度走：约束是否硬、场景是否多模态、算力预算多少。
| 方法 | 单次耗时量级 | 硬约束处理 | 强项场景 | 调参难点 |
|---|---|---|---|---|
| 采样 + 打分（Lattice） | 5～20 ms | 靠采样点天然满足，不保证 | 多障碍、多意图、需要可解释性 | 代价函数权重，权重一变行为全变 |
| 二次规划（QP） | 1～5 ms | 硬约束，可行域为空时要有兜底 | 结构化道路、跟车、车道内微调 | 可行域为空后的降级策略 |
| MPC（含动力学） | 10～50 ms | 硬约束 + 预测，最完整 | 高速、急弯、需要预判的执行器延迟 | 预测时域与权重矩阵，重了迟滞、轻了抖 |
选型上我一般这么定：低速、障碍多、 поведение 需要给人解释的，先上采样；高速、车道清晰、主要矛盾是平顺性的，上 QP 或 MPC。采样规划最大的好处不是性能，是可解释——出问题能指着某条候选轨迹说「它被哪个代价项否决了」。QP 和 MPC 出问题只能看求解器状态，黑匣子感很强。
还有一个讲义里常漏的点：QP 的可行域为空怎么办。车道窄、障碍贴边的时候很常见，求解器返回不可行，如果代码里没写降级，表现就是规划突然没输出，控制端保持上一帧轨迹继续跑，车就直直冲出去。兜底方案通常是三级：先放松横向边界 10 cm 重解，还不行就退化成「沿参考线减速停车」，最后一级是直接给最大减速度的停车轨迹。
3.3 采样密度、代价权重与安全裕度怎么给初值
参数表是讲义里最该有、却最少有的东西。下面这张是我一般会写进讲义的初值表，注意它只是初值，最终必须靠曲线调。
| 参数 | 建议初值 | 调大后的现象 | 调小后的现象 |
|---|---|---|---|
| 横向采样间隔 | 0.5 m | 障碍间缝隙过不去，漏解 | 候选数暴涨，耗时线性上升 |
| 横向采样范围 | ±3.5 m | 无谓候选多，容易选出激进轨迹 | 窄路绕障无解 |
| 纵向采样间隔 | 1.0 m 或 0.5 s | 速度剖面粗糙，加速度阶跃 | 耗时上升，收益递减 |
| 障碍物安全裕度 | 车宽/2 + 0.3 m | 窄通道全部无解 | 贴边擦碰 |
| 偏离参考线权重 | 1.0 | 宁可撞也不偏离，绕障能力差 | 到处乱飘，乘客晕 |
| 曲率惩罚权重 | 5.0 ～ 20.0 | 轨迹过于保守，绕障绕不过去 | 曲率跳变，方向盘抖 |
| 加速度惩罚权重 | 1.0 ～ 3.0 | 起步肉，跟车距离拉大 | 急加减速，乘坐不适 |
采样的候选数量要算清楚：横向 15 条 × 纵向 10 个时间点，就是 150 条候选，每条做一次碰撞检测和代价计算，5 ms 预算内做不完就得降采样或用粗筛加精筛两级。这个账在讲义里必须让学生自己算一遍，否则写出来的代码在工控机上直接超时。
代价函数还有一条不成文的规矩：所有项都要归一化到同一量级，否则权重表毫无意义。偏离参考线用米、曲率用 1/m、加速度用 m/s²，量纲不同直接加权，等于让数值大的项偷偷主导结果。常见做法是每项除以一个参考量（比如车道宽度、最大曲率、舒适加速度上限），把每项压到 0～1 再乘权重。
4. 控制层：纯跟踪、Stanley 与纵向标定的收敛验证
4.1 预瞄距离是纯跟踪唯一真正重要的参数
纯跟踪的公式讲义里都有：δ = arctan(2·L·sin α / L_d)，L 是轴距，α 是车辆朝向与「车到预瞄点」连线的夹角，L_d 是预瞄距离。公式本身没什么可讲的，真正决定车画不画龙的是 L_d 怎么随速度变。
L_d 太小，跟踪紧密但增益高，稍微有点定位噪声就抖；L_d 太大，弯道内侧切得厉害，稳态横向误差大。经验关系是 L_d = k·v + L_d0，k 取 0.8～1.2（单位秒，物理含义是预瞄时间），L_d0 取轴距的 1～2 倍。
| 车速 (m/s) | 预瞄时间 k=1.0 时 L_d | 适用场景 |
|---|---|---|
| 1.0 | 轴距 + 1.0 m | 泊车、窄通道，需要贴线 |
| 2.0 | 轴距 + 2.0 m | 园区主干道 |
| 3.0 | 轴距 + 3.0 m | 弯道开始需要提前切 |
| 5.0 | 轴距 + 5.0 m | 已接近纯跟踪的舒适上限 |
超过 5 m/s 还硬用纯跟踪，入弯会出现明显的「先外扩再内切」。这不是参数问题，是几何法的固有缺陷：它只看一个预瞄点，不看参考线的曲率信息。讲到这里应该顺势引出 Stanley 或前馈加反馈的结构，而不是继续调 L_d。
Stanley 的横向控制律是 δ = ψ_e + arctan(k·e / v)，ψ_e 是航向误差、e 是前轴到参考线的横向偏差。它的强项是低速贴线好，弱项是低速时 k/v 项会爆——v 趋近 0 的时候那一项趋于无穷，必须限幅并把 v 抬到一个下限（比如 0.5 m/s）再算。这是讲义里必须写死的工程处理，缺了它，车一停再起步就会猛地打一把方向。
4.2 纵向标定的顺序：先测死区，再标刹车，最后闭环
纵向 PID 看起来最简单，实际上最容易翻车，因为执行器不是线性的。标定顺序我一般按这四步走，顺序错了后面全是白工。
第一步，测执行器死区与延迟。给一串递增的加速度指令，记录车实际响应，找出「给了但不走」的区间和「给了之后多久才动」的延迟。低速车常见死区 0.05～0.15 m/s²，延迟 100～300 ms。这两个数直接决定 PID 的微分项该不该用（基本不该用，延迟大时微分只会放大噪声）。
第二步，标刹车。刹车的增益通常是非线性的，且前后段差异明显。用固定减速度指令做阶梯测试，拟合出「指令—实际减速度」曲线，做成查表。刹车标不准，跟车距离永远调不稳。
第三步，标驱动。驱动相对线性，但也存在低速扭矩不足的问题，同样做一张低速补偿表。
第四步才上闭环。初值给 kp = 0.5～1.5（单位是 m/s² 每 m/s 误差），ki = 0.05～0.2，kd = 0。前馈项直接把期望加速度送给执行器，PID 只修残差，这样积分项不会长时间饱和。
注意：纵向控制一定要有抗积分饱和和加速度限幅。没有限幅时，一旦前方目标丢失，积分项积累的误差会让车猛冲一下，这是最典型的翻车形态之一。
4.3 用三条曲线判断讲义里的结论成不成立
跑完仿真不要只看「有没有到终点」。我一般固定看三条曲线，缺一条都不算验证通过。
第一条，横向误差随弧长变化。关心三个数：RMS 值、峰值、峰值出现的位置。RMS 小于 0.1 m、峰值小于 0.3 m 且峰值出现在弯道入口，属于正常（入弯滞后是几何法的固有特性）；峰值出现在弯道出口，说明预瞄过大或曲率前馈不足。
第二条，曲率连续性。把规划曲率和实际执行曲率画在一起，看有没有阶跃。相邻点曲率差超过 0.02 /m 就会出现明显的方向盘抖动，这时候该回去查规划的曲率限幅，而不是调控制器。
第三条，加速度和 jerk。纵向加速度要压在 ±1.5 m/s² 以内（载人场景），jerk 控制在 2～5 m/s³。jerk 超标的位置往往就是速度剖面规划里采样间隔过大的地方，一眼能看出问题出在规划层而不是控制层。
这三条曲线要跟讲义里的推导对上：如果讲义说某个权重下曲率连续，而你跑出来是阶跃，不要怀疑仿真，先怀疑自己单位搞错了。
5. 避坑与排查：讲义对照实车时最常翻车的五处
5.1 弯道画龙：先查符号，再查预瞄
现象：直线上完全正常，一进弯方向盘左右小幅摆动，横向误差曲线呈正弦状，频率 1～3 Hz。
原因：九成是符号约定不一致。Frenet 的 d 正方向、转角的左正右负、航向误差的定义三者只要有一个对不上，控制器就会在「纠偏」和「过纠」之间来回振荡。剩下一成是预瞄距离相对车速偏小，增益过高。
解决：先在直线上故意给一个恒定横向偏差，看车往哪边打方向；再在定曲率圆弧上跑，把参考曲率和实际执行曲率叠在一张图上。两张图对完，符号问题基本现形。确认符号无误后再把预瞄时间 k 从 0.8 往 1.2 调，一次调 0.1，配合横向误差 RMS 观察。
5.2 规划轨迹看着顺、控制端抖成筛子
现象：把规划输出的轨迹点画出来非常平滑，但实车方向盘高频抖动，执行器温度上升明显。
原因：轨迹点之间的曲率是差分离散算出来的，点距不均匀（比如按时间采样但速度在变），导致相邻点曲率跳变。视觉上看点连线是平滑的，曲率序列却是一把锯齿。
解决：在规划输出前加一道曲率平滑，用滑动窗口对曲率做限幅滤波，或者把轨迹按等弧长重采样再算曲率。判别方法很简单：把曲率序列单独画出来，肉眼能看出锯齿就说明是这个问题，跟控制器一点关系都没有。
5.3 时间戳对不齐，等效于凭空加了一百毫秒延迟
现象：仿真里跟车很稳，实车上跟车距离忽远忽近，速度曲线滞后于目标曲线。
原因：规划用的是自己的时钟，控制用的是另一路时钟，两边差几十到上百毫秒。控制器拿到的是「过期」轨迹，用当前位姿去追一个已经过时的参考点，等效于增加了纯延迟。
解决：全链路统一到一个单调时钟，规划输出的每个轨迹点都带绝对时间戳，控制器按当前时刻插值取参考点，而不是直接取第一个点。另外加一个超时保护：轨迹时间戳落后当前时刻超过 200 ms 就降级为减速停车。
5.4 单位混用：弧度与度、m/s 与 km/h
现象：某个场景下转向突然打满或者完全不动，其他场景一切正常。
原因：某个模块用了度，另一个模块默认弧度；或者速度在一处是 km/h、另一处是 m/s。这类 bug 最坑的地方是它在数值上「看起来合理」——30 度当成 30 弧度会直接饱和，但 0.5 度当成 0.5 弧度只是偏一点，很难发现。
解决：在数据类型层面解决，不要靠注释。所有角度在接口上一律弧度并在变量名里带单位后缀，所有速度一律 m/s，转换只允许发生在最外层的人机界面。
5.5 仿真通过、实车失败：模型里少了三样东西
现象：仿真横向误差 RMS 0.05 m，实车 0.25 m，且在大曲率处偏差最大。
原因：仿真里的车辆模型太干净。缺的三样通常是：执行器延迟（转向从指令到到位 100～200 ms）、轮胎侧偏（低速时影响小，速度上来后不可忽略）、定位噪声（仿真用真值，实车定位有厘米级抖动）。
解决：在仿真回路里串一个纯延迟环节和一个一阶惯性环节，再给定位加高斯噪声重跑。如果加了这三样之后仿真结果跟实车接近，说明模型够用了；如果还差很远，问题多半在标定而不是算法。这一步做完，讲义的仿真章节才算真正闭环。
6. 两周把讲义变成自己的东西：一条可执行的复现路线
手里有讲义的人最容易犯的错，是从第一页开始按顺序读。我的习惯是反过来：先定一条能跑通的最小链路，再回头补理论。
第 1～2 天，只做参考线和 Frenet 转换。给一条带缓弯和一段急弯的路径，把全局坐标转成 s-d，再转回去，比对误差。转回去误差超过 1 cm 就说明转换公式或曲率近似有问题。这一步做完，坐标系和符号基本就钉死了。
第 3～4 天，只做纯跟踪，速度固定 1.5 m/s，跑一个标准 8 字。看横向误差曲线，把预瞄时间从 0.8 扫到 1.2，记录 RMS。这一步的目的是建立「参数—曲线」的直觉，而不是把误差调到最小。
第 5～7 天，加纵向 PID 和速度剖面，做定点停车。刹停精度控制在 ±0.3 m 以内就算过。期间把执行器死区和延迟测出来写进参数表，这两个数后面一直要用。
第 8～10 天，把采样规划接进来，做静态障碍绕行。先只放一个障碍，再放到三个，观察候选数和耗时的关系。这一步会第一次遇到「可行域为空」，把三级降级策略写出来。
第 11～14 天，做闭环联调，重点是把第 5 章那五条坑逐个复现一遍。故意把符号改反、故意让时间戳错位、故意混用单位，看自己能不能靠曲线定位问题。这个「故意制造故障」的练习，比顺利跑通十次有价值得多。
| 阶段 | 天数 | 通过标准 | 失败时优先查 |
|---|---|---|---|
| Frenet 往返 | 2 | 往返误差 < 1 cm | 曲率近似、符号 |
| 纯跟踪 8 字 | 2 | 横向 RMS < 0.15 m | 预瞄时间、定位噪声 |
| 纵向定点停车 | 3 | 停车误差 < 0.3 m | 死区、延迟、积分饱和 |
| 采样绕障 | 3 | 三障碍全通过，耗时 < 20 ms | 采样密度、代价权重 |
| 故障复现 | 4 | 五条坑全部定位成功 | —— |
最后说一个我自己的习惯。每次调完参数，我都会在讲义对应那页的空白处写一行：日期、改了什么、曲线怎么变的。半年后回头看，那些批注比讲义正文值钱得多，因为讲义告诉你「应该是什么」，批注告诉你「在你的车上是多少」。这套东西没有捷径，能少走弯路的部分，无非是把别人的翻车记录提前看一遍。希望帮到你。
本文还有配套的精品资源，点击获取
