控制算法 - Stanley 法

概述

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

扬州廋西湖

Stanley 方法

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

$$\begin{array}{}\text{(1)}& {\psi }_e=\psi -{\psi }_t\end{array}$$

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

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

$$\begin{array}{}\text{(2)}& \beta =\arcsin (\frac{k\cdot e_f}{v_x})\end{array}$$

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

$$\begin{array}{}\text{(3)}& \beta =\arctan (\frac{k\cdot e_f}{v_x})\end{array}$$

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

$$\begin{array}{}\text{(4)}& \begin{array}{cl}\delta & ={\psi }_e+\beta \\ & =\psi -{\psi }_t+\arctan ({\displaystyle \frac{k\cdot e_f}{v_x}})\end{array}\end{array}$$