1. 路径规划与速度规划的数据衔接逻辑在自动驾驶系统中路径规划Path Planning和速度规划Speed Planning是两个紧密耦合的核心模块。当车辆完成路径规划后生成的路径点序列需要转化为速度规划模块能够处理的输入数据。这个转换过程涉及多个关键数据维度的处理和重组。路径规划输出的典型数据结构包含二维/三维坐标序列x,y,z路径点曲率curvature航向角heading参考线偏移量lateral_offset路径点间距arc_length这些原始数据需要经过以下处理流程才能用于速度规划曲率-速度映射根据车辆动力学模型将路径曲率转换为最大允许通过速度障碍物投影将感知层的静态/动态障碍物映射到路径的Frenet坐标系交通规则编码将红绿灯、停止线等规则转换为路径上的速度约束点舒适度参数注入考虑加速度、加加速度jerk等舒适性指标关键提示路径规划输出的点间距需要保持均匀建议0.1-0.3米过大的间距会导致速度规划时无法准确计算曲率变化率进而影响舒适性。2. 速度规划专用数据结构的构建方法2.1 基础数据转换将路径规划输出转换为速度规划可用的ST图Station-Time需要以下计算步骤计算累计距离def calculate_s_accumulated(path_points): s [0.0] for i in range(1, len(path_points)): dx path_points[i].x - path_points[i-1].x dy path_points[i].y - path_points[i-1].y s.append(s[-1] math.sqrt(dx**2 dy**2)) return s曲率平滑处理使用滑动平均滤波器窗口大小通常5-7个点应用三次样条插值保证曲率连续限制最大曲率根据车辆最小转弯半径坡度数据融合将高精度地图的坡度信息投影到路径点计算等效曲率κ_effective √(κ² (tanθ / L)²) θ为坡度角L为车辆轴距2.2 约束条件编码速度规划需要处理的约束类型及编码方式约束类型数据结构处理方法曲率约束(s_start, s_end, κ_max)转换为最大速度v_max√(a_max/κ)障碍物约束(s, t_min, t_max)生成ST图中的禁行区域交通灯约束(s_stop, t_red_begin, t_red_end)生成时间窗口约束跟车约束(s_lead, v_lead, accel_lead)建立跟驰模型预测空间2.3 动态障碍物处理对于动态障碍物需要建立时空立方体S-T Cube表示预测轨迹离散化将障碍物预测轨迹采样为(s,t)点序列构建占用网格在ST图中标记被占用的区域安全边际计算纵向安全距离d_safe v_ego * t_reaction (v_ego² - v_obs²)/(2*a_max)横向安全距离保持至少0.5m净空3. 速度规划算法的数据接口设计3.1 输入数据结构规范推荐使用Protobuf格式定义接口message SpeedPlanningInput { repeated PathPoint path_points 1; // 路径点序列 repeated Obstacle obstacles 2; // 障碍物列表 TrafficLightInfo traffic_light 3; // 交通信号 VehicleState vehicle_state 4; // 车辆状态 double max_acceleration 5; // 最大加速度(m/s²) double max_deceleration 6; // 最大减速度(m/s²) double max_jerk 7; // 最大加加速度(m/s³) } message PathPoint { double s 1; // 累计距离 double x 2; // 全局X坐标 double y 3; // 全局Y坐标 double curvature 4; // 曲率(1/m) double heading 5; // 航向角(rad) }3.2 数据预处理流水线建立高效的数据预处理流水线需要考虑数据对齐确保所有传感器的数据时间戳同步建议使用PPSGPS时间坐标系统一将所有数据转换到车辆后轴中心坐标系时间预测补偿考虑算法处理延迟典型值100-200ms数据有效性检查路径点单调性检查曲率连续性检查障碍物生命周期管理4. 典型问题与调试技巧4.1 数据不连续问题排查当速度规划出现突变时按以下步骤检查路径规划数据检查曲率跳变python3 -c import matplotlib.pyplot as plt; import pickle; datapickle.load(open(path_data.pkl,rb)); plt.plot([p.curvature for p in data]); plt.show()验证坐标转换确保所有点都在局部坐标系下检查heading角是否连续不应有π到-π的跳变诊断工具推荐使用rqt_bag可视化ROS消息用PyKDL验证坐标系转换4.2 实时性优化策略对于计算密集型场景数据降采样在直线段使用较疏的点距0.5m在弯道保持密集点距0.1m并行计算#pragma omp parallel for for (int i 0; i path_points.size(); i) { // 曲率计算等耗时操作 }缓存优化预计算曲率-速度查找表对静态障碍物只计算一次投影5. 不同场景下的数据适配方案5.1 城市道路场景特征处理重点频繁的交通灯约束人行横道减速区公交车专用道限制数据增强方法def add_traffic_light_constraints(path, light): for s in find_stop_lines(path, light): yield SpeedConstraint( start_s s - 5.0, # 提前5米开始减速 end_s s, speed_limit 0.0, constraint_type HARD )5.2 高速公路场景特殊数据处理合流区冲突检测曲率补偿高速过弯时增加安全余量跟车时距调整根据车速动态变化速度规划参数调整highway_config: min_follow_distance: 2.0s # 时距从1.5s调整为2.0s curvature_safety_factor: 1.2 # 曲率安全系数增加20% max_lateral_accel: 1.8 m/s² # 提高横向加速度限制5.3 泊车场景数据转换特点使用Reeds-Shepp路径的连续曲率版本需要处理倒车时的速度符号考虑方向盘转角到路径曲率的转换特殊处理代码示例ParkingPathConverter::convertSteeringToCurvature( const VehicleState state, const ParkingPath path) { const double L wheelbase_; for (auto pt : path.points) { if (pt.gear GEAR_REVERSE) { pt.curvature * -1; // 倒车时曲率取反 } pt.curvature tan(pt.steering_angle) / L; } }在实际工程中我们发现在低速泊车场景下路径曲率的平滑处理比高速场景更为关键。一个实用的技巧是在路径点之间插入三次样条曲线确保曲率的一阶导数连续这样可以避免速度规划时产生不必要的方向盘抖动。