Apollo自动驾驶进阶：ST与ST迭代过程深度解析与优化实践

1次阅读

没有评论

共计 1941 个字符，预计需要花费 5 分钟才能阅读完成。

ST（时空）规划是自动驾驶系统的核心模块之一，负责在动态环境中生成安全、舒适的行驶轨迹。其核心目标是在时间（T）和空间（S）两个维度上，避开障碍物并满足车辆动力学约束，同时保证乘客舒适性。然而，在实际应用中，开发者常遇到以下问题：

规划效率低 ：传统方法在复杂场景下计算耗时高，难以满足实时性要求（如 100ms 内完成规划）。
迭代收敛慢 ：算法易陷入局部最优，尤其在多障碍物交互场景中。
鲁棒性不足 ：对传感器噪声或预测误差敏感，可能导致轨迹突变。

当前主流的 ST 规划算法可分为两类：

基于搜索的算法 （如 A *、Dijkstra）
优点：能保证全局最优性，适合离散化空间。
缺点：计算复杂度高，难以处理连续状态空间。
基于优化的算法 （如二次规划 QP、模型预测控制 MPC）
优点：可灵活处理约束，适合连续空间和动力学模型。
缺点：对初始值敏感，可能收敛到局部最优。

Apollo 采用混合策略：先用搜索算法生成粗解，再用优化算法精细化，兼顾效率与质量。

时空离散化 ：将时间轴（如 0 - 5 秒）和空间轴（如车道中心线）离散化为网格。
障碍物投影 ：将动态障碍物的预测轨迹映射到 ST 图中，标记不可行区域。
代价函数设计 ：综合考量安全性（距离障碍物）、舒适性（加速度 / 加加速度）和效率（行驶时间）。

分层优化 ：先优化路径（S 方向），再优化速度（T 方向）。
热启动 ：用上一帧的解作为当前帧初始值，加速收敛。
自适应步长 ：根据梯度变化动态调整学习率，避免振荡。

以下为简化版的 Python 代码，展示 ST 图搜索的核心逻辑：

def st_planning(st_grid, ego_state, obstacles):
    """
    st_grid: 时空网格（2D 数组，0= 可行，1= 障碍）ego_state: 车辆初始状态 [s, t, v]
    obstacles: 障碍物时空区域列表
    """
    # 初始化开放列表（优先队列）和关闭列表
    open_list = PriorityQueue()
    open_list.put((0, ego_state))  # (代价, 状态)
    closed_list = set()

    while not open_list.empty():
        current_cost, current_state = open_list.get()

        # 到达终点
        if current_state.s >= st_grid.shape[0] - 1:
            return reconstruct_path(current_state)

        # 生成邻域状态（考虑加速度限制）for delta_v in [-1, 0, 1]:  # 离散化速度变化
            next_v = current_state.v + delta_v
            next_s = current_state.s + next_v * TIME_STEP
            next_t = current_state.t + TIME_STEP

            # 检查边界和障碍物
            if (0 <= next_s < st_grid.shape[0] and 
                0 <= next_t < st_grid.shape[1] and 
                st_grid[next_s, next_t] == 0):

                # 计算新代价（示例：距离 + 加速度惩罚）new_cost = current_cost + \
                           abs(delta_v) * ACCEL_PENALTY + \
                           distance_to_goal(next_s)

                # 加入开放列表
                if (next_s, next_t) not in closed_list:
                    open_list.put((new_cost, State(next_s, next_t, next_v)))

    return None  # 无可行路径