Apollo自动驾驶右转场景的决策规划优化方案

1次阅读

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

自动驾驶车辆在路口右转时面临多重技术挑战。右转场景通常涉及复杂的交通参与者交互，包括对向直行车辆、行人过街、自行车穿行等动态障碍物。主要痛点可归纳为：

盲区处理 ：A 柱遮挡导致视觉盲区，需依赖多传感器冗余检测
轨迹平滑性 ：传统规划算法在曲率变化处易产生突变，影响乘坐舒适性
决策时效性 ：需在 100ms 内完成环境感知 - 决策 - 控制的闭环响应
规则合规性 ：不同地区右转交规差异（如红灯是否允许右转）

方案类型	传统方法	Apollo 优化方案
路径生成	固定半径圆弧 + 直线拼接	基于 Frenet 坐标系的五次多项式平滑
障碍物响应	完全停止等待	动态避让走廊 (Dynamic Corridor)
决策框架	有限状态机	分层强化学习 (Hierarchical RL)
控制执行	PID 跟踪	MPC+ 前馈补偿

Apollo 采用前向毫米波雷达、摄像头、激光雷达的异构数据融合：

雷达提供 100m 范围内动态物体相对速度
摄像头实现交通灯检测和行人识别
激光雷达点云用于构建局部高精地图

通过卡尔曼滤波进行时间对齐后，各传感器数据输入到感知模块的统一坐标系中。

右转路径在 Frenet 坐标系下分解为：

# Apollo 中的 Frenet 路径生成代码片段
class FrenetPath:
    def __init__(self):
        self.t = []  # 时间序列
        self.d = []  # 横向偏移
        self.s = []  # 纵向位移

    def to_cartesian(self, reference_line):
        # 将 Frenet 坐标转换回笛卡尔坐标系
        cartesian_points = []
        for i in range(len(self.s)):
            s, d = self.s[i], self.d[i]
            xy = reference_line.get_cartesian(s, d)
            cartesian_points.append(xy)
        return cartesian_points

采用 Quintic 多项式保证曲率连续：

l(s) = a0 + a1*s + a2*s² + a3*s³ + a4*s⁴ + a5*s⁵

通过约束条件：
– 起点 / 终点位置、速度、加速度
– 最大曲率限制（通常 <0.1 m⁻¹）
– 横向加速度限制（<1.5 m/s²）

以下展示 Apollo 中轨迹优化的关键实现：

// modules/planning/math/curve1d/quintic_polynomial_curve1d.cc
void QuinticPolynomialCurve1d::DeriveCoefficients(
    const double x0, const double dx0, const double ddx0,
    const double x1, const double dx1, const double ddx1,
    const double param) {
  // 计算五次多项式系数
  const double p = param;
  const double p2 = p * p;
  const double p3 = p2 * p;

  coeff_[0] = x0;
  coeff_[1] = dx0;
  coeff_[2] = ddx0 / 2.0;

  Matrix3d A;
  A << p3, p2 * p2, p2 * p3,
       3 * p2, 4 * p3, 5 * p2 * p2,
       6 * p, 12 * p2, 20 * p3;

  Vector3d b;
  b << x1 - coeff_[0] - coeff_[1] * p - coeff_[2] * p2,
       dx1 - coeff_[1] - 2 * coeff_[2] * p,
       ddx1 - 2 * coeff_[2];

  Vector3d x = A.colPivHouseholderQr().solve(b);
  coeff_[3] = x(0);
  coeff_[4] = x(1);
  coeff_[5] = x(2);
}

在北京亦庄测试基地的实测数据：