简介《决策规划控制讲义》是一份面向自动驾驶入门与进阶学习者的课程配套资料聚焦决策规划与控制算法这一核心模块帮助读者建立从环境感知到车辆执行的整体认知。内容围绕路径规划、行为选择与轨迹生成展开并延伸到PID、LQR、模型预测控制等控制方法同时涉及卡尔曼滤波、粒子滤波在多传感器融合中的应用以及故障检测、应急策略和实时性等工程考量。压缩包内为1个PDF文件共69.2MB以扫描讲义形式呈现便于按章节阅读和批注。已有973人学习说明该资料在相关课程学习者中具有一定参考价值。讲义还结合老王的手写笔记与实践案例适合对照课堂内容梳理算法脉络、补齐关键概念并为后续实验与项目实践提供思路。1. 决策规划控制讲义翻完三遍为什么还是不会调那一个参数一份两百多页的决策规划控制讲义最常见的结局是目录漂亮、公式齐全翻开第一天就停在第七页真到要写横向控制、要定采样密度、要解释车在弯里画龙的时候翻回去依然找不到那一页。问题不出在讲义写得差而在于讲义本身是「结论的集合」——它告诉你 Frenet 坐标系能把二维问题拆成两个一维问题却不会告诉你参考线曲率超过多少之后这种拆法开始失真。这篇笔记把一份决策规划控制讲义按「骨架、算法链、控制层、踩坑、复现路线」重新排一遍新手能照着搭出第一条跑得起来的链路熟手能看到每个参数背后的边界在哪。适合正在做低速园区车、记忆泊车、AGV 或移动机器人导航手里攥着讲义却落不了地的人。2. 讲义的骨架决策、规划、控制三层怎么切、接口怎么定2.1 三层的输出物不同别用同一套指标考核决策层回答的是「走哪条路、让不让、超不超」输出必须是离散量目标车道编号、行为原语跟车、换道、借道绕行、路口停车让行、期望速度区间。规划层回答「沿着这个意图走一条什么形状的曲线」输出是带时间戳的轨迹点序列常见时域 38 秒、点间隔 0.1 秒。控制层回答「这一拍油门给多少、方向盘打多少」输出加速度和曲率或直接是前轮转角。三层混在一起是讲义里最普遍的毛病。判断某个算法归哪一层我一般只看一件事优化变量里有没有控制量 u。只优化轨迹形状、把动力学当约束校核的归规划比如带曲率约束的二次规划、采样加打分把 u 直接作为决策变量、把状态 x 当约束滚着走的归控制MPC 就卡在这条线上讲义必须明确表态否则学生会把同一个 MPC 在两个章节里学两遍且互相矛盾。接口契约比算法更值得在讲义里单开一节。规划交给控制的东西缺一项就等着翻车要素交付要求不满足的后果时间戳与控制器时钟同源抖动小于 10 ms控制按错拍跟踪高速时等效于加了纯延迟速度剖面每点带 v 与 a首点与当前车速差小于 0.5 m/s起步一顿、跟车点头曲率序列相邻点曲率变化率限幅不允许阶跃方向盘高频抖动执行器过热坐标系声明显式写清全局系还是车辆系、单位是度还是弧度符号错误直线看不出来进弯反向打轮频率也要按层定别一律写 100 Hz。决策 12 Hz 就够人的换道意图本来就是秒级规划 510 Hz 重规划一次控制 50100 Hz低速园区车 2050 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 各吃哪一段场景讲义里这三种方法经常并列出现却不给选择依据学生看完只知道三个名词。实际选型按三个维度走约束是否硬、场景是否多模态、算力预算多少。方法单次耗时量级硬约束处理强项场景调参难点采样 打分Lattice520 ms靠采样点天然满足不保证多障碍、多意图、需要可解释性代价函数权重权重一变行为全变二次规划QP15 ms硬约束可行域为空时要有兜底结构化道路、跟车、车道内微调可行域为空后的降级策略MPC含动力学1050 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²量纲不同直接加权等于让数值大的项偷偷主导结果。常见做法是每项除以一个参考量比如车道宽度、最大曲率、舒适加速度上限把每项压到 01 再乘权重。4. 控制层纯跟踪、Stanley 与纵向标定的收敛验证4.1 预瞄距离是纯跟踪唯一真正重要的参数纯跟踪的公式讲义里都有δ arctan(2·L·sin α / L_d)L 是轴距α 是车辆朝向与「车到预瞄点」连线的夹角L_d 是预瞄距离。公式本身没什么可讲的真正决定车画不画龙的是 L_d 怎么随速度变。L_d 太小跟踪紧密但增益高稍微有点定位噪声就抖L_d 太大弯道内侧切得厉害稳态横向误差大。经验关系是 L_d k·v L_d0k 取 0.81.2单位秒物理含义是预瞄时间L_d0 取轴距的 12 倍。车速 (m/s)预瞄时间 k1.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.050.15 m/s²延迟 100300 ms。这两个数直接决定 PID 的微分项该不该用基本不该用延迟大时微分只会放大噪声。第二步标刹车。刹车的增益通常是非线性的且前后段差异明显。用固定减速度指令做阶梯测试拟合出「指令—实际减速度」曲线做成查表。刹车标不准跟车距离永远调不稳。第三步标驱动。驱动相对线性但也存在低速扭矩不足的问题同样做一张低速补偿表。第四步才上闭环。初值给 kp 0.51.5单位是 m/s² 每 m/s 误差ki 0.050.2kd 0。前馈项直接把期望加速度送给执行器PID 只修残差这样积分项不会长时间饱和。注意纵向控制一定要有抗积分饱和和加速度限幅。没有限幅时一旦前方目标丢失积分项积累的误差会让车猛冲一下这是最典型的翻车形态之一。4.3 用三条曲线判断讲义里的结论成不成立跑完仿真不要只看「有没有到终点」。我一般固定看三条曲线缺一条都不算验证通过。第一条横向误差随弧长变化。关心三个数RMS 值、峰值、峰值出现的位置。RMS 小于 0.1 m、峰值小于 0.3 m 且峰值出现在弯道入口属于正常入弯滞后是几何法的固有特性峰值出现在弯道出口说明预瞄过大或曲率前馈不足。第二条曲率连续性。把规划曲率和实际执行曲率画在一起看有没有阶跃。相邻点曲率差超过 0.02 /m 就会出现明显的方向盘抖动这时候该回去查规划的曲率限幅而不是调控制器。第三条加速度和 jerk。纵向加速度要压在 ±1.5 m/s² 以内载人场景jerk 控制在 25 m/s³。jerk 超标的位置往往就是速度剖面规划里采样间隔过大的地方一眼能看出问题出在规划层而不是控制层。这三条曲线要跟讲义里的推导对上如果讲义说某个权重下曲率连续而你跑出来是阶跃不要怀疑仿真先怀疑自己单位搞错了。5. 避坑与排查讲义对照实车时最常翻车的五处5.1 弯道画龙先查符号再查预瞄现象直线上完全正常一进弯方向盘左右小幅摆动横向误差曲线呈正弦状频率 13 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且在大曲率处偏差最大。原因仿真里的车辆模型太干净。缺的三样通常是执行器延迟转向从指令到到位 100200 ms、轮胎侧偏低速时影响小速度上来后不可忽略、定位噪声仿真用真值实车定位有厘米级抖动。解决在仿真回路里串一个纯延迟环节和一个一阶惯性环节再给定位加高斯噪声重跑。如果加了这三样之后仿真结果跟实车接近说明模型够用了如果还差很远问题多半在标定而不是算法。这一步做完讲义的仿真章节才算真正闭环。6. 两周把讲义变成自己的东西一条可执行的复现路线手里有讲义的人最容易犯的错是从第一页开始按顺序读。我的习惯是反过来先定一条能跑通的最小链路再回头补理论。第 12 天只做参考线和 Frenet 转换。给一条带缓弯和一段急弯的路径把全局坐标转成 s-d再转回去比对误差。转回去误差超过 1 cm 就说明转换公式或曲率近似有问题。这一步做完坐标系和符号基本就钉死了。第 34 天只做纯跟踪速度固定 1.5 m/s跑一个标准 8 字。看横向误差曲线把预瞄时间从 0.8 扫到 1.2记录 RMS。这一步的目的是建立「参数—曲线」的直觉而不是把误差调到最小。第 57 天加纵向 PID 和速度剖面做定点停车。刹停精度控制在 ±0.3 m 以内就算过。期间把执行器死区和延迟测出来写进参数表这两个数后面一直要用。第 810 天把采样规划接进来做静态障碍绕行。先只放一个障碍再放到三个观察候选数和耗时的关系。这一步会第一次遇到「可行域为空」把三级降级策略写出来。第 1114 天做闭环联调重点是把第 5 章那五条坑逐个复现一遍。故意把符号改反、故意让时间戳错位、故意混用单位看自己能不能靠曲线定位问题。这个「故意制造故障」的练习比顺利跑通十次有价值得多。阶段天数通过标准失败时优先查Frenet 往返2往返误差 1 cm曲率近似、符号纯跟踪 8 字2横向 RMS 0.15 m预瞄时间、定位噪声纵向定点停车3停车误差 0.3 m死区、延迟、积分饱和采样绕障3三障碍全通过耗时 20 ms采样密度、代价权重故障复现4五条坑全部定位成功——最后说一个我自己的习惯。每次调完参数我都会在讲义对应那页的空白处写一行日期、改了什么、曲线怎么变的。半年后回头看那些批注比讲义正文值钱得多因为讲义告诉你「应该是什么」批注告诉你「在你的车上是多少」。这套东西没有捷径能少走弯路的部分无非是把别人的翻车记录提前看一遍。希望帮到你。本文还有配套的精品资源点击获取