前轮反馈控制(Stanley) 法

1.定义：

前轮反馈控制（Front wheel feedback）又称 Stanley 控制 ，其核心思想是基于前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算。

2.核心思想：

 Stanley方法是一种基于横向跟踪误差为前轴中心到最近路径点的距离的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可 以直观的获得控制车辆方向盘转角的控制变量。

Ø 前轮转角控制变量由两部分构成：一部分是航向误差引起的转角，即当前车身方向与参考轨迹最近点的切线方向的夹角；另一部分是横向误差引起的转角，即前轮中心到参考轨迹最近点的横向距离。

3.算法分析与推导

在不考虑横向跟踪误差的情况下，前轮偏角应当与给定路径参考点的切线方

向一致。其中，表示车辆航向与最近路径点切线方向之间的夹角，在没有

任何横向误差的情况下，前轮方向与所在路径点的方向相同（即参考点的切线方向）。

(编程中实现为参考点位置的的航向角-实际位置的航向角)

在不考虑航向跟踪偏差的情况下，横向跟踪误差越大，前轮转向角越大，假

设车辆预期轨迹点在距离最近点dt,方向为最近点切线的点汇合，根据几何

关系得出如下非线性比例函数：

其中d(t)与车速相关，用车速v(t)、增益参数k表示。

当横向跟踪误差很小时，上式改写为

针对上式采用拉氏变换求解微分方程，

因此横向误差指数收敛于ey(0)，参数k决定了收敛速度。

代码实现注意事项：

在实现的过程中要将代码分小模块写好，主要是利用前轮转角实现预瞄点的跟踪，因此前轮转角由两部分组成。

学习分享，来源于b站ALLY前辈的视频，本博客主要用于记录学习。