Lattice Planner算法在自动驾驶中的轨迹优化实践

1. Lattice Planner算法基础解析第一次接触Lattice Planner时我被它优雅的数学表达和实际效果惊艳到了。这个算法本质上是在Frenet坐标系下玩的一个采样-评估游戏就像在停车场找车位时大脑会自动生成几条可能的路线然后选择最合适的那条。Frenet坐标系是理解这个算法的关键。想象你开车时导航地图上那条蓝色的参考线就是Frenet坐标系的s轴而垂直于这条线的方向就是l轴。这种表示方法比传统的笛卡尔坐标系更符合人类驾驶直觉。我常用的转换公式是这样的// Frenet转笛卡尔坐标 Vector2d frenet_to_cartesian(double s, double l, const ReferenceLine ref) { auto ref_point ref.GetReferencePoint(s); double x ref_point.x() - l * sin(ref_point.heading()); double y ref_point.y() l * cos(ref_point.heading()); return {x, y}; }实际项目中我发现坐标系转换的精度会直接影响最终轨迹质量。有次因为参考线采样不够密导致转换后的轨迹出现锯齿状抖动车辆就像喝醉了一样左右摇摆。后来我们改用了三次样条插值来处理参考线问题才得到解决。2. 轨迹生成的核心机制生成候选轨迹是Lattice Planner最精彩的部分。这里有个很妙的技巧把横向和纵向运动解耦处理。就像跳舞时上半身动作和腿部动作可以分开设计最后再组合成完整舞步。横向轨迹生成我通常用五阶多项式class QuinticPolynomial: def __init__(self, xs, vxs, axs, xe, vxe, axe, T): # 计算五次多项式系数 self.a0 xs self.a1 vxs self.a2 axs / 2.0 A np.array([ [T**3, T**4, T**5], [3*T**2, 4*T**3, 5*T**4], [6*T, 12*T**2, 20*T**3] ]) b np.array([ xe - self.a0 - self.a1*T - self.a2*T**2, vxe - self.a1 - 2*self.a2*T, axe - 2*self.a2 ]) x np.linalg.solve(A, b) self.a3, self.a4, self.a5 x纵向轨迹则要考虑更多实际约束。有次测试时车辆在弯道加速导致严重侧滑就是因为纵向速度规划没考虑横向加速度限制。现在我都会用这个约束条件最大横向加速度 (纵向速度)² × 曲率 ≤ 阈值3. 代价函数设计实战技巧代价函数就像给不同候选轨迹打分的评分表。早期版本我们只考虑了舒适性指标结果车辆在紧急情况下反应迟钝。后来加入了安全性和效率的权衡形成了现在的多目标优化方案。这是我常用的代价函数结构struct CostWeights { double time_weight 0.1; // 时间成本 double jerk_weight 1.0; // 舒适度 double offset_weight 10.0; // 车道居中 double speed_weight 2.0; // 速度保持 double collision_weight 100.0;// 碰撞风险 }; double calculate_total_cost(const FrenetPath path, const CostWeights weights, double target_speed) { double cost 0.0; // 计算各项代价 cost weights.time_weight * path.time_cost(); cost weights.jerk_weight * path.jerk_cost(); // ...其他代价项 return cost; }实际调参时有个小技巧先用对数尺度粗调0.1,1,10再用线性尺度微调。记得有次调参时把jerk权重设得过大结果车辆变得像新手司机一样起步停车都特别温柔乘客反而觉得晕车。4. 工程优化与性能提升当候选轨迹数量超过500条时算法实时性就会成为瓶颈。我们团队踩过的坑包括盲目增加采样密度导致计算爆炸还有多线程同步带来的性能损耗。现在我们的优化方案是三层级并行处理轨迹生成层GPU加速采样代价评估层多CPU核并行碰撞检测层KD-Tree空间索引这是我们的并行计算框架关键代码def parallel_trajectory_evaluation(trajectories, obstacles): with ThreadPoolExecutor() as executor: # 第一阶段并行计算动力学代价 future_cost {executor.submit(calc_dynamic_cost, t): t for t in trajectories} # 第二阶段并行碰撞检测 future_collision {executor.submit(check_collision, t, obstacles): t for t in trajectories} # 等待所有任务完成 wait(list(future_cost.keys()) list(future_collision.keys()))实测下来这种方案能在100ms内处理上千条候选轨迹。有个特别有意思的发现有时候减少采样数量反而能提高规划质量因为过多的相似轨迹会导致优化陷入局部最优。5. 典型场景解决方案在城市道路测试中有几个经典场景特别考验Lattice Planner的鲁棒性cut-in场景前车突然变道插入。我们的解决方案是动态调整代价函数权重前车距离50米时碰撞代价权重指数级增加引入刹车舒适度代价项避免急刹横向采样偏向远离cut-in车辆的方向拥堵跟车场景这里最大的挑战是stop-and-go时的舒适性。我们开发了速度规划平滑算法vectordouble smooth_speed_profile(const vectordouble raw_speeds) { // 使用滑动窗口平均 const int window_size 5; vectordouble smoothed; for (int i 0; i raw_speeds.size(); i) { double sum 0.0; int count 0; for (int j max(0,i-window_size); j min(iwindow_size,raw_speeds.size()-1); j) { sum raw_speeds[j]; count; } smoothed.push_back(sum / count); } return smoothed; }弯道场景这里最容易出现轨迹曲率不连续的问题。我们引入了曲率约束的轨迹修正算法if (max_curvature threshold) { // 重采样更低速的纵向轨迹 adjust_longitudinal_speed(); // 使用贝塞尔曲线平滑 apply_bezier_smoothing(); }6. 调试工具链搭建好的调试工具能省去50%的开发时间。我们自研的可视化工具可以实时显示所有候选轨迹按代价着色参考线和Frenet坐标系障碍物投影最优轨迹的详细代价分解这是我们的可视化代码框架class Visualization: def plot_trajectories(self, trajectories, best_idx): plt.figure(figsize(12,6)) # 绘制所有轨迹 for i, traj in enumerate(trajectories): color grey if i ! best_idx else red alpha 0.3 if i ! best_idx else 1.0 plt.plot(traj.x, traj.y, colorcolor, alphaalpha) # 绘制代价热力图 # ...其他可视化元素 plt.show()在调试参数时我们会记录不同参数组合下的规划耗时轨迹舒适度评分紧急避障成功率 然后使用帕累托前沿分析找到最优参数组合。7. 前沿改进方向最近我们在试验一些创新性的改进方案效果令人振奋自适应采样策略根据场景复杂度动态调整采样密度。在开阔道路减少采样点在复杂交叉口增加采样密度。实现代码大致如下vectordouble adaptive_sampling(double complexity_score) { double base_density 1.0; // 基础采样间隔 double adaptive_factor 1.0 0.5 * complexity_score; return linspace(0, max_offset, int(base_samples * adaptive_factor)); }学习式代价函数用强化学习自动调整代价权重。这个方案在仿真中表现惊艳但在实车测试时发现对噪声过于敏感还在持续优化中。轨迹预测耦合将障碍物预测轨迹直接融入采样过程。比如对可能cut-in的车辆预先在其预测路径周围增加采样点。这需要精确的预测模块配合我们现在的实现是def generate_prediction_aware_samples(prediction): samples [] for t in prediction.time_steps: for obj in prediction.objects: # 在预测位置周围增加采样点 samples.extend(create_samples_around(obj.position_at(t))) return samples这些改进让我们的Lattice Planner在复杂城市场景中的通过率提升了近40%但每个方案都带来了新的技术挑战比如实时性下降、参数调试复杂度增加等。