路径跟踪之Pure Pursuit控制算法

Pure Pursuit是一种几何跟踪控制算法，也被称为纯跟踪控制算法。该算法最早由R. Wallace在1985年提出，其思想是基于当前车辆的后轮中心位置（车辆质心），在参考路径上向（称为前视距离）的距离匹配一个预瞄点，假设车辆后轮中心可以按照一定的转弯半径行驶至该预瞄点，然后根据前视距离、转弯半径、车辆坐标系下预瞄点的朝向角之间的几何关系来计算前轮转角。

如上图所示，在三角形OAC中，根据正弦定理可得：

根据车辆的运动学方程，有：

所以，前轮转角为：

车辆坐标系下预瞄点的朝向角可以由AC的方向角减去车身航向角获得。

定义横向误差为后轮中心位置和预瞄点在横向上的误差，如上图中的。

当前轮转角很小是，横向误差可以近似为：

所以，该算法近似于一个P控制器，跟踪效果由前视距离决定。

利用Matlab可以得到该算法跟踪一个圆形轨迹的效果如下图所示。

减小前视距离，仿真结果如下图所示，可以看出跟踪结果出现了一定程度的震荡，这是因为前视距离减小后，近似P控制器的比例系数增加，从而造成动态响应出现超调和震荡。

如果预瞄点如下图所示，那么大于90度，车辆后轮的运行轨迹应该是大圆，绕了一个大圈，是在向后跟踪，此时的前轮转角计算公式仍然不变。

 

在三角形OAC'中，根据正弦定理，有：

所以，前轮转角计算公式仍为：

最后，再看下等于90度的情况，车辆后轮的运行轨迹应该是右上那个半圆。

如上图所示，有：

很容易求得前轮转角的大小为：

通过以上分析，不难发现，三种情况下的前轮转角计算公式均适用，同样“近似P控制器”这个结论在上述三种情况下也都成立。