Apollo EM Planner轨迹规划原理与工程实践解析

这几年聊自动驾驶轨迹规划，绕不开两个名字：一个是端到端，一个是Apollo的EM Planner。前者代表着大家对“一个网络吃掉传感器输入，直接输出方向盘和油门”的想象力，后者则是在真实规则系统里服役最久、也最能讲清楚“自动驾驶规划到底在解决什么问题”的方案。我第一次用Apollo做规划方向的项目时，EM Planner是我的默认选择，它不花哨，但能把一条安全、平滑、可执行的轨迹从底层逻辑到上层实现讲得明明白白。这篇文章我就把这套框架彻底拆开，从原理到工程实现，再到我实际调试中踩过的坑，一起聊透。

如果你是在校学生、刚转行做自动驾驶规划的工程师，或者已经在用Apollo但只停留在“能跑通Demo”阶段，这篇内容应该能帮你少走不少弯路。EM Planner不是一个能“背下来就完事”的算法，它背后关于路径决策、速度决策、凸优化、动态规划的设计哲学，才是真正值钱的东西。

1. 为什么轨迹规划要“解耦”：EM Planner要解决的真正问题

1.1 规划问题的本质：多个目标在互相打架

先想一个问题：自动驾驶轨迹规划到底在求什么？

从数学上看，规划模块要找到一组随时间变化的车辆状态序列，让它从当前位姿到达目标位姿，同时满足安全性、舒适性、交规、车辆动力学四类约束。这四个约束不是协同合作关系，而是互相打架的关系。

• 安全约束要求离障碍物越远越好，但道路空间就这么宽，离障碍物太远可能就压线了。
• 舒适性要求加速度变化率（jerk）越小越好，但前车突然急刹时，你必须瞬间把减速度拉满。
• 交规要求限速、不能压实线、斑马线前要停车，但导航目标可能要求你连续变道。

如果把所有目标直接揉进一个非线性优化问题里，变量规模会非常大：以100毫秒一个轨迹点、规划未来8秒来看，就是80个点的位置和速度，总共几百个变量，再加上几十条约束。用通用非线性求解器在车载计算单元上实时求解，很难保证每次都收敛到可行解。

这也是为什么纯粹的数值优化在工程上不那么吃香，而Apollo选择了另一种思路：把问题拆碎、降维、分层。

1.2 “解耦”的核心：把路径和速度分开算

EM Planner最核心的设计思想，不是“用某个高大上的优化器”，而是“把路径和速度分开算，再交替迭代”。

具体来说，规划问题被拆成两个子问题：

• 路径子问题：在给定参考线的前提下，找到一条横向偏移曲线l(s)，表示在纵向距离s处，车辆偏离参考线多少。这个子问题本质上一维的——只在横向上寻找合理偏移。
• 速度子问题：在路径已经确定的前提下，找到速度曲线s(t)，表示在每个时间点t上车辆应该到达哪个纵向位置。这个子问题也是一维的——只在纵向上寻找合理速度。

一维优化问题的求解难度，比二维优化低一个量级。更重要的是，拆开后可以针对每个子问题选择最合适的算法：路径用动态规划（DP）+二次规划（QP），速度也用DP+QP。每个阶段都有成熟、稳定的求解方案。

1.3 EM在这里到底是什么意思

严格说，EM是统计学里的期望最大化算法，用于含隐变量的极大似然估计。Apollo的论文里提到EM Planner，实际上是借用了EM“交替迭代、逐步逼近”的精神，而不是严格的数学EM。

在EM Planner里，路径规划器和速度规划器会来回迭代：

• 第一轮，先假设一个初始速度（比如匀速），在SL图上规划路径。
• 路径确定了，把障碍物投影到ST图上，规划速度。
• 速度结果反哺路径规划，让路径在避开障碍物时考虑速度变化的影响。
• 如此反复几轮，直到路径和速度不再发生明显变化。

这个“交替迭代、收敛到可行解”的过程，就是EM Planner名字的由来。你不需要把它严格对应到统计学公式，只要理解它是一个解耦的、迭代的优化框架就够了。

2. SL与ST两个坐标系：整个EM Planner的地基

2.1 为什么规划要在Frenet坐标系下做

如果你看过Apollo的规划代码，会发现里面几乎没有直接用全局XY坐标做规划的地方，所有障碍物、参考线、轨迹点都要先转换到Frenet坐标系。

Frenet坐标系就是沿参考线展开的坐标系：用s表示沿参考线的纵向距离，用l表示相对参考线的横向偏移。为什么非要用它？我举个例子你就明白了。

在全局XY坐标里，一条车道可能是一条斜线，甚至经过弯道后变成一条弧线。你想表达“车辆保持在车道中心”，在XY坐标下得写一个复杂的几何函数；但在Frenet坐标下，这就是一个非常朴素的约束：l = 0。

更关键的是，道路的语义（车道中心线、车道边界、停止线位置）在Frenet坐标系下都能用一个沿s的一维函数表示。凸优化里的一维边界约束、路径平滑性约束都变得极其直观。

所以你可以这样理解：Frenet坐标系把“道路几何”和“车辆运动”解耦了。道路几何被参考线吸收，规划模块只需要关心车辆相对参考线的位移和速度。

2.2 SL图与ST图各自管什么

在EM Planner内部，两个坐标系分别服务于两个阶段：

SL图相当于一个“航拍图”，把参考线拉直后，看车辆左右两侧有哪些障碍物、车道边界在哪里。路径规划的任务就是在SL图上找一条横向偏移曲线，绕过障碍物且尽量保持在车道中心。

ST图则是一个“时空图”，横轴是未来时间，纵轴是车辆沿路径的纵向位置。动态障碍物的预测轨迹投影到ST图上后，会形成一个禁止进入的“时空区域”。速度规划的任务就是在ST图上找一条s(t)曲线，避开所有障碍物区域，同时满足加速度、加加速度和限速约束。

有了这两个坐标系，EM Planner才能做到“路径决策不考虑时间，速度规划再考虑时间”的分层设计。

2.3 参考线：一切规划的基准

我必须强调一下参考线在EM Planner里的重要性，因为很多人第一次看代码时容易忽略它，导致后面看路径规划一头雾水。

参考线（Reference Line）一般来自全局路径规划，它是一条沿着车道中心或导航路径的平滑曲线。EM Planner的所有障碍物投影、路径偏移、ST图构建，全部以参考线为基准。如果参考线本身不平滑、不连续，那后面所有优化结果都会被带偏。

我在实际调试中就遇到过一个问题：高架匝道的地图参考线在某处有突变，导致EM Planner规划出的路径在每一轮迭代时横向偏移都跳变，车辆在高架上画龙。排查了很久才发现问题根本不在EM Planner，而在上游参考线生成。这个经验后面我会细说。

3. 路径规划链路：DP找“大致走哪”，QP把路径打磨平滑

3.1 参考线平滑：先保证“地基”质量

Apollo在进入路径规划之前，会先对参考线做平滑处理。这个步骤叫Reference Line Smoother，早期版本里常用FemPosDeviationSmoother，通过二次规划或样条优化来生成一条曲率连续、几何平滑的参考线。

为什么必须要平滑？因为地图模块给出的参考线往往来自道路中心线的折线拼接，在曲率上是不连续的。车辆执行轨迹时，横向加速度与曲率直接相关，曲率突变意味着转向角速度突变，车会明显顿挫。

FemPosDeviationSmoother的目标函数通常包含三部分：

• 位置偏差代价：平滑后的点不能偏离原始参考线太远，否则就失去参考意义。
• 平滑性代价：相邻三点之间的夹角变化尽量小。
• 紧凑性代价：相邻点之间的距离尽量均匀。

这里的“平滑性”和“偏差”是一对矛盾：平滑性权重拉高，参考线会变得很圆滑，但可能偏离实际车道中心；偏差权重拉高，参考线能贴近地图，但平滑效果就没了。工程上需要针对不同道路类型（高速、城市快速路、园区道路）分别标定权重。

3.2 DP路径决策：在离散采样网格上找最低成本走廊

参考线准备好之后，进入路径规划第一阶段：DP（动态规划）路径决策。

这一步的思想非常朴素：既然连续空间里的最优路径很难直接求，那就在横向上离散采样，把连续问题变成在网格上搜索最优路径的问题。

具体做法是沿参考线每10米（这个步长可配置）采样一个纵向位置，每个纵向位置上再以0.5米为间隔采样多个横向偏移候选点。这些候选点形成一张网格图，任意相邻两个纵向截面上的候选点之间都可以连成一条“路径段”。

动态规划在这个网格上做序列决策：每到达一个候选点，都要积累一段成本，最终选择一条累计成本最低的路径作为DP结果。成本函数通常包含以下几项：

• 障碍物距离代价：路径点离障碍物越近，代价越高。这个代价一般不是线性增长的，而是带一个安全分界点，距离小于安全阈值后代价指数上升。
• 车道偏移代价：离车道中心越远，代价越高，这能保证路径不会无故压线或骑线。
• 曲率代价：路径段的转角越大，代价越高。
• 连续性代价：相邻路径段之间的横向变化率不能太大，否则车辆无法平滑执行。

DP路径决策得到的并不是最终轨迹，而是一条“走廊”或者叫“引导线”。它的作用是告诉后续QP优化：安全的、值得走的路径大致在哪里，哪些区域绝对不能进。

3.3 QP路径优化：在凸空间里打磨出平滑曲线

DP给出的路径往往有折角、不平滑，直接给控制模块执行会把车辆开成“蛇形走位”。所以EM Planner在DP后面接了一个QP路径优化器。

QP要做的事情是：在DP结果形成的凸走廊内，求解一个二次规划问题，输出一条曲率连续、符合车辆运动学约束的平滑路径。

目标函数一般是这样的形式：

• 最小化位置二阶导（曲率）和三阶导（曲率变化率），保证路径平滑。
• 最小化与DP参考线的偏差，保证路径在DP决策的“安全走廊”内。
• 最小化与车道中心的偏差。

约束条件包括：

• 横向边界：路径点必须在DP决定的横向边界内，不能超出车道边界。
• 曲率约束：路径曲率不能超过车辆最小转弯半径对应的极限值。
• 曲率变化率约束：曲率变化率不能超过转向机构响应能力。

为什么这里要用QP而不是继续用更复杂的非线性优化？因为二次规划问题有成熟的求解器，能保证在几十毫秒内返回最优解，而且解是全局最优（在凸假设下），不存在陷入局部最优的问题。这在车载计算单元上是至关重要的。

3.4 路径规划里常见的坑：障碍物膨胀与投影误差

路径规划阶段最容易出问题的不是算法本身，而是障碍物数据的处理。

第一个坑是障碍物没有做尺寸膨胀。规划时车辆和障碍物都被抽象为质点，但实际车辆有四五米长、两米宽。如果不把障碍物边界膨胀半个车宽、再加安全余量，QP优化出来的路径可能贴着障碍物边缘走，实车根本执行不了。

第二个坑是障碍物投影到SL图时没有考虑车辆朝向。低速复杂场景下，车辆斜着通过狭窄区域时，质心投影位置和实际占用区域差异很大，会导致路径规划结果偏保守或者过于激进。

我个人的经验是：在SL图上做路径规划时，至少要进行“半车宽+安全距离”的膨胀，同时在路径评估阶段加入车辆当前朝向与障碍物的相对夹角判断。这样做之后，窄路会车、绕桩场景的路径质量会明显提升。

4. 速度规划链路：ST图上的两步走

4.1 构建ST图：把所有“纵向不能去”的区域画出来

路径规划完成后，车辆要走的路已经确定，接下来就是速度规划：决定车辆在路径上每个时刻该以多快的速度前进。

速度规划的第一步是构建ST图。ST图的横轴是未来时间（比如未来8秒），纵轴是沿参考线的纵向距离s。所有会影响纵向运动的对象都要投影到这个图上：

• 动态障碍物：根据预测模块给出的未来轨迹，计算它未来每个时刻占据的纵向区间，在ST图上画出对应的时间-位置区域。
• 静止障碍物：在ST图上表现为一段纵向位置区间在所有时间内都存在。
• 停止线、人行横道：“虚拟障碍物”，在ST图上表现为从某个时间开始的纵向位置约束。
• 限速区域：不是障碍区域，而是速度上限约束。

ST图的质量直接决定速度规划的效果。如果预测轨迹不准确，ST图上画出的障碍物区域就会偏移，速度规划给出的结果自然也是错的。这也是为什么EM Planner对预测模块的依赖如此之强。

4.2 DP速度决策：先找一条“大势正确”的粗略速度曲线

与路径规划一样，速度规划也分DP和QP两步。

DP速度决策的思路是：在时间轴上离散采样（比如每1秒一个时间节点），在每个时间节点上采样若干个速度候选值（或者纵向位置候选值），然后用动态规划搜索一条累计代价最低的s(t)曲线。

代价函数通常包含：

• 与障碍物的时空距离代价：如果与障碍物区域的时间差或空间差过小，代价急剧上升。
• 加速度代价：加速度越大，舒适性越差。
• 加加速度代价：jerk越大，体感越差。
• 参考速度偏差代价：偏离道路限速或期望行驶速度时，代价增加。
• 停车必要性代价：在必须停车的位置（如红灯前、行人前），应尽快将速度降下来。

DP输出的速度曲线是离散、粗糙的，但它的“轨迹形态”已经很接近合理速度曲线了：什么时候需要减速、什么时候可以加速、什么时候需要停车，这个决策已经定型。

4.3 QP速度优化：把决策变成可执行的光滑速度曲线

DP速度决策得到的曲线存在加速度跳变，直接执行会对乘坐舒适性造成很大影响。所以EM Planner再用QP对速度曲线做平滑优化。

QP速度优化的目标函数通常包含：

• 最小化加速度变化率（jerk），这是舒适性的关键。
• 最小化与DP速度曲线的偏差，保证决策意图不丢失。
• 最小化加速度，减少冲击感。

约束条件包括：

• 速度上下限：不能超过限速、不能倒车。
• 加速度上下限：受油门/刹车能力限制。
• 加加速度上下限：受执行器响应和舒适性限制。
• 位置约束：不能进入ST图上的障碍物时空区域。

QP速度优化输出的是每个规划时刻的s、s_dot（速度）、s_ddot（加速度），再配合路径规划输出的横向偏移，就能合成完整的二维轨迹。

4.4 速度规划里最典型的失败案例：跟车急刹时jerk爆炸

我在仿真里调试过很多次这样的场景：主车以80公里时速跟车，前车突然制动，预测模块给出的障碍物区域在ST图上压缩得很紧凑，QP为了避开障碍物区域，会生成一个非常急促的减速曲线，jerk可能达到十几甚至几十米/立方秒，车内人员的感受就是“被猛地拽了一下”。

这个问题光在QP层面解决很难，因为约束已经摆在那里了。我的处理方式是：

• 在DP阶段就加入“最小安全时距”约束，车辆与前车的目标时间距离不要小于2秒，这比1秒时距对预测误差的容忍度高一倍。
• 在ST图上给障碍物区域加时间余量，相当于把减速起点提前。
• 提高QP目标函数里jerk项的权重，让求解器优先保证舒适性。

这三板斧下去，绝大多数急刹场景的舒适性都能改善很多。

5. 从论文到代码：Apollo里的EM Planner究竟长什么样

5.1 代码结构：从Scenario到Planner的调用链

如果你打开Apollo的modules/planning目录，会发现EM Planner并不是一个孤立的类，而是嵌在整个场景调度体系里的。以Apollo 5.0以后的代码结构为例：

• ScenarioManager：负责判断当前场景，比如车道保持、变道、交叉口、人行道等。
• OnLanePlanning：在道路上的统一规划入口，内部调用PublicRoadPlanner。
• PublicRoadPlanner：是EM Planner在公开道路上的具体实现。

核心的路径和速度优化器在不同的Apollo版本里类名略有不同，大致对应关系如下：

整个调用链可以理解为：环境信息进来之后，ScenarioManager先判断场景，再交给PublicRoadPlanner，后者依次调用路径DP、路径QP、速度DP、速度QP四个优化器，最后把结果合成一条带时间戳的轨迹，发布给控制模块。

5.2 核心参数：调哪些参数能改变轨迹行为

很多初学者拿到EM Planner的代码，不知道该动哪些参数。我整理了一份最常用的调参清单，按影响程度排序：

我的建议是：调参不要一上来就动一堆，永远一次只调一个参数，然后跑同一组测试场景做对比。否则出问题时你根本不知道是谁引起的。

5.3 怎么看中间结果：可视化比看代码更高效

调试EM Planner时，光看最终轨迹远远不够。你真正需要的是四个中间结果：

• 路径DP生成的“走廊”长什么样，是否覆盖了安全区域。
• 路径QP平滑后的路径和DP走廊的偏差有多大。
• 速度DP在ST图上选择的区域是否合理。
• 速度QP输出的速度曲线与DP结果的偏差是否在可接受范围。

Apollo的DreamView和Planning可视化插件已经支持输出这些中间结果。如果没有跑仿真环境，我也建议把每个优化器的输出单独打印日志，用离线脚本重放分析。我排查过的最难缠的轨迹问题，最后都是通过对比DP走廊和QP输出才定位到根因的。

6. EM Planner的边界与工程演化方向

6.1 也要知道它的软肋

EM Planner的核心优势是解耦、高效、可解释性很强，但它也有明显的边界，我在实际项目里深有体会。

第一个软肋在于离散化的精度受限。DP在SL图和ST图上的采样步长决定了搜索粒度。采样太粗容易漏掉最优解，采样太细计算量又上去了。面对非常规的障碍物布局（比如斜向停车、不规则施工区域），DP的网格搜索可能找不到一条合理的走廊，导致后续QP无解。

第二个软肋是规划结果对预测模块过度依赖。EM Planner的ST图完全依赖障碍物预测轨迹来构建，预测误差一大，速度规划就跟着出错。目前预测模块通常假设障碍物按恒定速度或恒定加速度运动，真实世界里突然切入、减速让行这类行为很难被准确预判。

第三个软肋是交互博弈能力偏弱。EM Planner是典型的“感知-预测-规划”串行结构，它把所有交通参与者当作“会移动的障碍物”，而不是“有意图的参与者”。在无保护左转、人车混行、拥堵合流这些需要博弈的场景里，串行结构往往表现得僵硬保守。

6.2 工程上如何绕开这些坑

针对这三个软肋，我在工程上摸索出了一些有效的应对方式：

• 针对离散化精度，可以在DP之前先做一次“障碍物密度检测”，在障碍物密集区域自动加密采样，在空旷区域降低采样密度，兼顾效果和效率。
• 针对预测依赖，可以在ST图的障碍物区域基础上，按预测置信度动态外扩。低置信度障碍物的区域扩张时间范围，相当于为预测误差预留缓冲。
• 针对交互博弈，现阶段最务实的做法是引入场景化策略缝合。常规巡航用EM Planner，接近博弈场景（如无保护转弯）时切换到更慢但更能灵活决策的决策状态机，等脱离冲突区域再切回EM Planner。

6.3 我个人的调参与使用习惯

最后分享一些我在实际使用EM Planner过程中积累的经验：

第一，一定要建立场景回归测试集。把典型的高频场景录成bag包，包括高速跟车、城市路口、前车切入、行人横穿。每次改参数、改权重后回放全套bag，对比关键指标——横向偏差、纵向加速度、jerk最大值、是否压线、是否与障碍物过于贴近。没有这套回归测试，改参数约等于盲人摸象。

第二，调参前先把中间结果可视化工具链搭好。我自己会写一个离线脚本，读planning的debug输出，自动画出路径DP走廊、QP路径、ST图、速度曲线四张图。每次调参前后对比这四张图，比只看最终轨迹直观得多。

第三，在DP和QP之间要留出“足够的信任空间”。DP是按离散点搜索的，它给出的走廊本来就偏保守；QP如果过于追求贴合DP走廊，会把一些原本可行的平滑路径也排除掉。我的做法是把QP对DP参考线的跟随权重设得比重适中，允许QP在一些平滑路径更优的地方适度偏离DP走廊，只要不突破安全边界就行。

第四，也是最重要的一点：EM Planner不是“一套参数走天下”的算法。高架高速场景的cost权重、城市拥堵场景的权重、园区的低速场景的权重完全是三个世界。Apollo里按场景配置这些参数是合理的，但不要试图用一套权重去适配所有场景，这不是算法的问题，是人类驾驶的多样性本来就是这么复杂。

第五，如果你在课程设计或科研项目里用EM Planner做基础，千万不要一开始就想着魔改核心优化器。先把标准流程跑通，再针对一个具体问题（比如参考线平滑参数、ST图障碍物外扩策略、速度QP的jerk权重）做一个小改进，用数据证明改进有效，这比大改框架更容易出成果，也更符合工程迭代的节奏。

我记得有一次做高架汇入场景的调参，为了一个犹豫不决的汇入决策，我在仿真里连续跑了两天，最后发现问题的根源不在EM Planner，而在于上游预测给汇入车辆的置信度太高，导致ST图上目标车辆的区域过宽，主车始终找不到汇入间隙。把预测置信度调整和ST图外扩策略统一之后，汇入成功率一下就上来了。那次之后我更确信一件事：在自动驾驶系统里，模块边界上的问题永远比模块内部的问题更值得花时间研究。EM Planner庞大的框架里，真正决定最终轨迹质量的，往往也就是边界上的那几个参数。