自动驾驶路径规划与速度规划的数据衔接技术

1. 路径规划与速度规划的数据衔接

在自动驾驶决策规划模块中，路径规划和速度规划是两个紧密关联的核心环节。路径规划确定了车辆在空间中的行驶轨迹，而速度规划则决定了车辆沿着这条轨迹行驶的时间特性。两者之间的数据衔接质量直接影响到最终轨迹的平滑性和可行性。

1.1 路径规划输出解析

路径规划模块（如piecewise jerk path optimizer）通常会输出以下关键数据：

• 横向偏移量(l)：车辆相对于参考线的横向位置
• 一阶导数(l')：横向偏移对纵向距离的变化率，反映路径曲率
• 二阶导数(l'')：曲率的变化率，影响转向的平顺性

这些数据通常以离散点序列的形式表示，每个点对应参考线上的一个纵向位置(s)。在实际应用中，我们还需要考虑以下衍生参数：

• 航向角变化：通过l'计算得到
• 曲率半径：由l''推导得出
• 路径长度：通过积分计算

1.2 速度规划的数据需求

速度规划模块需要从路径规划结果中提取以下关键信息：

1. 几何特性数据：

   • 路径曲率及其变化率
   • 最大允许横向加速度
   • 路径的平滑度评估

2. 约束条件数据：

   • 路径边界约束
   • 障碍物投影信息
   • 交通规则限制区域

3. 优化目标数据：

   • 参考线偏离权重
   • 舒适性代价函数参数
   • 效率优化指标

2. 数据转换与预处理

2.1 坐标系转换

路径规划通常在Frenet坐标系下进行，而速度规划需要将部分数据转换到笛卡尔坐标系：

1. 位置转换：

   cpp复制CartesianPoint frenetToCartesian(const FrenetPoint& frenet, const ReferenceLine& ref) {
       CartesianPoint cartesian;
       auto ref_point = ref.GetReferencePoint(frenet.s);
       cartesian.x = ref_point.x() - frenet.l * sin(ref_point.heading());
       cartesian.y = ref_point.y() + frenet.l * cos(ref_point.heading());
       return cartesian;
   }
   

2. 速度/加速度转换：
   需要考虑路径曲率的影响，使用如下转换关系：

   code复制v_cartesian = v_frenet / (1 - k * l)
   a_cartesian = a_frenet / (1 - k * l) + v_frenet² * (k' * l + k * l') / (1 - k * l)³
   

2.2 数据重采样与平滑

由于路径规划的输出可能是非均匀采样的，需要进行重采样处理：

1. 等距采样：

   • 沿参考线方向按固定间隔(如0.1m)重新采样
   • 使用三次样条插值保证高阶连续性

2. 曲率平滑：

   python复制def smooth_curvature(k_original, window_size=5):
       kernel = np.ones(window_size)/window_size
       return np.convolve(k_original, kernel, mode='same')
   

3. 导数一致性检查：
   确保l、l'、l''之间的数学关系成立，避免数值计算带来的不一致性。

3. 关键数据结构构建

3.1 路径-速度映射表

构建一个将路径信息映射到速度规划需求的数据结构：

3.2 约束条件编码

将路径边界和障碍物约束编码为速度规划可用的形式：

1. ST图构建：

   • 将障碍物投影到路径上
   • 计算沿路径的占据区域
   • 转换为速度-时间坐标系下的约束

2. 动态障碍物处理：

   cpp复制struct DynamicObstacleConstraint {
       double s_start;  // 障碍物起始位置
       double s_end;    // 障碍物结束位置
       double t_start;  // 出现时间
       double t_end;    // 消失时间
       double safe_margin; // 安全裕度
   };
   

4. 优化目标传递

4.1 代价函数参数

将路径优化的目标平滑传递到速度规划：

1. 舒适性代价：

   code复制comfort_cost = w_jerk * (jerk)^2 + w_acc * (acc)^2
   

2. 参考线偏离代价：

   code复制ref_cost = w_l * (l - l_ref)^2 + w_dl * (dl/ds - dl_ref/ds)^2
   

3. 效率代价：

   code复制efficiency_cost = w_t * T + w_v * (1 - v/v_desired)^2
   

4.2 权重自适应策略

根据路径特性动态调整速度规划权重：

• 高曲率路段：增加舒适性权重
• 直线路段：增加效率权重
• 换道区域：增加轨迹跟踪精度权重

5. 实际应用中的问题与解决方案

5.1 数据同步问题

问题现象：
路径规划更新频率(如10Hz)与速度规划频率(如20Hz)不一致导致的数据不同步。

解决方案：

1. 使用双缓冲机制：
   • 一个缓冲区用于读取
   • 另一个用于写入更新
2. 时间戳校验：

   cpp复制if (abs(current_time - path_data.timestamp) > threshold) {
       // 触发数据重新请求
   }
   

5.2 曲率突变处理

问题现象：
路径规划输出的曲率在连接点处可能出现突变，导致速度规划产生不合理的加速度需求。

解决方案：

1. 曲率变化率限制：

   code复制if (|dk/ds| > threshold) {
       // 触发路径重新优化
   }
   

2. 速度自适应平滑：
   • 在曲率突变点前提前减速
   • 使用S曲线速度过渡

5.3 实时性优化

性能瓶颈：
大规模数据转换和预处理可能影响实时性。

优化措施：

1. 增量式更新：
   • 只处理变化的部分路径
2. 并行计算：

   cpp复制#pragma omp parallel for
   for (int i = 0; i < path_points.size(); ++i) {
       // 数据处理
   }
   

3. 预计算缓存：
   • 缓存常用转换结果
   • 使用查找表加速计算

6. Apollo框架中的实现参考

在Apollo自动驾驶框架中，这一数据转换过程主要在Planning模块的ReferenceLineProvider和PathData类中实现：

1. 关键数据结构：

   proto复制message PathPoint {
       optional double s = 1;
       optional double l = 2;
       optional double dl = 3;  // l'
       optional double ddl = 4; // l''
       optional double kappa = 5; // 曲率
   }
   

2. 数据流处理：

   • PathOptimizer生成优化后的PathData
   • SpeedOptimizer通过PathData构建STGraph
   • 最终生成完整的轨迹

3. 性能优化技巧：

   • 使用protobuf进行高效序列化
   • 零拷贝数据共享
   • 基于RTTI的动态数据处理

在实际工程实现中，还需要特别注意以下几点：

1. 数据类型一致性检查
2. 单位系统统一(弧度/度，米/厘米等)
3. 异常处理机制
4. 数据有效性验证

路径规划到速度规划的数据构建是自动驾驶决策规划中的关键桥梁，其质量直接影响后续模块的性能表现。通过合理的数据结构设计和高效的转换算法，可以确保两个模块之间的平滑衔接，为车辆提供既安全又舒适的行驶轨迹。