第三章 Pure Pursuit（纯追踪）算法：几何原理、前视距离选取、算法实现与仿真

各位同学，欢迎来到路径跟踪控制的第三讲。今天咱们聊聊一个非常经典、也非常直观的算法——Pure Pursuit，也就是纯追踪算法。

说实话，我刚开始做自动驾驶控制的时候，第一个上手的跟踪算法就是它。为什么？因为它简单、直观，而且效果还不错。你想想看，一个算法能让你用几何的方式去理解车辆怎么追着路径跑，这本身就很有意思。

3.1 几何原理：从自行车模型说起

Pure Pursuit 的核心思想，说白了就是：让车辆像追着一个点跑一样。

想象一下，你开着一辆车，前方路上有一个红色的标记点。你的任务就是让车的前轮始终指向那个点，然后开过去。这就是 Pure Pursuit 最朴素的几何解释。

我们来看一下它的数学模型。这里我假设你已经熟悉了自行车模型（Bicycle Model），如果不熟也没关系，我们简单回顾一下。

在自行车模型中，车辆的后轴中心是控制点，前轮转角决定了车辆的转弯半径。Pure Pursuit 算法在这个模型上，定义了一个“前视点”（Look-ahead Point），这个点位于目标路径上，距离车辆后轴中心一个固定的距离——我们称之为前视距离（Look-ahead Distance）。

算法的目标就是：计算一个前轮转角，使得车辆能够沿着一条圆弧，从当前位置行驶到前视点。

这个几何关系可以用一个简单的公式表达：

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

其中：

δ：前轮转角（我们要算的东西）
L：车辆轴距
α：车辆航向与前视点方向之间的夹角
ld：前视距离

嗯，这里要注意一点：这个公式是从圆弧几何推导出来的。我当年第一次看到这个公式时，觉得它美极了——一个简单的几何关系，就把路径跟踪问题变成了一个纯几何问题。

核心理解： Pure Pursuit 的本质是“用圆弧去拟合路径”。前视点越远，圆弧越平缓；前视点越近，圆弧越弯曲。

为了让你更直观地理解，我画了一张图：

这张图里，红色的车辆正在追踪绿色的前视点。橙色的虚线是前视距离 ld，紫色的线是车辆当前的航向，红色的弧线是算法规划出来的行驶轨迹。你看，只要算出 α，就能得到转角 δ。

3.2 前视距离的选取：算法的灵魂

好，几何原理讲完了。现在我要说一个关键问题：前视距离 ld 怎么选？

我个人习惯把前视距离比作“驾驶员的视野”。你开车时，如果只看眼前1米，那方向盘会打得飞快，车会左右摇摆；如果你看500米远，那方向盘几乎不动，但过弯时可能会冲出道路。

Pure Pursuit 也是一样。前视距离的选择直接影响跟踪性能：

前视距离	特点	问题
过小	响应快，跟踪精度高	容易振荡，稳定性差
过大	响应平滑，稳定性好	跟踪精度低，过弯会切内线
适中	平衡精度和稳定性	需要根据车速和曲率调整

我在项目中遇到过一个问题：某次在高速场景测试，车速到了80km/h，前视距离设成了10米。结果车辆在弯道里疯狂振荡，差点失控。后来我把前视距离调到了25米，问题就解决了。

所以，前视距离不能是固定的。常见的做法是让它随车速变化：

ld = k * v + ld_min

其中：

v：当前车速
k：比例系数（通常取0.3~0.5）
ld_min：最小前视距离（防止低速时太小）

我的经验： 对于乘用车，k 值我一般从0.3开始调。如果发现弯道跟踪有延迟，就减小 k；如果发现振荡，就增大 k。另外，别忘了加上 ld_min，我通常设5米左右。

还有一种更高级的做法：根据路径曲率来调整前视距离。曲率大的弯道，前视距离小一点；直道上，前视距离大一点。这个思路我在一个园区物流项目里用过，效果不错。

3.3 算法实现：从公式到代码

好了，理论讲完了，咱们来写代码。下面是一个 Pure Pursuit 的核心实现，我用 Python 写的，方便你理解。

import numpy as np

class PurePursuitController:
    def __init__(self, wheelbase, k=0.3, ld_min=5.0):
        self.L = wheelbase          # 轴距
        self.k = k                  # 前视距离系数
        self.ld_min = ld_min        # 最小前视距离
    
    def compute_steering_angle(self, vehicle_state, path_points, velocity):
        """
        计算前轮转角
        :param vehicle_state: [x, y, yaw] 车辆当前位置和航向
        :param path_points: 路径点列表 [[x1,y1], [x2,y2], ...]
        :param velocity: 当前车速
        :return: 前轮转角 (弧度)
        """
        # 1. 计算前视距离
        ld = self.k * velocity + self.ld_min
        
        # 2. 找到前视点
        look_ahead_point = self._find_look_ahead_point(
            vehicle_state, path_points, ld
        )
        
        if look_ahead_point is None:
            return 0.0  # 没找到前视点，保持直行
        
        # 3. 计算夹角 alpha
        dx = look_ahead_point[0] - vehicle_state[0]
        dy = look_ahead_point[1] - vehicle_state[1]
        
        # 将向量转换到车辆坐标系
        alpha = np.arctan2(dy, dx) - vehicle_state[2]
        
        # 4. 计算前轮转角
        steering_angle = np.arctan2(2 * self.L * np.sin(alpha), ld)
        
        return steering_angle
    
    def _find_look_ahead_point(self, vehicle_state, path_points, ld):
        """找到距离车辆后轴中心为 ld 的路径点"""
        min_dist = float('inf')
        target_idx = 0
        
        # 先找到最近的路径点索引
        for i, point in enumerate(path_points):
            dist = np.hypot(
                point[0] - vehicle_state[0],
                point[1] - vehicle_state[1]
            )
            if dist < min_dist:
                min_dist = dist
                target_idx = i
        
        # 从最近点开始，向前搜索距离为 ld 的点
        accumulated_dist = 0.0
        for i in range(target_idx, len(path_points) - 1):
            segment_dist = np.hypot(
                path_points[i+1][0] - path_points[i][0],
                path_points[i+1][1] - path_points[i][1]
            )
            accumulated_dist += segment_dist
            if accumulated_dist >= ld:
                # 线性插值找到精确位置
                ratio = (accumulated_dist - ld) / segment_dist
                x = path_points[i+1][0] - ratio * (path_points[i+1][0] - path_points[i][0])
                y = path_points[i+1][1] - ratio * (path_points[i+1][1] - path_points[i][1])
                return [x, y]
        
        # 如果路径不够长，返回最后一个点
        return path_points[-1]

这段代码里，我做了几件事：

根据车速计算前视距离
在路径上找到距离车辆后轴中心为 ld 的点
计算车辆航向与前视点方向的夹角 α
用公式算出前轮转角

注意： 找前视点时，我用了线性插值。为什么？因为路径点通常是离散的，直接取最近的点会导致转角跳变。插值后，前视点会平滑移动，控制效果会好很多。我曾经因为没做插值，导致车辆在弯道里一抖一抖的，排查了半天才发现是这个问题。

3.4 仿真验证：看看效果

代码写完了，咱们跑个仿真看看。我设了一个简单的圆形路径，半径20米，车速分别设为5m/s和10m/s。

先看低速情况：

# 仿真参数
wheelbase = 2.5        # 轴距 2.5m
k = 0.3                # 前视距离系数
ld_min = 5.0           # 最小前视距离
velocity = 5.0         # 车速 5m/s (18km/h)

# 创建控制器
controller = PurePursuitController(wheelbase, k, ld_min)

# 仿真结果（这里只展示关键数据）
# 最大横向误差: 0.12m
# 平均横向误差: 0.05m
# 转角最大值: 0.15 rad

再看高速情况：

velocity = 10.0        # 车速 10m/s (36km/h)

# 仿真结果
# 最大横向误差: 0.35m
# 平均横向误差: 0.18m
# 转角最大值: 0.22 rad

你看，车速提高后，跟踪误差变大了。这是因为前视距离变长后，车辆对路径的“跟随性”变差了。但好处是转角变化更平滑，车辆不会剧烈摆动。

这里有个权衡：你要精度，还是要舒适性？ 如果是泊车场景，精度优先，前视距离可以设小一点；如果是高速巡航，舒适性优先，前视距离要大一些。

避坑指南： 我曾经在一个项目中，把前视距离系数 k 设成了0.8，结果车辆在弯道里严重切内线，差点撞到路沿。后来我加了一个限制：前视距离不能超过路径曲率半径的1.5倍。这个经验分享给你。

3.5 Pure Pursuit 的优缺点

最后，咱们客观地评价一下这个算法。

优点：

实现简单，代码量少
几何意义清晰，容易调试
在低速场景下表现优秀
对路径平滑度要求不高

缺点：

高速时跟踪精度下降
前视距离需要手动调参
在曲率变化大的路径上表现不佳
没有考虑车辆动力学约束

说白了，Pure Pursuit 是一个“够用但不完美”的算法。它在很多实际项目中都能用，尤其是低速的园区物流、自动泊车等场景。但如果你要做高速的自动驾驶，可能需要更高级的算法，比如 Stanley 或者 MPC。

不过，我建议你先把 Pure Pursuit 吃透。为什么？因为它包含了路径跟踪最核心的思想——用几何关系去解控制问题。这个思想，在后面讲其他算法时也会用到。

好了，这一章就到这里。记住，算法是死的，人是活的。多调参、多试错，你才能真正理解它。

课后练习： 把上面的代码复制到你的环境里，换一条正弦曲线路径试试。观察不同前视距离下的跟踪效果。你会发现，前视距离对振荡的抑制效果非常明显。

📚 课程目录

第三章 Pure Pursuit（纯追踪）算法：几何原理、前视距离选取、算法实现与仿真

3.1 几何原理：从自行车模型说起

3.2 前视距离的选取：算法的灵魂

3.3 算法实现：从公式到代码

3.4 仿真验证：看看效果

3.5 Pure Pursuit 的优缺点