自动驾驶（七十一）---------Pure Pursuit轨迹追踪

       前面介绍过很多轨迹规划，也有很多控制相关的知识，例如pid调节、kalman 滤波、MPC等等。今天再学习一种轨迹追踪的方法，轨迹追踪是在有轨迹规划的基础上，如何实现车辆按照规划的轨迹行驶的问题，属于控制的范畴。

 1. 自行车模型和阿克曼转向几何

       这部分内容我很早就介绍过：车辆运动学模型 , 这里想简单复习一下，所谓运动学模型就是已知车辆的速度转角等信息，计算车辆在当前速度下位置和姿态的变化。

                                         

       

        精确描述单车模型中几何关系的核心公式如下：

       其中，δ 为前轮转角，L 为轴距(wheel base)，R 为给定转向角运动时，无人车形成的轨迹圆的半径，具体参数和推导看我之前的博客。MPC就是基于这样的假设，估计车辆的未来一段时间的运行轨迹，那么有没有其他模型或者方法来估算车辆的行驶轨迹呢？

2. Pure Pursuit轨迹追踪

        Pure Pursuit轨迹追踪是一种基于几何追踪的纯追踪方法，我们也是从自行车模型出发，以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角 ， 假设车辆是可以沿着一条经过目标路点的圆弧行驶，如下图所示：

                                        

         特别的我们不再是通过转角和三角关系计算车辆的行驶轨迹，而是假设车辆会按照这样的一段圆弧来行驶，这和我们在MPC中估算的轨迹不同。知道了车辆的估计轨迹模型了，下一步就是如何把这个方法运用到车辆控制中来。我们的MPC是把估算的轨迹和我们的目标轨迹向比，优化出对应的控制参数，这里又是怎么理解呢？

3. 理解和公式计算

        我们有一条规划好的轨迹，也假设了车辆在一定的时间内按照第二步的圆弧行驶，这样我们可以分阶段来实现轨迹，我们先选取一段较小长度的轨迹，把轨迹的终点作为这一小段的目标，目标就是让车经过这个点，所以就很自然的要求圆弧经过这个点，再通过几何计算就可以计算出对应的半径和前轮的转角了。

       Pure pursuit方法的依据是使如上图所示的单车模型以合适的前轮转向 δ 运动，并恰好使无人车后轴中心经过当前的路点。 这样一来，我们就可以根据当前的路点以及单车几何模型计算当前的期望前轮转向角 δ。那么更具正弦定理我们可以推导出如下转换式：

                                

       综上推导出： 其中 κ 是计算出来的圆弧的曲率，那么前轮的转角 δ 的表达式为：

       我们可以得出纯追踪算法控制量的的最终表达式：

       这里我们把时间考虑进来，在知道t时刻车身和目标路点的夹角 α(t)α(t) 和距离目标路点的前视距离 ldld 的情况下，由于车辆轴距 LL 固定，我们可以利用上式估计出应该作出的前轮转角 δδ ，为了更好的理解纯追踪控制器的原理，我们定义一个新的量：elel —— 车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：

              圆弧的弧度就可重写为：   

       考虑到本质是横向上的CTE，由上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为，这个P控制器受到参数 （即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说， 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即，那么前轮的转角公式就变成了：

            

       那么纯追踪控制器的调整就变成了调整系数k，通常来说，会使用最大，最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡），