横向控制 - Pure Pursuit

原理

阿克曼模型

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\varphi }\end{array}\right]=\left[\begin{array}{c}v\cos \varphi \\ v\sin \varphi \\ \frac{v}{L}\tan \delta \end{array}\right]$$

前轮转角$\delta$和转弯半径$R$的关系

$$\frac{1}{R}=\frac{\tan \delta }{L}=\kappa$$

Pure-pursuit 思想是对于路径上的预瞄点，假设后轮中心点可以按照一定的转弯半径$R$行驶到该预瞄点，那么根据后轮中心点到预瞄点的距离$l_d$，就可以求得$\delta$。

首先基于后轴线，后轴中点和预瞄点做等腰三角形。显然该三角形的地角$\pi /2-\alpha$。由于车辆纵轴线垂直于后轴线，因此可知等腰三角形底边与车辆纵轴线夹角为$\alpha$。

接着，作等腰三角形的角分线，该线将等腰三角形分为了两个全等的直角三角形，$2\alpha$顶角被平分。因此有

$$\sin \alpha =\frac{\frac{l_d}{2}}{R}=\kappa \frac{l_d}{2}=\frac{l_d\tan \delta }{2L}$$

最终可得

$$\delta =\arctan \frac{2L\sin \alpha }{l_d}$$

方案一

根据几何关系，有如下关系
$$\begin{array}{rl}(R-g_y{)}^2+g_x^2& =R^2\\ g_x^2+g_y^2& =l_d^2\end{array}$$
做恒等变换有
$$\kappa =\frac{1}{R}=\frac{2g_y}{l_d^2}$$
到这里可以直接根据$\kappa$计算前轮转角
$$\delta =\arctan (L\kappa )=\arctan \frac{2g_{y}L}{l_d^2}$$
注释：这里的预瞄点的横向坐标和横向误差是一样的故也有材料写$e_y$

预瞄点选择

以后轴中心为原点做圆，取车前圆与轨迹的交点（可能为插值点），该点即为预瞄点。
由前面的讨论可知，Pure Pursuit本质上是一个与预瞄距离有关的比例控制器($K_p=2L/l^2,e_y=e_y$ )，当预瞄距离(lookahead distance) $l_d$越小时，比例控制器系数$K_p$越大。由基本的控制原理不难理解，系统的响应越快，跟踪性能更灵敏；而当预瞄距离(lookahead distance)越大时，比例控制器系数$K_p$越小，系统的响应较慢，容易造成较大的跟随误差。