企业官网建设流程全解析

做自动驾驶路径规划这块，EM Planner算是一个绕不开的经典框架。它是目前主流开源自动驾驶方案中比较典型的一套分层解耦式规划算法，核心思路是先靠动态规划把“连续的巨大解空间”压缩成一条大体靠谱的可行路径走廊，然后交给二次规划在这个走廊里做平滑与优化，最终产出一条兼顾安全、舒适和可执行性的轨迹。这篇博客我结合自己用C++落地一套精简版EM Planner的实际经历，把整个算法从原理到实现拆开讲一遍，重点讲清楚动态规划和二次规划到底在干什么、谁先谁后、为什么非要分两段，以及那些平时文档里不太会写的细节和坑。适合刚切入路径规划、或者正准备在项目里实现规划模块的同学收藏阅读。

1. EM Planner的整体思路：为什么要“动态规划+二次规划”两段式？

1.1 路径规划问题的本质与解空间复杂度

先说清楚路径规划到底难在哪。车辆在道路上行驶，横向要避开障碍物跟边界，纵向要保证不撞车、不闯红绿灯、能平稳刹停，整车还要满足最大转向角、最大加速度和加加速度限制。理论上这些条件可以全部写进一个巨大的全局优化问题里，一次性求出最优轨迹，很多学术论文也确实是这么做的。但进入工程落地就会发现问题非常棘手：约束非线性、目标函数非凸、解空间维度高，一个典型的规划周期里解空间可能包含几十个维度，每个维度又需要高分辨率离散化，组合之后的数量级直接爆炸，实时求解根本跑不动。

EM Planner给出的答案是“分层分治”。它不试图一次性解一个巨型问题，而是把整个轨迹规划拆成路径规划和速度规划两个子问题，每个子问题内部再按照“先粗后细”的方式走两遍：先用动态规划（DP）快速找到一个粗糙但物理可行的解，再用二次规划（QP）在这个解的邻域内精细打磨。两层计算都是成熟且数值稳定的算法，工程上能严格控制在几十毫秒的计算周期内。

1.2 解耦思维：路径与速度分开规划，SL与ST坐标系

路径规划和速度规划为什么可以拆开？这背后依赖的是坐标系变换。车辆实际运动是在笛卡尔坐标系（XY）下表达的，但路径和速度的耦合很重：弯道上速度快了就会侧滑，障碍物旁边速度快了又会碰撞。为了解耦，业界常规做法是把坐标转换到Frenet坐标系下。简单理解就是：以参考线（通常是道路中心线或引导线）为基准，纵向位置记作s，表示沿参考线走了多远，横向位置记作l，表示偏离参考线多少。这样“路径”就变成了一条l关于s的曲线，描述车辆怎么偏移；“速度”变成了s关于时间t的曲线，描述车辆沿道路方向怎么加速减速。

这种转换最大的价值在于把原本纠缠在一起的问题拆成了两个独立的一维问题。路径规划只需要回答“在哪个s位置，我应该偏移多少l”，只需要关注障碍物的横向距离、曲率变化这些几何因素；速度规划只需要回答“在哪个时刻，我应该到达哪个s位置”，只需要关注前车的距离、限速、加速度约束。两者通过最终轨迹合并时再做一次几何和时间的同步即可。

1.3 DP与QP的配合逻辑

既然已经拆成了路径和速度两个子问题，为什么每个子问题内部还要再拆一次DP和QP？只用QP不行吗？关键在于非凸约束。路径规划里障碍物会导致可行域中存在大量凹形区域，一个目标函数如果直接把“离障碍物越远越好”写进去，整个问题是非凸的，QP求解器要么收敛到局部极值，要么跑不出结果。而DP擅长在离散化的网格上做全局搜索，它可以通过穷举相邻层的状态来找到一个能绕开障碍物的粗略路径，这个解虽然不够平滑，但大概率位于“正确解附近”的凸可行域里。

于是配合逻辑就很清晰了：DP负责“找路”，通过网格搜索确定路径的拓扑结构，该绕就绕、该让就让，它不关心曲率是否平滑，只关心大方向对不对；QP负责“修路”，以DP结果作为中心线，在它周围划定一个局部通道，然后在这个小范围内做二次规划，最小化曲率变化、横向加速度变化等平滑性指标，同时把硬约束加进去，保证最终路径满足车辆运动学限制。速度规划同理，DP在ST图上给出一个粗略的速度决策（何时巡航、何时跟车、何时刹停），QP再在这个决策附近打磨加速度和加加速度，让乘车体验更平顺。

2. 动态规划阶段：先在栅格里搜索“可行解”

2.1 路径DP：在SL空间里做状态转移

路径DP的核心操作是构建一个沿s方向延伸的离散网格。我做精简版时把规划范围设成前方约80米，每隔0.5米取一个s采样列，也就是160列；每一列上再按横向偏移l采样，范围是[-3.5, 3.5]米，步长0.25米，大概29个候选状态。这样一个网格就定义了DP搜索的所有状态空间，某个格子代表“车辆行驶到s位置时，横向偏移为l”。

DP的流程是逐列递推的。第一列每个点的代价就是初始代价，从第二列开始，对当前列每一个状态点，遍历前一列所有状态点，计算从上一列那个点转移到当前这个点的累计代价。转移代价包含几个核心项：

• 障碍物代价：当前格子中心到障碍物的距离越近，代价越高，进入安全缓冲区内则直接设为无穷大，保证搜索不会穿过障碍物；
• 横向偏移代价：偏离参考线越远，代价越大，让车辆尽量保持在车道中间行驶；
• 横向变化代价：前后两列的l值之差反映横向速度，差得越大代价越高，防止路线左右剧烈抽动；
• 曲率代价：由相邻三个点的几何关系估算曲率，让路径不至于突然拧成一个锐角。

每一列计算时只保留累计代价最小的那个“父状态”指针，这样走到最后一列时，沿着最小代价路径回溯，就能得到一条从起点到终点穿过网格的离散路径点序列。整个过程的时间复杂度是列数乘以每列状态数的平方，160乘29乘29的计算量在C++里是微秒量级，非常轻量。

2.2 速度DP：在ST图里做时间-位置决策

速度规划的DP与路径DP结构上非常相似，但坐标系换成了ST。横轴是时间t（规划未来约8秒，以0.1秒为间隔采样），纵轴是纵向位置s（也就是车辆沿道路走的距离）。障碍物通过路径规划给出的横向位置被投影到ST图上，比如前车以某个速度匀速行驶，它在ST图上对应一条斜线段，斜线段上方区域表示纵向位置会被前车占据，规划轨迹不能穿过这些占据区域。

速度DP的状态就是每个时刻可以选择一个s位置。同样地，对当前时刻的每个s采样点，遍历上一时刻的每个s采样点，计算转移代价。速度DP的代价函数重点在：

• 障碍物代价：当前采样点的s位置落入障碍物占据区域，代价极高；
• 速度代价：期望速度与当前速度的偏差，偏差越大代价越高，引导车辆尽量贴近目标速度巡航；
• 加速度代价：由前后两个时刻的s差得到速度，再由速度差得到加速度，超过一定阈值时代价指数级上升；
• 加加速度代价：相邻加速度的差变化不能太剧烈，保证乘坐舒适性。

速度DP本质上是把“车辆如何随时间前进”的问题离散成一个线性搜索，它在ST图上找出一条连续的、不与障碍物相交的可行路径。跑完之后，同样通过回溯得到时间-位置序列，转换成速度曲线后就能看出车辆在哪个时间段加速、哪个时间段跟车或者刹停。

2.3 DP代价函数的设计与权重调节心得

代价函数是整个DP阶段最消耗精力、也最体现经验的部分。我在调参过程中最大的感受是：各项代价之间的权重严重依赖场景，量纲不统一会导致某个维度被支配。比如横向偏移代价量级是米，动辄1到3，而曲率代价如果直接使用曲率值，量级可能只有0.01到0.1，如果不做归一化，曲率的约束实际上起不到任何作用。我的做法是先对每一项做归一化处理，把它除以该维度可能出现的最大或典型值，再乘上可调节的权重系数。这样每个权重在0到1之间调节时，语义更直观。

另外一点是防止DP结果被障碍物边界“挤压”到不可行区域。单纯的障碍物距离代价会让路径贴着安全边界走，安全冗余不足。我通常会把障碍物代价设计成分段函数：距离大于安全距离时代价为零或极小，进入安全边界内时代价随距离缩短快速上升，这样既不会让远处障碍物过度影响路径选择，又能严格保证近处安全性。DP阶段的核心是给出拓扑正确的决策，而不是追求平滑，所以代价设计的原则应该是“大方向不能错，细节留给QP”。

3. 二次规划阶段：把粗糙轨迹打磨成可用轨迹

3.1 路径QP的目标函数与约束形式

DP给出的路径点虽然能绕过障碍物，但直接拿来用是不行的，点与点之间横向偏移变化往往很突兀，曲率也未必满足车辆转向能力限制。路径QP要解决的就是这个问题。它不再重新探索整条路径，而是以DP结果为基准，构建一个带有限制条件的二次规划模型。

我实现的路径QP变量选择是每个采样点的横向偏移量及其一阶、二阶差分，也就是l值、dl/ds和d2l/ds2。目标函数由几部分组成：当前点与DP结果基准点的偏差平方和、相邻点横向偏移变化率平方和、以及曲率变化率平方和。分别对应三个目标：不能让QP结果离DP结果太远、横向速度不能过大、曲率变化不能过于猛烈。约束条件包括：每个点的l必须落在DP规划的可行走廊边界内（一般以DP点为中线上限偏移0.5到1.0米），横向偏移变化率不超过最大侧向速度对应的值，曲率不超过车辆最小转弯半径对应的值。

写成人人能懂的类比：DP是预算草稿，告诉你这笔钱大概该怎么花，QP是财务审核，在“不超出预算上下浮动范围”的前提下，把每一笔支出安排得尽量均衡平滑。

3.2 速度QP与舒适性优化

速度QP的结构和路径QP几乎对称，但变量换成了每个时刻的s位置、速度v和加速度a。目标函数重点惩罚三项：实际速度与目标速度的偏差、加速度平方、加加速度平方。约束项则包括：加速度上下限、加加速度上下限、速度不超过道路限速、以及每个时刻的s位置不能越过ST图上的障碍物边界。

这里有个实际工程中的细节：QP约束只能表达线性约束，但ST图里的障碍物边界往往是不规则的凸多边形。为了让QP可以处理，常规做法是先把边界离散成一系列线性不等式，或者把ST图中的安全边界做线性近似。实现上，我选择在DP结果给出的安全通道内建立一个“沿时间方向的分段线性边界”，对每一段约束取最大和最小s可到达范围，这样QP求解时速度极快且不容易出现无解情况。速度QP跑完，我们得到的就是一条时间上连续、加加速度受限、完全避障的速度曲线。

3.3 求解器选型与数值稳定注意事项

二次规划求解器是另一个必须认真选择的组件。路径QP的维度通常在300个变量左右，速度QP大概在240个变量左右，这种规模用主流开源求解器都能轻松处理，关键看实时性和数值稳定性。我对比过纯手写梯度下降和调用现成求解器两种方案，结论是别自己造轮子。现成的开源求解器在稀疏矩阵存储、KKT条件求解、非可行解检测上都做得足够成熟，实现成本低且算法收敛可靠。

数值稳定方面有两个容易踩的坑。第一个是变量尺度差异。路径QP里横向偏移量是米，很小，但曲率约束换算成数值时会放大到几十；速度QP里s位置是几十米，而加速度约束只有几，这些量级差会导致矩阵病态。我的解决办法是对变量做归一化预处理：s除以规划长度，l除以横向走廊宽度，v除以最大速度，a除以最大加速度，求解完成后再换算回物理单位。第二个是约束条件的线性无关性。如果两个约束几乎重复，求解器内部会面临退化问题，轻则迭代次数增多，重则直接报错。所以我在生成QP矩阵前会做一次简单的冗余检查，筛掉方向一致且右端项相近的重复约束。

4. 精简版EM Planner的C++实现细节

4.1 整体模块划分与数据流

精简版EM Planner在工程上可以拆成四个清晰模块：参考线处理、路径规划器、速度规划器、轨迹合并器。输入是车辆当前位姿、参考线点集、障碍物列表，输出是一条未来若干秒的离散轨迹点序列。

数据流是流水线式的：参考线处理模块先把输入的点集做平滑和坐标变换，生成Frenet坐标系下的基础参考信息；路径规划器按前面说的DP+QP流程，在SL坐标系下输出一条平滑路径，记录每个s处的l值；速度规划器拿到这条路径后，把障碍物投影到ST图，再经过DP+QP输出每个时刻的s值；轨迹合并器把路径纵坐标s和横向偏移l按时间序列合并，转换回XY坐标，附带每个点的速度和加速度，送给下游控制模块。这种流水线结构的好处是每个模块可以单独调试，路径规划出了问题不会牵累速度规划，给排查带来很大方便。

4.2 核心数据结构与内存复用

实现C++版本时，我一开始把每个模块内部的数据独立管理，调试起来当然直观，但跑下来发现频繁分配和释放vector占用了不少耗时，尤其在嵌入式环境下还会有内存碎片问题。优化之后我引入了统一的内存池，所有网格节点、DP回溯数组、QP矩阵稀疏索引都预分配好最大容量，规划周期内只做覆盖写入。

核心数据结构可以这样定义：

struct FrenetPoint { double s; double l; double dl; double ddl; }; struct SpeedPoint { double t; double s; double v; double a; }; struct TrajectoryPoint { double x; double y; double theta; double v; double a; double relative_time; }; struct DpGrid { // 横向: 离散化索引, 纵向: s方向索引 std::vector<std::vector<double>> cost; std::vector<std::vector<int>> parent; };

注意多段轨迹的连续性问题。轨迹合并时，本周期规划出来的第一个点必须与上一周期执行到的位置对齐，否则控制模块会出现方向盘急打或者急加速减速的跳变。所以每个模块保存一个上一周期的末端状态，并作为当前周期的硬约束输入。

4.3 关键实现细节：网格采样与代价函数计算

网格采样实现时要特别注意边界情况。以路径DP为例，并不是全部八十米内每列都采样同样的横向范围，在近距离处车辆机动能力强，可以多采几个横向候选点；远距离处车辆还到不了，采样范围可以适当收窄。这种非均匀采样可以在保证近处决策质量的同时减少状态数量。代价函数计算时，障碍物需要投影到Frenet坐标系下的各个s位置，投影过程本身可以用分段线性插值实现，因为参考线点已经是稠密的。

下面给一段路径DP转移代价的伪代码结构：

for (int col = 1; col < columns; ++col) { for (int cur = 0; cur < level_states; ++cur) { double best_cost = kInf; int best_prev = -1; for (int prev = 0; prev < level_states; ++prev) { double cost = earlier_cost[prev] + obstacle_cost(cur, col) + lateral_offset_cost(cur) + lateral_change_cost(prev, cur, col) + curvature_cost(prev, cur, col); if (cost < best_cost) { best_cost = cost; best_prev = prev; } } cost[col][cur] = best_cost; parent[col][cur] = best_prev; } }

这里有个易被忽略的性能优化点：障碍物代价在每一列其实只随当前格子的位置变化，与上一状态无关，所以可以提前算好整张二维表，循环里直接查表，少做重复计算。我这版实测下来DP部分只占整个规划周期的百分之十不到，绝大部分时间花在QP求解和坐标变换上。

4.4 实时性优化与多线程设计

实时性直接决定这套算法能不能上车。我最初实现时整个规划周期平均耗时五十毫秒，确实能跑，但在高速场景对这个周期很不满意。后来做了几项优化：一是把QP求解放到独立线程里，因为求解过程不依赖外部输入，只需要DP结果作为基准，完全可以并行；二是把坐标变换和代价函数表计算做成批处理，用循环展开加SIMD指令加速；三是调整QP求解器内部迭代次数上限，在精度允许范围内提前终止收敛。优化之后平均周期降到二十毫秒左右，最坏情况也能稳定在三十毫秒以内，这个数值对大部分L2级别辅助驾驶系统已经足够了。

考虑双周期冗余设计也是必要的。实际运行中容易出现某个周期求解器不收敛或者结果异常跳变，这时候直接使用上一周期完整结果会比临时拼凑一段残缺轨迹安全得多。我在轨迹合并器里做了一层逻辑：如果本周期轨迹校验未通过，就沿用上一周期轨迹末端延伸一个周期的旧轨迹，同时向状态机上报退化标志，让决策层知道当前处于降级运行状态。

5. 实际运行中的常见问题与排查

5.1 轨迹抖动与相邻周期过渡不自然

这个问题在初版调试时几乎是必然出现的。现象是车辆在直线路段上行驶，规划轨迹每个周期都在轻微左右摇摆，虽然不是大幅画龙，但乘车人能明显感觉到横向晃动。查下来原因基本出在DP代价函数的横向偏差权重太小，DP结果在不同周期因为障碍物投影的细微变化，会选择不同的横向偏移通道，QP结果也跟着来回移动。

解决办法分两步：第一步提高横向偏移代价在总代价中的占比，让车辆在没有障碍物压力时尽量贴着参考线走；第二步在QP目标函数里加入与上一周期轨迹的一致性约束，也就是限制本周期轨迹与上一周期轨迹在相同s位置上的l差值上限。这个约束本质上是把时间一致性转化为空间一致性，效果很明显，加完之后抖动大幅下降。还有一个经验是障碍物投影到ST图时要做时间上的低通滤波，障碍物位置预测的小幅跳变不要直接传导给规划结果。

5.2 QP无解与约束冲突的处理

QP无解的原因通常是约束集合本身就不一致，常见场景是前方近距离突然插入一辆车，安全走廊被压缩得很窄，同时车辆又因为速度过快无法在剩余距离内刹停到避开区。理论上这是规划问题本身无解，任何算法都变不出安全轨迹，但工程上必须优雅降级，不能直接崩溃或者输出空轨迹。

我的处理策略是分层次放软约束。首先去掉加加速度和加速度的舒适性约束，改为惩罚项引入目标函数，先保证硬安全约束有解；如果还是无解，就把安全走廊边界从“严格避障”放宽为“靠近障碍物但存在碰撞风险”，同时向上层模块上报紧急制动请求。这套策略本质上是在安全与舒适之间做优先级排序：绝对安全优先，舒适性其次，必须牺牲舒适性时毫不犹豫地牺牲。

5.3 参数标定与场景适配

最后聊参数标定，这是最不引人注意但最影响落地效果的部分。EM Planner里的每一项cost权重和每个约束阈值，几乎没有一个能通过纯理论推导得到最优值，全部依赖在真实场景中反复试车。我踩过的坑是“同一套参数在不同道路场景下表现差异悬殊”，比如高速匝道的曲率约束必须比城市道路更严格，否则车辆在匝道上会频繁触发限速或路径修正。

我的做法是把参数做成场景相关的配置项，用状态机识别当前场景类型（高速、城市、园区、停车等），规划开始时加载对应参数集。场景切换时还要做参数过渡，不能瞬间切换，否则车辆状态会突变。另外每个版本参数调整时在车上必须记录下完整的规划输入输出日志，离线回放并对比轨迹质量，光靠主观感受去调参，效率太低也容易漏掉边界情况。

在实际落地这套精简版EM Planner的过程中，我最大的体会是：动态规划加二次规划的组合并不复杂，真正决定算法上限的是工程实现中那些细节——代价函数的量纲统一、求解器的数值稳定、周期间的连续性约束、参数与场景的适配。这些琐碎的东西没有算法论文里那么高大上，但每一处都直接影响车辆跑在路上到底稳不稳、安全边界到底够不够。如果你正在做相关实现，建议先从静态场景跑通全流程，再逐步加入动态障碍物和复杂道路，最后再花时间做参数标定与极端情形演练。这套路径规划算法本身是成熟的，只有在工程细节上狠狠下功夫，才能把纸上算法变成真正能让人放心乘坐的轨迹。