纯跟踪算法（Pure persuit）

纯跟踪局部路径规划器

在得到局部路径之后，本项目使用纯跟踪算法通过横向控制器反馈控制小车的转角，从而达到跟踪局部路径的效果，经实验确认，跟踪效果很好。

纯跟踪算法原理如下：

在搭建模型时可以将小车看作车辆运动学自行车模型

运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，车辆运动学模型能反映车辆位置、速度、加速度等与时间的关系。基于运动学模型设计出的控制器也能保证对车辆底层执行层下发的指令是符合车辆运动学约束的。对于运动学自行车模型，我们一般做出如下假设：

不引入任何会影响到车辆动作的力

车辆只在xy平面上运动，在z轴（垂直方向）上的侧倾，前后俯仰等不考虑。

车辆结构就像自行车，左右前轮的转角和转速一致，后轮也是一样。所以两个前轮和后轮各可只用一个轮胎来描述。（转向角小）

前后轮的各自速度矢量和各自轮胎朝向一致，即假设轮胎无侧滑。

而一般只有在低速状态下，轮胎产生的侧向力很小可忽略，所以该模型也主要应用于低速场景。

模型简化图如下：

                              

 

其中符号表示物理意义如下

 

公式推导：

通过正弦定理可以推出：

                

即：

                            

  

其中k为转弯圆弧的曲率。由上图还可以推出：

                                  

那么：

        

 

由上式可知，本控制器的本质是对转角进行控制，以减少横向误差为目标的横向控制器。其中2L/ld2可视为控制器P的参数，L为车辆的轴距，为设定的预瞄距离。本控制器的控制效果主要取决于预瞄距离的取值，一般来说预瞄距离越长，控制效果越平滑，预瞄距离越短，控制效果会越精确（同时也会带来一定的震荡），预瞄距离的选取也和当前车速有关，以下式为例：

                       

其中v为当前车速，g为一可调节的参数，lf为一预设前视距离。

在一些文献中，预瞄距离的选取方式表现如下形式：

                                                                                      

 

上式中常数A=1/(2amax)，amax为最大制动加速度。Av2表示最短车辆制动距离。B表示车辆遇到异常时需要的反应时间，Bv为对应的反应距离，C表示车辆的最小转弯半径。

本算法在实际应用中，通常不要求跟踪的目标点到本车后轴中心的距离切实等于预瞄距离。而是会选择采样好的一系列目标点中到后轴中心距离最接近预瞄距离的那个点来近似跟踪。这样做的好处是可以不需要目标轨迹的函数方程来求解真实预测距离坐标，极大地提升了算法的效率。

算法在运用到车辆之前，已经通过实验测试，测试结果如下：

根据实验得出结论：通过纯跟踪法，处理过小拐角处我们可以基本上无误的跟踪曲线。并且在实际车辆运行过程中，我们发现也可以做到基本无误的跟踪。

 部分参考如下博客(165条消息) 无人驾驶之车辆控制（1）纯跟踪（Pure Pursuit）算法与Stanley算法_纯跟踪算法在高速效果不佳_小郑同学爱学习的博客-CSDN博客