自动驾驶|横向控制|纯跟踪控制|Stanley控制|后轮反馈控制

1 车辆运动学模型

自行车模型假设：

1）不考虑车辆在垂直方向(Z轴方向)的运动，即假设车辆的运动是一个二维平面上的运动。

2) 假设车辆左右侧轮胎在任意时刻都拥有相同的转向角度和转速；这样车辆的左右两个轮胎的运动可以合并为一个轮胎来描述。

3）假设车辆行驶速度变化缓慢，忽略前后轴载荷的转移。

4) 假设车身和悬架系统都是刚性系统。

5）假设车辆的运动和转向是由前轮驱动的。

由上述假设可得自行车模型的运动微分方程如下式：

2 pure pursuit纯跟踪控制

算法核心思想：基于当前车辆后轮中心 位置，在参考路径path上向匹配一个预瞄点C，假设车辆后轮中心点A可以按照一定的转弯半径𝑅 行驶抵达该预瞄点（定圆运动），然后根据预瞄距离ld， 转弯半径𝑅，车辆坐标系下预瞄点的朝向角𝛼 之间的几何关系来确定前轮转角δf 。如下图所示：

满足的正弦定理：

化简得到：

阿克曼转向方程：

可到前轮转向角为：

为了更深的理解纯追踪控制器的原理，定义横向误差为预瞄点到车辆的距离：

联立上式和前轮转角控制公式，并考虑小角度的情况：

由此，可以看出纯跟踪算法近似于一个P控制器：

跟踪效果由预瞄距离ld 决定。

预瞄距离和弯道曲率以及车速相关，就像人开车一样，车速越快，我们人眼眺望的距离就远；弯道越急，人眼眺望的距离更近。

3 stanley控制

Stanley方法是一种基于横向跟踪误差（cross-track error：e 为前轴中心到最近路径点(p x, py)的距离）的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量，其中包含横向误差e和航向误差θe

横向误差控制量为：

综合控制量为：

为比例（横向误差）+积分（角度误差）控制。

使用线性自行车运动模型，可以得到横向误差的变化率：

根据几何关系有：

当误差较小时：

因此横向误差指数收敛于e(t)=0，参数k决定了收敛速度。对于任意横向误差，微分方程都单调的收敛到0。

4 后轮反馈控制

后轮反馈式（Rear wheel feedback）是利用 后轮中心 的 路径跟踪偏差量 来进行转向控制量计算的方法。

Frenet坐标系使用道路的中心线作为参考线，使用参考线的 切线向量 和 法线向量 建立坐标系
那么基于参考线的位置，就可以使用 纵向距离 和 横向距离 来描述任意位置
同时纵向和横向的速度、加速度、加加速度等信息也更便于计算

frenet坐标系下状态方程为：

e为横向误差，ψe 为角度误差。