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

1. 项目概述

Pure Pursuit算法作为经典的路径跟踪控制方法，在自动驾驶领域已有三十余年应用历史。这项技术最早由美国国防高级研究计划局（DARPA）在1980年代资助的自动驾驶项目中提出，最初用于军事车辆的自主导航。如今，随着乘用车自动泊车功能的普及，Pure Pursuit因其实现简单、鲁棒性强的特点，成为APA（自动泊车辅助）系统中轨迹跟踪环节的理想选择。

我在参与某主机厂自动泊车项目时，发现传统PID控制在低速泊车工况下存在超调严重、收敛速度慢的问题。经过多轮仿真验证，最终采用改进型Pure Pursuit算法作为跟踪控制器，在保证精度的同时将横向误差控制在10cm以内。本文将分享基于Carsim-Simulink联合仿真环境的完整实现过程，包括关键参数调试心得和实际工程中的避坑经验。

2. 核心原理拆解

2.1 Pure Pursuit几何模型

算法的核心思想可类比人类驾驶行为：当车辆需要跟随路径时，驾驶员会下意识地看向前方某点，并调整方向盘使车辆朝向该点行驶。Pure Pursuit将此过程数学模型化：

1. 前视距离(Ld)：虚拟目标点与车辆当前点的距离，相当于驾驶员的视线距离
2. 曲率计算：根据车辆当前位置、航向角与前视点坐标，计算所需转向曲率
3. 阿克曼转向：将曲率转换为前轮转角指令

具体推导过程如下（以自行车模型为例）：

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

其中：

• κ：路径曲率
• α：车辆当前航向与目标点连线的夹角
• L：车辆轴距
• δ：前轮转角

2.2 泊车场景的特殊处理

与传统路径跟踪不同，自动泊车场景有三个显著特点：

1. 低速工况：车速通常低于5km/h，动力学特性更接近几何模型
2. 大曲率路径：直角泊车时曲率变化剧烈
3. 末端精度要求高：最终停车位置误差需<5cm

针对这些特点，我们做了以下改进：

• 动态前视距离：Ld = min(1.5, 0.3*v + 0.5) 根据车速自适应调整
• 曲率滤波：对相邻路径点曲率进行滑动平均处理
• 末端切换逻辑：距离终点1m时切换为PID精确控制

3. 仿真环境搭建

3.1 Carsim车辆模型配置

使用Carsim 2019.1版本，关键参数设置：

注意：必须勾选"External Driver"选项以接收Simulink控制指令，同时建议将仿真步长设为0.01s以保证控制精度。

3.2 Simulink控制框架

搭建如图所示的层级结构：

code复制PurePursuit Controller
├── Path Preprocessor
│   ├── Path Smoothing (Quintic Spline)
│   └── Curvature Calculator
├── Core Algorithm
│   ├── Lookahead Distance Generator
│   └── Steering Angle Calculator
└── Post-Processing
    ├── Rate Limiter (max 30 deg/s)
    └── Feedforward Compensator

关键模块实现细节：

1. 路径预处理：对A*算法生成的原始路径进行五次样条插值，确保曲率连续
2. 曲率补偿：基于车辆动力学参数添加前馈补偿项
3. 转向速率限制：防止方向盘瞬时转角过大

4. 参数调试方法论

4.1 前视距离优化

通过参数扫描法确定最优Ld：

调试心得：

• 低速时Ld过大会导致"割线"现象
• 高速时Ld过小会引起轨迹振荡
• 动态公式中a值影响速度敏感度，b值决定基础前视量

4.2 曲率滤波设计

采用窗口大小为5的Savitzky-Golay滤波器，对比不同阶数效果：

实际工程中选择3阶滤波器，在性能和实时性间取得平衡。

5. 典型问题解决方案

5.1 路径抖动问题

现象：跟踪过程中车辆频繁左右微调
排查步骤：

1. 检查原始路径曲率连续性（最大曲率变化率应<0.2 m⁻¹/m）
2. 验证控制周期一致性（使用Simulink Execution Time Viewer）
3. 检测转向系统延迟（Carsim中查看Steer_Actuator延迟参数）

最终发现是Carsim与Simulink的时钟同步问题，通过以下方法解决：

• 在Simulink配置中设置Fixed-step solver
• 勾选"Carsim S-Function"的同步选项
• 添加10ms的指令缓冲队列

5.2 末端 overshoot

问题描述：车辆在接近终点时越过目标位置
优化方案：

1. 引入双模式切换逻辑：
   • 当距离>1m时使用Pure Pursuit
   • 距离≤1m时切换为PD控制（Kp=0.8, Kd=0.2）
2. 添加终点速度规划：

   code复制v_end = v_max * (d/1.5)^0.5
   

3. 设置机械制动触发条件（纵向误差>15cm）

6. 实车适配经验

将算法从仿真移植到实车时需注意：

1. 转向系统标定：

   • 测量转向传动比（通常12-16:1）
   • 标定方向盘死区（普通车辆约±3°）
   • 测试最大转向速率（家用车约500°/s）

2. 定位误差补偿：

   • 添加基于IMU的航向角修正
   • 对轮速脉冲信号进行滑动平均滤波
   • 设置0.1m的位置更新阈值

3. 执行器保护：

   c复制if(fabs(steer_cmd - steer_actual) > 30deg)
       trigger_emergency_stop();
   

实测数据显示，经过适配后的系统在标准车位（2.5×5m）中可实现：

• 平均横向误差：4.3cm
• 最大误差：8.7cm
• 泊入时间：23.5s（含安全检查耗时）

7. 进阶优化方向

对于追求更高性能的开发者，建议尝试：

1. 曲率前馈补偿：

   math复制δ_ff = L·κ + Kv·ay
   

   其中ay为横向加速度，Kv为调谐参数（典型值0.003-0.005）

2. 模型预测控制融合：

   • 将Pure Pursuit作为MPC的参考生成器
   • 用MPC处理动态障碍物避让
   • 需注意实时性约束（求解时间<50ms）

3. 轮胎松弛效应补偿：
   低速时添加基于松弛长度的修正项：

   code复制δ_comp = σ/(L·v)  (σ≈0.1m for normal tire)
   

我在某豪华车型项目上实测发现，结合MPC后可将极端工况（如倾斜车位）下的成功率从82%提升至96%。