1. Pure Pursuit算法核心思想解析

Pure Pursuit算法的核心思想源自人类驾驶员的行为模式。想象一下你开车时的场景：眼睛会自然看向前方某个目标点，双手控制方向盘使车辆朝着该点行驶。这种"看远控近"的驾驶策略，正是Pure Pursuit算法的灵感来源。

几何模型构建的关键在于三个要素：

• 预瞄点（Lookahead Point）：在参考路径上距离车辆当前位置一定距离的点
• 预瞄距离（Lookahead Distance）：车辆当前位置到预瞄点的距离
• 转向圆弧：车辆沿圆弧行驶时能够恰好到达预瞄点

算法通过简单的几何关系建立控制模型。假设车辆后轴中心为A点，预瞄点为C点，车辆将沿着以O为圆心、半径为R的圆弧行驶。根据几何关系可得：

R = L / (2 * sin(α)) δ = arctan(2 * L * sin(α) / ld)

其中：

• L：车辆轴距（已知参数）
• α：预瞄点与车辆航向的夹角
• ld：预瞄距离（动态参数）
• δ：前轮转向角（控制输出）

这个简洁的公式就是Pure Pursuit的数学核心，它将复杂的路径跟踪问题转化为几何计算问题。

2. 算法优缺点深度分析

2.1 核心优势

对外界干扰的鲁棒性是Pure Pursuit最突出的优点。在实际测试中我们发现：

• 对路径噪声不敏感：即使参考路径有轻微波动，算法仍能保持平稳跟踪
• 参数调节直观：主要调节预瞄距离一个参数即可适应不同场景
• 计算效率高：单次迭代计算量极小，适合嵌入式平台实现

我在Autoware项目中的实测数据显示，在60km/h速度下，算法单次计算耗时不超过0.5ms（i7-1185G7处理器）。

2.2 固有局限性

急弯场景的适应性不足是主要痛点。当路径曲率突然增大时：

1. 固定预瞄距离会导致转向不足
2. 动态调整预瞄距离可能引发振荡
3. 横向误差会短暂增大到危险范围

我们在测试场用S形弯道做的对比实验显示，Pure Pursuit在曲率>0.1m⁻¹的弯道中，最大横向误差可达0.5m，而MPC算法能控制在0.2m以内。

2.3 典型应用场景

根据工程经验，Pure Pursuit最适合：

• 低速场景（<30km/h）：园区物流车、农业机械
• 平缓路径：高速公路、结构化道路
• 计算资源受限的平台：STM32等MCU

3. 预瞄距离优化策略

3.1 速度自适应方法

最基础的优化是使预瞄距离随速度线性变化：

ld = k * v + ld0

其中k=0.3-1.0，ld0=2-5m是常见参数范围。但这种方法在弯道表现不佳。

3.2 曲率补偿方法

更先进的方案是引入路径曲率补偿：

ld = min(ld_max, k1*v + k2/|κ| + k3*ey)

其中：

• κ：路径曲率（通过三点拟合计算）
• ey：当前横向误差
• k1,k2,k3：调节权重

实测表明，这种组合策略可使弯道跟踪精度提升40%以上。在Autoware的实现中，相关参数为：

ld_velocity_ratio: 2.4 # k1 ld_curvature_ratio: 120.0 # k2 ld_lateral_error_ratio: 3.6 # k3

3.3 动态限幅策略

为避免预瞄距离突变，需要设置合理边界：

ld = clamp(ld, min_ld, max_ld);

典型值为min_ld=3m，max_ld=15m。特殊场景如倒车时需单独设置：

reverse_min_lookahead_distance: 7.0

4. 工程实现关键细节

4.1 预瞄点选取算法

精确的预瞄点计算直接影响控制效果。Autoware采用两步法：

1. 粗搜索：找到路径上距离车辆最近的点

auto closest_idx = motion_utils::findNearestIndex(path, current_pose);

2. 精确定位：沿路径向前搜索，找到第一个距离大于ld的点

for(int i=closest_idx; i<path.size(); i++){ if(calcDistance(path[i], current_pose) > ld){ target_idx = i; break; } }

4.2 曲率计算优化

三点曲率估计法既简单又有效：

double calcCurvature(Point p1, Point p2, Point p3){ // 向量叉积计算曲率 double cross = (p2.x-p1.x)*(p3.y-p2.y) - (p2.y-p1.y)*(p3.x-p2.x); double d1 = sqrt(pow(p2.x-p1.x,2) + pow(p2.y-p1.y,2)); double d2 = sqrt(pow(p3.x-p2.x,2) + pow(p3.y-p2.y,2)); return 2*cross/(d1*d2*(d1+d2)); }

注意要设置最小距离阈值（通常4-6m），避免近距离点噪声干扰。

4.3 控制输出平滑处理

转向角滤波必不可少。我们采用二阶低通滤波器：

δ_filtered = 0.8*δ_prev + 0.2*δ_current

同时要限制转向角变化率：

double delta_rate = (delta - prev_delta)/dt; if(fabs(delta_rate) > MAX_DELTA_RATE){ delta = prev_delta + sign(delta_rate)*MAX_DELTA_RATE*dt; }

5. 不同场景下的参数调优

5.1 高速公路场景

特点：高速度（>80km/h）、大曲率半径 推荐参数：

k1 = 0.6 k2 = 100.0 k3 = 2.0 min_ld = 10.0 max_ld = 30.0

5.2 城市道路场景

特点：中低速（30-50km/h）、频繁启停 推荐参数：

k1 = 0.4 k2 = 80.0 k3 = 3.0 min_ld = 5.0 max_ld = 15.0

5.3 停车场场景

特点：低速（<10km/h）、急弯多 推荐参数：

k1 = 0.3 k2 = 50.0 k3 = 5.0 min_ld = 3.0 max_ld = 8.0

6. 典型问题排查指南

6.1 振荡问题

症状：车辆方向持续左右摆动解决方案：

1. 增大预瞄距离（增加k1或ld0）
2. 降低转向比例增益
3. 检查路径平滑度（建议使用5点移动平均滤波）

6.2 转向不足

症状：车辆过弯时切弯解决方案：

1. 减小k1使预瞄距离更短
2. 增大k2加强曲率补偿
3. 检查最大转向角限制是否合理

6.3 响应延迟

症状：车辆动作总是慢半拍解决方案：

1. 检查控制周期（建议>20Hz）
2. 验证传感器数据时延
3. 适当减小预瞄距离

7. 进阶优化方向

7.1 路径重采样策略

原始路径点密度不均会导致控制抖动。建议采用等距重采样：

vector<Point> resamplePath(const vector<Point>& path, double ds){ vector<Point> new_path; double accumulated = 0.0; for(int i=0; i<path.size()-1; i++){ double seg_len = distance(path[i], path[i+1]); while(accumulated < seg_len){ double ratio = accumulated/seg_len; new_path.push_back(interpolate(path[i], path[i+1], ratio)); accumulated += ds; } accumulated -= seg_len; } return new_path; }

7.2 速度自适应曲率计算

动态调整曲率计算窗口能提升精度：

double adaptive_window = max(5.0, 0.5*current_speed);

7.3 预测轨迹生成

生成预测轨迹可提前发现问题：

vector<Pose> predictTrajectory(Pose current, double delta, double speed, int steps){ vector<Pose> traj; for(int i=0; i<steps; i++){ double yaw_rate = speed * tan(delta) / WHEELBASE; current.x += speed * cos(current.yaw) * DT; current.y += speed * sin(current.yaw) * DT; current.yaw += yaw_rate * DT; traj.push_back(current); } return traj; }

8. 与其他算法的对比融合

8.1 与PID对比

• Pure Pursuit：基于几何，天然抗干扰，但理论保证弱
• PID：误差驱动，需要精细调参，易振荡

实际工程中常采用串级控制：外层Pure Pursuit生成目标曲率，内层PID跟踪曲率。

8.2 与MPC融合

前沿方案采用混合架构：

1. Pure Pursuit提供初始猜测
2. MPC在局部进行优化
3. 权重动态调整：高速侧重Pure Pursuit，弯道侧重MPC

这种架构在保持计算效率的同时，提升了弯道性能。某量产方案数据显示，跟踪误差可降低35%。

9. 实车调试经验分享

9.1 调试准备

必备工具清单：

1. 高精度GPS/RTK（厘米级定位）
2. 转向角传感器（分辨率<0.5°）
3. 数据记录仪（100Hz以上采样率）
4. 可视化工具（如rqt_plot）

9.2 调试步骤

分阶段验证法：

1. 静态测试：验证算法输出是否符合预期
2. 低速测试（<10km/h）：检查基础功能
3. 中速测试（30-50km/h）：优化参数
4. 高速测试（>80km/h）：验证稳定性

9.3 安全注意事项

必须实现的安全机制：

1. 转向角速率限制
2. 紧急停止开关
3. 故障检测（如NaN检查）
4. 看门狗定时器

10. 未来发展方向

10.1 自适应参数优化

采用在线学习技术，使参数能随以下因素自适应：

• 车辆载重
• 轮胎磨损状态
• 路面附着系数

10.2 多模态感知融合

结合视觉语义信息增强预瞄点选择：

• 识别车道线类型（实线/虚线）
• 检测障碍物侵入区域
• 估计路面摩擦系数

10.3 车路协同应用

在V2X环境下的进化方向：

1. 获取前方道路精确曲率
2. 预见性速度规划
3. 协同避障策略

某测试场数据显示，结合V2X信息可使预瞄距离优化提前3秒，横向误差降低40%。