第四章：纯跟踪控制（Pure Pursuit）

各位同学，今天我们来聊聊纯跟踪控制。这个名字听起来挺玄乎，其实说白了就是——让机器人像狗追飞盘一样，去追着路径上的某个点跑。

我记得刚入行那会儿，带我的老工程师跟我说：「小X，路径跟踪这事儿，别想太复杂。你让车盯着前面一个点跑，跑到了再盯下一个，这不就完了？」我当时还觉得他太草率，后来才发现，这恰恰是纯跟踪的精髓所在。

4.1 基本原理

纯跟踪的核心思想，我可以用一句话概括：在当前车辆位置前方，选取一个目标点（我们叫它「前视点」），然后控制车辆朝着这个点行驶。

你想想看，这跟人走路其实很像。你走路的时候，不会盯着脚下那一亩三分地，而是会看着前方几米远的地方。为什么？因为这样走起来更稳，不容易跑偏。

具体到数学上，纯跟踪算法会计算一条从当前车辆后轴中心到前视点的圆弧。这个圆弧的曲率，就是我们要输出的控制量。嗯，这里要注意，曲率直接决定了方向盘该打多少。

核心公式：

曲率 κ = 2 * sin(α) / Ld

其中：

α 是车辆航向与前视方向之间的夹角
Ld 是前视距离

我在项目中遇到过一个问题：当车辆速度很慢时，这个公式算出来的曲率会特别大，导致方向盘猛打，车身剧烈晃动。后来怎么解决的？我加了一个速度相关的限幅，说白了就是低速时限制最大转角。

4.2 前视距离的选择

前视距离 Ld 是纯跟踪里最关键的参数，没有之一。选大了会怎样？车会「切弯」，走捷径，路径跟踪精度变差。选小了呢？车会左右摇摆，像喝醉了酒一样。

我个人习惯的做法是：让前视距离与车速成正比。车速越快，看得越远；车速越慢，看得越近。这很符合直觉，对吧？

车速范围	前视距离	效果
0 - 0.5 m/s	0.3 - 0.5 m	适合原地转向、低速调整
0.5 - 1.5 m/s	0.5 - 1.0 m	日常巡检速度，精度尚可
1.5 - 3.0 m/s	1.0 - 2.0 m	快速通行，会有一定切弯

我的调试技巧：

先固定一个中等前视距离（比如 1.0m），让车跑起来。如果发现路径跟踪有振荡，就增大前视距离；如果发现转弯时切弯太严重，就减小前视距离。来回调两三次，基本就能找到感觉。

我曾经在一个仓库项目里吃过亏。当时为了追求跟踪精度，把前视距离设得很小，结果车在转弯时疯狂抖动，差点把货架撞倒。后来我把前视距离从 0.3m 调到 0.8m，问题立刻解决了。所以，别一味追求精度，稳定性更重要。

4.3 算法实现步骤

纯跟踪的实现步骤，其实就五步。我习惯用伪代码来梳理逻辑，这样写代码时不容易漏掉边界情况。

// 纯跟踪算法伪代码
function purePursuit(robot_pose, path_points, lookahead_distance):
    // 步骤1：找到距离车辆最近的路点
    nearest_idx = findNearestPoint(robot_pose, path_points)
    
    // 步骤2：从最近点开始，沿着路径向前找前视点
    lookahead_point = findLookaheadPoint(path_points, nearest_idx, lookahead_distance)
    
    // 步骤3：如果找不到前视点（路径太短），就用终点
    if lookahead_point is None:
        lookahead_point = path_points[-1]
    
    // 步骤4：计算车辆到前视点的夹角 α
    alpha = calculateAlpha(robot_pose, lookahead_point)
    
    // 步骤5：计算曲率并输出转向指令
    curvature = 2 * sin(alpha) / lookahead_distance
    steering_angle = curvatureToSteering(curvature)
    
    return steering_angle

这里有个坑，我必须要提醒你：步骤2中找前视点的时候，一定要沿着路径方向找，而不是直线距离。否则在弯道上，你可能会找到路径外侧的点，导致控制异常。

注意：前视距离不能小于车辆的最小转弯半径。否则，算法会要求车辆做出物理上无法实现的转弯，导致控制失效。

4.4 优缺点分析

纯跟踪控制，我用它做过不下十个项目。它的优缺点，我心里门儿清。

优点

实现简单：代码量少，调试参数少，适合快速原型验证
平滑性好：生成的轨迹是圆弧，转向变化连续，不会出现突变
鲁棒性强：对路径的平滑度要求不高，即使路径有噪声也能正常工作
计算量小：只需要做简单的几何计算，嵌入式平台上也能跑得很流畅

缺点

精度有限：在高速或大曲率弯道上，切弯现象明显，跟踪误差较大
参数敏感：前视距离需要根据车速和路径曲率动态调整，否则效果很差
不考虑未来：只盯着一个前视点，没有全局规划的概念，容易陷入局部最优
不适合倒车：纯跟踪天生是为前向行驶设计的，倒车时表现很差

我个人觉得，纯跟踪最适合的场景是：低速、中等精度、路径曲率变化不大的场合。比如室内巡检机器人、AGV 小车、割草机器人等。如果你要做高速赛车或者精密对接，那还是看看别的算法吧。

4.5 知识体系图

下面这张图，是我自己总结的纯跟踪控制知识体系。你可以把它当作一张「地图」，学完本章后，对照着检查自己有没有遗漏的知识点。

这张图把纯跟踪的核心知识点串起来了。你从「基本原理」出发，理解几何关系；然后掌握「前视距离」这个关键参数；接着把「实现步骤」的代码写出来；最后用「优缺点分析」来评估它是否适合你的项目。一条线下来，思路就清晰了。

好了，纯跟踪控制的内容就讲到这里。下一章我们会聊另一种经典算法——Stanley 控制，它跟纯跟踪的思路不太一样，但同样非常实用。