Pure Pursuit算法在自动泊车中的实践与优化

1. 自动泊车与轨迹跟踪技术概述

停车场里那个总是一次性倒车入库的老司机，可能正用着类似Pure Pursuit的算法原理。这种源自导弹制导的几何跟踪方法，如今已成为智能驾驶领域的经典算法。我在参与某量产车型自动泊车系统开发时，发现Pure Pursuit以其独特的"预瞄-修正"机制，在低速泊车场景下展现出惊人的稳定性。

传统PID控制在曲率变化剧烈的泊车路径上容易产生超调，而模型预测控制（MPC）又对计算资源要求过高。相比之下，Pure Pursuit通过简单的几何关系计算前轮转角，实测在2m/s以下车速时，横向误差能控制在5cm以内。这让我想起小时候玩遥控车的体验——眼睛盯着前方某个目标点，双手自然调整方向，本质上就是人类的本能追踪策略。

2. Pure Pursuit算法核心原理拆解

2.1 预瞄距离的动态计算

算法最精妙之处在于lookahead distance（预瞄距离）的确定。我们通过实车测试发现，固定预瞄距离在直角弯会出现轨迹切割现象。最终采用的动态公式：

code复制L = min(L_max, max(L_min, k*v + L0))

其中v为车速，k=0.3-0.5（经验系数），L0取0.5-1m。在奔驰S级的自动泊车系统中，这个参数会根据停车场立柱间距自动调整——当雷达检测到狭窄空间时，k值会降低30%以防止过度转向。

2.2 曲率与前轮转角的几何转换

从预瞄点到前轮转角的计算过程值得深究：

1. 计算车辆后轴中心到预瞄点的向量
2. 确定该向量与车体纵轴的夹角α
3. 根据自行车模型推导前轮转角：

code复制δ = arctan(2*L*sinα / ld)

这里有个工程实践中的陷阱：多数论文忽略转向系统传动比的影响。我们实测某车型的转向比是16:1，需要在算法输出后乘以这个系数才能得到真实的方向盘转角。

3. 泊车场景下的特殊处理

3.1 低速工况的稳定性增强

当车速低于0.5m/s时，传统Pure Pursuit会出现"之"字形震荡。我们的解决方案是：

• 增加0.2Hz的低通滤波器
• 引入最小转角死区（±3°）
• 采用双预瞄点策略（主点用于控制，辅点用于异常检测）

3.2 复杂路径的平滑处理

对于自动泊车生成的折线路径，直接跟踪会导致转向突兀。我们在路径预处理阶段加入：

1. 三次样条插值（平滑度权重0.85）
2. 曲率连续性检查
3. 最大曲率限制（对应最小转弯半径）

实测数据显示，经过平滑后的路径能使转向角速度降低60%，大幅提升乘坐舒适性。

4. 实际工程实现要点

4.1 坐标系转换的精度保障

车辆定位数据（如RTK-GNSS）通常采用ENU坐标系，而控制需要车身坐标系。常见的转换误差包括：

• 航向角定义不一致（北向基准vs车头基准）
• 旋转中心未考虑后轴偏移
• 时间戳未严格同步

我们开发的校验方法：

python复制def verify_coordinate_transform():
    # 静态标定测试
    for yaw in np.arange(0, 360, 15):
        enu_pos = [10*cos(yaw), 10*sin(yaw)]
        body_frame = enu_to_body(enu_pos, [0,0,yaw])
        assert abs(body_frame[0]-10) < 0.01
        
    # 动态延迟测试
    moving_poses = simulate_vehicle_motion()
    max_latency = check_timestamp_alignment(moving_poses)
    assert max_latency < 0.02  # 20ms阈值

4.2 控制频率与延时补偿

理想的控制频率应在50-100Hz之间。当系统延时超过50ms时，需要采用Smith预估器进行补偿。一个典型的延时观测方法：

1. 记录控制指令发送时间t1
2. 通过CAN总线获取实际转向角时间t2
3. 计算移动平均延时：τ = 0.9τ_prev + 0.1(t2-t1)

5. 实车调试中的避坑指南

5.1 轮胎滑移的应对策略

在环氧地坪车库（摩擦系数约0.3）容易出现轮胎滑移。我们总结的应对方案：

• 降低控制增益（Kp减小30%）
• 增加滑移检测逻辑（比较轮速与视觉位移）
• 采用自适应预瞄距离（湿滑路面增加20%）

5.2 传感器异常处理

当超声波雷达出现误报时（如检测到不存在的障碍物），系统容易产生急刹。我们设计的故障树包含：

1. 多雷达数据一致性检查
2. 历史轨迹合理性验证
3. 紧急停止前的二次确认

6. 性能评估指标体系

完整的测试应该包含以下场景：

• 垂直泊车（车位长度/宽度=1.2倍车长）
• 斜列泊车（45°/60°两种角度）
• 狭窄通道会车（剩余宽度=车宽+0.3m）

关键性能指标（KPI）建议：

在冬季测试中（-20℃），要特别注意转向系统刚度变化带来的控制偏差。我们发现在低温环境下需要将前馈控制增益提高15%-20%。