1. Pure Pursuit算法在自动泊车中的应用概述
Pure Pursuit（纯追踪）算法是一种经典的路径跟踪控制算法，广泛应用于自动驾驶领域，特别是在低速场景如自动泊车中表现出色。这个算法的核心思想非常简单直观——就像人类驾驶员会盯着前方某个点作为目标点进行转向一样，算法也会在路径前方选择一个"预瞄点"，然后计算如何转向才能让车辆朝着这个点移动。
在自动泊车场景中，Pure Pursuit算法相比其他控制方法有几个显著优势：
- 计算效率高：只需要简单的几何计算，适合实时控制
- 参数直观：主要调节参数只有预瞄距离，调试方便
- 适应性强：对路径形状没有特殊要求，能处理各种曲线
2. 泊车场景的路径规划与运动控制
2.1 平行泊车与垂直泊车的路径特点
平行泊车和垂直泊车虽然都是倒车入库，但路径规划上有明显差异：
平行泊车路径特点：
- 通常采用"S"形或分段圆弧路径
- 初始阶段需要较大转向角斜向进入
- 中段需要逐渐回正方向
- 末段可能需要微调位置
垂直泊车路径特点：
- 多采用"L"形或连续圆弧路径
- 转向角通常需要保持较大值较长时间
- 进入车位时需要快速回正方向
- 对初始位置要求相对宽松
2.2 自行车模型与运动学约束
在车辆控制中，我们通常使用自行车模型来简化车辆运动学：
code复制dx = v * cos(θ) * dt
dy = v * sin(θ) * dt
dθ = (v / L) * tan(δ) * dt
其中：
- (x,y)是车辆后轴中心位置
- θ是车辆航向角
- v是车速（倒车时为负值）
- L是轴距
- δ是前轮转向角
这个模型虽然简化，但能准确描述低速情况下的车辆运动特性，特别适合泊车场景。
注意：在实际编程实现时，要特别注意倒车情况下(v为负)的转向逻辑。此时转向角δ的符号应与前进时相反，否则会导致控制方向错误。
3. MATLAB实现详解
3.1 代码结构与参数配置
完整的Pure Pursuit泊车仿真代码主要包含以下几个部分：
- 参数配置区：用户可以修改的关键参数
matlab复制PARKING_MODE = 'parallel'; % 'parallel'或'perpendicular'
LOOKAHEAD_DIST = 2.5; % 预瞄距离(米)
VELOCITY = -1.0; % 倒车速度(m/s)，负值表示倒车
DT = 0.1; % 仿真步长(秒)
WHEELBASE = 2.8; % 车辆轴距(米)
MAX_STEER = deg2rad(45); % 最大转向角(弧度)
STOP_THRESH = 0.3; % 停止阈值(米)
- 路径生成区：根据泊车模式生成参考路径
- 主仿真循环：实现Pure Pursuit控制逻辑
- 可视化函数：绘制车辆、车位和轨迹
3.2 Pure Pursuit核心算法实现
算法核心代码位于主仿真循环中：
matlab复制% 寻找最近的路径点
[~, closest_idx] = min(sqrt((ref_x - current_x).^2 + (ref_y - current_y).^2));
% 在参考路径上寻找预瞄点
idx = find(path_dist >= LOOKAHEAD_DIST, 1, 'first');
if isempty(idx)
target_idx = length(ref_x);
else
target_idx = idx;
end
% 计算转向角
alpha = atan2(goal_y - current_y, goal_x - current_x) - state(3);
L = sqrt((goal_x - current_x)^2 + (goal_y - current_y)^2);
delta = atan2(2 * WHEELBASE * sin(alpha), L);
这段代码实现了Pure Pursuit的核心几何计算：
- 首先找到车辆当前位置在参考路径上的最近点
- 然后沿着路径向前寻找距离等于预瞄长度的点作为目标点
- 最后根据车辆与目标点的几何关系计算转向角
3.3 倒车逻辑的特殊处理
泊车场景都是倒车入库，这带来几个需要特别注意的问题：
- 速度符号：必须确保速度为负值，否则运动学模型会出错
- 转向响应：倒车时转向响应与前进相反，需要特别注意控制逻辑
- 预瞄点选择：倒车时预瞄点应该在车辆后方，但Pure Pursuit算法本身不区分前后
在代码中，我们通过固定预瞄距离并在接近终点时锁定最后一个路径点来保证稳定性：
matlab复制% 如果已经接近终点，直接锁定最后一个路径点作为目标
if dist_to_goal < LOOKAHEAD_DIST
goal_x = ref_x(end);
goal_y = ref_y(end);
end
4. 参数调优与性能分析
4.1 关键参数影响分析
预瞄距离(LOOKAHEAD_DIST)：
- 值过大：车辆会过早切入弯道，可能撞上路沿
- 值过小：车辆会频繁调整方向，轨迹振荡
- 建议值：泊车场景一般2.0-3.5米
车速(VELOCITY)：
- 绝对值越大：控制响应越滞后，需要更大的预瞄距离
- 绝对值越小：控制更精确，但仿真时间变长
- 建议值：1.0-2.0 m/s
仿真步长(DT)：
- 值过大：离散误差明显，控制不精确
- 值过小：计算量增加，仿真速度慢
- 建议值：0.05-0.1秒
4.2 不同泊车模式的参数差异
| 参数 | 平行泊车 | 垂直泊车 |
|---|---|---|
| 预瞄距离 | 较小(2.0-2.5m) | 较大(2.5-3.5m) |
| 最大转向角 | 较小(30-40°) | 较大(40-45°) |
| 路径曲率 | 中等 | 较大 |
| 速度 | 较慢(-1.0m/s) | 可稍快(-1.5m/s) |
5. 常见问题与调试技巧
5.1 典型问题排查指南
问题1：车辆无法准确入库
- 检查参考路径是否合理生成
- 验证初始位置是否在控制器可校正范围内
- 调整预瞄距离，尝试减小10-20%
问题2：轨迹振荡严重
- 减小车速绝对值
- 适当增大预瞄距离
- 检查转向角是否超过物理限制
问题3：车辆方向相反
- 确认速度值为负(倒车)
- 检查转向角计算符号是否正确
- 验证参考路径朝向是否合理
5.2 调试技巧与经验分享
- 可视化调试：实时显示预瞄点和车辆轨迹关系，直观理解控制行为
- 分阶段验证：先测试直线跟踪，再测试曲线，最后测试完整泊车路径
- 参数渐变法：从保守参数开始，逐步向激进参数调整，找到稳定边界
- 记录关键数据：保存每次仿真的状态历史，便于对比分析
一个实用的调试代码片段：
matlab复制% 在仿真循环中添加调试输出
if mod(step_count, 10) == 0
fprintf('Step %d: Pos=(%.2f,%.2f), Yaw=%.1f°, Delta=%.1f°, Dist=%.2fm\n',...
step_count, state(1), state(2), rad2deg(state(3)),...
rad2deg(delta), dist_to_goal);
end
6. 算法扩展与改进方向
6.1 自适应预瞄距离
固定预瞄距离在复杂路径中表现有限，可以考虑根据车速和路径曲率动态调整：
matlab复制% 根据车速和曲率动态调整预瞄距离
lookahead = max(1.5, min(3.5, abs(VELOCITY) * 2.5));
lookahead = lookahead * (1 + 0.5 * abs(curvature));
6.2 考虑车辆动力学约束
基础Pure Pursuit只考虑运动学，实际车辆有动力学限制，可以添加：
- 转向速率限制
- 加速度限制
- 轮胎摩擦圆约束
matlab复制% 转向速率限制
max_steer_rate = deg2rad(30); % 最大30°/s
delta = constrain(delta, prev_steer - max_steer_rate*DT,...
prev_steer + max_steer_rate*DT);
6.3 与高级路径规划算法结合
Pure Pursuit作为跟踪控制器，可以与以下规划算法结合：
- Hybrid A*：处理复杂障碍物环境
- RRT*：高维空间规划
- 多项式螺旋线：生成更平滑路径
在实际项目中，我尝试将Pure Pursuit与Hybrid A*结合，先规划出粗略路径，再用Pure Pursuit跟踪，效果显著优于单独使用任何一种方法。特别是在狭窄空间泊车时，这种组合方法能可靠地完成各种复杂泊车任务。