Apollo EM Planner 速度规划实战DPQP 两阶段算法在 ST 图中的 5 步实现自动驾驶系统中的速度规划模块直接决定了行驶的平顺性和安全性。本文将深入解析 Apollo 开源框架中 EM Planner 的速度规划实现聚焦于动态规划DP与二次规划QP两阶段算法在 ST 图中的应用。通过五个核心步骤的代码级拆解读者将掌握工业级速度规划的实现细节。1. ST 图构建与障碍物投影ST 图Speed-Time Graph是速度规划的核心数据结构其纵轴表示沿参考线的纵向位移s横轴表示时间t。构建 ST 图的第一步是将动态障碍物投影到这个二维空间中。障碍物预测与投影代码示例def project_obstacle_to_st(obstacle_traj, ref_line): st_boundaries [] for t in np.arange(0, 8.0, 0.1): # 时间分辨率0.1秒 obs_s, obs_l ref_line.get_frenet_position( obstacle_traj.position(t)) st_boundaries.append( (t, obs_s - obstacle.length/2, obs_s obstacle.length/2)) return polyfit_st_boundary(st_boundaries)关键参数说明时间分辨率通常取 0.1-0.2 秒障碍物膨胀需考虑车辆安全距离Apollo 默认 0.5m投影精度与参考线采样间隔一致通常 0.1-1.0mST 图栅格化参数对比参数城市道路高速公路单位时间范围8.012.0秒距离范围150300米时间分辨率0.10.2秒距离分辨率0.51.0米提示障碍物投影时需考虑预测不确定性通常会在时间维度上对障碍物区域进行膨胀处理膨胀系数随预测时间增加而增大。2. DP 粗搜索动态规划路径生成动态规划阶段的目标是在离散化的 ST 网格中找到一条避开所有障碍物的最低成本路径。Apollo 采用分层递进的方式进行搜索DP 核心流程状态采样在时间维度上等间隔采样如 0.5 秒节点扩展每个状态节点向下一时刻的多个速度状态扩展代价计算评估每个扩展路径段的综合成本路径回溯从终点反向选择最小成本路径代价函数组成def dp_cost(current_node, next_node): # 障碍物代价 obs_cost 1.0 if in_obstacle(next_node) else 0.0 # 速度代价相对于参考速度 speed_cost (next_node.speed - ref_speed)**2 # 加速度代价 accel_cost ((next_node.speed - current_node.speed) / (next_node.t - current_node.t))**2 # 横向偏移代价考虑换道场景 lateral_cost abs(next_node.l - target_lane_center)**2 return (w_obs * obs_cost w_speed * speed_cost w_accel * accel_cost w_lateral * lateral_cost)DP 搜索参数优化经验速度采样策略采用非均匀采样在参考速度附近增加采样密度障碍物代价权重动态调整对近距离障碍物增大惩罚系数平滑性约束通过限制相邻状态间的最大速度差实现3. QP 问题构建从粗解到精解DP 提供的粗解定义了 QP 问题的可行域。二次规划阶段的目标是在这个凸空间内找到平滑且符合动力学约束的速度曲线。QP 问题数学表述 [ \begin{aligned} \min_{s(t)} \quad \int_0^T w_1(\dddot{s})^2 w_2(\ddot{s})^2 w_3(\dot{s} - v_{ref})^2 dt \ \text{s.t.} \quad s(0) s_0, \dot{s}(0) v_0, \ddot{s}(0) a_0 \ \dot{s}(t) \in [0, v_{max}] \ \ddot{s}(t) \in [a_{min}, a_{max}] \ s(t) \notin \mathcal{O}_i \quad \forall i, t \end{aligned} ]QP 约束类型实现约束类型Apollo 实现方式典型值初始状态等式约束当前实际状态速度限制不等式约束0-20m/s加速度限制不等式约束-4.0~2.0 m/s²障碍物避让线性化约束安全距离0.5mOSQP 求解器配置solver osqp.OSQP() settings { verbose: False, eps_abs: 1e-5, eps_rel: 1e-5, max_iter: 5000, polish: True } solver.setup(P, q, A, l, u, **settings)注意QP 问题的约束线性化是关键步骤Apollo 采用一阶泰勒展开将非线性障碍物约束转化为线性约束每次迭代后更新线性化点。4. 多项式回归与速度平滑获得离散的速度规划点后需通过多项式回归生成平滑的速度曲线。Apollo 采用五次多项式进行拟合五次多项式系数求解 [ s(t) \sum_{k0}^5 c_k t^k ] 边界条件包括初始状态位置、速度、加速度末端状态位置可选速度、加速度多项式拟合代码实现PiecewiseJerkSpeedProblem::FitPolynomial( const std::vectorstd::pairdouble, double st_points) { // 构建最小二乘问题 Eigen::MatrixXd A(st_points.size(), 6); Eigen::VectorXd b(st_points.size()); for (int i 0; i st_points.size(); i) { double t st_points[i].first; A.row(i) 1, t, t*t, t*t*t, t*t*t*t, t*t*t*t*t; b(i) st_points[i].second; } // 添加边界条件约束 Eigen::MatrixXd A_eq(3, 6); A_eq 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0; // 求解带约束的最小二乘问题 return SolveQP(A, b, A_eq, initial_conditions); }拟合效果评估指标最大加速度偏差应小于 0.5 m/s²加加速度Jerk应小于 2.0 m/s³位置误差 RMS应小于 0.1m速度误差 RMS应小于 0.2m/s5. Apollo 参数调优实战EM Planner 的性能高度依赖参数配置。以下是关键参数的实际调优经验DP 参数配置apollo/modules/planning/conf/planning_config.pb.txtdp_st_speed_config { unit_t: 0.5 // 时间分辨率(s) unit_s: 1.0 // 距离分辨率(m) dense_dimension_s: 41 dense_unit_s: 0.5 sparse_unit_s: 1.0 speed_weight: 0.0 accel_weight: 10.0 jerk_weight: 10.0 obstacle_weight: 1.0 reference_weight: 0.0 go_down_buffer: 5.0 go_up_buffer: 5.0 }QP 参数调优技巧权重调整原则高优先级约束如安全使用大权重1e5舒适性相关权重通常在 1e2-1e3 量级效率相关权重在 1e0-1e1 量级典型问题处理震荡现象增大 Jerk 权重约束冲突检查障碍物投影准确性求解失败放松次要约束或增大求解器迭代次数场景适配参数表场景类型speed_weightaccel_weightjerk_weight备注城市跟车1.05.010.0侧重平顺性高速巡航5.03.05.0侧重速度保持紧急避障0.11.01.0侧重安全性路口起步2.010.020.0抑制急加速实际部署中建议通过大量场景测试绘制参数灵敏度曲线找到各权重的帕累托最优前沿。