控制算法-Stanley法

发表于 2020-02-23 更新于 2021-08-30 分类于控制算法，路径跟踪阅读次数：

概述

本文主要介绍Stanley方法的推导，该方法也称为前轮位置反馈。该方法基于车辆几何关系，算法简单，适合低速下的车辆路劲跟踪。

Stanley方法是一种基于航迹误差\(e_{f}\)的非线性反馈函数,该航向误差是车辆前轴中心点与目标路径最近点\((c_x,c_y)\)的距离。由上图几何关系得

\[ \psi_e = \psi - \psi_{t} \tag{1} \]

其中，\(\psi\)表示车辆横向角，\(\psi_{t}\)表示离车辆前轴中心最近目标路径点处的航向角。

前轮速度轴与目标路径航向之间的夹角\(\beta\)为

\[ \beta = \arcsin(\frac{k\cdot e_{f}}{v_x}) \tag{2} \]

但是在论文中都是使用反正切函数求\(\beta\)角，可能原因是系数\(k\)可以任意选择，选择可是的系数\(k\)，确实可以转化为

\[ \beta = \arctan(\frac{k\cdot e_{f}}{v_x}) \tag{3} \]

结合等式(1)和(3)得

\[ \begin{array}{cl} \delta &= \psi_e + \beta \\ &= \psi - \psi_{t} + \arctan(\dfrac{k\cdot e_{f}}{v_x}) \end{array}\tag{4} \]