介绍
纯跟踪算法是一种路径跟踪算法，它通过计算曲率来将车辆从其当前位置移动到某个目标位置。该算法的整个重点是选择一个距离车辆在路径上前方一定距离的目标位置。纯跟踪算法这个名称来源于我们用来描述该方法的类比。我们倾向于认为车辆正在追逐其前方某个距离的路径上的点——它正在追求那个移动点。这种类比通常用来将此方法与人类驾驶方式进行比较。我们倾向于看着汽车前面一段距离并朝着那个位置行驶。这种预瞄性距离会随着我们行驶而改变，以反映道路的扭曲和视觉遮挡。
推导
纯跟踪算法是一种几何确定曲率的方法，以驱动车辆到达被称为目标点的选择路径点。这个目标点是路径上距离当前车辆位置一个观察距离的点。构建连接当前点和目标点的弧线，该弧线的弦长为预瞄距离，并作为确定连接两点的唯一弧线的第三个约束。可以将预瞄距离视为类比于人类驾驶员可能朝向的距离前方的一点，以跟踪路径。
点(x,y)为预瞄点，弦长为d。有以下公式：
\[ \begin{align} x^2+y^2=l^2 \\ x+d=r \end{align} \]
和弧形有关的公式：
\[ \begin{align} 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} \\ \gamma=\frac{2x}{l^2} \end{align} \]
曲率\(\gamma\)通过x和弦长l计算得到
实现
实现的难点在于怎样处理路径信息（通信，图形，从规划器更新新的路径信息）
路径表示
通常路径表示为一连串离散点。点的信息包含如下：
- 全局坐标系下的x坐标
- 全局坐标系下的y坐标
- 全局坐标系下的朝向角
- 当前路径点的曲率
- 当前点距离起始路径的直线距离
通信和路径管理
跟踪器和规划器不一定在同一机器运行。
跟踪器使用旧的路径驱动车辆。规划器完成规划就会发送新的路径到跟踪器。新的路径可能覆盖旧的路径。
我们还必须注意，必须为跟踪器与车辆上的传感器模块的接口做一些准备。 在我们的实现中，跟踪器有一个到中央车辆控制器的接口，它可以通知跟踪器当前的车辆姿态。 跟踪器可以将其驾驶请求发送到该中央控制器以执行车辆。
纯跟踪算法
步骤如下：
- 得到当前车辆定位位置
- 寻找路径点上离车辆最近的点
- 寻找目标点
- 把目标点转换到车辆坐标系下
- 计算曲率并请求车辆将转向设置为该曲率
- 更新车辆位置
- 得到当前车辆定位位置。可以根据里程计来得到。
- 寻找路径点上离车辆最近的点。在几何推导中指出，目标点将在车辆的预瞄距离内。可能有多个点距车辆当前位置一个预瞄距离。车辆应转向距其当前位置一个预瞄距离的最近点。因此，将首先找到距离车辆最近的路径点，然后从该点开始搜索距车辆预瞄距离为1的点，并沿着路径开始。
- 寻找目标点。通过向上移动路径并计算该路径点与车辆当前位置之间的距离来找到目标点。 路径点位置记录在全局帧中；该计算是在全局坐标中完成的。
- 把目标点转换到车辆坐标系下。找到目标点后，必须将其转换为车辆的局部坐标。曲率的几何推导是在车辆坐标中完成的，并且对车辆的曲率命令在车辆坐标中是有意义的。
- 更新车辆位置。
流程图如下：
算法参数
改变预瞄距离的影响
预瞄距离小的时候跟踪会出现震荡，大的时候会出现偏离路径。
给定路径曲率的非唯一先行
找到路径曲率与最佳预瞄距离之间的封闭关系非常有用。我们试图为一个任意曲率的圆找到一个最佳预瞄的解决方案。一旦找到这种关系，那么就可以随着路径曲率的变化而改变预瞄。
找到这个问题的解决方案，意味着预瞄距离和曲率之间存在一对一的关系。图中的圆是一条曲率不变的路径，因此应该有一个可用于路径跟踪的预瞄距离。
圆内的等腰三角形显示了从起始点延伸到目标点的三角形底面的预瞄距离，三角形的边只是圆的半径。一个能满足这个等腰三角形的预瞄距离将满足前面所说的曲率条件，因此定义了这个圆的曲率。但从图中可以很容易地看出，许多预瞄距离都会满足曲率条件。给出一个预瞄距离，我们就可以定义一个曲率，但给出一个曲率，预瞄距离就不确定了。事实上，在0到2r之间的预瞄距离都是同样可接受的。
问题
- 突变的曲率会导致请求很大的速度，导致车辆的尾部打滑。
- 由于转向中的一阶滞后，车辆不会像期望的那样快速接近路径。
autoware.ai实现
差速底盘Python实现
https://github.com/helywin/PythonRobotics/blob/master/PathTracking/pure_pursuit/pure_pursuit_diff.py
PID控制线速度
\[ \begin{gather} a = K_p * (v_{target} - v_{current}) \\ v = v + a * \Delta t \end{gather} \]