自动驾驶决策规划控制全链路解析：从行为决策到运动规划

简介：这是一份《决策规划控制讲义》PDF，源自“忠厚老实的老王”主讲的自动驾驶课程，面向算法工程师、车辆工程学生及对智能驾驶原理感兴趣的学习者，系统梳理了决策规划与控制的核心知识框架。资源为1个PDF文件，大小约69.2MB，扫描版手写笔记保留了讲课中的推导细节与重点标注。内容涵盖路径规划、行为选择与轨迹生成，以及PID、LQR、模型预测控制等主流控制算法；同时涉及传感器融合中卡尔曼滤波与粒子滤波的运用，并对安全性与实时性设计展开讲解。讲义穿插实际应用案例与实验思路，便于将理论和工程实践相结合，帮助读者建立起从环境感知到车辆执行闭环的完整认知。该讲义已有970人学习，适合作为系统复习或快速入门自动驾驶控制方向的参考资料。

1. 决策规划控制讲义.pdf：先搞清楚它到底在讲什么

第一次拿到这份讲义，我以为是本科教材的扫描件，翻到中间才意识到自己判断错了。它把自动驾驶里最容易让人迷失的三个模块——行为决策、运动规划、反馈控制——串成了一条完整链路。这两年我接触过不少从感知转规控的工程师，也包括一些刚进自动驾驶公司的校招生，大家卡住的从来不是某一个公式，而是不知道该往哪个方向深挖。这份讲义的价值在于，它把后端逻辑讲得足够清楚，读完你至少能回答三个问题：车在路口为什么要那样变道、轨迹是怎么从一堆点变成平滑曲线的、方向盘转角误差到底该由谁负责。

它适合两类人：一是准备进入规控方向、想建立全局认知的工程师；二是已经在做其中一块，但经常和上下游扯皮的从业者。如果你属于前者，建议按顺序读；如果你是后者，可以直接翻到自己对应的章节对照排查。

2. 先把讲义读薄：决策层与全局路径规划的认知框架

2.1 行为决策到底在决策什么

很多人以为行为决策就是“判断变道还是不变道”，这是最常踩的误解。行为决策解决的是一个状态转移问题：当前车处在一个什么场景、下一个动作该是什么。讲义里给出的主干结构是一个有限状态机，配合场景分类器，状态大致包括巡航、跟车、停车、变道、汇入、绕障。每个状态定义了允许执行的动作集合，状态之间通过条件转移。

实际落地时你会发现，状态机设计最难的从来不是转移条件，而是状态的粒度和恢复策略。粒度过粗，比如只分“走”和“停”，在城市道路就是一场灾难；粒度过细，比如把“左侧第三车道有慢车”这种场景定义成独立状态，状态数量会爆炸，维护成本高到项目组放弃。

我一般会建议按“纵向意图 × 横向意图”来拆分，而不是按场景拆。纵向只有“保持、减速、停车、加速”四种，横向只有“保持、左变道、右变道、绕行”四种，组合起来最多 16 个状态，大部分产品 10 个以内就够用了。常见的做法是加上一个“待定”状态，给仲裁层留余地。

2.2 全局路径规划：别只盯着 A*

全局路径规划是决策的上游输入，它回答的问题是“从 A 到 B 走哪条路”。讲义的这套链路里，全局规划输出的是车道级的参考路径，不是像素级的轨迹。这条参考路径会被下游局部规划拿来当作约束和参考线。

这里我想多说一句，很多新手一提到路径规划就想到 A*、Dijkstra，这在离散网格上是对的，但自动驾驶里的全局规划是在车道网络上做的，不是栅格栅格地图。车道网络里每个节点是一个车道段，每条边是车道间的连接关系，A* 跑的其实是这个拓扑图。这个区别非常关键：同样的 A*，网格上的启发函数和车道网络上的启发函数完全不一样。

在车道网络里跑 A*，需要考虑三个信息：代价、约束、时间。代价一般是距离、限速、拥堵系数加权；约束是禁止掉头、单行道这种；时间是从出发时刻开始预估的到达时刻，用来避免把拥堵路段的代价算得过于乐观。

def astar_on_lane_graph(graph, start_lane, goal_lane): open_set = {start_lane} came_from = {} g_score = {start_lane: 0.0} f_score = {start_lane: heuristic(start_lane, goal_lane)} while open_set: current = min(open_set, key=lambda x: f_score.get(x, float('inf'))) if current == goal_lane: return reconstruct_path(came_from, current) open_set.remove(current) for next_lane, edge_cost in graph.neighbors(current): tentative_g = g_score[current] + edge_cost.cost(t=current_time_hint) if tentative_g < g_score.get(next_lane, float('inf')): came_from[next_lane] = current g_score[next_lane] = tentative_g f_score[next_lane] = tentative_g + heuristic(next_lane, goal_lane) open_set.add(next_lane) return None

这段代码的启发函数 h 用的是车道段质心的大圆距离或平面欧氏距离。如果你跑的是城市道路，更推荐用大圆距离，因为路网不一定在同一个平面投影下。另一个细节是 edge_cost.cost(t=...) 这个接口，它预留了时间维度，目的是让拥堵预测可以按时刻切换代价。做全局规划时，建议把这类动态代价做成独立接口，尽量不要把路况耦合进静态地图表里，否则后面做路况更新会很痛苦。

2.3 坐标系约定：整个链路最容易翻车的地方

决策规划控制三个模块里，坐标系可能是让所有模块工程师吵架的唯一共同话题。讲义里一共会出现三个坐标系：世界系、车体系、Frenet 系。规划层在 Frenet 系工作，控制层在车体系工作，而感知输出通常在世界系。每一次跨模块交接，都要做坐标变换，而误差往往就藏在变换里。

Frenet 系的特点是沿着参考线定义纵向 s 和横向 l，这样做的好处是横向控制问题被解耦成了一个一维问题。但 Frenet 系的公理只适用于参考线曲率变化不太剧烈的情况，如果参考线是停车场里的直角弯，Frenet 系下的横向偏差会瞬间失真。

工程上我一般会做一次坐标一致性检查：在所有模块的输入结构里，强制指定坐标系标签，模块内不做隐式假设。这个检查看起来很简单，但真救过我的项目。某次联调，局部规划输出的是车体系轨迹，控制模块当成 Frenet 系来跟踪，结果横向误差呈正弦发散，排查了两天，最后发现是一个结构体字段没改名。

3. 局部轨迹规划实操：从采样到代价函数的完整闭环

3.1 横向纵向为什么必须在 Frenet 里解耦

局部轨迹规划的目标是在一个短时间窗口里生成一条满足动力学约束、避开障碍物的轨迹。这条轨迹有两个维度：纵向速度轮廓和横向偏移。讲义里的核心做法是把这两个维度拆开，分别生成候选集，再组合评估。

横向轨迹的生成基于当前车辆相对参考线的横向状态，包括横向位置、横向速度、横向加速度，纵向轨迹则是当前车速和期望车速之间的转移。纵向和横向各自生成一组多项式或五次样条曲线，然后配对成完整轨迹。

为什么要在 Frenet 系解耦？因为纵向决策（加速、刹车）和横向决策（变道、绕行）在物理上本来就相对独立，它们在笛卡尔坐标系里耦合的非线性程度非常高，直接用 xy 坐标生成轨迹，你很难对横向偏移做精确约束。而在 Frenet 系，横纵向各是一条一维曲线，约束写起来很直观，代价函数也容易设计。

采样是另一个重点。我一般会用密集一点的横向采样配合稀疏的纵向采样，比如横向每 0.5 米采样一条，纵向只取三档目标速度，让组合数量可控。如果你横向采样过密、纵向也过密，候选轨迹数量会轻松破千，评估环节的算力消耗会直线上升。

3.2 轨迹生成与碰撞检测：用边界框而不是质点

生成候选轨迹后，第一关是碰撞检测。碰撞检测有两种做法：质点和边界框。质点检测速度快，但只能用在状态非常稀疏的 demo 里；真实路况建议直接上边界框。边界的来源是车辆的半宽、半长，加上一个安全余量，在 Frenet 系里对每个采样点计算车辆四个角点的位置，再和障碍物做相交判断。

def sample_trajectories(lateral_offset_range, target_speeds): candidates = [] for d_offset in lateral_offset_range: lateral = quintic_polygon(d_start, d_end=d_offset, T=8.0) for v_target in target_speeds: longitudinal = quartic_polygon(s_start, s_dot_start, v_target, T=8.0) candidates.append(Trajectory(lateral, longitudinal)) return candidates def is_collision(trajectory, obstacles): for s, l, t in trajectory.sample_every(0.1): corners = vehicle_corners(s, l, heading_at(s)) for obs in obstacles: if any(in_polygon(corner, obs.polygon) for corner in corners): return True return False

quartic_polygon 的输入是起点位置、起点速度、目标速度和时间 T，输出是一条纵向 s-t 曲线。这里有一个容易被忽略的点：加加速度 jerk 是三次导，不是二次导，很多算法的横向舒适度指标用的是 jerk 的三次方积分，如果你代码里写成了加速度平方积分，横向指标的数值会差一个量级。

碰撞检测的另一个坑是时间戳对齐。轨迹是随时间变化的，障碍物的预测位置也是随时间变化的，你必须要在同一时刻 t 上做比较。如果在代码里取了障碍物的当前时刻位置来和未来的轨迹段比较，低速场景还看不出问题，高速场景下的误检率会高到你怀疑算法本身。

3.3 代价函数设计和权重：哪个指标最影响实车体验

候选轨迹经过碰撞过滤后，剩下的都“能跑”，它们之间的区别只有好和更好的问题。代价函数的每一项都对应一种驾驶性格：横向偏移项让车贴近参考线，纵向加速度项让车速变化平缓，jerk 项影响乘坐体验，终点偏移项降低变道的急切程度。

我维护过一套线上轨迹规划的权重配置，踩过大坑。横向偏移误差的权重设得过高，车在车道内像被轨道吸住一样，遇到轻微弯道会频繁微调方向，方向盘抖动明显。后来把横向偏移权重下调约三分之一，路径预留了更大的误差空间，抖动消失，乘坐体验提升明显。换道完成时间约束也是同样道理，这个权重调高会让变道动作变得激进，在城市车流里经常导致后车急刹。

一个公认的起点配置如下表，但注意这只是起点：

调参的顺序有讲究。我一般先调纵向，再调横向，最后调交互项。如果你反过来，会发现横向调了半天，纵向一变，车感又全变了，前面白调。

4. 控制层落地：PID、LQR 与 MPC 的选用边界

4.1 三个算法的定位差异：不是越复杂越好

讲义的控制部分把 PID、LQR、MPC 放在同一章，这是有原因的。它们解决的是同一类问题——轨迹跟踪，但适用场景完全不同。PID 零建模成本，调三个系数，适合工况简单、车速不高的场景；LQR 需要线性化模型，适合高速公路这种工况相对稳定的场景；MPC 能带约束，适合城市道路、避障频繁切换的场景。

选型看的是约束密度。你的系统里有几条硬约束？如果有——比如方向盘转角有限、加速度有限——MPC 的优势就体现出来了，因为它在优化里面直接带了约束项，而 LQR 只能通过调权重间接限制输出。如果约束很少、路况简单，PID 的成本优势会非常明显，没必要上 MPC。

研发效率也是一个考量。MPC 的调参时间通常是 LQR 的三到四倍。如果你在做一个项目周期紧张的产品，先上 LQR 把系统跑起来，再去评测是否需要 MPC，这是更务实的路径。

4.2 纵向控制的油门刹车标定与速度环

纵向控制最容易被低估，因为看起来就是把速度误差喂给 PID。但实际上场的翻车案例里，有一大半根因不是算法，而是执行力标定。同样的一个刹车指令，空载和满载、平路和下坡产生的减速度完全不同。

我一般会把执行器的响应建模成“指令查表映射到加速度”，在标定阶段采集不同车速、不同踏板深度下实际加速度的数据，生成一张二维查表，控制器的输出先过表，再发给执行器。这样做还有一个好处：把执行器差异隔离在标定层，速度环 PID 的整定可以对所有车辆共用一套初始参数。

纵向 PID 的积分项处理值得单独拿出来说。车速的积分项在长时间低速行驶时容易积累出较大的稳态误差，导致起步蹿动。工程上的常见做法是加抗积分饱和，把积分限幅设成油门踏板正常范围内的中等值，同时在车速低于某阈值时强制清零积分项。这个阈值一般取 1～2 km/h 之间，具体看车型。

4.3 横向控制的 LQR 参数整定与 MPC 代价矩阵设计

横向控制是我最愿意花时间讲的部分，因为它的参数量不大，但每个量都有明确物理含义。LQR 的核心是两个矩阵 Q 和 R，Q 指定状态误差的权重，R 指定控制量的惩罚。横向误差的权重取大一点，车会贴着参考线走；控制量惩罚取大一点，转向会变得迟钝。

工程经验值：R 的初值设在转向角最大值的平方分之一附近，Q 里横向偏差权重设成 R 的百倍量级，航向角偏差权重比横向偏差小一到两个量级。这样设置的原因很简单，横向偏差是一阶积分量，它的权重如果不压过航向角，车会表现为“偏着走但很稳”。

MPC 的调参复杂度比 LQR 高一个维度，因为它在权重之外还有预测时域和控制时域。预测时域设得越长，控制器越“有远见”，但计算量也增大；控制时域设得越短，系统越保守。话虽如此，最影响实车表现的是约束的定义方式。约束太紧，比如转角率设成硬约束，系统容易退化；我一般会把运动学约束设成软约束，通过惩罚系数引导，只在真正接近物理极限时才收紧边界。

5. 避坑：从讲义到仿真与实车的常见翻车点

5.1 轨迹跳变：明明每段轨迹都平滑，接起来却突兀

现象：单帧轨迹内部很平滑，但两个控制周期之间的轨迹首尾不连贯，导致方向盘高频修正，乘坐感受明显发硬。

原因：相邻两个周期的规划起点，一个来自上一帧轨迹的末端，一个来自当前实际位姿，两者存在位姿偏差。规划时没有考虑。

解决：在轨迹生成前做一次位姿对齐，用当前实际状态作为规划起点，而不是用上一帧输出的末端，同时给横向起点加一段小的过渡约束，避免因为对齐产生的剧烈抖舵。实车调试时，把起点对齐逻辑放在采样之前，而不是之后。

5.2 笛卡尔系与 Frenet 系的曲率符号问题

现象：高速弯道里横向误差发散，跟踪曲线呈“外抛”趋势，LQR 参数无论怎么调都改善不了。

原因：参考线的曲率符号在 Frenet 变换时取反了。左转和右转时，曲率的正负号定义不一致，导致横向运动的解算符号错位。

解决：在坐标系变换函数里加一个自检——对同一参考线分别用左转和右转的用例跑一遍，检查横向偏移符号是否符合直觉。这个用例纳入回归测试，每次改动坐标变换代码都跑一遍，能拦下大部分重复问题。

5.3 决策层和控制层的时间戳不同步

现象：决策输出的变道指令，控制层延迟了约 150 毫秒才响应，体感就是“车想动但没立刻动”，在路上体现为变道动作拖沓。

原因：决策层的输出经过消息队列传递，队列在拥堵时出现了积压，时间戳没有及时更新，控制层拿到的还是一帧旧指令。

解决：所有模块间的指令传递统一带时间戳，并且控制层在拿到指令后校验时间戳，超过一定阈值就丢弃等待新指令。另外可以在决策输出时把“期望执行时刻”写进指令，控制层按这个时刻校准，而不是按接收时刻。

5.4 仿真里表现好，实车一上就失控

现象：同样的轨迹跟踪算法，仿真场景里横向误差稳定在 10 厘米内，实车跑起来误差到了 30 厘米，偶尔还会出现小幅震荡。

原因：仿真里的执行器响应模型过于理想化，真实方向盘有转向延迟、摩擦，轮胎的侧偏特性也没有被建模进去。

解决：在仿真里给执行器加上延迟环节和一阶惯性环节，并把轮胎侧偏刚度设成实车标定的低置信区间值做鲁棒性测试。我一般会让仿真的横向误差指标直接放宽到实车目标值的 1.5 到 2 倍，只有在这个条件下依然稳定的参数，才敢拿到实车上去。

5.5 舒适度指标和碰撞安全指标打架

现象：为了降低 jerk 值，轨迹规划把转向动作拉得特别长，结果变道过程穿越了对向车流，安全间距被压缩到临界值。

原因：代价函数里舒适度权重压过了避障权重，规划器在“宁愿离得近一点，也不要动作急一点”的方向上跑偏了。

解决：把避障相关约束从代价项提升为硬约束，物理上不允许低于最小安全间距，舒适度只能在安全边界内优化。我后面所有项目都遵循这条原则：安全阈值全部做成硬约束，舒适度永远排在第二位，这个顺序不能倒。

6. 把讲义变成能力：离线评测与回放验证的进阶手法

读讲义和会做规控之间，差的不是第二遍第三遍阅读，而是一套能让自己快速验证理解的工具链。我习惯把讲义里的每个算法块拆出来，配一个离线评测的脚本，核心指标就三样：横向误差均值、jerk 的超过阈值比例、碰撞检测误报率。

先把录制的实车数据切成时间片，拿同一段数据反复跑不同的参数组合，误差数据画成曲线，一眼就能看出哪组参数在哪个路段失效。这里我强烈建议把“路段场景”作为标签加进评测报告里，因为同一组参数在直线高速和连续弯道上的表现可能完全不同，只看全局平均值会掩盖局部问题。

仿真环境的选择也有讲究。我一般先用轻量级的单车模型做纯算法验证，验证逻辑正确性；再用高保真仿真模拟器做执行器细节验证。两层分开跑，定位问题会快很多，因为单车模型里出的问题基本只会是算法问题，不会混进轮胎模型或悬架特性的干扰。

最后建议你把讲义里的重要公式都自己手动推导一遍，然后和原文对照。亲手推过的公式，调试时你会知道它的适用边界在哪里。我的习惯是每接手一个项目，先把参考线坐标系和代价函数权重这两块重新看一遍，再把离线评测脚本跑一遍，确认没有因为版本迭代引入回归问题。这不是流程洁癖，是吃过亏之后形成的习惯。希望这篇笔记能帮你在规控这条路上少走一些弯路，把讲义里的知识真正变成能落地的能力。

本文还有配套的精品资源，点击获取