企业官网建设流程全解析

Apollo EM Planner 速度规划实战：DP+QP 两阶段算法在 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.0m）

ST 图栅格化参数对比：

提示：障碍物投影时需考虑预测不确定性，通常会在时间维度上对障碍物区域进行膨胀处理，膨胀系数随预测时间增加而增大。

2. DP 粗搜索：动态规划路径生成

动态规划阶段的目标是在离散化的 ST 网格中，找到一条避开所有障碍物的最低成本路径。Apollo 采用分层递进的方式进行搜索：

DP 核心流程：

1. 状态采样：在时间维度上等间隔采样（如 0.5 秒）
2. 节点扩展：每个状态节点向下一时刻的多个速度状态扩展
3. 代价计算：评估每个扩展路径段的综合成本
4. 路径回溯：从终点反向选择最小成本路径

代价函数组成：

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 约束类型实现：

OSQP 求解器配置：

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_{k=0}^5 c_k t^k ] 边界条件包括：

• 初始状态：位置、速度、加速度
• 末端状态：位置（可选速度、加速度）

多项式拟合代码实现：

PiecewiseJerkSpeedProblem::FitPolynomial( const std::vector<std::pair<double, 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); }

拟合效果评估指标：

1. 最大加速度偏差：应小于 0.5 m/s²
2. 加加速度（Jerk）：应小于 2.0 m/s³
3. 位置误差 RMS：应小于 0.1m
4. 速度误差 RMS：应小于 0.2m/s

5. Apollo 参数调优实战

EM Planner 的性能高度依赖参数配置。以下是关键参数的实际调优经验：

DP 参数配置（apollo/modules/planning/conf/planning_config.pb.txt）：

dp_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 权重
  • 约束冲突：检查障碍物投影准确性
  • 求解失败：放松次要约束或增大求解器迭代次数

场景适配参数表：

实际部署中，建议通过大量场景测试绘制参数灵敏度曲线，找到各权重的帕累托最优前沿。