Apollo EM Planner 路径-速度解耦实战DPQP 两阶段规划 100ms 内完成轨迹生成自动驾驶规划算法是连接感知与控制的桥梁其核心任务是在复杂动态环境中生成安全、舒适且符合车辆动力学的轨迹。百度 Apollo 开源的 EM Planner 作为工业级解决方案通过路径-速度解耦策略和 DPQP 两阶段优化实现了在 100ms 周期内完成高质量轨迹生成。本文将深入解析该框架的工程实现细节并提供可运行的 Python 仿真代码。1. 路径-速度解耦的工程意义在高速行驶场景中传统耦合式规划器面临计算复杂度爆炸的问题。EM Planner 创新性地采用分治策略将三维时空规划问题分解为两个二维优化问题路径规划在 Frenet 坐标系的 SL 维度沿参考线的纵向距离 S 与横向偏移 L进行静态障碍物避让速度规划在 ST 维度时间 T 与路径上的纵向距离 S处理动态障碍物的时空交互这种解耦带来三个显著优势计算效率提升单个 QP 问题的变量数从 O(n³) 降至 O(n²)模块化设计路径和速度模块可独立迭代优化故障隔离单个模块失效不会导致系统完全崩溃# Frenet坐标系转换示例 def cartesian_to_frenet(x, y, ref_line): 将笛卡尔坐标(x,y)转换到参考线ref_line的Frenet坐标(s,l) # 寻找最近参考点 min_idx np.argmin([(x-pt[0])**2 (y-pt[1])**2 for pt in ref_line]) s ref_line.cum_dist[min_idx] # 计算横向偏移 ref_x, ref_y ref_line.points[min_idx] ref_theta ref_line.headings[min_idx] dx, dy x - ref_x, y - ref_y l -dx * np.sin(ref_theta) dy * np.cos(ref_theta) return s, l2. 路径规划从粗解到精修2.1 SL 投影与静态环境构建将动态障碍物投影到参考线是路径规划的关键预处理步骤。通过假设障碍物在规划周期内保持静止将三维问题降维处理对每个障碍物轨迹点进行 Frenet 坐标转换沿参考线建立占用栅格图标记不可通行区域红色和风险区域黄色class StaticObstacleMap: def __init__(self, ref_line, obstacles): self.ref_line ref_line self.samples np.linspace(0, ref_line.length, 500) self.occupancy np.zeros_like(self.samples) # 0free, 1risky, 2blocked def update(self, obstacles): for obs in obstacles: s_list [cartesian_to_frenet(x,y,self.ref_line)[0] for x,y in obs.trajectory] min_s, max_s min(s_list), max(s_list) # 标记阻塞区域 idx (self.samples min_s) (self.samples max_s) self.occupancy[idx] 2 # 标记缓冲区域 idx (self.samples min_s-5) (self.samples max_s5) self.occupancy[idx] np.maximum(self.occupancy[idx], 1)2.2 DP 路径决策Lattice 图搜索动态规划在离散化的 Lattice 图上进行多行点采样和代价评估参数取值说明横向采样间隔0.2-0.5m取决于车道宽度纵向采样间隔5-10m随车速增加代价函数权重[1.0, 0.3, 0.1][障碍物距离, 曲率, 偏移量]def dp_path_planning(static_map): 在SL空间进行动态规划路径搜索 # 构建Lattice图 s_samples np.linspace(0, static_map.ref_line.length, 50) l_samples np.linspace(-2, 2, 5) # 横向采样 # 初始化DP表 dp_cost np.full((len(s_samples), len(l_samples)), np.inf) dp_parent np.full((len(s_samples), len(l_samples)), -1) # 起点初始化假设起点在车道中心 dp_cost[0, len(l_samples)//2] 0 # 前向传播 for i in range(1, len(s_samples)): for j in range(len(l_samples)): # 评估前一列所有节点到当前节点的转移代价 for k in range(len(l_samples)): cost dp_cost[i-1,k] transition_cost(s_samples[i-1], l_samples[k], s_samples[i], l_samples[j]) if cost dp_cost[i,j]: dp_cost[i,j] cost dp_parent[i,j] k # 回溯最优路径 path [] j np.argmin(dp_cost[-1]) for i in range(len(s_samples)-1, -1, -1): path.append((s_samples[i], l_samples[j])) j dp_parent[i,j] return path[::-1]2.3 QP 路径优化凸空间内的精细调整DP 结果提供了障碍物决策和可行隧道QP 在此约束空间内进行平滑优化$$ \begin{aligned} \min_{l(s)} \quad \int \left[ w_1\left(\frac{d^2l}{ds^2}\right)^2 w_2\left(\frac{dl}{ds}\right)^2 w_3(l-l_{ref})^2 \right] ds \ \text{s.t.} \quad l(s) \in \text{可行区域} \ \left|\frac{dl}{ds}\right| \leq \tan(\theta_{\max}) \ \left|\frac{d^2l}{ds^2}\right| \leq \kappa_{\max} \end{aligned} $$def qp_path_optimization(dp_path, static_map): 基于DP粗解进行二次规划优化 # 提取参考线 s [p[0] for p in dp_path] l_ref [p[1] for p in dp_path] # 构建QP问题 n len(s) H np.zeros((3*n, 3*n)) # 最小化jerk、曲率和偏移 f np.zeros(3*n) # 添加平滑性目标 for i in range(1, n-1): # 三阶差分近似jerk H[3*i, 3*i-3:3*i3] [1, -3, 3, -1] # 添加跟踪目标 for i in range(n): H[3*i2, 3*i2] 0.1 # 轻惩罚偏离参考线 # 添加约束 A [] b [] for i in range(n): # 边界约束 if static_map.occupancy[i] 2: # 硬约束避开阻塞区域 A.append(np.eye(3*n)[3*i2]) b.append(0 if l_ref[i] 0 else 1) # 求解QP res solve_qp(H, f, A, b) return res.x[2::3] # 提取优化后的横向偏移3. 速度规划时空博弈的艺术3.1 ST 图构建与动态障碍物投影将动态障碍物预测轨迹映射到 ST 图是速度规划的基础对每个障碍物轨迹计算其在路径上的 S 坐标随时间 T 的变化在 ST 图上绘制障碍物的时空占据区域根据交互类型标记为超车区或跟车区class StGraph: def __init__(self, path, obstacles, t_horizon8.0): self.s_samples np.linspace(0, path[-1][0], 100) self.t_samples np.linspace(0, t_horizon, 50) self.occupancy np.zeros((len(self.t_samples), len(self.s_samples))) # 投影动态障碍物 for obs in obstacles: for t_idx, t in enumerate(self.t_samples): pred_s obs.predict_position_at(t) s, _ cartesian_to_frenet(pred_s.x, pred_s.y, path) s_idx np.argmin(np.abs(self.s_samples - s)) self.occupancy[t_idx, s_idx] 1 # 添加安全边际 safe_dist max(5, 0.5 * ego_speed**2 / 4.0) # 假设4m/s²减速度 min_s max(0, s - safe_dist) max_s min(path[-1][0], s safe_dist) min_idx np.argmin(np.abs(self.s_samples - min_s)) max_idx np.argmin(np.abs(self.s_samples - max_s)) self.occupancy[t_idx, min_idx:max_idx] 13.2 DP 速度决策拓扑选择在 ST 图上进行动态规划搜索核心是定义合理的状态转移代价安全性代价与障碍物的时空距离舒适性代价加速度和加加速度效率代价偏离期望速度的惩罚def dp_speed_planning(st_graph, ref_speed): 在ST空间进行动态规划速度搜索 # 初始化DP表 dp_cost np.full(st_graph.occupancy.shape, np.inf) dp_parent np.full(st_graph.occupancy.shape, -1) # 起点初始化 dp_cost[0,0] 0 # 前向传播 for t_idx in range(1, len(st_graph.t_samples)): for s_idx in range(len(st_graph.s_samples)): if st_graph.occupancy[t_idx, s_idx] 0: continue # 跳过障碍物区域 # 评估可能的前驱状态 for prev_s_idx in range(max(0, s_idx-5), min(len(st_graph.s_samples), s_idx1)): dt st_graph.t_samples[t_idx] - st_graph.t_samples[t_idx-1] ds st_graph.s_samples[s_idx] - st_graph.s_samples[prev_s_idx] v ds / dt a (v - (st_graph.s_samples[prev_s_idx] - st_graph.s_samples[max(0,prev_s_idx-1)])/dt)/dt # 转移代价计算 cost dp_cost[t_idx-1, prev_s_idx] cost 0.1 * (v - ref_speed)**2 # 速度惩罚 cost 0.3 * a**2 # 加速度惩罚 cost 0.01 * (s_idx - prev_s_idx)**2 # 平滑惩罚 if cost dp_cost[t_idx, s_idx]: dp_cost[t_idx, s_idx] cost dp_parent[t_idx, s_idx] prev_s_idx # 回溯最优路径 speed_profile [] s_idx np.argmin(dp_cost[-1]) for t_idx in range(len(st_graph.t_samples)-1, -1, -1): speed_profile.append((st_graph.t_samples[t_idx], st_graph.s_samples[s_idx])) s_idx dp_parent[t_idx, s_idx] return speed_profile[::-1]3.3 QP 速度优化动力学可行解基于 DP 提供的拓扑结构QP 在凸可行区域内进行精细优化$$ \begin{aligned} \min_{s(t)} \quad \int \left[ w_1\left(\frac{d^3s}{dt^3}\right)^2 w_2\left(\frac{ds}{dt} - v_{ref}\right)^2 \right] dt \ \text{s.t.} \quad \frac{ds}{dt} \in [0, v_{\max}] \ \frac{d^2s}{dt^2} \in [a_{\min}, a_{\max}] \ s(t) \notin \text{障碍物区域} \end{aligned} $$def qp_speed_optimization(dp_speed, st_graph): 基于DP速度剖面进行二次规划优化 t [p[0] for p in dp_speed] s_ref [p[1] for p in dp_speed] n len(t) # 构建QP问题 H np.zeros((3*n, 3*n)) # 最小化jerk和速度偏差 f np.zeros(3*n) # 添加平滑性目标 for i in range(1, n-1): # 三阶差分近似jerk H[3*i, 3*i-3:3*i3] [1, -3, 3, -1] # 添加速度跟踪目标 for i in range(n): H[3*i1, 3*i1] 0.1 # 速度项 # 添加约束 A [] lb [] ub [] # 速度约束 for i in range(n): # 0 v v_max A.append(np.eye(3*n)[3*i1]) lb.append(0) ub.append(30.0) # 假设限速30m/s # a_min a a_max if i 0: dt t[i] - t[i-1] a_row np.zeros(3*n) a_row[3*i1] 1/dt a_row[3*(i-1)1] -1/dt A.append(a_row) lb.append(-3.0) # 最大减速度3m/s² ub.append(2.0) # 最大加速度2m/s² # 求解QP res solve_qp(H, f, A, lb, ub) s_opt res.x[::3] v_opt res.x[1::3] return list(zip(t, s_opt, v_opt))4. 轨迹生成与性能优化4.1 路径-速度合成将优化后的路径和速度剖面融合为时空轨迹沿路径插值获取每个 s 对应的 (x,y) 坐标根据速度剖面计算每个时间点 t 的 s 值组合生成 (t, x, y, θ, v, a) 的轨迹序列def generate_trajectory(path, speed_profile): 合成最终轨迹 # 路径插值器 s_path [p[0] for p in path] x_path [p[1] for p in path] y_path [p[2] for p in path] x_interp interp1d(s_path, x_path, kindcubic) y_interp interp1d(s_path, y_path, kindcubic) # 速度插值器 t_speed [p[0] for p in speed_profile] s_speed [p[1] for p in speed_profile] s_interp interp1d(t_speed, s_speed, kindcubic) # 生成轨迹点 trajectory [] for t in np.linspace(0, t_speed[-1], 100): s float(s_interp(t)) x float(x_interp(s)) y float(y_interp(s)) # 计算航向角使用中心差分 ds 0.1 x1, y1 x_interp(sds), y_interp(sds) theta np.arctan2(y1-y, x1-x) # 计算速度s对t的导数 v float(s_interp(t, 1)) trajectory.append((t, x, y, theta, v)) return trajectory4.2 实时性保障技巧在 Apollo 的实际实现中采用以下方法确保 100ms 时限热启动使用上一帧的规划结果作为当前帧优化初值并行计算路径 DP 和速度 DP 可并行执行增量更新仅对受环境变化影响的部分重新规划分层精度远场区域使用粗粒度采样提示在 Python 原型中可通过 Numba JIT 编译器加速数值计算部分典型情况下可将 DP 搜索时间从 50ms 降至 5ms。5. 前沿改进方向工业界对 EM Planner 的改进主要集中在三个方面动态障碍物交互引入博弈论模型处理变道协商使用强化学习优化代价函数权重计算效率提升基于 GPU 的并行 DP 实现神经网络替代 QP 求解器舒适性优化考虑乘客晕动症的加加速度约束路面不平度融合的轨迹平滑# 强化学习优化的代价函数示例 class RL_Cost(nn.Module): def __init__(self): super().__init__() self.net nn.Sequential( nn.Linear(6, 32), # 输入障碍物距离、速度差、曲率等 nn.ReLU(), nn.Linear(32, 1) ) def forward(self, state): return self.net(state) # 输出代价估计