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速度也用DPQP。每个阶段都有成熟、稳定的求解方案。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坐标系l横向偏移s纵向位移路径规划静态障碍物、低速动态障碍物ST坐标系t时间s纵向位移速度规划动态障碍物、停车线、限速区域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版本里类名略有不同大致对应关系如下功能常见类名作用路径DP决策DpPolyPathOptimizer在SL图上生成粗路径走廊路径QP优化QpSplinePathOptimizer / FemPathOptimizer平滑路径并满足运动学约束速度DP决策DpStSpeedOptimizer在ST图上生成粗速度曲线速度QP优化QpSplineStSpeedOptimizer平滑速度曲线并满足时间约束整个调用链可以理解为环境信息进来之后ScenarioManager先判断场景再交给PublicRoadPlanner后者依次调用路径DP、路径QP、速度DP、速度QP四个优化器最后把结果合成一条带时间戳的轨迹发布给控制模块。5.2 核心参数调哪些参数能改变轨迹行为很多初学者拿到EM Planner的代码不知道该动哪些参数。我整理了一份最常用的调参清单按影响程度排序参数名所在位置作用trajectory_time_lengthgflags配置规划视野长度默认7-8秒调大会增加计算量max_acceleration/max_decelerationplanning_config.pb.txt纵向加速度/减速度上限max_velocityplanning_config.pb.txt当前道路允许的最大速度path_l_scan_beg/end路径采样配置横向采样范围path_l_scan_resolution路径采样配置横向采样步长越细越精准但越耗时障碍物安全距离conf里各cost权重路径和速度规划中的安全余量QP中jerk权重QP目标函数配置舒适性偏好权重越大动作越柔和我的建议是调参不要一上来就动一堆永远一次只调一个参数然后跑同一组测试场景做对比。否则出问题时你根本不知道是谁引起的。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庞大的框架里真正决定最终轨迹质量的往往也就是边界上的那几个参数。