控制算法 - Stanley 法

概述

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

Stanley 方法

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

(1)ψe=ψψt

其中,ψ 表示车辆横向角,ψt 表示离车辆前轴中心最近目标路径点处的航向角。

前轮速度轴与目标路径航向之间的夹角 β

(2)β=arcsin(kefvx)

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

(3)β=arctan(kefvx)

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

(4)δ=ψe+β=ψψt+arctan(kefvx)