Control-Pure Pursuit

将车辆模型简化为自行车模型，如下图，根据​转向，在低速和平缓转向情况下，车辆的转向角可近似求解：

其中，​是车辆轴距，​是车辆后轴中心转向半径。

 纯跟踪控制算法是一种典型的横向控制算法，在​中有多次应用。该算法思想是基于车辆后轴中心位置，在参考路径上前​​的距离匹配一个预瞄点，假设车辆后轴中心可以按照一定转弯半径行驶到达该预瞄点。该算法根据预瞄距离、转弯半径与预瞄点坐标等几何关系来求解转向角。

 如图所示，车辆的航向角为​，车辆后轴中心和预瞄点间的连线与车辆航向的夹角为​，则车辆后轴中心行驶圆弧所对应的夹角为​。可得：

点​为后轴中心点，点​为预瞄点，由正弦定理得：

于是：

一般预瞄距离​是关于速度的函数：

结合公式(1)，可得转向角：

如果是在车体坐标系中，并且车辆的定位位置是在后轴中心：

其中，​是在车体坐标系的坐标。