无人驾驶车辆控制（三）：纯跟踪算法（Pure Pursuit）_用pure pursuit 控制速度 在转弯处不行怎么办

       无人驾驶车辆横纵向控制（Vertical and horizontal Control）一般采用纵向PID控制方法，横向纯跟踪方法（Pure Pursuit）。高阶算法有线性二次型调节器（LQR）和模型预测控制（MPC），这些在本系列博客中都会讲解，但本节我们趁热打铁，基于无人驾驶车辆控制（二）：阿克曼转向来讲解横向控制中常用到的纯跟踪算法（PPC）。

       进入正文前先说下，纯跟踪算法中的难点。①在纯跟踪算法中存在一个预瞄距离Ld,并且Generally，是需要调整的。会使用最大，最小前视距离来约束它，越大的前视距离意味着轨迹跟踪越smooth，小的前视距离会使得追踪的轨迹更加精确，但是呢，容易震荡。②拐弯处需要根据转弯大小做适当的减速。③第一个索引点的确定

      OK，接下来上硬菜。

一、理论推导

      接上周学习的阿克曼转向理论，继续画图推导！

求 前轮转角，需要知道 轴距，theta角和预瞄距离Ld。前面说了，Ld 需要调整，轴距是车辆本身参数可以测量得到，剩下的Theta一般可以由惯性测量单元IMU直接得到车辆的位姿信息（即使没有IMU，也可以通过计算得到。已知k时刻坐标和k+1时刻坐标，参考点坐标。分别从k时刻引出两条向量，求出夹角theta）

也有人说，纯跟踪算法其实就是横向的P调节器。有一定道理，因为Theta很小时，Theta与sin(theta)，tan(theta)和arctan(theta)都是等价无穷小，那么前轮转角delta≈2L/Ld乘以theta。那么前轮转角与theta成正比，即为P，P＝2L/Ld！

二、代码讲解

本次分别使用matlab，simulink和Python分别实现纯跟踪算法。下次更新simulink搭建纯跟踪模型和ROS系统下小海龟模型轨迹跟踪联合仿真快速验证控制算法（强烈推荐！V流程，MIL→RCP→HIL仿真），还不赶紧关注收藏走一波~

 

首先，易于理解的Simulink，

然后，网上有男神Adam shan的python代码，见https://blog.csdn.net/AdamShan/article/details/80555174

基于此，写出.m文件，也很简单，不再赘述，后面提供资源下载，供学习参考

最后，Python代码实现。说下思路吧，首先定义Horizon，也就是预瞄距离，先定为6米。然后获取车辆后轴中心点的位置，也就是手绘图中提到的A点，获取局部路径。然后，发布速度指令的Topic：Twist_pub。定义当前车辆pose，velocity和waypoint信息

进入LOOP，更新频率rate设为20Hz。然后判断当前车辆pose，velocity和waypoint信息，计算控制车辆指令TwsitCommand,计算后发布！

那么怎么算呢？就是定义的CalculateTwistCommand

设定lad，累加器，累加到大于预瞄距离Horizon时选取这个路径点作为targetWaypoint。然后获取车辆航向角yaw，和后轮侧偏角alpha，然后计算前轮转角theta。

OK，代码放一起，供参考

https://download.csdn.net/download/yuzistu_CSDN/12866385