纯跟踪学习笔记（1）算法原理理解

前言

最近在学习在Gazebo仿真环境下实现差速小车依据全局规划器发布的全局路径然后通过pure pursuit（纯跟踪）对路径进行跟踪。因笔者对控制算法了解的也不多，本文着重记录听教学视频的做的笔记，并附上个人的一些见解，仿真将放在下一篇文章之中。若有错误之处，请大家批评指正，也欢迎大家讨论学习。

具体学习的课程可以点下面这个链接：https://www.bilibili.com/video/BV1Ly4y177dF/?spm_id_from=333.337.search-card.all.click

算法思想

基于当前车辆后轮中心位置，在参考路径Ld（自定义）的距离匹配一个预瞄点，假设后轮中心可以按照一定的转弯半径R行驶抵达该预瞄点，然后根据预瞄点距离Ld，转弯半径R，车辆坐标系下预瞄点的朝向角2a之间的几何关系确定前轮转角。

具体推导

上图中，A点为车体后轮的中心点，B点为车体前轮的中心点，C点为预瞄点，Ld为预瞄点的距离，L为小车的轴距，delta为前轮的转向角度。由算法思想可知，最终我们需要求解的就是转向角度delta。

因为阿克曼模型是前轮转向，后轮直驱的，所以给前轮一个固定的转向角后，车体行走轨迹是一个圆弧，即图中的虚线部分。

而小车沿着这个圆弧从A点走到C点，所以OC=OA=R 。在沿着这条圆弧行走的过程中，车身始终与圆弧相切，所以BA垂直于OA 。所以角CAO=pai / 2 - a，而三角形CAO是等腰三角形，所以角COA=pai - 2 * ( pai / 2-a ) = 2a 。

由三角形的正弦定理可知，Ld / sin2a = R / sin( pai/2 - a )，推出 R = Ld / 2 sina 为1式。

由阿克曼转向可知delta左边那个角为 pai / 2，因为 delta + 角 OBA = 角 BOA + 角 OBA = pai / 2，所以角BOA = delta ，所以 tan delta = tan 角BOA = L / R 为2式。

联立1，2两式得出，delta = arctan（ 2L * sina / Ld）。由此我们就得出了转向角delta的求解式 。

实现PP算法的代码逻辑：

1、确定车辆当前位置；

2、在规划出的路径中，找到离车最近的点；

3、找到预瞄点；

4、换算预瞄点坐标为车辆坐标系；

5、计算转向角delta，控制车辆运动；

6、更新车辆状态。

More Question

在我们编写代码时，如何去求解预瞄点呢？首先，搞清楚为什么会提出这个问题。因为当我们拿到一条参考路径时，肉眼看是一条线，但实际上他是由若干个轨迹点组成的，而每一个轨迹点之间是有间隔的，如果我们求预瞄点时，按照以车体本身为圆心，Ld为半径画圆找此圆与参考路径曲线的交点为预瞄点这个方法去求是不正确的，因为如果这个圆与参考路径曲线的交点正好在两个轨迹点的中间，该如何选取呢。

解决方法：

1、遍历轨迹点，找到与当前小车位置最近的轨迹点序号；

2、从该轨迹点向轨迹前方搜索，找到与预瞄距离最接近的一个轨迹点，确认为预瞄点。

总结

对纯跟踪算法一些很细节的东西，笔者也模棱两可，还在学习中，欢迎大家指出笔者对PP算法一些理解误区。