1 介绍

1.1 概述

纯追踪控制器的核心是沿着朝向前视点的弧线行驶。

理解和可视化纯追踪控制器的更直观的方式是下面的 GIF：

使用纯追踪控制器进行路径跟踪的快速演示：

1.2 发展历史

• 第一个应用是在Terragator上，Terragator是一个六轮驱动的机器人，用于80年代初的户外视觉实验。

• 当Terragator的工作转移到新的NavLab项目时，弧线命令算法也随之转移。在NavLab项目中，提出了包括纯追踪在内的多种路径跟踪算法，包括五次多项式方法和“控制理论”方法。所有这些算法的测试表明，纯追踪方法是最为稳健和可靠的方法。

• Amidi在其硕士论文中包含了他对前述三种方法的比较结果。这项研究帮助确定了纯追踪算法作为通用跟踪算法的潜力。

• 1992年，卡梅隆大学发表Pure Pursuit算法论文，该算法在机器人导航领域取得了巨大成功。
  卡梅隆大学 Pure Persuit 算法

• 进入21世纪后，Pure Pursuit算法得到了进一步的发展和扩展。研究人员开始将其与其他路径规划和控制算法结合使用，如 Stanley 控制器和 MPC（Model Predictive Control）等，以提高路径跟踪的精度和稳定性。同时，算法的计算效率也得到了优化，使其能够在更加复杂和动态的环境中应用。

• 随着自动驾驶技术的发展，Pure Pursuit算法被广泛应用于无人驾驶汽车和各种机器人系统中。特别是在实际道路测试中，Pure Pursuit作为一种基础的路径跟踪算法，展示了其在不同场景下的可靠性和稳定性。在此期间，研究者们进一步优化了算法的参数调节策略，使其在处理不平坦路面、动态障碍物和其他复杂条件时更加鲁棒。

1.3 EKF vs MPC vs Pure Pursuit

算法	原理	应用场景	优点	缺点
EKF	非线性滤波技术	目标跟踪、导航、自动驾驶等	能处理非线性系统，精确估计系统状态	强非线性时可能带来较大误差
MPC	基于模型的优化控制策略	工业过程控制、机器人导航、自动驾驶等	处理复杂动态系统，实时性强，适应性强	计算量大，对模型准确性要求高
Pure Pursuit	几何跟踪控制算法	自动驾驶、机器人路径跟踪	算法简单、易于实现，实时调整前轮转角	对预瞄点选择敏感，极端场景下表现不佳

1.4 PP 前探距离的影响

特性	较小前探距离	较大前探距离
路径跟踪精度	较高 因为机器人会频繁调整方向以适应路径	较低 因为较长的直线行驶可能导致对小曲率的忽略
系统稳定性	较高 因为机器人能够快速适应路径变化	较低 因为较大的前探距离可能导致较大的路径偏差
响应速度	较快 因为机器人更频繁地更新目标点	较慢 因为机器人在较长距离内保持当前目标点
转弯半径	较小 因为机器人更频繁地进行小角度转弯	较大 因为机器人在较长距离内保持直线行驶，可能导致较大的转弯半径

2 理解

普渡大学–control-algorithms/basic-pure-pursuit

Originally written by Sarah Xiang from VRC team 97963A and VEXU team ILLINI
普渡大学–control-algorithms/basic-pure-pursuit

准备

• 掌握反馈控制回路（如PID）的工作知识
• 一个功能齐全的位置跟踪（里程计）系统
• 基本编程知识（数组和循环）
• 高中数学，例如三角学

导入必要的库

import numpy as np
import matplotlib.pyplot as plt
import math
import matplotlib.animation as animation
from IPython import display

什么是 Pure Pursuit 控制器？

纯追踪控制器是一种用于轮式移动机器人的自动转向方法。它是一种转向方法，这意味着它可以计算轮式机器人保持在预先计算的路径上所需的角速度。假设线速度为常数。因此，如果您希望在机器人接近目标时减慢速度，则需要您选择的额外速度控制器（比例控制器这样简单的东西就可以起作用）。

使用纯追踪控制器进行路径跟踪的快速演示：

在上面的动画中，虚线灰色线是机器人需要遵循的预先计算的路径，实线黑色线是机器人的轨迹。大黄点和短红线代表机器人的当前位置和航向。还有另一条实线连接机器人“追赶”的点和机器人的当前位置，但在这个演示中很难看到。虽然机器人的轨迹与路径并不完全匹配，但纯追踪控制器仍然表现得相当不错。

限制

如上所示，由于前瞻距离不为零，纯追踪控制器无法完美地追踪路径。路径包含的尖角越多，性能越差。在存在尖角的情况下，有两种方法可以实现更好的性能：

• 在路径生成过程中优化路径
• 对纯追踪控制器本身进行改进

如何工作

纯追踪控制器的关键在于计算路径上一定距离的目标点 $l_d$ ，称为前瞻距离，从机器人的当前位置开始。然后，使用目标点计算机器人向该点移动所需的适当角速度。随着机器人向前移动，将选择路径上的另一个目标点，并更新机器人的角速度。由于机器人永远无法到达这个目标点，并且目标点停留在路径上，因此最终结果将是机器人跟踪路径。

为了数学上找到机器人要跟随的适当目标点，以机器人当前位置为中心，以半径 $l_d$ 画一个圆。这个圆与路径的交点就是潜在的目标点。我们将在文档后面的部分再次讨论算法的这一部分。

理解和可视化纯追踪控制器的更直观的方式是下面的 GIF：

胡萝卜和驴子之间的距离始终是固定的，所以驴子会追着胡萝卜，按照骑手想要的方向移动。

在我们实现的纯追踪控制器中，路径是一个由等距点（一维数组）表示的二维数组。基本算法是使用 for 循环遍历每对点，以确定每条线段内是否有任何交点。如果找到目标点，则遵循最合适的目标点。这些步骤将重复进行，直到到达路径的末端。

线圆交点

让我们先来看看选择目标点背后的基本数学原理——线圆交点。有多种方法可以找到交点，我们发现这种方法（下面的屏幕截图）最容易实现和调试。

在上面介绍的方法中，圆被假定为以原点为中心。在我们的应用中，圆将以机器人的当前位置为中心 [currentX, currentY]，定义线的两个点将是任意点 pt1 = [x1, y1] 和 pt2 = [x2, y2]。因此，要应用此方法，我们首先需要从 [x1, y1] 和 [x2, y2] 中减去 currentX 和 currentY 以使系统以原点为中心。同样，我们执行这样的偏移，以便我们可以使用上面屏幕截图中的方法，该方法要求圆以原点为中心。

线圆交点与边界

你可能已经注意到了，在处理上述算法时，有些地方不太对劲。在某些条件下，尽管找到的交点位于由 pt1 和 pt2 定义的无限直线上，但它们并不完全在 [x1, y1] 和 [x2, y2] 的范围内。请考虑下面所说明的情况：

尽管找到了交点，但它们并不在路径中的任何线段上。这些不是我们希望机器人跟随的点！那么我们如何防止这种情况发生呢？

解决方案很简单。在找到交点之后，我们需要检查它们的 x 值是否在 [min(x1, x2), max(x1, x2)] 的范围内，以及它们的 y 值是否在 [min(y1, y2), max(y1, y2)] 的范围内。也就是说，为了使解有效并且 intersectFound 为 True，需要满足以下条件：

判别式 >= 0
min(x1, x2) <= sol_x <= max(x1, x2)
min(y1, y2) <= sol_y <= max(y1, y2)

你可以在下面的代码单元中修改你的 line_circle_intersection 函数。这里也提供了完成并注释的代码。
（旁注：在 VEXCode 中使用 (a >= x) && (x >= b)。）

选择目标点

线与圆相交（只有一条线段）的所有可能结果如下：

情况 1：
无交集，如果是这种情况，判别式将为负数。

情况 2：
发现交点，但它们不在(x1, y1)和(x2, y2)之间。判别式为正，但我们仍应将这种情况视为“无交点”。

情况 3：
(x1, y1)在和的范围内有且仅有一个交点(x2, y2)。判别式为正，机器人应沿着找到的交点行进。

情况 4：
存在两个交点，并且它们都位于 (x1, y1) 和 (x2, y2) 之间。在这种情况下，我们需要确定哪个点更适合机器人跟随。我们可以使用的一种方法是计算交点与第二个点 (x2, y2) 之间的距离（路径沿着 (x1, y1) -> (x2, y2) 的方向），并选择离第二个点更近的点（换句话说，就是离路径末端更近的点）。

极端情况：
在某些极端情况下，当机器人通过急转弯时，它可能会与多个线段产生多个交点（如下图所示）。我们可以编写程序，让机器人跟随它找到的第一个有效点。这样，在路径重叠自身或存在多个急转弯的极端情况下，机器人就不会完全跳过路径的一部分。为了防止机器人在路径上向后走，我们可以创建一个变量 lastFoundIndex 来存储它刚刚经过的点的索引。每次循环运行时，它将只检查位于 path[lastFoundIndex] 之后的点。这样，机器人已经通过的线段将不会被再次检查交点。在没有找到新交点的情况下（机器人偏离路径），机器人将跟随 lastFoundIndex 处的点。

让我们更仔细地看看 lastFoundIndex 以及选择目标点的函数应该如何工作。

当机器人第一次进入路径（第一次循环迭代）时，lastFoundIndex = 0。线-圆交点搜索从 path[0] 开始，找到一个交点并将其选为机器人前进的目标点。

在第一次循环迭代结束时，由于目标点位于 path[0] 和 path[1] 之间，lastFoundIndex 更新为 0。当第二次循环迭代开始时，机器人已经向 path[1] 靠近。由于 lastFoundIndex 仍然是 0，线-圆交点搜索再次从 path[0] 开始。这一次，与路径有两个交点：一个位于 path[0] 和 path[1] 之间，另一个位于 path[1] 和 path[2] 之间。

按照正常程序，算法会选择位于 path[0] 和 path[1] 之间的交点作为目标点，并跳出搜索循环。从下图可以看出，这不是一个很好的选择，因为这会导致机器人向后移动。为了避免这种不良的目标点选择，我们可以添加一个额外的条件来评估算法找到的目标点：只有当选定的目标点与路径中的下一个点之间的距离比机器人当前位置与路径中的下一个点之间的距离更短时，搜索循环才能跳出。如果上述陈述不成立，则搜索循环继续。如果机器人未能在下一条线段中找到交点，我们也可以增加 lastFoundIndex。这将防止机器人向后移动，因为当找不到交点时，path[lastFoundIndex] 将成为目标点。

等效的伪代码：

if pt_to_pt_distance (goalPt, path[i+1]) < pt_to_pt_distance (currentPos, path[i+1]) :  
    break
else:  
    lastFoundIndex += 1
    continue

在下图所示的情况下，很明显目标点 goalPt（左侧交点）与 path[1] 之间的距离大于 currentPos（当前位置）与 path[1] 之间的距离（两个距离都用虚线红线标出）。因此，搜索循环继续，下一个搜索循环迭代将选择位于 path[1] 和 path[2] 之间的交点。

在第二次循环迭代结束时，lastFoundIndex 更新为 1。当第三次循环迭代开始时，下一次搜索将从 path[1] 开始，因此将跳过位于 path[0] 和 path[1] 之间的交点。被标记为寻找目标点而省略的路径部分用棕色标出。

下面的代码单元可以用来你自己实现 goal_pt_search 算法（对于不熟悉 Python 的人，提供了注释过的代码）。已提供一个示例路径。

向目标点移动 - 第 1 部分

现在我们已经确定了目标点，下一步将是让机器人朝该点移动。让我们首先看看如何让机器人移动到一个固定的靶标点。考虑下图所示的情况，机器人位于 [currentX, currentY]，我们希望让它移动到点 [targetX, targetY]：

机器人需要同时执行两个动作：向目标点移动和向目标点转向。为了实现这一点，我们需要分别计算线性速度和转向速度，并将它们相加以获得最终速度。在这种情况下，获取线性误差是容易的。在上面的图中，机器人需要行驶的总距离用虚线灰线标出，其值为 sqrt(pow(targetY - currentY, 2) + pow(targetX - currentX, 2)) ，$\sqrt{(x_2-x_1{)}^2+(y_2-y_1{)}^2}$
然而，获取转向误差则稍微复杂一些。机器人需要转动的总量用蓝色标出。由于转向方向是逆时针，蓝色角度（从这一点起将被称为 turnError）应该是正数。我们可以通过执行减法来获得这个角度的大小：

turnError 可以通过从灰色角度（即 absTargetAngle）减去橙色角度（即 currentHeading）来计算。灰色角度是由向量 [targetX-currentX, targetY-currentY] 与全局 X 轴形成的角度。从现在起，我们将称这个灰色角度为 absTargetAngle。我们可以使用内置的 atan2 函数来计算它的值。由于 atan2 的输出范围是 -180 到 180，但我们的笛卡尔坐标系的范围是 0 到 360，如果它是负数，我们可以简单地在 atan2 的输出上加 360。

在我们继续之前，有一些特殊情况值得一提。如果 currentHeading 是 1 度，而 absTargetAngle 是 359 度，我们会得到 turnError = absTargetHeading - currentHeading = 1 - 359 = -358 度，这完全是无意义的。为了使函数尽可能高效，如果 turnError 最终大于 180 度，机器人应该从另一个方向转向目标。

下面的代码单元是供你编写 find_min_angle 函数的空间，该函数计算机器人朝目标点转向所需的最小转向误差。如果你的实现是正确的，返回的角度在所有情况下应该保持在 [-180, 180] 的范围内。如果你更熟悉使用旋转和取模（%）而不是航向，可以随意以那种方式实现该函数。后面部分提供的动画函数默认使用航向，但也会提供与旋转和取模兼容的代码。

向目标点迈进 - 第2部分

得到了线性误差和转向误差之后，我们可以根据这些，计算机器人左右两侧的速度。由于转向误差在机器人逆时针转向时为正，左侧的速度应该是 linearVel - turnVel，右侧的速度应该是 linearVel + turnVel。在下面的代码示例中，使用了一个基本的比例控制器来计算这两个速度。请注意，动画代码假定 linearVel 和 turnVel 的单位是 velocityUnits::pct。

总结一下，移动到目标点的算法如下：
1.计算线性误差。
2.计算转向误差，确保它在 [-180, 180] 范围内。
3.根据线性误差和转向误差，使用比例控制器计算左右两侧的速度。
4.将计算出的速度应用到机器人上，使其朝目标点移动。

把所有东西放在一起

人们很自然地认为，纯追踪控制器本质上只是反复向位于路径上的目标点移动，但将线性误差设置为剩余路径的长度。根据我们以往的经验，这种基本实现对于 VRC（虚拟机器人竞赛）来说效果相当不错。原始的纯追踪控制器，用于自行车模型车辆，以一种略有不同且更复杂的方式计算角速度。下一节将简要讨论其对差分驱动机器人的适应性。在将所有步骤结合起来之前，让我们快速回顾一下这些组件是什么：

• 带边界的线-圆交点
• 目标点搜索和选择
• 找到合适的转向误差并计算转向速度

你可能想知道在哪里计算线性（向前）速度。答案是：它可以是任何值。机器人的线性速度可以单独用你选择的控制器来计算。你可以让它保持恒定，与路径上剩余的距离成比例，或者应用一些花哨的算法。如前所述，纯追踪控制器是一个自动转向控制器，输出适当的角速度。

有一个参数极大地影响控制器的性能，那就是预瞄距离 $l_d$。你可能认为这个距离越小越好，但小的预瞄距离容易有更多的振荡。另一方面，大的预瞄距离经常造成“切角”问题。根据经验，我发现对于 VRC 机器人，典型的预瞄距离范围从 0.7 英尺到 1.5 英尺。我建议尽量不要将预瞄距离设置在 0.5 英尺以下。尽管模拟看起来更完美，在现实世界中，机器人将无法很好地使用 0.5 英尺或更短的预瞄距离运作。

下面的代码单元可用于自己实现完整的算法。提供的示例路径是一个闭环，因此恒定纵向速度在此路径下工作得很好。我们鼓励您尝试不同的控制律和不同的路径。对于非闭环的路径，您需要考虑如何让机器人在路径末端正确停止。有很多方法可以实现这一点，留给读者自己去弄清楚。

附加信息

回到角速度

如上所述，原始的纯追踪控制器以不同的方式计算角速度。期望机器人行驶一个弧线，这个弧线同时与它的当前位置和目标点相交，并有一个额外的约束条件，即这个弧线必须与连接机器人前后轮的线相切。在行驶这个弧线的过程中，机器人的角速度保持不变。在我们上面的实现中，为了使机器人到达目标点，它的角速度随着转向误差的变化而变化。差异在下图中进行了说明：

要将这种弧线行驶行为适应到差分驱动机器人上，考虑以下问题：假设机器人的线速度保持不变，并且已知机器人的宽度，机器人的左右两侧需要有什么样的速度差才能产生这种弧线轨迹？

为了计算适当的转向速度，我们首先需要找到我们希望机器人行驶的弧线的半径（在下图中表示为 R）。

使用三角函数很容易计算。利用底部的直角三角形，我们可以得到：
$R=\frac{lookaheaddistance/2}{sin(turnerror)}$

接下来，考虑下面的图表：

将机器人左侧行驶的距离表示为 $L_l$ ，右侧行驶的距离表示为 $L_r$。假设机器人在时间 Δt 后完成了弧线的行驶。由于机器人左侧的速度为 linearVel − turnVel，右侧的速度为 linearVel + turnVel，我们可以得到以下方程：

$L_l=(linearVel-turnVel)\cdot \Delta t=(R-\frac{W}{2})\cdot (2\cdot turnError)$

$L_r=(linearVel+turnVel)\cdot \Delta t=(R+\frac{W}{2})\cdot (2\cdot turnError)$

将第一个方程除以第二个方程，我们得到：
$\frac{(linearVel-turnVel)\cdot \Delta t}{(linearVel+turnVel)\cdot \Delta t}=\frac{\left(R-\frac{W}{2}\right)\cdot (2\cdot turnError)}{(R+\frac{W}{2})\cdot (2\cdot turnError)}$

$\frac{(linearVel-turnVel)}{(linearVel+turnVel)}=\frac{\left(R-\frac{W}{2}\right)}{(R+\frac{W}{2})}$

进一步简化为：
$turnVel=\frac{W}{2R}\cdot linearVel$

用 $\frac{\text{look ahead distance}/2}{sin(\text{turn error})}$ 替代 $R$，获得最终公式：

$turnVel=\frac{W\cdot \sin (turnerror)}{\text{look ahead distance}}\cdot linearVel$

生成航点

纯追踪控制器不会生成机器人要遵循的路径。相反，输入到函数中的所有路径都必须预先计算。

thomasfermi/Algorithms-for-Automated-Driving/PurePursuit

thomasfermi/Algorithms-for-Automated-Driving/PurePursuit
https://github.com/thomasfermi/Algorithms-for-Automated-Driving

汽车横向控制，纯追踪控制器的实现。

在本节中，我们希望控制前轮转角 $\delta$，使车辆沿着给定的路径行驶。这被称为车辆横向控制。

在纯追踪方法中，在期望路径上识别出一个目标点（TP），该点距离车辆有一个前视距离 $l_d$。角度 $\delta$ 的选择是根据运动学自行车模型来确定的，使得车辆能够到达目标点。

前视距离是一个参数，通常根据速度 $v$ 来选择，通常通过 $l_d=K_{dd}v$ 来确定，其中常数 $K_{dd}$ 需要进行调试。我们还可以设置最小和最大前视距离，以避免在非常高和非常低的速度下出现不理想的行为。

接下来，我们绘制自行车模型和需要跟随的给定路径。我们还在后轮中心周围绘制了一个半径为 $l_d$ 的圆。该圆与路径的交点就是我们的目标点 TP。根据运动学自行车模型，车辆将沿着橙色弧线移动，而这条弧线是由前轮转角 $\delta$ 决定的。我们的目标是选择一个 $\delta$，使得橙色的车辆轨迹能够到达目标点。

由于上面的图形是通过编程生成的（使用tikz），我们可以更改直至车辆轨迹经过目标点：

$\delta =25°$

$\delta =20°$

$\delta =15°$

$\delta =11.3°$

但比尝试一堆不同的 $\delta$ 值更优雅的解决方案是，我们可以根据下图中的品红色三角形实际计算出最佳的 $\delta$ 值。

首先，我们注意到从瞬时旋转中心 (ICR) 到目标点 (TP) 的距离等于 $R$，因为 TP 位于围绕 ICR 的半径为 $R$ 的橙色圆上。因此，品红色三角形是 等腰三角形，且 ${\gamma }_2={\gamma }_3$。从图中可以看到 ${\gamma }_3+\alpha =90°$。因此 ${\gamma }_2={\gamma }_3=90°-\alpha$。由于三角形内角和为 $180°$，我们有

$$180°={\gamma }_1+{\gamma }_2+{\gamma }_3={\gamma }_1+(90°-\alpha )+(90°-\alpha )$$

这意味着 ${\gamma }_1=2\alpha$。根据 正弦定理

$$\frac{l_d}{\sin ({\gamma }_1)}=\frac{R}{\sin ({\gamma }_2)}$$

这里，我们用到了后轮到目标点 TP 的距离是 $l_d$。如果我们将 ${\gamma }_1=2\alpha$ 和 ${\gamma }_2=90°-\alpha$ 代入上述公式，得到

$$\frac{l_d}{\sin (2\alpha )}=\frac{R}{\sin (90°-\alpha )}$$

根据 三角加法公式，我们有 $\sin (90°-\alpha )=\cos (\alpha )$ 和 $\sin (2\alpha )=\sin (\alpha +\alpha )=2\sin (\alpha )\cos (\alpha )$。因此，我们可以进一步简化公式，得到

$$\frac{l_d}{2\sin (\alpha )\cos (\alpha )}=\frac{R}{\cos (\alpha )}$$

从而得出 $R=l_d/(2\sin (\alpha ))$。对于运动学自行车模型，我们之前推导出车轮转角 $\delta$ 是 $R$ 的函数，公式为 $\delta =\arctan (L/R)$，其中 $L$ 是车轮基地，即车轮之间的距离。将其与新得出的 $R$ 公式结合，我们最终得到

$$\delta =\arctan \left(\frac{2L\sin (\alpha )}{l_d}\right)$$

这就是我们需要选择的转角 $\delta$，以到达目标点！我们现在可以写出纯跟踪算法了：

纯跟踪算法 – 对于每个时间点：

• 计算前视距离 $l_d$，公式为 l_d = np.clip(K_dd * speed, min_ld, max_ld)。函数 np.clip 的文档可在 这里 查阅。K_dd、min_ld 和 max_ld 是可调参数。
• 找到目标点 TP，该点为后轮周围半径 $l_d$ 圆与期望路径的交点。
• 使用目标点坐标 (x_tp, y_tp)，确定 $\alpha$，公式为 alpha=arctan2(y_tp,x_tp)。
• 使用公式 $ \delta = \arctan \left(\frac{2 L \sin(\alpha)}{l_d}\right) $ 计算纯跟踪前轮角度 $\delta$。
• 行动：转动方向盘以将前轮角度设为 $\delta$。

练习

在本次练习中，您将实现纯跟踪和PID控制。

如果您没有完成车道检测的章节，可能没有设置Python环境，也没有下载练习代码。在这种情况下，请访问附录进行设置。

开始工作，打开 code/tests/control/target_point.ipynb 并按照指示进行操作。接着，打开 code/tests/control/control.ipynb 并继续操作。本次练习使用Jupyter Notebook中的简单车辆模拟器来测试代码。如果成功完成这些练习，您还可以在Carla仿真中运行控制器：

• 通过执行 CarlaUE4.exe (Windows) 或 CarlaUE4.sh (Linux) 启动Carla。
• 执行 python -m code.tests.control.carla_sim --ex 观察控制算法的运行。如果省略 --ex 标志，将看到示例解决方案。
• 默认情况下，从Carla的HD地图中查询车道中心并将其作为参考路径提供给控制器。但如果运行 python -m code.tests.control.carla_sim --ex --ld，将使用您的 LaneDetector：左右车道边界的平均值 $(y_l(x)+y_r(x))/2$ 将作为参考路径提供给控制器。请注意，在 carla_sim.py 中有一个关于正确调用 LaneDetector 构造函数的 “TODO” 项。应确保仿真运行时没有错误。由于同时运行Carla仿真和 LaneDetector 会消耗大量硬件资源，除非您的计算机性能非常强大，否则仿真可能每秒只运行几帧。

PDF–DAWGMA–Team 1712 – Pure Pursuit Controller

PDF–DAWGMA–Team 1712

摘要

自适应纯追踪控制器使机器人能够快速、平稳且准确地跟随路径。与基于时间流逝提供目标轮速的运动轮廓法不同，纯追踪基于机器人相对于其想要跟随的路径的位置来提供目标速度。这使得它非常稳健：如果控制器的参数调整不当，机器人起始位置错误，地毯不平坦，甚至如果它撞击到其他机器人，机器人仍能自我校正以跟随路径并到达目标。我们要感谢卡内基梅隆大学的作者们提供的算法概述，可以在此处找到：链接。我们强烈建议将其作为本白皮书的补充阅读。卡梅隆大学 Pure Persuit 算法

概述

纯追踪控制器的核心是指导机器人沿着从当前位置到目标点的弧线行驶。这个目标点被称为前探点，它是距离机器人前视距离的路径上的一个点。随着机器人沿着路径移动，它瞄准前探点，前探点随着机器人沿路径移动。通过这种方式，机器人可以被认为是“追踪”前探点。这一行动类似于人类驾驶员在路上看一个点并朝那个点瞄准。

实施纯追踪控制器有三个部分：里程计、生成路径和跟随路径。让我们从里程计开始。

里程计

里程计是确定机器人的XY位置。最简单的方法是使用附加在驱动轮上的编码器来测量机器人移动了多远，并使用陀螺仪来测量方向。通过每秒重复这个数学运算50次，机器人可以得到其当前位置的公平近似。数学公式如下：

$distance=(changeinleftencodervalue+changeinrightencodervalue)/2$

$xlocation+=distance\ast cosine(robotangle)$

$ylocation+=distance\ast sine(robotangle)$

这给出了相对于其起始位置的机器人位置，默认情况下是(0, 0)。如果您将起始位置更改为机器人在场地上的起始位置，那么计算出的位置将是场地中心的（相对于场地）。当连续运行路径时，使用场地中心坐标可以减少由于机器人没有精确停止而引起的误差累积，因为机器人仍然知道它在场地上的位置并且可以在下一个路径中自我校正。
场地中心坐标还可以使绘制路径更加直观。您也可以使用其他形式的里程计，例如视觉或激光雷达（本文中未讨论）。

路径生成

路径是由一系列XY坐标（称为航点）组成的，首先通过概述一些机器人要跟随的航点。然后，软件将更多的航点注入到路径中并平滑路径。然后计算并存储每个航点的以下信息：

• 该点沿路径的距离
• 机器人在该点的目标速度
• 该点路径的曲率

在创建路径时，用户还设置了机器人的最大速度和加速度。以下是创建路径航点的可视化：

插值点

拥有密集的点可以提高速度设定点和曲率测量的准确性。此外，我们使用的平滑算法在有很多点的情况下效果最佳。我们的注入算法计算每个路径线段（包括起点，不包括终点）内可以适应多少等间距点，然后沿着线段插入它们。它对路径中的每个线段都这样做。当它到达末尾时，它还没有添加路径的终点，所以它将终点添加到点列表中。以下是这个算法的伪代码版本：

spacing = 期望点之间的距离
new_points = 用于存放新点的数组

// 对路径中的每个线段进行操作：
for every line segment in the path:
    vector = end_point - start_point
    num_points_that_fit = Math.ceil(vector.magnitude() / spacing)
    vector = vector.normalize() * spacing
    for (i = 0; i < num_points_that_fit; i++):
        add (start_point + vector * i) to new_points

// 添加路径中的最后一个点到new_points

我们发现，6英寸的间隔效果很好。如果您有不同的插入点方法，请随意使用。

平滑

为了平滑路径，我们使用了一个来自团队2168的算法。team2168/robot/FalconPathPlanner，该算法接收一个XY坐标的二维数组，并返回坐标的更平滑版本。它接收三个参数：weight_data（我们称之为a）、weight_smooth（b）和tolerance。这个算法平滑的程度取决于点之间的间距以及a、b和tolerance的值。更大的b意味着更平滑的路径。我们发现b的值在0.75到0.98之间工作得很好，a设置为1-b，tolerance=0.001。您可以针对每个路径调整b的值，或者对所有路径保持常数。以下是算法，以防您使用纸张无法查看链接：

public double[][] smoother(double[][] path, double weight_data, double weight_smooth, double tolerance)
{

    //copy array
    double[][] newPath = doubleArrayCopy(path);

    double change = tolerance;
    while(change >= tolerance)
    {
        change = 0.0;
        for(int i=1; i<path.length-1; i++)
            for(int j=0; j<path[i].length; j++)
            {
                double aux = newPath[i][j];
                newPath[i][j] += weight_data * (path[i][j] - newPath[i][j]) + weight_smooth * (newPath[i-1][j] + newPath[i+1][j] - (2.0 * newPath[i][j]));
                change += Math.abs(aux - newPath[i][j]);	
            }					
    }

    return newPath;
}

其他平滑路径的方法（如五次样条）也可以工作，只要最终结果是一系列平滑、密集的XY点。

点之间的距离

您需要存储每个点沿路径的距离。点之间的距离变化用于计算每个点的目标速度和曲率。这个距离不一定等于注入算法中使用的间距，因为当路径被平滑处理时，点会移动。找到路径上某点的距离就是简单地保持点之间距离的累积和：

$distanceatpointi=distanceatpoint(i-1)+distance\_formula(pointi,point(i-1))$

路径的曲率

某点处的路径曲率用于调整机器人的速度。在急转弯时（急=大曲率），我们希望机器人减速。我们可以通过找到穿过该点及其两侧各点的圆的半径来计算该点的曲率。然后曲率就是半径的倒数。

路径曲率的计算:
给定三个点 P(x1, y1)，Q(x2, y2) 和 R(x3, y3)，我们试图找到以 C(a, b) 为中心，半径为 r 的圆，使其经过 P、Q 和 R。我们可以写出 r = |PC| = |QC| = |RC|。解决此问题的公式如下：
$k_1=\frac{0.5\times (x_1^2+y_1^2-x_2^2-y_2^2)}{(x_1-x_2)}$

$k_2=\frac{(y_1-y_2)}{(x_1-x_2)}$

$b=\frac{0.5\times (x_2^2-2\times x_2\times k_1+y_2^2-x_3^2+2\times x_3\times k_1-y_3^2)}{(x_3\times k_2-y_3+y_2-x_2\times k_2)}$

$a=k_1-k_2\times b$

$r=\sqrt{(x_1-a{)}^2+(y_1-b{)}^2}$

$\text{curvature}=\frac{1}{r}$

曲率 c = 1/r
​
这个公式有以下边缘情况：

• 如果出现除以零的错误，解决方法是在 $x1=x2$ 时，给分母添加一个足够小的值（0.001）。
• 然后公式给出的近似值非常接近，没有错误。
• 如果答案是 NaN，这意味着半径是无穷大，曲率是 0，路径是直线。

对于路径上的每个点，我们取该点及其两侧的点，并将它们代入此公式。对于起点和终点，它们没有另一侧的点，曲率是 0。

速度

第一部分：最大速度
路径上的每个点都有一个目标速度，机器人尝试达到这个速度。机器人使用离它最近的点的目标速度来计算目标左右轮速度。在计算某点的目标速度时，我们考虑到该点的曲率，以便机器人在急转弯时减速。同时也会考虑为路径定义的最大速度。每个点的速度设置为以下两者的最小值：(path max velocity, k / curvature at point)
其中 k 是一个常数，大约在 1-5 之间，基于你希望机器人在转弯时的速度有多慢。这降低了机器人在转弯时的速度，同时确保机器人的目标速度永远不会超过最大值。下面的图表展示了某条路径上每个点的目标速度可能的样子：

第二部分：加速度限制
当机器人沿路径移动时，我们希望它遵循速度设定点，但也要遵守最大加速度。我们希望目标速度平滑变化，而不是从 0 瞬间达到 100。我们希望将上面计算出的蓝色目标速度线变成下面的橙色线：

这产生了一个问题。机器人的目标速度基于它最接近的点，在这张图中，路径起点的点速度为 0，机器人将永远不会开始移动。为了解决这个问题，我们去掉平滑的加速度，只留下减速度：

这样，由于起点的目标速度非零，机器人实际上开始移动。但是，由于目标速度不再遵守最大加速度，我们必须通过一个速率限制器。实时中，当机器人沿路径移动时，速率限制器限制输入目标速度的变化速率，使得实际目标速度平滑，但机器人确实在尝试移动。本质上，它将第二条橙色线变回第一条。速率限制器的逻辑在计算速度的方程之后描述。

第三部分：计算
为了计算每个点的目标速度，我们模拟机器人以最大加速度从末端到开始沿着路径加速，其速度受到第一部分计算出的最大速度的限制。因此，某点的目标速度是以下两者的最小值：该点的当前速度，以及机器人从最后一点开始可以达到的最大速度。你可以想象一个机器人以受限于蓝色速度的加速度反向沿着路径加速会产生上面图表中的橙色线。

要找到目标速度，我们使用运动学方程 $v_{f}^2 = v_{i}^2 + 2 \times a \times d $。重新排列后，我们得到 $v_f$，即在给定点上可达到的最大速度，其中 $v_i$ 是前一点的速率，$a$ 是最大加速度，$d$ 是点之间的距离：

$v_f=\sqrt{v_i^2+2\times a\times d}$

要计算新的目标速度，首先将最后一点的速度设置为0。然后，从倒数第二点开始，以索引 $i$ 向后移动：

distance = distance_formula(point(i + 1), point i)

点 $i$ 的新速度 $=\min (\text{点 }i\text{ 的旧目标速度},\sqrt{(\text{点 }(i+1)\text{ 的速度}{)}^2+2\times a\times \text{距离}})$

速率限制器

速率限制器接收你想限制的输入值和你希望输入变化的最大速率，然后返回一个尝试达到与输入相同值但变化速度受限的输出值。它将第一个参数限制在后两个参数的范围内。

max change = change in time between calls to rate limiter * max rate
output += constrain(input − last output, − max change, max change)

速率限制器在“跟随路径”部分中使用。

跟随路径

跟随路径的算法如下：

• 找到最近的点
• 找到前探点
• 计算到前探点的弧线曲率
• 计算目标左右轮速度
• 使用控制循环实现目标左右轮速度

最近点
找到最近点的关键是计算到所有点的距离，并选择最小的。我们建议从上一个最近点的索引开始搜索，这样最近点只能沿路径向前移动。稍后，我们将需要查找这个点的目标速度。

前视点
前视点是距离机器人一定前视距离的路径上的点。我们通过找到以机器人位置为中心，半径为前视距离的圆与路径线段的交点来找到前视点。这里提供了如何找到圆和线段的两个可能交点的代码：交点检测算法。它计算两个 t 值，其中 t 的范围是 0 - 1，表示交点沿线段的比例位置。如果 t < 0 或 t > 1，则表示圆不与线段相交。

接下来是算法的代码示例，以防你在纸上阅读：
计算线段与圆的交点

1. E 是线段的起点。
2. L 是线段的终点。
3. C 是圆心（机器人位置）。
4. r 是圆的半径（前瞻距离）。

计算：
$d=L-E$（射线的方向向量，从起点到终点）
$f=E-C$（从圆心到射线起点的向量）

a = d.Dot(d)
b = 2 * f.Dot(d)
c = f.Dot(f) - r * r
discriminant = b * b - 4 * a * c

if (discriminant < 0)
    // 则没有交点。
}else{
    discriminant = sqrt(discriminant)
    t_1 = (-b - discriminant) / (2 * a)
    t_2 = (-b + discriminant) / (2 * a)

    if (t1 >= 0 && t1 <=1){
        //return t1 intersection
    }
    if (t2 >= 0 && t2 <=1){
        //return t2 intersection
    }
    // 否则，没有交点
}

然后，找到交点：
$Point=E+(tvalueofintersection)\ast d$

前视点的分数索引只是 t 值加上线段起点的索引。在下图中，前视点的分数索引是 1.5。

要找到前视点，遍历路径的线段，查看线段和前视圆之间是否有有效的交点（0 ≤ t ≤ 1）。新的前视点是第一个有效交点，其分数索引大于上一个前视点的索引。这确保了前视点永远不会向后移动。如果没有找到有效的前视点，则使用上一个前视点。为了优化搜索，你可以从上一个前视点的索引开始。

我们根据路径的弯曲程度选择大约 12-25 的前视距离，对于更弯曲的路径使用较短的前视距离。然而，前视距离也可以根据路径的曲率或机器人的目标速度沿路径变化。我们还没有这样做，但这是可能的。下图显示了不同前视距离的影响：

弧线的曲率

纯追踪控制器的核心是沿着朝向前视点的弧线行驶。以下是如何计算该弧线的曲率。（在我们的计算中，我们将使用正值表示向右转）：

在上图中，X和Y轴与机器人对齐。L是前视距离，(x, y)是前视点，r是弧线的半径，D仅仅是r和x之间的差值。根据基本数学，我们有：

$x^2+y^2=L^2$
$x+D=r$

然后以下代数给出：

$D=r-x$
$(r-x{)}^2+y^2=r^2$
$r^2-2rx+x^2+y^2=r^2$
$2rx=L^2$
$r=\frac{L^2}{2x}$

曲率的定义是半径的倒数，所以：
$curvature=\frac{2x}{L^2}$

我们可以选择L，前视距离，但我们如何找到x？x是相对于机器人到前视点的水平（相对于机器人）距离。这可以通过计算前视点到由机器人位置和方向定义的假想线上的距离来找到。见下图：

我们使用点斜式找到“机器人线”的方程：

(y − robot y)/(x − robot x) = tan(robot angle)
Converting this to the form ax + by + c = 0 gives:
a = − tan(robot angle)
b = 1
c = tan(robot angle) * robot x − robot y

点线距离公式是：
$d=\frac{|a\cdot x+b\cdot y+c|}{\sqrt{a^2+b^2}}$

插入我们的系数和前视点的坐标给出：
$x=\frac{|a\cdot {\text{lookahead}}_x+b\cdot {\text{lookahead}}_y+c|}{\sqrt{a^2+b^2}}$

然而，这并没有给我们足够的信息。考虑以下情况：

在这些情况下，前视点到机器人的距离相同，但在一个情况下机器人需要向右转，在另一个情况下向左转。我们需要知道前视点在哪一边。如果我们将机器人想象成一个向量（红色）和机器人到前视点作为一个向量（橙色），我们可以取向量的叉乘的符号来计算前视点是在左边还是右边。如果答案是正数，点就在右边；如果是负数，就在左边。

如果 R 是机器人的位置，B 是“机器人线”上的另一个点，L 是前视点，那么点所在的一边就是向量叉乘的符号： $\overline{RB}$ and $\overline{RL}$

$\text{side}=\mathrm{signum}(\text{cross product})=\mathrm{signum}\left(\left(B_y-R_y\right)\times \left(L_x-R_x\right)-\left(B_x-R_x\right)\times \left(L_y-R_y\right)\right)$

虽然我们知道A和L，但我们没有B，但我们可以通过以下方式在机器人线上创建另一个点：

$B_x=R_x+\cos (\text{robot angle})$
$B_y=R_y+\sin (\text{robot angle})$

在主方程中，有表达式 $ B_{x} - R_{x} $ 和 $ B_{y} - R_{y} $ , 当代入上述方程时，简化为 $ \cos(\text{robot angle})$ and $\sin (\text{robot angle})$，因此整个方程简化为：

$\text{side}=\mathrm{signum}(\text{cross product})=\mathrm{signum}(\sin (\text{robot angle})\times (L_x-R_x)-\cos (\text{robot angle})\times (L_y-R_y))$

要获得有符号的曲率，只需做 curvature * side。有符号的曲率然后告诉机器人转多少以及向哪个方向转。

轮速

虽然我们不能直接控制机器人行驶的曲率，但我们可以控制左右轮的速度。为了计算使机器人以正确的曲率和正确的速度行驶的目标左右速度，我们需要知道三件事：
(1)曲率，(2)目标速度，(3)轮距（机器人上轮子之间的水平距离）。

(1)我们上面计算了曲率。
(2)要获得目标速度，取与最近点相关联的目标速度，并不断通过速率限制器来获得加速度限制的目标速度。
(3)轮距是从你的机器人上测量的。由于转弯时的摩擦损失，你想使用比实际大几英寸的轮距。

V= 目标机器人速度
L= 目标左轮速度
R= 目标右轮速度
C= 弧线的曲率
W= 机器人的角速度
T= 轮距

对于一个滑移转向（履带驱动）机器人，以下方程是成立的：
V = (L + R)/2
W = (L − R)/T
V = W/C

组合这三个方程（这留给读者作为练习）给出：
L = V * (2 + CT)/2
R = V * (2 − CT)/2

控制轮速

计算出目标左右轮速度后，我们需要对驱动系统的每侧应用正确的功率以使它们达到该速度。我们的实现使用前馈和反馈控制器的组合来实现。前馈项将与目标速度成比例的功率应用上，并在加速或减速时加上一些额外的功率以克服惯性：

$FF=K_v\ast targetvel+K_a\ast targetaccel$

这里， target vel 是左右轮计算出的速度，并且 target accel 是通过取 target vel 的导数得到的。

$K_v$ 是前馈速度常数，它表示对于给定的目标速度应用多少功率。$K_a$，前馈加速度常数，对加速度做同样的工作。

反馈项应用功率以纠正测量轮速和目标轮速之间的误差：

$FB=K_p\ast (targetvel-measuredvel)$

$K_p$ （比例反馈常数）越大，机器人就越尝试纠正速度误差。

然后给轮子的功率是 (FF + FB)。我们独立地在驱动的每侧运行这个控制器。

调整

$K_v,K_a,K_p$ 的值是多少？为了计算这些，首先为机器人创建一条长的直线路径，使用已经创建的纯追踪代码。确保你可以访问测量轮速和目标轮速与时间的图表，例如在下面的示例中，展示了每个步骤应该是什么样子：

$K_v$: 选择一个 $K_v$ 大约等于 1/(top robot speed)。将其他常数设置为0，运行路径。调整 $K_v$ 直到目标和测量速度在机器人以恒定速度行驶时匹配。

$K_a$: $K_a$ 从0.002左右开始。调整直到目标和测量速度在加速、巡航和减速时匹配得相当好（不必完美）。

$K_p$: 我们使用了0.01。你可以让它更大，因为机器人将更准确地跟踪目标速度。但要警告：如果增加太多，它会变得抖动。

停止

我们的控制器在机器人的最近点是路径上的最后一个点时停止。因为我们的点间隔为6英寸，这意味着机器人在路径末端前约3英寸处停止。这种停止行为对我们来说效果很好，因为它意味着机器人的惯性会把它带到我们的终点，而不是越过它。纯追踪控制器的一致性足以容易地调整路径，如果它没有停在你确切想要的地方。

注意，如果你的路径以急转弯结束，这使得机器人很难在正确的位置停下来，同时面向正确的方向。这个控制器在急转弯和路径末端的跟踪效果最差。为了解决这个问题，你可以：

• 减少前视距离
• 调整路径，使其不再有急转弯
• 保留急转弯，但调整路径直到机器人最终到达你想要它去的地方
• 虚拟地将路径延伸到实际终点之外，以便有效的前视距离不会缩小到0.

可视化的重要性

在编程纯追踪控制器时，数据可视化非常有帮助。算法的每一步都能从图表或图解中受益。这使得检查数学运算是否按预期工作变得容易。更有用的是，机器人模拟器允许你可视化算法的工作并测试更改，然后再将其应用到真实机器人上。这是我们的模拟器的图片：

我们通过模拟两个电机（每个驱动侧一个），然后使用滑移转向运动学来计算机器人的传感器将读取的内容。如果你想尝试实现机器人模拟器，可以搜索这些主题。

结论

纯追踪控制器为快速、准确、可靠地跟随路径提供了一种稳健的方法。由于算法的可靠性，你不必担心调整常数，这节省了时间和精力。你的机器人可能会磨损，地毯可能不平坦，但只要编码器和陀螺仪读数准确，它仍然会跟随路径。我们非常兴奋，因为我们的算法和相关的路径绘制应用在这个赛季两次获得了控制创新奖。以下是一些算法运行的视频：

• Far blue
• Close red
• Middle red

祝编程愉快！

参考文献

• 卡内基梅隆大学算法概述
  https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_19921.pdf

• 维基百科：点到直线的距离
  https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_an_equation

• StackOverflow：圆线段碰撞检测算法
  https://stackoverflow.com/questions/1073336/circle-line-segment-collision-detection-algorithm

• StackOverflow：判断点在直线哪一侧
  https://stackoverflow.com/questions/1560492/how-to-tell-whether-a-point-is-to-the-right-or-left-side-of-a-line

• 给定三点的圆 - 方法 2
  http://www.qc.edu.hk/math/Advanced%20Level/circle%20given%203%20points.htm%28method%202%29

• KHEngineering/SmoothPathPlanner GitHub 仓库
  https://github.com/KHEngineering/SmoothPathPlanner

• ChiefDelphi 论坛关于虚拟路径延伸的讨论
  https://www.chiefdelphi.com/forums/showthread.php?p=1735028

LemLib–Pure Pursuit

LemLib–Pure Pursuit
https://github.com/LemLib/LemLib

Pure Pursuit 是一种路径跟踪算法，可让机器人快速跟踪路径。它的工作原理是在机器人周围画一个假想的圆圈，然后找到圆圈和路径的交点。然后机器人将驶向交点。如下面的 GIF 所示：

调整 Pure Pursuit 非常简单。如果您希望机器人更紧密地跟随路径，请减少前瞻距离。如果您希望机器人更松散地但更快地跟随路径，请增加前瞻距离。一个好的起点是 10-15 英寸，但这将根据每个动作而有所不同。

Nav2–Regulated Pure Pursuit

Nav2–Regulated Pure Pursuit

受控的 Pure Pursuit 控制器实现了 Pure Pursuit 控制器的变体，专门针对服务/工业机器人的需求。它通过路径曲率调节线性速度，以帮助减少盲角高速时的超调，从而使操作更加安全。它还比 Pure Pursuit 的任何其他变体更好地遵循路径。它还具有启发式方法，可以在接近其他障碍物时减速，以便您可以在附近发生潜在碰撞时自动减慢机器人的速度。它还实现了自适应前瞻点功能，可以通过速度进行缩放，从而在更大范围的平移速度下实现更稳定的行为。

请参阅软件包README以获取更多完整信息。

如果您使用此存储库中的受监管纯追踪控制器算法或软件，请在您的论文中引用此工作：

Macenski、S. Singh、F. Martin、J. Gines，《机器人路径跟踪的受控纯追踪》。《自主机器人》，2023 年。

纯跟踪算法（Pure persuit）

纯跟踪算法（Pure Pursuit）在差速机器人上的应用
双舵轮AGV轨迹跟踪Pure Pursuit算法模型分析、python代码实现
Pure Persuit算法基础

自行车模型

差速轮模型

双舵轮模型

参考

1、卡梅隆大学 Pure Persuit 算法
2、普渡大学–control-algorithms/basic-pure-pursuit
3、ros_motion_planning–同大 Yang Haodong
4、thomasfermi/Algorithms-for-Automated-Driving/PurePursuit
github: https://github.com/thomasfermi/Algorithms-for-Automated-Driving
5、PDF–DAWGMA–Team 1712
6、MathWorks–Pure Pursuit Controller
7、阿木实验室 pure_pursuit算法原理
8、team2168/robot/FalconPathPlanner
9、Introduction to Pure Pursuit Tracking Algorithm
10、Pure Pursuit
11、LemLib–Pure Pursuit
12、Nav2–Regulated Pure Pursuit
13、纯跟踪算法（Pure Pursuit）在差速机器人上的应用
14、双舵轮AGV轨迹跟踪Pure Pursuit算法模型分析、python代码实现
15、Pure Persuit算法基础