1. 几何路径跟踪方法(Pure Pursuit和Stanley Method)

1 几何车辆模型(Geometric Vehicle Model)

用于几何路径跟踪的阿克曼转向车辆的一个常见简化是自行车模型，或者叫单车模型。自行车模型简化了四轮汽车，将两个前轮和两个后轮组合在一起，形成一个两轮模型。自行车模型的第二个简化假设是，车辆只能在平面上进行移动。这些简化导致前轮转向角和后轴轨迹曲率之间将遵循简单的几何关系，如图1所示。

图1 几何单车模型

根据图1有几何关系：

其中是前轮的转向角，是前轴和后轴之间的距离(轴距)，是后轴在给定转向角下运行所沿的圆半径。此模型可近似描述汽车在低速和适度转向角下的运动。

2 Pure Pursuit方法

图2 Pure Pursuit几何模型

对于移动机器人和自动驾驶系统，Pure Pursuit（纯追踪）法及其变种是最常用的路径跟踪方法之一。Pure Pursuit法中，需要计算后轴位置连接车辆前方路径目标点的圆弧曲率。目标点位置由预瞄距离确定，其坐标如图2所示。从图中可以发现，仅使用车辆航向方向与预瞄点方向之间的角度，即可确定车辆的转向角。这是因为，将正弦定律应用于图2，可得到如下结果：

将上面最后的式子写成曲率形式有：

其中， κ 为后轴轨迹曲率。另一方面，根据前面几何车辆模型中的第一个公式，有：

将该式的曲率用前面的公式代替，就可以得到Pure Pursuit方法的控制律为：

为了更好地理解该控制律，定义新的变量 eld 代表预瞄目标点到车辆航向向量的距离，即：

则曲率公式可重写为：

Pure Pursuit方法的控制律变成：

从该式可以看出Pure Pursuit控制方法本质上是一个关于预瞄横向误差的非线性比例控制器，而该控制增益和相关，即通过调节不同的预瞄距离可对于不同的控制性能效果。一般而言，预瞄距离和车辆行驶速度有关。假设预瞄距离和车速成正比，则Pure Pursuit控制律为:

其中，为可调控制参数。

一般而言，随着控制参数的增加，预瞄距离ld增加，车辆行驶过程中更不容易震荡、轨迹更平滑。而当控制参数减小时，预瞄距离ld减小，车辆行驶过程中横向控制误差将减小，同时也更容易导致闭环不稳定或者震荡。在车辆进入弯道时，过大的控制参数或者预瞄距离，将导致出现内切的现象。因此可以看到，和一般的控制算法一样，Pure Pursuit控制也要在稳定性和控制精度之间做妥协（trade off）。但是Pure Pursuit控制对道路曲率扰动具有良好的鲁棒性。

高速下转向曲率的快速变化易使车辆产生侧滑，系统模型与实际车辆特性相差较大会导致跟踪性能恶化，因此Pure Pursuit算法多适用于较低车速和小侧向加速度下的路径跟踪控制。

3 Stanley方法

图3 Stanley几何模型

Stanley方法是斯坦福大学无人车项目在DARPA挑战赛中使用的路径跟踪控制方法。该方法的反馈控制律是前轴中心到最近参考路径点横向误差的非线性函数，并且能够实现控制误差的指数收敛。

Stanley方法有不同的变种形式，这里只介绍其中一种，该控制律由两部分组成。第一项通过将转向角设置为航向误差来保持车轮与给定路径的方向一致，如图3所示，横向误差为：

其中，为车辆航向角, 为参考路径点的切线角度。当横向误差不为零时， Stanley控制律的第二项和横向误差及车速有关。完整的Stanley控制律为:

其中，为控制增益参数。从该控制律也可以看到，随着前轴横向误差的增大，前轮转角控制量将使车辆更靠近参考轨迹线。

一般而言，随着控制参数的增加，横向控制误差将会减小。和Pure Pursuit方法一样，Stanley方法同样是在控制精度和稳定性之前进行trade off。

参考文献：

[1] Snider J M . Automatic Steering Methods for Autonomous Automobile Path Tracking[J]. Robotics Institute, 2011.