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.