4. Pure Pursuit深度解析：前视距离选择、曲率计算、速度自适应调节

Pure Pursuit，说白了就是让机器人像开车一样去追一个前方的虚拟点。这个算法在机器人圈子里火了这么多年，不是没有道理的。我最早接触它是在做AGV小车项目的时候，当时被它的简洁和鲁棒性惊艳到了——你想想看，一个算法只需要调一个参数，就能让车跑得像模像样，这多难得。

但别被它的简单外表骗了。真正要把Pure Pursuit用好，你得吃透三个核心问题：前视距离怎么选、曲率怎么算、速度怎么跟着路况变。今天我们就一个一个掰开揉碎了讲。

核心观点： Pure Pursuit的本质是一个几何追踪器，它不关心全局路径怎么来的，只关心「我现在该往哪转」。这个特性让它特别适合与全局规划器解耦。

4.1 前视距离：一个参数定乾坤

前视距离（lookahead distance）是Pure Pursuit的灵魂。我见过太多人一上来就问「这个参数设多少合适」，其实这个问题本身就问错了。

前视距离不是固定的，它应该随着车速和路径曲率动态变化。为什么？你想想看：

车速快的时候，前视距离要拉长，给机器人足够的反应时间
弯道急的时候，前视距离要缩短，不然容易切弯切到沟里去
直道上可以适当拉长，让行驶更平滑

我在项目中遇到过最典型的坑：把前视距离设成固定值，结果低速转弯时机器人疯狂振荡，高速直道时又像喝醉了酒一样左右摇摆。嗯，后来我改用自适应前视距离，问题就解决了。

常用的自适应策略有两种：

策略	公式	适用场景
速度线性映射	`ld = k * v + ld_min`	直道为主，弯道较少
曲率自适应	`ld = ld_base / (1 + alpha * \|curvature\|)`	多弯道，路径曲率变化大

我的经验： 如果你刚开始调参，建议先用速度线性映射。把k设为0.3~0.5，ld_min设为0.5米左右。跑一圈看看效果，再微调。

4.2 曲率计算：从几何到控制量

曲率计算是Pure Pursuit的数学核心。说白了，就是根据机器人当前位置、前视点位置，算出一个圆弧半径，然后转成转向角。

公式其实很简单：

// 计算曲率
double dx = lookahead_point.x - robot_pose.x;
double dy = lookahead_point.y - robot_pose.y;
double alpha = atan2(dy, dx) - robot_pose.theta;
double ld = sqrt(dx*dx + dy*dy);
double curvature = 2.0 * sin(alpha) / ld;

// 转成转向角
double steering_angle = atan(curvature * wheelbase);

这里有个细节很多人会忽略：alpha是前视点相对于机器人朝向的角度差。如果这个角度差太大，说明前视点选得太远了，或者路径拐弯太急。我曾经遇到过因为前视距离设得太大，导致alpha超过90度，结果机器人直接往反方向打方向盘——那场面，啧啧。

注意： 当ld非常小（比如机器人已经接近目标点）时，曲率计算会变得不稳定。建议加一个最小前视距离保护，比如ld = max(ld, 0.1)。

4.3 速度自适应调节：让机器人跑得又快又稳

速度控制是Pure Pursuit里最容易出彩也最容易翻车的地方。我的原则是：弯道慢、直道快、急弯更慢。

常用的速度调节策略：

曲率限速： 根据当前路径的曲率动态调整最大速度
前视点距离限速： 如果前视点离得近，说明快到终点了，减速
加速度限制： 防止速度突变导致机器人「点头」或「甩尾」

我一般这样实现：

// 曲率限速
double max_speed_by_curvature = v_max / (1.0 + beta * fabs(curvature));

// 前视点距离限速
double max_speed_by_distance = v_max * (ld / ld_max);

// 最终速度
double target_speed = min(max_speed_by_curvature, max_speed_by_distance);
target_speed = clamp(target_speed, v_min, v_max);

这里beta是个经验值，我一般取2~5。值越大，弯道减速越明显。你可以根据机器人的最大转向角来调——转向角越大，beta可以设小一点。

避坑指南： 我曾经在测试时发现机器人过弯时速度降得太慢，结果直接冲出路径。后来加了加速度限制，让速度变化更平滑。具体做法是：actual_speed = lerp(actual_speed, target_speed, dt / tau)，其中tau是时间常数，一般设0.2~0.5秒。

4.4 Pure Pursuit完整流程图

下面这张图展示了Pure Pursuit从输入到输出的完整流程。你可以看到，前视距离、曲率、速度三者是相互耦合的——前视距离影响曲率计算，曲率又反过来影响速度调节。

4.5 实战调参建议

说了这么多理论，最后给点实在的。如果你现在就要调Pure Pursuit，按这个顺序来：

先固定前视距离，设成1.0米左右，跑一圈看看基本行为
再调速度自适应，让机器人在弯道能自动减速
最后优化前视距离自适应，让行驶更平滑

记住一个原则：宁慢勿快，宁稳勿抖。Pure Pursuit不是赛车算法，它追求的是稳定追踪。我见过太多人一上来就想让机器人跑得飞快，结果调了三天参数还是原地打转。

小技巧： 在Nav2中，你可以通过lookahead_distance和min_lookahead_distance参数来设置前视距离的范围。我个人习惯把lookahead_time设成1.5~2.0秒，这样前视距离会自动随速度变化。

好了，Pure Pursuit的核心内容就这些。记住，算法是死的，人是活的。多跑几圈，多看看数据，你自然就能找到最适合你机器人的那组参数。