Lattice Planner实战避坑指南：从Frenet坐标推导到参考线平滑，我的实车调试血泪史

Lattice Planner实战避坑指南：从Frenet坐标推导到参考线平滑，我的实车调试血泪史

当第一次看到Lattice Planner生成的轨迹在实车上完美执行时，那种成就感让人难以忘怀。但在这之前，我经历了无数个调试到凌晨的夜晚，踩过各种意想不到的坑。本文将分享我在Lattice Planner实车部署过程中遇到的关键技术难题及解决方案，希望能为同行提供一些参考。

1. Frenet坐标转换：那些让人抓狂的二阶导数

Frenet坐标系是Lattice Planner的基础，但也是最容易出错的部分。在实际调试中，我发现90%的轨迹异常都源于Frenet坐标转换的不准确，特别是二阶导数的计算。

1.1 为什么二阶导数如此关键

在笛卡尔坐标系与Frenet坐标系的转换中，二阶导数直接影响轨迹的平滑性和车辆控制的稳定性。一个常见的误区是只关注位置(s,d)和速度(s',d')的计算，而忽略了加速度(s'',d'')的重要性。

典型错误表现：

• 车辆在转弯时出现不必要的抖动
• 轨迹在参考线曲率变化大的地方出现突变
• 控制模块频繁报出加速度超限警告

1.2 正确的推导方法

经过多次验证，我发现最可靠的推导方式是采用运动学分析方法。以下是关键公式：

s' = vx * cos(Δθ) / (1 - kr * d) d' = (vy - vx * sin(Δθ)) / (1 - kr * d)

其中Δθ是车辆航向角与参考线航向角的差值，kr是参考线曲率。

二阶导数的推导更为复杂，需要考虑曲率变化率：

s'' = [ax * cos(Δθ) - vx * sin(Δθ) * (ω - kr * s')] / (1 - kr * d) + s' * (kr' * d + kr * d') / (1 - kr * d) d'' = [ay - ax * sin(Δθ) - vx * cos(Δθ) * (ω - kr * s')] / (1 - kr * d) + d' * (kr' * d + kr * d') / (1 - kr * d)

提示：在实际编码时，建议先实现这些公式的单元测试，用已知的输入输出验证正确性。

1.3 调试技巧

1. 可视化工具必不可少：同时绘制笛卡尔和Frenet坐标系下的轨迹
2. 分步验证：先验证静态转换，再测试动态场景
3. 边界条件测试：特别关注d=0和曲率突变的情况

2. 参考线不平滑：轨迹阶跃的罪魁祸首

参考线质量直接决定了Lattice Planner的输出质量。我曾遇到一个棘手问题：车辆在特定路段总是出现不自然的摆动，最终发现是参考线不平滑导致的。

2.1 问题现象分析

不平滑的参考线会导致：

• 轨迹在笛卡尔坐标系中出现明显的折线
• 曲率不连续，导致控制模块难以跟踪
• 速度规划出现跳变

实测数据对比：

2.2 参考线平滑方案

经过多次尝试，我总结出以下有效的平滑方法：

1. 分段多项式平滑

   • 使用五次多项式保证曲率连续
   • 关键点约束位置、方向和曲率

2. 优化目标设计

   cost = w1*位置误差 + w2*曲率 + w3*曲率变化率

   权重选择建议：

   • 城市道路：w2 > w3 > w1
   • 高速道路：w3 > w2 > w1

3. 实时平滑策略

   • 预计算全局参考线
   • 局部实时微调

2.3 实车验证要点

1. 在不同速度下测试平滑效果
2. 特别关注曲率变化大的区域（如路口）
3. 监控控制模块的跟踪误差

3. 横向采样参数：±0.5不是金科玉律

Apollo默认的横向采样参数（如±0.5m）被很多人视为标准配置，但在实际道路测试中，我发现盲目套用这个值会导致诸多问题。

3.1 参数影响分析

横向采样参数直接影响：

• 轨迹的多样性
• 避障能力
• 乘坐舒适性

不同场景下的最优参数：

3.2 动态调整策略

我开发了一套基于规则的动态调整方法：

1. 根据车速调整范围

   lateral_range = base_range * (1 + 0.5*(v - v0)/v0);

2. 根据道路宽度调整密度

   • 宽路：稀疏采样
   • 窄路：密集采样

3. 考虑障碍物分布

   • 单侧障碍：非对称采样
   • 双侧障碍：中心偏移

3.3 调参经验分享

1. 先从保守参数开始，逐步扩大范围
2. 记录每次参数变更的效果
3. 建立参数与场景的映射关系

4. 轨迹拼接：时间一致性陷阱

轨迹拼接(Stitching)是保证规划连续性的关键，但时间处理不当会导致严重的控制问题。

4.1 典型问题场景

1. 规划周期延迟：100ms的规划延迟在60km/h车速下意味着1.67m的位置误差
2. 控制指令堆积：多个轨迹段时间重叠导致控制冲突
3. 紧急制动失效：时间戳错误使制动指令延迟

4.2 解决方案

时间对齐策略：

1. 统一使用全局时间戳
2. 预测车辆未来状态作为规划起点

   planning_start = current_pose.predict(dt)

轨迹重叠处理：

1. 新旧轨迹相似度检测
2. 渐进式切换权重
3. 异常情况安全处理

实测对比数据：

4.3 调试建议

1. 可视化时间轴上的轨迹序列
2. 模拟不同延迟场景
3. 压力测试极端情况

5. 实车调试的血泪教训

经过三个月的实车调试，我总结了以下宝贵经验：

1. 不要相信仿真结果：在仿真中完美的轨迹，实车可能完全无法执行
2. 分阶段验证：先验证无障碍物场景，再逐步增加复杂度
3. 数据记录至关重要：每次测试都要完整记录所有中间状态
4. 控制-规划协同调试：单独优化规划模块效果有限

必备调试工具链：

• 高精度定位设备
• 实时轨迹可视化工具
• 数据回放分析系统
• 参数快速调整接口

在经历了无数次失败后，当我第一次看到车辆平稳地通过复杂路段时，所有的努力都变得值得。Lattice Planner的实车部署没有银弹，唯有不断试错和优化才能获得理想的效果。