城市复杂路况下的轨迹规划实战：EM Planner深度调优与场景化应用

如果你正在为自动驾驶车辆在十字路口犹豫不决、在拥堵车流中跟车顿挫、或者面对突然切入的车辆时规划轨迹不够丝滑而头疼，那么这篇文章或许能给你一些不一样的思路。我们不再重复那些教科书式的算法原理，而是直接切入实战，聊聊如何让Apollo EM Planner在真实的城市复杂场景中真正“聪明”起来。对于规控算法开发者而言，理解算法框架只是第一步，更重要的是掌握在不同场景下调整其“行为”的钥匙——那些隐藏在配置文件中的参数，以及如何根据具体路况动态调整策略。今天，我们就以EM Planner为核心，对比Lattice、Hybrid A*等常见规划器的特性，深入探讨如何针对典型城市场景进行精细化调优。

1. 规划器选型：为何EM Planner更适合动态城市环境？

在开始调参之前，我们必须清楚手中的工具适合解决什么问题。城市自动驾驶场景的核心挑战在于高动态性和强交互性。静态的障碍物避让相对简单，难的是预测其他交通参与者的意图，并做出既安全又高效的决策。

Lattice Planner通过采样一组预定义的轨迹库，并从中选择成本最低的一条。它的优势在于计算速度快，结构清晰。

# 简化的Lattice轨迹采样示例（概念性代码）
def sample_lattice_trajectories(current_state, target_state):
    trajectories = []
    for lateral_offset in [-0.5, 0.0, 0.5]:  # 横向偏移采样
        for time_horizon in [2.0, 4.0, 6.0]:  # 时间终点采样
            traj = generate_quintic_polynomial_trajectory(
                current_state, 
                target_state.with_lateral_offset(lateral_offset),
                time_horizon
            )
            trajectories.append(traj)
    return trajectories

然而，在十字路口这种需要精确空间拓扑选择的场景，有限的采样可能错过最优解，或者因为采样粒度问题导致轨迹不够平滑。Hybrid A*则擅长在复杂的、非结构化的静态环境中（比如狭窄停车场）搜索出一条可行的路径，但它对动态障碍物的处理能力较弱，且计算量较大，难以满足城市道路高频重规划的需求。

EM Planner采用了路径-速度解耦迭代优化的思路。它将问题拆解：先在Frenet坐标的S-L平面（纵向-横向）规划一条空间路径，解决“往哪走”的问题；再在S-T平面（纵向-时间）规划速度曲线，解决“何时走”的问题。两者迭代，直至收敛。这种解耦大幅降低了问题的维度，使得它既能像Lattice一样快速响应，又能像优化器一样产出高质量的平滑轨迹，尤其擅长处理动态障碍物的时空交互。

注意：选择规划器不是非此即彼。在一些架构中，可以分层使用，例如用Hybrid A*做全局粗糙路径搜索，再用EM Planner进行局部精细轨迹优化。

为了更直观地对比，我们来看一个关键场景下的表现差异：

场景特征	Lattice Planner 表现	Hybrid A* Planner 表现	EM Planner 表现
十字路口无保护左转	轨迹库可能缺乏恰好匹配的样本，导致决策犹豫或轨迹不自然；对动态车流预测的融合较生硬。	搜索空间巨大，实时性差；难以将动态车辆预测有效转化为搜索代价。	优势明显：E-Step可规划出贴合转弯几何的路径，M-Step能精确计算与对向车流的时空间隙，做出加速通过或减速让行的决策。
密集车流跟车	跟车距离控制可能因采样离散而呈阶梯状变化，舒适性一般。	不适用，主要用于静态路径搜索。	可通过ST图精确规划速度曲线，实现平滑的加速度控制，跟车距离稳定，且能提前预判前车减速而柔和制动。
车辆近距离切入	重规划可能触发轨迹突变（切换到另一个差异很大的采样轨迹），导致车辆急刹或猛打方向。	重规划耗时长，无法满足实时避障要求。	E-Step和M-Step可快速迭代，通过微调横向路径或调整速度曲线来规避，轨迹变化更连续、舒适。
复杂几何道路（如环岛）	依赖于参考线质量，若参考线曲率不连续，生成的轨迹可能不平滑。	能搜索出通过性路径，但路径可能不够平滑，且不考虑动力学约束，需要后处理。	Frenet坐标系天然贴合道路，E-Step的QP优化能保证高阶平滑（C2连续），输出可直接用于控制的轨迹。

这张表清晰地告诉我们，对于以动态交互为核心的城市道路，EM Planner在平衡实时性、平滑性、动态避障能力方面具有综合优势。但这并不意味着它开箱即用，其性能高度依赖于参数配置和对场景的理解。

2. 深入EM Planner内核：关键参数场景化调优指南

EM Planner的强大，源于其可配置的代价函数和约束条件。调优的本质，是调整算法在不同场景下的“价值观”和“行为边界”。我们结合Apollo开源代码中的一些关键配置，来谈谈实战经验。

2.1 E-Step（路径优化）调优：塑造车辆的“空间偏好”

E-Step首先用动态规划（DP）搜索一条粗略的路径，再用二次规划（QP）进行平滑。DP的代价函数权重，直接决定了车辆是更“激进”还是更“保守”。

• 横向偏移代价 (weight_lateral_offset)：这个权重控制车辆对车道中心的“黏性”。在高速巡航时，可以适当提高，让车辆稳稳居中。但在拥堵跟车时，如果前车稍微压线，你或许希望车辆能稍微横向偏移以获取更好的视野，这时可以略微降低该权重，或引入对前车轨迹的跟随代价。
• 障碍物距离代价 (weight_lateral_obstacle_distance)：这是安全的核心。权重必须足够高以确保避障。但有个技巧：这个代价函数通常不是简单的距离倒数，而是设计为分段函数。例如，在安全距离外代价很小，进入警戒距离后代价急剧上升。调整这个分段函数的阈值，比单纯调整权重更有效。在城市混合交通中，对于行人、自行车，这个警戒距离应该设置得比普通车辆更大。
• 平滑性代价 (weight_lateral_derivative 等)：它控制路径的曲率及其变化率（类似方向盘转角和转速）。十字路口转弯时，需要权衡平滑性与对转弯几何的贴合度。权重太低，路径可能贴合几何但曲率突变；权重太高，路径可能过于平滑而偏离理想转弯路径，甚至切到对向车道。我的经验是，在转弯前提前（例如距离路口20米）适度降低平滑性权重，让DP更自由地搜索，进入弯心后再恢复高权重以保证舒适性。

// Apollo配置示例 (modules/planning/conf/planning_config.pb.txt)
task_config: {
  task_type: PATH_BORROW_DECIDER
  dp_poly_path_config: {
    lateral_offset_cost: 1.0          // 横向偏移代价权重
    lateral_obstacle_cost: 10.0       // 横向障碍物代价权重
    lateral_derivative_cost: 100.0    // 横向一阶导（偏向）代价
    lateral_second_order_derivative_cost: 1000.0 // 横向二阶导（曲率）代价
    // 障碍物距离代价分段函数配置
    obstacle_cost_param: {
      buffer: 0.5    // 安全缓冲距离
      weight: 10.0
    }
  }
}

DP之后，QP会在DP路径生成的“走廊”内进行平滑。这里的约束是关键：

• 走廊边界 (boundary_buffer)：DP路径向左右扩展的宽度。在狭窄路段，这个缓冲区必须收窄，否则QP可能无解。但在高速场景，为了应对不确定性，缓冲区可以适当加宽。
• 曲率约束 (curvature_constraint)：必须与车辆动力学匹配。乘用车的最大曲率通常对应最小转弯半径。在调参时，务必确保这个值不超过车辆物理极限，并留有余量。

2.2 M-Step（速度优化）调优：定义车辆的“时间性格”

如果说E-Step决定了车辆的行驶路线，那么M-Step就决定了它在这条线上是“老司机”还是“新手”。

• 时间代价 (weight_time) vs. 舒适度代价 (weight_acceleration, weight_jerk)：这是一对核心矛盾。提高时间代价权重，车辆会更倾向于快速到达目的地，可能带来更频繁的急加速和急刹车（高jerk）。在城市拥堵跟车场景，应显著提高舒适度权重（尤其是jerk），降低时间权重，让跟车动作如行云流水。而在高速巡航且车流稀疏时，可以适当提高时间权重以提升效率。
• 效率代价 (weight_velocity)：鼓励车辆接近期望速度。但这个期望速度不应该是固定的地图限速。在下匝道或前方有慢车的场景，应该由行为决策层动态提供一个降低的期望速度，引导M-Step提前平顺减速，而不是等到很近时急刹。
• ST图上的障碍物投影与处理：这是EM Planner处理动态障碍物的精髓。M-Step会将障碍物投影到S-T图上，形成“时空禁区”。这里的关键参数是障碍物的膨胀大小和预测轨迹的不确定性处理。对于预测轨迹不确定性高的目标（如突然减速的车辆），应该沿时间轴扩大其禁区。在代码中，这通常体现在对障碍物ST多边形的膨胀逻辑上。

提示：不要孤立地调整E-Step或M-Step。例如，在路口遇到对向左转车辆，如果只调整M-Step减速，可能一直等不到通行窗口。此时需要联动：当M-Step发现长时间等待（超过阈值）时，应反馈给E-Step，触发一次重新规划，或许就能找到一条绕行（如稍微右偏）的路径。

3. 典型城市场景的解决方案与避坑指南

理论结合实践，我们剖析几个让算法开发者夜不能寐的典型场景。

3.1 十字路口无保护左转

这是城市驾驶的“终极测试”之一。难点在于：1) 几何路径复杂；2) 与对向直行车流存在时空冲突；3) 需要明确的“博弈”或“礼让”决策。

• EM Planner策略：

  1. E-Step生成多条候选路径：不仅仅是一条，可以基于不同的通过意图（激进、保守）生成2-3条路径，例如一条贴近中心线的标准左转路径，一条更靠右以预留更大安全空间的路径。
  2. M-Step进行时空评估：将每一条候选路径与对向车流的预测轨迹进行ST投影。计算每条路径的“可通过时间窗口”。
  3. 决策与选择：如果存在安全的时间窗口（且加速度/jerk在舒适范围内），选择最有效（时间代价低）的路径-速度组合，果断通过。如果安全窗口很小或不存在，则选择“等待”策略，即在停止线后规划一条近乎停止的速度曲线，并持续评估。等待超过一定时间后，可触发更“主动”的路径（如探出车头）以寻求合作。

• 调参要点：

  • 增加对向车辆预测轨迹的时间不确定性膨胀，避免过于冒险的穿插。
  • 在代价函数中，为“停车等待”引入一个随时间增长的渐进式惩罚，防止车辆永远等待下去。
  • 调整QP中起点的速度、加速度边界条件，使车辆能从静止状态平稳启动。

3.2 拥堵路段启停跟车

目标是舒适、平稳，减少“油门-刹车”的频繁切换，同时保持合理的安全距离。

• EM Planner策略：M-Step是核心。它不再仅仅看当前时刻的距离，而是在ST图上规划一条平滑的速度曲线，这条曲线会提前预判前车的运动趋势。
• 调参要点：
  • 大幅提高jerk代价权重：这是提升舒适性的最直接手段。限制加加速度，让加速度的变化更柔和。
  • 使用基于时距（Time Headway）的安全距离模型：在ST图中，安全区域的下边界不应是固定距离，而应是 s_lead - (safe_time_headway * v_ego + min_distance)。这样，跟车距离会随自车速度动态变化。
  • 平滑前车状态：直接使用感知模块输出的前车速度可能会有噪声。对前车速度进行低通滤波或状态估计，作为M-Step的输入，可以避免自身速度规划的高频抖动。

# 简化的基于时距的安全距离计算（用于ST图下边界）
def compute_st_lower_bound(ego_s, lead_s, lead_v, time_headway=1.5, min_gap=2.0):
    """
    ego_s: 自车纵向位置
    lead_s: 前车纵向位置
    lead_v: 前车速度（滤波后）
    time_headway: 期望时距，例如1.5秒
    min_gap: 最小静态距离，例如2.0米
    """
    estimated_lead_future_s = lead_s + lead_v * prediction_horizon
    safe_distance = lead_v * time_headway + min_gap
    return estimated_lead_future_s - safe_distance

3.3 旁车紧急切入

这是对系统反应速度和轨迹平滑性的双重考验。

• EM Planner策略：得益于其迭代框架，EM Planner可以快速响应。当感知到旁车有切入意图（如跨线）时：

  1. M-Step首先反应：尝试在现有路径上通过减速来避让。如果减速可以避免碰撞（在ST图上无重叠），则生成一条舒适减速曲线。
  2. E-Step协同调整：如果仅靠减速无法避让（例如切入车距离太近），M-Step会反馈冲突给E-Step。E-Step会在下一个规划周期（通常100ms内）重新规划路径，进行小幅度的横向避让。由于迭代优化，新的路径会与上一周期的路径保持连续性，避免方向盘突变。

• 调参要点：

  • 降低路径变化代价的权重，让E-Step在必要时更愿意横向调整。
  • 在障碍物投影时，对切入车辆使用更激进的膨胀策略（尤其是横向），并考虑其加速度不确定性，提前预留空间。
  • 确保规划器的运行频率足够高（Apollo默认100ms），以保证在紧急情况下能及时更新轨迹。

4. 超越调参：架构思维与性能优化

参数调优是精细活，但有时我们需要从更高维度思考。EM Planner的性能也受限于其输入和整体架构。

4.1 预测模块的质量是上限 规划器再优秀，也依赖于准确的障碍物未来轨迹预测。在十字路口，对行人意图的误判；在跟车时，对前车减速趋势的漏报，都会导致规划器做出错误决策。与其盲目调整规划器参数去“覆盖”预测误差，不如花精力提升预测模块的准确性，或者让规划器能处理预测的不确定性（如多模态预测）。

4.2 参考线生成至关重要 EM Planner在Frenet坐标系下工作，参考线是其“舞台”。一条曲率不连续、跳变的参考线，会让E-Step的QP优化非常困难，导致输出轨迹抖动。确保参考线生成模块（通常基于高清地图和路由）输出足够平滑（至少C2连续）的参考线，是规划稳定的前提。在复杂路口，可以考虑生成多条候选参考线供规划器选择。

4.3 实时性与计算效率 EM Planner的DP搜索和QP优化都是计算密集型。在资源受限的硬件平台上，需要优化：

• 网格分辨率：纵向(delta_s)和横向(delta_l)采样间隔。间隔大，计算快，但可能错过最优解；间隔小，精度高，但计算慢。需要折中。
• 规划长度：规划 horizon 太长会增加计算量，太短则前瞻性不足。可以采用动态规划长度，在高速时加长，在拥堵时缩短。
• 热启动：利用上一周期的优化结果作为本次优化的初始值，可以大幅提升QP的收敛速度。

4.4 与行为决策的接口 EM Planner是一个执行层规划器，它需要清晰的行为指令。例如，“向左换道”、“在停止线前停车”、“跟驰前车”。这些指令应该转化为具体的代价函数权重或约束条件。建立一个丰富、明确的行为决策-规划接口，比让规划器自己“猜”要可靠得多。

最后想说的是，自动驾驶轨迹规划没有“银弹”参数。EM Planner提供了一个强大而灵活的框架，但其最佳性能的发挥，依赖于对场景的深刻理解、对参数作用的精准把握，以及上下游模块的紧密配合。最好的调优方式，是在大量真实路测数据或高质量仿真场景中，反复迭代验证。每次参数的调整，都问问自己：这会让我的车在真实路上开得更像一位经验丰富、安全稳妥的老司机吗？记住，舒适、安全和可预测，永远是城市自动驾驶的核心。