pure pursuit算法

0, 设置预瞄距离

1，从期望轨迹中找预瞄点B，方法是从期望轨迹上找距离A点等于预瞄距离L的点。

2，A点与B点坐标可能都是基于某个全局坐标系，所以需要将B点坐标从全局坐标系转换至车辆坐标系上

3，计算转弯半径(参考)

4，计算转向角(参考)   以及控制参数(参考)

 

纯追踪控制器其实是一个横向转角的P控制器，这个P控制器受到参数前视距离的影响很大，如何调整前视距离变成纯追踪算法的关键，

通常来说，前视距离被认为是车速的函数，在不同的车速下需要选择不同的前视距离。一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，

ld=kvx

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