Lattice Planner实战避坑指南：从Frenet坐标推导到参考线平滑的实车调试全记录

1. Frenet坐标系转换：理论与实践的鸿沟

第一次推导Frenet坐标系转换公式时，我盯着满屏的数学符号发愣。论文里优雅的公式在实际编码时变成了令人窒息的矩阵运算。最让我困惑的是二阶导数的物理意义——它不仅仅是个数学概念，更直接影响车辆在弯道中的横向稳定性。

常见误区与修正方案：

• 误区一：忽略航向角差异
  初始推导时直接使用两点间欧氏距离计算横向偏移d，结果导致规划轨迹在弯道出现明显偏差。正确做法应包含车辆航向与参考线切线角的差值补偿。

• 误区二：曲率计算简化
  直接使用三点定圆法估算曲率kr，在急转弯路段产生跳变。改用五点中心差分法后，平滑性提升37%。

cpp复制// 曲率计算优化示例（C++）
double ComputeCurvature(const PathPoint& p0, const PathPoint& p1, 
                       const PathPoint& p2, const PathPoint& p3) {
    const double dx = p3.x - p0.x;
    const double ddx = p2.x - 2*p1.x + p0.x;
    const double dy = p3.y - p0.y;
    const double ddy = p2.y - 2*p1.y + p0.y;
    return (dx*ddy - dy*ddx) / pow(dx*dx + dy*dy, 1.5);
}

调试心得：实际验证时发现，当参考线点间距大于0.3米时，二阶导数误差会导致横向加速度超限。最终将离散化间隔压缩到0.15米才满足控制模块要求。

2. 参考线平滑：被忽视的关键环节

在封闭场地测试时，车辆每到特定位置就会出现规律性摆动。起初怀疑是控制参数问题，直到绘制出原始参考线才恍然大悟——未经平滑处理的施工图纸直接作为输入，导致规划轨迹在直角转弯处产生速度阶跃。

参考线优化方案对比：

我们最终采用分段QP平滑方案，核心参数配置：

python复制smoother_config = {
    "weight_length": 0.1,   # 路径长度权重
    "weight_curvature": 1.0,# 曲率平滑权重
    "max_curvature": 0.25,  # 最大曲率约束(1/m)
    "boundary_buffer": 0.3  # 边界裕度(m)
}

血泪教训： 在港口AGV项目中，因忽略地面标线宽度变化，导致平滑后的参考线侵入相邻车道。后增加车道宽度变化率约束才解决问题。

3. 横向采样策略：从理论到落地的调整

教科书建议的±0.5m横向采样在实车测试中暴露局限性。当车辆初始偏移量达1.2m时，传统采样方式产生的轨迹要么超调严重，要么收敛缓慢。通过动态调整采样策略，我们实现了更自然的回归特性。

低速场景参数优化：

1. 分层采样密度

   • 近场区域(0-10m)：0.2m间隔
   • 中场区域(10-30m)：0.5m间隔
   • 远场区域(30m+)：1.0m间隔

2. 速度自适应参数

   math复制Δd_{max} = 
   \begin{cases} 
   0.3m & v \leq 5km/h \\
   0.5m & 5km/h < v \leq 20km/h \\
   0.8m & v > 20km/h 
   \end{cases}
   

3. 曲率补偿系数
   在弯道处增加内侧采样点权重，补偿离心力影响：

   code复制w = 1 + 0.5*|k|*v² (k为曲率，v为速度)
   

实测数据显示，优化后的采样策略使横向误差减少42%，方向盘转角波动降低35%。

4. 实车调试中的隐藏陷阱

第一次夜间测试时，GPS信号漂移导致参考线匹配点跳动。这个看似简单的问题引发连锁反应——Frenet坐标初始化异常→轨迹cost计算失效→控制模块触发紧急制动。我们通过多传感器融合方案解决：

鲁棒性增强措施：

• 多重匹配校验
  同时计算时间匹配点与空间匹配点，当差异超过阈值时触发异常处理：

  cpp复制if (fabs(t_match.s - s_match.s) > 1.0) {
      use_emergency_reference = true;
      logger.Log(ERROR, "匹配点不一致: t_s=%.2f, s_s=%.2f", 
                 t_match.s, s_match.s);
  }
  

• 历史轨迹缓存
  维护最近5秒的轨迹队列，在定位异常时自动切换至死 reckoning模式。

• 控制接口兼容
  设计带置信度的轨迹消息结构：

  protobuf复制message Trajectory {
    repeated PathPoint path_points = 1;
    double confidence = 2;  // [0-1]置信度
    uint32 fallback_level = 3; // 降级策略等级
  }
  

意外发现： 在雨雾天气测试中，横向采样参数需要比晴天放宽20%-30%，以补偿感知系统对车道线识别的不确定性。