Lattice Planner算法在自动驾驶中的轨迹规划实战

1. Lattice Planner算法基础从理论到实践第一次接触Lattice Planner时我被它优雅的数学表达和实际效果的反差震撼到了。这个算法就像一位经验丰富的赛车手能在瞬息万变的道路环境中快速规划出最优路线。简单来说Lattice Planner是一种基于采样的轨迹规划方法它通过在Frenet坐标系下生成大量候选轨迹然后用代价函数筛选出最合适的那条。Frenet坐标系是这个算法的灵魂所在。想象一下你在高速公路上开车传统的地图坐标笛卡尔坐标系就像用经纬度描述位置而Frenet坐标系则更符合人类驾驶直觉——它用沿着道路走了多远s值和距离车道中心线偏移多少l值来描述位置。这种表示方法让轨迹规划变得直观很多。在实际项目中我发现Frenet坐标转换有几个关键点需要注意参考线质量直接影响规划效果就像GPS导航依赖地图精度一样曲率计算要足够精确特别是在弯道区域坐标系转换时的数值稳定性需要特别处理2. 轨迹生成的核心技术细节2.1 横向轨迹生成实战横向规划就像是决定车辆在车道内的左右位置。我通常使用五阶多项式来生成横向轨迹因为它能更好地满足起终点状态约束。在实际编码时Eigen库的矩阵运算能大幅简化计算过程。这里有个实际项目中的参数设置经验// 横向轨迹参数示例 const float D_ROAD_W 1.0f; // 横向采样间隔 const float MAX_ROAD_W 3.0f; // 最大横向偏移 const float DT 0.2f; // 时间分辨率横向jerk加加速度的控制特别重要。有次测试时忽略了这点导致乘客在变道时感到明显不适。后来我们调整了代价函数中的jerk权重舒适性立即提升了不少。2.2 纵向轨迹的速度规划纵向规划解决的是开多快的问题。在高速场景下我习惯设置多个速度采样点// 纵向速度采样设置 const float TARGET_SPEED 30.0f; // m/s const float D_T_S 5.0f; // 速度采样间隔 const int N_S_SAMPLE 3; // 采样数量实际应用中发现速度规划必须考虑前车动态。我们后来改进了算法加入了基于雷达数据的预测模块使得跟车更加平稳。一个常见的坑是加速度突变这会导致乘坐体验大打折扣。3. 代价函数设计的艺术代价函数就像是算法的指挥棒决定了什么样的轨迹会被选中。经过多次迭代我们的代价函数包含了这些关键要素横向代价包括jerk、时间和终点偏移量纵向代价考虑加速度、速度偏差和时间效率安全代价障碍物距离惩罚项舒适度代价加速度变化率限制// 代价计算示例 float calc_total_cost(const FrenetPath path) { float lat_cost KJ * path.d_dd.back() KT * path.t.back(); float lon_cost KV * abs(TARGET_SPEED - path.s_d.back()); float obs_cost check_collision(path) ? 0 : COLLISION_PENALTY; return KLAT*lat_cost KLON*lon_cost obs_cost; }调试代价函数时我发现权重设置需要大量实车测试。最初我们过于强调效率导致车辆动作太激进后来增加了舒适性权重找到了更好的平衡点。4. 复杂场景下的实战挑战4.1 城市道路的应对策略在城市道路测试时我们遇到了几个典型问题密集障碍物导致规划失败率高频繁的启停导致舒适性下降复杂路口难以找到合适参考线解决方案是引入了自适应采样机制当检测到复杂环境时自动增加采样密度和扩展采样范围。同时优化了参考线生成算法使用三次样条曲线拟合更平滑的基准路径。4.2 高速场景的优化高速公路场景的特点是速度高、预测难度大。我们做了这些改进延长规划时间跨度MAXT从3s增加到5s加入交通流预测模块优化碰撞检测算法使用OBB有向包围盒代替简单圆形检测// 高速场景参数调整 const float HIGHWAY_MAXT 5.0f; const float HIGHWAY_DT 0.25f; // 更粗的时间分辨率 const float SAFETY_DISTANCE 50.0f; // 安全跟车距离5. 性能优化实战技巧5.1 计算效率提升当第一次在实车上跑算法时计算延迟让人抓狂。通过以下优化我们将计算时间从200ms降到了50ms以内使用KD-Tree加速障碍物查询实现多线程并行评估轨迹采用自适应采样策略简单场景减少采样预生成常用轨迹模板5.2 内存优化方案在嵌入式平台部署时内存限制是个大挑战。我们通过以下方式解决使用内存池管理轨迹对象减少不必要的中间变量存储量化浮点数为定点数运算采用环形缓冲区存储历史轨迹// 内存池实现示例 class FrenetPathPool { public: FrenetPath* allocate() { if (pool.empty()) expandPool(); auto* path pool.back(); pool.pop_back(); return path; } void deallocate(FrenetPath* path) { path-clear(); pool.push_back(path); } private: std::vectorFrenetPath* pool; };6. 实际项目中的经验分享在完成三个自动驾驶项目后我总结了这些血泪教训参考线质量决定上限再好的规划算法也救不了糟糕的参考线。我们开发了专门的参考线优化模块确保其连续性和合理性。参数不是万能的试图用同一组参数应对所有场景是徒劳的。最终我们实现了场景自适应的参数调整策略。实时性 vs 质量找到平衡点很关键。我们的方案是分层处理——简单场景快速响应复杂场景允许更长的计算时间。测试要充分有次更新后没测试雨天场景结果车辆在湿滑路面上规划出了危险轨迹。现在我们的测试覆盖了20种典型场景。硬件影响很大同样的算法在不同计算平台表现可能天差地别。建议早期就确定硬件方案并进行针对性优化。7. 前沿改进方向最近我们在尝试这些创新方法学习式采样用强化学习优化采样区域选择减少无效采样轨迹预测融合结合预测模块输出生成更安全的候选轨迹多目标优化使用Pareto前沿处理相互冲突的优化目标不确定性建模考虑传感器噪声和预测不确定性生成鲁棒性更强的轨迹在最新项目中我们还将Lattice Planner与行为规划深度整合通过上层决策指导下层采样范围取得了不错的效果。这种分层架构既保持了灵活性又提高了规划效率。