自动驾驶笔记-轨迹跟踪之②预瞄距离调优：从理论到实践的Pure Pursuit进阶

1. 预瞄距离：轨迹跟踪的"方向盘"

第一次调预瞄距离时，我盯着屏幕上蛇形走位的仿真车直挠头——明明算法公式都写对了，为什么车辆总是画龙？后来才发现，预瞄距离这个参数就像开车时的视线距离：看得太近容易频繁修正方向（轨迹震荡），看得太远又会导致转弯不及时（轨迹偏离）。这让我想起考驾照时教练的叮嘱："别老盯着车头看，要看远方的路！"

在Pure Pursuit算法中，**预瞄距离（Lookahead Distance）**决定了车辆"看向"轨迹上多远的目标点。它直接影响两个关键指标：

• 跟踪精度：距离越小，车辆越紧跟轨迹细节
• 行驶平稳性：距离越大，转向动作越柔和

实际调参时，我发现固定预瞄距离会遇到典型问题：

• 低速时转向过于敏感（像新手司机紧握方向盘）
• 高速时转向响应滞后（像疲劳驾驶反应迟钝）
• 急弯处容易"切弯"（轨迹截断）
• 长直道出现"之字形"震荡

2. 动态预瞄距离的黄金法则

2.1 速度自适应公式

经过几十次实车测试，我最推荐这个动态公式：

python复制def calculate_lookahead(current_speed):
    min_dist = 2.0  # 最低预瞄距离(米)
    max_dist = 8.0  # 最高预瞄距离(米)
    k_speed = 0.3   # 速度系数(实测0.2-0.5较优)
    
    return min(max_dist, max(min_dist, k_speed * current_speed))

这个公式的妙处在于：

1. 低速保障：当车速<5m/s时保持2米基础距离，避免过度敏感
2. 线性扩展：车速每增加1m/s，预瞄距离增加0.3米
3. 上限保护：超过8米后不再增加，防止长距离预瞄失效

实测数据对比（某园区自动驾驶接驳车）：

2.2 曲率补偿策略

遇到急弯时，单纯速度适配还不够。我在某物流车项目中发现，当转弯半径<10米时，需要额外补偿：

python复制def curvature_compensation(curvature):
    base_comp = 1.5  # 基础补偿量
    curve_thresh = 0.1  # 曲率阈值(1/m)
    
    if curvature > curve_thresh:
        return base_comp * (curvature / curve_thresh)
    return 0

补偿逻辑解析：

• 当检测到曲率>0.1（即半径<10米）时触发
• 补偿量随曲率线性增加
• 实际应用时要与速度公式叠加使用

3. 典型场景调优实战

3.1 十字路口左转

这是最考验算法的场景之一。我们团队在调参时发现：

• 入弯阶段：提前100米开始渐进减小预瞄距离（从6m→3m）
• 弯心阶段：保持最低预瞄距离（2-3m）
• 出弯阶段：随车速提升逐步恢复

关键技巧是建立距离-曲率映射表：

python复制turn_comp_map = [
    {"dist_to_turn": 100, "lookahead": 6.0},
    {"dist_to_turn": 50,  "lookahead": 4.5},
    {"dist_to_turn": 20,  "lookahead": 3.0},
    {"dist_to_turn": 0,   "lookahead": 2.5}
]

3.2 长直道加速

高速场景容易暴露两个问题：

1. 预瞄点抖动：由于定位误差，远距离目标点坐标波动
2. 控制延迟：大预瞄距离导致转向响应慢

我们的解决方案：

• 增加路径平滑滤波（Savitzky-Golay滤波器）
• 设置转向死区（<3°不动作）
• 采用指数移动平均处理预瞄点

4. 实车调试避坑指南

去年在港口AGV项目里踩过的坑，总结出这些经验：

硬件层面：

• 定位误差>10cm时，预瞄距离应>3米
• 转向执行器延迟>200ms时，需降低速度系数k_speed
• IMU安装偏移会导致曲率计算误差

软件层面：

• 一定要做预瞄点可视化（如下图）
• 建议采样频率≥10Hz
• 警惕路径规划与跟踪的耦合问题

调试时我习惯用这个检查清单：

1. [ ] 低速直线测试（2m/s）
2. [ ] 中速弯道测试（5m/s）
3. [ ] 高速变道测试（8m/s+）
4. [ ] 紧急避障测试
5. [ ] 连续S弯测试

记得第一次夜测时，因为没考虑车灯照射范围对预瞄距离的影响，导致车辆在黑暗弯道偏离轨迹。后来我们增加了环境光强补偿系数，才解决这个问题。