1. 动态规划在自动驾驶路径规划中的应用原理
动态规划（Dynamic Programming）作为自动驾驶路径规划的核心算法之一，其本质是通过空间换时间的策略，将复杂问题分解为相互关联的子问题。在Apollo开源框架中，DP路径规划主要解决结构化道路（如高速公路）上的最优路径搜索问题。
1.1 动态规划的核心思想
动态规划算法有效性的关键在于两个特性：
- 最优子结构：全局最优解包含子问题的最优解
- 重叠子问题：不同决策路径会重复访问相同的子问题
以二维网格地图为例，当我们计算从起点(0,0)到终点(2,2)的最短路径时，会反复计算中间节点(1,1)的最优解。通过维护一个DP表格记录每个节点的最小代价，可以将时间复杂度从指数级降低到多项式级。
1.2 自动驾驶中的特殊考量
与传统路径规划不同，自动驾驶场景需要额外考虑：
- 车辆动力学约束：最小转弯半径、最大加速度等
- 道路拓扑结构：车道线、交通规则等语义信息
- 障碍物预测：动态障碍物的运动轨迹预测
Apollo采用SL坐标系（Frenet Frame）将三维路径规划问题解耦为二维优化问题：
- S轴：沿参考线的纵向距离
- L轴：垂直于参考线的横向偏移
这种表示方法显著降低了问题复杂度，使DP算法能在实时性要求下运行。
2. DP路径规划的完整实现流程
2.1 环境建模与代价函数设计
在Apollo的实现中，路径规划首先需要构建量化的搜索空间：
python复制class DPRoadGraph:
def __init__(self, reference_line, obstacles):
self.s_resolution = 1.0 # 纵向分辨率(米)
self.l_resolution = 0.5 # 横向分辨率(米)
self.s_samples = int(reference_line.length / self.s_resolution)
self.l_samples = 5 # 每纵向位置考虑的横向位置数
self.cost_table = np.full((self.s_samples, self.l_samples), float('inf'))
self.obstacles = self._preprocess_obstacles(obstacles)
代价函数通常包含多个加权项：
- 横向偏移代价（保持车道中心）
- 曲率代价（平滑度）
- 障碍物距离代价
- 速度匹配代价
python复制def calculate_cost(self, s, l, prev_s, prev_l):
lateral_cost = self._calc_lateral_cost(l)
curvature_cost = self._calc_curvature_cost(s, l, prev_s, prev_l)
obstacle_cost = self._calc_obstacle_cost(s, l)
return 0.3*lateral_cost + 0.4*curvature_cost + 0.3*obstacle_cost
2.2 状态转移与回溯搜索
Apollo采用分层递推的方式填充DP表格：
python复制def run_dp(self):
# 初始化起点
self.cost_table[0][self.l_samples//2] = 0
# 前向递推
for s in range(1, self.s_samples):
for l in range(self.l_samples):
min_cost = float('inf')
# 遍历前一状态所有可能位置
for prev_l in range(self.l_samples):
cost = self.cost_table[s-1][prev_l] + \
self.calculate_cost(s, l, s-1, prev_l)
if cost < min_cost:
min_cost = cost
self.predecessor[s][l] = prev_l
self.cost_table[s][l] = min_cost
# 回溯最优路径
path = []
current_l = np.argmin(self.cost_table[-1])
for s in reversed(range(self.s_samples)):
path.append((s * self.s_resolution,
(current_l - self.l_samples//2) * self.l_resolution))
current_l = self.predecessor[s][current_l]
return path[::-1]
关键实现细节：Apollo在实际应用中会使用五次多项式连接相邻DP节点，确保生成的路径满足车辆运动学约束。
3. 速度规划的动态规划实现
3.1 ST图表示与问题建模
速度规划在Apollo中被建模为ST图（时间-距离图）上的优化问题：
- 横轴：时间（Time）
- 纵轴：沿路径的距离（Distance）
障碍物在ST图中表现为禁行区域，规划目标是在满足动力学约束条件下，找到从起点到终点的最优速度曲线。
python复制class SpeedDP:
def __init__(self, path, obstacles, max_speed):
self.t_resolution = 0.1 # 时间分辨率(秒)
self.s_resolution = 0.1 # 距离分辨率(米)
self.max_accel = 2.0 # 最大加速度(m/s^2)
self.max_decel = -3.0 # 最大减速度(m/s^2)
self.obstacles = self._project_obstacles(path, obstacles)
3.2 多目标代价函数设计
速度规划的代价函数需要考虑：
- 行程时间（尽快到达）
- 舒适性（加速度平滑）
- 安全性（与障碍物距离）
- 能耗效率（匀速最优）
python复制def calc_speed_cost(self, s, t, prev_s, prev_t):
dt = t - prev_t
ds = s - prev_s
speed = ds / dt if dt > 0 else 0
# 加速度计算
prev_speed = (prev_s - self._get_prev_prev_s(prev_s, prev_t)) / dt
accel = (speed - prev_speed) / dt
# 各代价项
time_cost = dt * 0.1
jerk_cost = abs((accel - self._get_prev_accel(prev_s, prev_t)) / dt) * 0.3
obstacle_cost = self._calc_st_obstacle_cost(s, t) * 0.6
return time_cost + jerk_cost + obstacle_cost
3.3 动态规划求解过程
速度DP的实现与路径DP类似，但需要考虑更多物理约束：
python复制def solve(self):
# 初始化
self.cost_table[0][0] = 0
# 递推填充
for t_idx in range(1, self.t_samples):
for s_idx in range(self.s_samples):
min_cost = float('inf')
# 遍历可能的前一状态（考虑加速度限制）
min_prev_s = max(0, s_idx - self._max_s_change(t_idx, True))
max_prev_s = min(self.s_samples-1, s_idx - self._max_s_change(t_idx, False))
for prev_s_idx in range(min_prev_s, max_prev_s+1):
cost = self.cost_table[t_idx-1][prev_s_idx] + \
self.calc_speed_cost(s_idx, t_idx, prev_s_idx, t_idx-1)
if cost < min_cost:
min_cost = cost
self.predecessor[t_idx][s_idx] = prev_s_idx
self.cost_table[t_idx][s_idx] = min_cost
# 回溯最优速度曲线
speed_profile = []
current_s = np.argmin(self.cost_table[-1])
for t in reversed(range(self.t_samples)):
speed_profile.append((t * self.t_resolution,
current_s * self.s_resolution))
current_s = self.predecessor[t][current_s]
return speed_profile[::-1]
4. 工程实践中的关键问题与解决方案
4.1 计算效率优化
原始DP算法的时间复杂度为O(n²)，在长距离规划时可能无法满足实时性要求。Apollo采用以下优化策略：
-
分层规划：
- 第一层：粗分辨率全局规划（100米级别）
- 第二层：细分辨率局部规划（10米级别）
-
启发式剪枝：
python复制def heuristic_pruning(self, s, l):
# 剔除明显偏离最优路径的节点
min_cost_in_column = np.min(self.cost_table[s])
return self.cost_table[s][l] > min_cost_in_column + self.pruning_threshold
- 并行计算：利用GPU加速DP表格的填充过程
4.2 局部最优与震荡问题
DP算法容易陷入局部最优，特别是在复杂场景中。常见解决方案包括：
- 多起点策略：从多个候选起点开始规划，选择全局最优
- 记忆化搜索：记录历史规划结果，避免短时间内的决策震荡
- 后优化处理：对DP结果进行二次平滑优化
python复制def post_smoothing(self, path):
# 使用二次规划对DP路径进行平滑
qp_solver = QPSolver()
smoothed_path = qp_solver.solve(
objective=PathSmoothnessObjective(),
constraints=[
PathDeviationConstraint(max_deviation=0.5),
CurvatureConstraint(max_curvature=0.1)
],
init_solution=path
)
return smoothed_path
4.3 实际部署注意事项
-
参数调优经验：
- 横向分辨率通常设置为车道宽度的1/5
- 时间分辨率与控制系统周期保持一致（通常0.1秒）
- 代价权重需要根据车型和场景调整
-
实时性保障：
- 设置最大规划时长（如100ms超时）
- 异常情况下回退到保守策略
-
与预测模块的协同：
- 动态障碍物需要投影到SL和ST空间
- 考虑预测不确定性带来的风险代价
python复制def handle_dynamic_obstacles(self, predictions):
for obj in predictions:
# 计算障碍物占据的SL区域
sl_polygon = self._convert_to_sl_polygon(obj.trajectory)
# 添加安全余量
buffered_polygon = sl_polygon.buffer(self.safety_margin)
self.dynamic_obstacles.append({
'polygon': buffered_polygon,
'probability': obj.probability
})
在真实车辆部署时，我们发现DP算法对参数敏感性较高，需要通过大量实车测试来验证不同场景下的表现。特别是在复杂路口场景中，需要结合语义信息（如交通灯状态）动态调整代价函数。