做自动驾驶规划绕不开Apollo绕不开EM Planner。不管你是刚入门的学生还是在企业里做量产项目的工程师只要翻过Apollo规划模块的源码大概率都会对着em_planner这个目录发过怵这名字是什么意思为什么路径和速度要分开算DP和QP接在一起解决什么问题这篇文章就专门把它讲透。我会从算法思想、数学建模、代码落地到工程调试逐层拆解尽量让一个刚接触规划的人也能顺着这条线把Apollo EM Planner的骨架和血肉都理解清楚。先说结论EM Planner的核心思路是把“轨迹规划”这个非凸、高维、多约束的复杂问题拆成“路径规划横向”和“速度规划纵向”两个相对独立的问题然后用动态规划做粗解、二次规划做精解再通过“横向-纵向交替优化”的方式逼近全局最优。这个思路本身并不复杂但工程实现里全是细节踩坑踩到怀疑人生也是常有的事。下面我们一个一个环节来说。1. 为什么是EM算法思想与问题拆解逻辑1.1 轨迹规划到底难在哪先看一个简单的场景自车以15m/s巡航前方100米处出现一个静止障碍物旁边车道没车。人要开很简单——打灯、变道、加速或减速通过。但让机器自己算问题立刻变得棘手路径和速度是耦合的。你既要决定往左还是往右绕还要决定是减速绕还是加速冲过去。横向偏移量、纵向速度、时间三者的变化必须同时协调。道路环境是非凸的。道路上有多个障碍物时可行区域往往是几个不连通的空间普通的凸优化工具无法直接求解。约束数量极多。动力学约束、曲率约束、限速、交通规则、障碍物距离余量、舒适性指标动辄上百条。如果一上来就把路径和速度联合求解这个问题在数学上是个非凸、高维、多目标的最优化问题直接硬解根本解不动尤其是放在车端那个实时性要求极高的控制器里更是天方夜谭。EM Planner的聪明之处就是把“一步到位”拆成了“分步逼近”。1.2 用“期望最大化”的思维看懂EMEMExpectation-Maximization原本是统计学里用来求解含隐变量概率模型的迭代算法——E步先给定参数求隐变量期望M步再在隐变量已知条件下更新参数反复迭代收敛。Apollo把这个思路借到了轨迹规划里E步Expectation固定当前的速度决策在SL坐标系下做路径规划找到一条最优的横向轨迹M步Maximization固定上一步得到的路径在ST坐标系下做速度规划找到这条路径上最优的纵向运动两层循环交替迭代第一步没有显式的“概率模型”但主循环里还有一层EM规划循环反复做路径规划和速度规划让两者互相逼近行为上就是典型的交替优化。这里要提一句我个人的理解Apollo的EM不是一个严格意义上的期望最大化算法它更多是借用了“两部分隐变量互相迭代收敛”的思想。工程上真正的价值在于——每一步都只解一个“凸的、低维的”子问题解起来快而且问题本身被拆成了好调试的两层出问题的时候很容易定位。1.3 SL坐标系和ST坐标系理解EM的前提条件讲EM Planner之前必须先理解Frenet坐标系下的两个投影平面。SL坐标系S是沿参考线方向的弧长StationL是偏离参考线的横向距离Lateral。这个坐标系用来描述“路径在哪里拐、横向偏移多大”是路径规划的主场。ST坐标系S同样是弧长T是时间Time。这个坐标系用来描述“什么时刻到达哪个里程位置”是速度规划的主场。为什么不在笛卡尔坐标系x-y里直接做因为道路是弯的用x-y坐标写出来的避障约束、车道边界约束全都是一堆隐式方程解起来又慢又绕。而SL和ST坐标把“沿着路走”和“何时走到”彻底解耦约束表达非常直观。EM Planner整个路径和速度迭代优化的框架就是建立在这两个坐标系之上的。2. 路径规划SL坐标系下的DP粗解与QP精解2.1 路径规划的输入与目标路径规划的输入通常包括ReferenceLine参考线由路由模块生成经过平滑处理后是自车行驶的基准线一般为离散点序列障碍物信息感知与预测模块给出的障碍物当前状态和预测轨迹被投影到SL坐标系下每个障碍物都带有一段SLBoundary占据的弧长区间和横向区间道路边界左侧和右侧可行驶区域的边界线同样投影到SL坐标系下自车状态当前的纵向位置s0、横向偏移l0、横摆角、曲率等。路径规划的目标就是在满足避障约束、动力学约束、边界约束的前提下找到一条从当前状态到目标状态的平滑路径。但正因为可行域可能非凸Apollo选择了“DP动态规划定方向QP二次规划做细化”的两段式结构。2.2 第一段DP怎么在采样点里找到“方向”DP路径规划的核心是在SL平面内做离散化搜索。具体做法是沿参考线方向每隔一段距离比如1米取一个采样层在每个采样层上横向偏移一定的范围比如左右各8米步长0.5米形成一张节点网。每个节点就代表一条候选路径可能经过的一个点。DP需要在每个节点上计算代价然后用动态规划递推找到总代价最小的一条路径。代价函数一般包含以下几项障碍物代价路径点离障碍物边界越近代价越高。通常用指数函数或分段线性函数来描述“碰撞风险”和“贴边风险”横向偏移代价离参考线越远的点代越高。这个代价体现了“尽量走在路中间”的偏好同时也能抑制绕路平顺性代价相邻路径点之间的横向变化量包括一阶差和二阶差越大代价越高。二阶差对应加速度变化实际可以理解为控制方向的“平滑度”曲率代价某些实现会把曲率也作为代价项保证查出来的路径是车辆能跟得上的。DP阶段的输出是一条“指方向”的粗略路径。它保证了避障正确、整体走向合理但不够平滑——因为采样步长大、离散化精度低路径点之间存在折线感。如果直接把DP结果交给控制模块方向盘会一抖一抖的谁都坐不住。所以还要有第二段。关于DP采样参数我自己的经验值是这样调的采样层间隔0.5~1.0米横向采样步长0.25~0.5米横向范围±5~8米。范围太窄绕不过大障碍物太宽搜索空间暴涨实时性扛不住。在高速场景我倾向把横向步长放宽到0.5米因为在高速下0.25米的步长差异对舒适性几乎没有感觉但计算量能砍掉近一半。2.3 第二段QP如何把“折线”磨成“平滑曲线”路径QP的输入是DP给出的路径和由它生成的决策边界。这一步不再做全局搜索而是把问题建模成一个凸二次规划问题变量每个离散S点处的横向偏移l(s)以及它的一阶导数l(s)、二阶导数l(s)、三阶导数l(s)视具体实现而定。目标函数是一个典型的代价和[ \min \sum_i \Big( w_1 l_i^2 w_2 l_i^2 w_3 (l_i - l_{ref,i})^2 \Big) ]其中l_i是横向加速度项它和曲率相关控制路径的转向剧烈程度l_i是横向加加速度项控制路径的“柔和度”这项权重调大一点路径会更平滑但也会更“懒”避障会显得迟钝(l_i - l_{ref,i})是偏离DP参考线的代价避免QP解出和DP方向相差太大的路径。约束条件包括边界约束每个s点处的l必须在上下边界内。边界来自道路边界、障碍物膨胀后的边界以及DP路径的邻域连续性约束相邻采样点的l、l、l必须满足连续性保证路径是几何连续且曲率连续的曲率约束l不能超过车辆动力学允许的最大值否则车辆无法跟踪首末状态约束起点必须匹配自车当前状态终点需要满足设定的目标状态比如回归参考线。QP解出来的路径就是一条既避障、又平滑、还符合动力学约束的最终路径。从我的经验看工程上最影响行车舒适性的三个权重参数分别是w2横向加加速度权重、w1横向加速度权重和w3参考线贴合权重。三者需要根据场景动态调整高速公路和园区低速场景参数差异很大直接上同一套参数多半要翻车。2.4 实践中最容易踩的坑路径规划这部分我做过不少测试也趟了不少坑这里挑两个典型的说第一个坑是“QP解出来的路径贴着障碍物边沿”。原因往往出在DP上——DP阶段为了躲避高代价的障碍物选了极端绕行路径导致生成的边界太紧QP在紧边界里优化自然会把路径贴着边界推。解决办法不是去调QP而是要检查DP避障的代价值是不是设置得太激进给DP留出“离障碍物更远”的奖励项不要让它钻边角。第二个坑是“采样间隔和QP离散间隔不匹配导致的轨迹扭曲”。比如DP采样1米、QP采样0.5米边界映射如果没做插值QP在0.5米采样点上的边界会突变。我的习惯是固定一个主采样间隔比如0.5米DP和QP统一使用或者QP在做边界生成时对DP结果做线性插值保证边界连续。3. 速度规划ST坐标系下的决策与优化3.1 路选好了还得决定开多快路径规划解决了“往哪里开”的问题接下来要解决“开多快、什么时候加速、什么时候刹车”的问题。这部分在ST坐标系下进行。要先构造S-T图横轴是时间纵轴是沿参考线方向的弧长位置把预测的障碍物轨迹投影到这张图上就得到了静态/动态障碍物在时空中的占据区域。举个例子前方30米处有一个静止障碍物对应到ST图上就是一条从t0到t∞、s始终等于30的一条竖带宽度是障碍物长度加安全余量。如果前方有一辆以10m/s匀速行驶的前车它在ST图上就是一个从0, 20出发、以10m/s斜率向上延伸的平行四边形斜带。我们的任务是画出一条从0, s0出发的曲线要求这条曲线不穿过任何障碍物带同时满足速度、加速度、加加速度的物理约束。和路径规划一样速度规划也分DP和QP两段。我见过不少人只看QP而忽略DP觉得DP只是给初值其实不对——DP解决的“何时变道避让还是减速跟车”这类决策问题QP只是把决策翻译成平滑的运动曲线决策错了QP救不回来。3.2 速度决策DP如何在ST网格里“闪转腾挪”速度DP的做法和路径DP类似在T轴方向每隔一段离散时间如0.5秒取一层在S轴方向按一定间隔采样形成一个ST网格。每个网格点代表“在t时刻到达s位置”的一种可能。DP从起点向目标方向递推计算每条可用路径的总代价。代价函数通常包括碰撞代价进入障碍物占据区域的路径代设为无限大完全禁止穿越跟车代价如果落在前车轨迹的阴影区即离前车太近代会显著增加加速度代价相邻时刻速度变化即加速度太大代会增大让DP倾向于平缓加减速向目标速度靠拢的代价比如当前道路限速80而规划期望速度是70那么偏离这个期望速度的代就会增加倒车代价城市道路场景尽量避免倒车所以反向运动的代一般设为无穷大或极大。DP输出的结果是一系列(t_i, s_i)离散点把它们做差分就能得到每个时刻的速度曲线。有了这条速度曲线就完成了“速度决策”同时也圈定了后续QP的可行隧洞让QP的搜索范围大大收窄。3.3 速度优化QP如何让速度曲线变得体面DP速度曲线在时间离散和空间采样上不够光滑且往往会有比较大的加速度突变。速度QP的作用就是在DP给出的“允许区域”内平滑出一条兼顾舒适、安全、效率的速度曲线。速度QP的变量通常定义在每个离散时间点上的s、v、a和jerk加加速度。目标函数形如[ \min \sum_i \Big( w_a a_i^2 w_j jerk_i^2 w_v (v_i - v_{ref})^2 \Big) ]约束条件包括速度边界0 ≤ v ≤ v_max限速每个时刻点的速度上限可能不同比如弯道限速低、直道限速高加速度边界a_min ≤ a ≤ a_max对应于刹车的最大减速度和动力的最大加速度加加速度边界jerk_min ≤ jerk ≤ jerk_max这是舒适性的核心约束位置边界由DP决策生成的s上下界约束QP不能跑出DP划定的可行空间终点约束在规划周期的末端自车需要达到某个目标速度或目标位置比如在停止线前刹停。QP解出来的速度曲线和路径曲线做时间对齐后就得到了完整的轨迹轨迹上每个点不仅有位置坐标还有期望速度、期望加速度、期望时间戳控制模块拿这些信息去做方向盘和油门刹车的跟踪就可以了。3.4 速度规划的几个典型问题我在调速度规划时碰到过三个最典型的场景第一前车急刹场景。前车以极短时间从20m/s减速到0ST图上它的轨迹带会突然变得很“陡”。如果DP采样时间间隔太大很可能漏掉骤变区域的可行通道导致QP解出来是一段急刹或直接规划失败。解决方案是把DP的时间采样间隔缩小比如从0.5秒降到0.2秒或者对前车的加速度做保守估计把预测轨迹的信噪比压低一点。第二加塞场景。旁车突然切入本车道ST图上的占据区域变化剧烈。反应慢的DP会在前一帧把路径规划到“完全避让”后一帧又改成“减速跟随”导致速度轨迹前后跳变。我在实战中加了一个“决策平滑”缓冲如果前后两帧的速度曲线速度差超过阈值限制一次只修正一定比例保证输出的速度曲线是连续的。第三DP和QP目标不一致的问题。DP的代价函数里慢速靠边和快速通过可能是等价的但QP的优化方向可能偏向其中一种造成“DP想绕行QP想减速直行”的矛盾。根因是DP决策阶段没有把QP的目标项比如参考速度偏差充分纳入代价我在项目中会把DP代价函数再加上“终速偏差”项让DP在起始阶段就为QP的目标速度做准备。4. 工程落地Apollo代码结构与调试要点4.1 先找到EM Planner的代码入口如果你用的是Apollo开源版本规划模块的核心代码一般位于modules/planning/目录下。EM Planner相关的主要文件夹是modules/planning/planner/em/EM Planner的策略入口包含EMPlanner::Plan()等核心方法modules/planning/tasks/被EM Planner调用的各类子任务比如路径规划的path_reuse_decider、dp_poly_path速度规划的speed_bounds_decider、dp_st_speed、qp_spline_st_speed等modules/planning/common/参考线、轨迹、规划状态等公共数据定义modules/planning/reference_line/参考线生成与平滑相关逻辑。我建议新手刚开始不要直接去追Plan()的大循环而是先从跑通一次EMPlanner::Plan()的完整调用栈开始。方法是在Plan()里加几行日志把路径规划结果、速度规划结果的关键输出打出来看一遍整个数据流。这比在代码里到处反复翻找要高效得多。4.2 常用配置与权重参数在哪改EM Planner的配置通常在modules/planning/conf/planning_config.pb.txt以及各类task对应的proto配置里。调参的关键点有路径采样与QP权重在dp_poly_path和piecewise_jerk_path等task配置中速度DP的代价权重在dp_st_speed配置中速度QP的权重在qp_spline_st_speed或类似task的配置中限速、参考速度在planning_config.pb.txt的default_reference_line_info等字段中。有一点要特别注意改配置和改代码是两条完全不同的调试阶段。初期做参数调优只动conf下的文本文件先跑仿真看效果如果仿真里发现算法本身的行为不对比如DP完全没有规避障碍物再回到代码里看逻辑。不建议一上来就对着代码改权重否则出问题时变量太多根本定位不了。4.3 用仿真和回放数据加速迭代关于环境搭建有几个和热词里提到的方向相关的经验想分享CarSim、NI和VTD这类联合仿真课题本质上都是把Apollo的规划输出接到车辆动力学模型上形成“规划-控制-车辆-场景”闭环。我自己实践下来Apollo和VTD联合仿真时最容易出问题的地方不在规划模块而在坐标系对齐和时间同步。VTD输出的世界坐标系位置、航向角如果和Apollo内部用的参考线坐标系不一致EM Planner的SL投影会直接算错出现轨迹乱飞的现象。如果已经有实车或路测采集的数据包建议优先用cyber_recorder回放数据来调EM Planner。回放数据可以反复暂停、对比不同参数下的表现比每次跑实车节约大量时间。关于数据集网上有很多公开的自动驾驶数据集可以用来做场景提取但要注意Apollo的规划模块对输入数据的质量要求很高。我自己试过把某些公开数据集直接灌进去因为参考线生成、地图投影的方式和采集时的坐标系不一致结果规划出来的轨迹惨不忍睹。想用公开数据集测Apollo规划务必要先做坐标系转换和地图语义对齐。4.4 环境与权限类的部署问题有热词提到“apollo当前环境有namespace缺”“怎么给hfplat账号添加编辑权限”这类问题虽然看起来和算法无关但实际开发中确实很常见。我统一说下经验如果启动Apollo后提示namespace缺失或者module状态异常首选看cyber的中控日志确认是不是cyber进程没有正常启动其次用cyber_monitor检查各channel数据流是否通畅。规划模块对上游topic的依赖很强状态机没跑起来EM Planner是不会有有效输出的。“给账号添加编辑权限”这类问题很多是工作区挂载权限或Docker容器权限配置导致的。最稳妥的方案是保证用户被加入docker用户组项目目录所有者为当前用户再重新构建镜像后进入开发容器不要直接对宿主机系统的系统目录乱改权限。另一个容易被忽略的点Apollo各版本之间依赖差异很大如果仓库是从其他渠道拷贝来的经常会在编译阶段报各种头文件或proto版本不匹配。解决思路是先检查WORKSPACE文件里依赖的版本号再对照官方文档确认当前分支的编译依赖而不是盲目升级或降级系统中的基础库。5. 常见问题与排查经验速查下面这张表把我的实战排查经验和一些常见情况整理在一起给遇到类似问题的朋友一个快速定位的思路现象可能的根因排查方向推荐处理规划出的轨迹抖动明显QP权重设置不合理或参考线抖动先录轨迹看抖动频率是否与参考线刷新频率一致加大w2横向加加速度权重对参考线做更平滑的处理遇到静态障碍物绕不过去障碍物投影到SL边界时膨胀过大检查感知模块输出的障碍物长宽以及规划配置里障碍物膨胀参数调小膨胀半径或为低速场景单独设置更小的安全距离车辆停在原地无规划输出上游感知/预测channel断流用cyber_monitor检查各个topic的帧率和时间戳修复上游数据链路很多情况是时间不同步速度曲线出现尖角ST图上DP决策点过密或加速度约束没生效检查dp_st_speed的离散密度与QP的jerk权重适当增加加速度、jerk权重或限制DP搜索的相邻速度变化量动态避障轨迹频繁左右摆DP/QP对障碍物预测轨迹敏感度过高看预测轨迹的置信度确认是否把不确定性较大的预测当成硬约束降低预测轨迹的置信度权重把不确定区域当作半约束软处理弯道速度过高限速信息未正确绑定到参考线检查地图模块是否给出弯道曲率限速在速度规划的边界生成阶段根据曲率动态计算限速值三个额外的实用技巧一、环境变量和日志级别。调试EM Planner时把日志级别调到DEBUG会输出海量信息建议先用INFO跑场景定位到具体行为异常后再针对那个task打开局部DEBUG避免一上来就被刷屏淹没。二、可视化调试。Apollo自带的DreamView里可以查看SL图、ST图以及路径速度曲线的对比。我平时调试时习惯把DP粗解、QP精解、参考线三条曲线同时显示行为异常一眼就能看出来是哪一段出了问题效率比看纯数据高太多了。三、轨迹平滑的终极兜底方案。如果QP输出仍然不够平滑还可以再加一层后处理——轨迹平滑器如planning/tasks/optimizers/下的平滑模块用样条或多项式对输出轨迹做一次全局平滑。但这只是兜底不能一直靠它掩盖算法问题。我自己一般会把后处理平滑量控制在很小的范围内如果某次平滑量异常大说明前面的代价函数或者约束还需要再调。写在最后EM Planner是一套把“非凸问题拆成凸问题、把非线性问题拆成线性问题”的典型工程方案。它比起端到端网络那种黑盒方法要“笨”不少但胜在可解释、可调试、约束透明这也正是它能在量产项目里长期存活的原因。我早期学Apollo规划时花了很多时间在数学推导上但后来发现真正帮我理解这个算法的其实是跑仿真和实车遇到问题、再回去查代码和数学表达式的过程。建议你也多动手跑几个场景把DP、QP每一层的输入输出都打印出来看一遍很多东西会瞬间通透。后面我会再写一篇关于Lattice Planner和EM Planner的详细对比讲清楚两者各自的适用场景和取舍。如果你正在做自动驾驶规划方向或者准备入门不妨先把EM Planner的路径与速度“两个循环”彻底吃透。我试下来这套基本功几乎所有自动驾驶规划项目都用得上值得把时间花在上面。