参考资料

1. Stanley算法

之前学习了纯追踪算法和PID算法在轨迹跟踪上的应用，现在学习Stanley算法。

1.1 算法思想

前轮反馈控制(Front wheel feedback)也就是常说的Stanley方法，其核心思想是基于车辆前轴中心点的路径跟踪偏差量对方向盘转向控制量进行计算。

Stanley方法是一种基于横向跟踪误差(前轴中心到最近路径点的距离，注意，这里横向跟踪误差的定义与前文pure pursuit和PID中的轨迹跟踪定义的不太一样，后两者是基于后轴中心定义的)的非线性反馈函数,并且能实现横向跟踪误差指数收敛于0。

1.2 公式推导

在这里插入图片描述

前轮转角控制变量 $\delta$ 由两部分构成:

一部分是航向误差引起的转角,即当前车身方向与参考轨迹最近点的切线方向的夹角 $\theta_e$ ;
另一部分是横向误差引起的转角,即前轮速度方向与参考轨迹最近点的切线方向所成的夹角 $\delta_e$ 。

根据上图展示的几何关系，易得
$\tag{1} \delta =\theta_e+\delta_e$

显然， $\delta_e$ 满足
$\tag{2} \delta_e= \arctan{\frac{ke_y}{d}}= \arcsin{\frac{ke_y}{v}}$
其中， $d$ 是与车速 $v$ 相关的。

这里值得注意的是， $\delta_e$ 在许多论文中都是表示成
$\tag{3} \delta_e = \arctan{\frac{ke_y}{v}}$
猜测的一个可能原因是小角度假设，当 $\delta_e$ 很小时公式(3)是成立的；另外一个可能原因是因为增益系数 $k$ 可以任意选择，当 $k$ 选择恰当时，公式(3)是可以成立的。后面最终表达式也选用公式(3)的形式。

另外，根据几何关系， $\theta_e$ 满足
$\tag{4} \theta_e = \psi_t-\psi$
其中， $\psi$ 表示车辆航向角， $\psi_t$ 表示离车辆前轴中心最近目标路径点处的航向角。

结合等式(1)(3)(4)，得前轮转角控制量 $\delta$ 的最终表达式为
$\tag{5} \delta = \psi_t-\psi+\arctan{\frac{ke_y}{v}}$

1.3 横向误差变化率

横向误差的变化率为
$\tag{6} \dot e_y = -v\sin{\delta_e}$

带负号是因为在公式(5)的控制下，横向误差会越来越小，因此横向偏差变化率会有负号。

其中 $\sin \delta_e$ 根据几何关系可知：
$\tag{7-1} \sin \delta_{e}=\frac{e_{y}}{\sqrt{d(t)^{2}+e_{y}^{2}}}$
根据公式(3)，进一步也有
$\tag{7-2} \sin \delta_{e}=\frac{k e_{y}}{\sqrt{v^{2}+\left(k e_{y}\right)^{2}}}$

故有:
$\tag{8} \dot{e}_{y}=\frac{-k v e_{y}}{\sqrt{v^{2}+\left(k e_{y}\right)^{2}}}=\frac{-k e_{y}}{\sqrt{1+\left(\frac{k e_{y}}{v}\right)^{2}}}$
当横向跟踪误差 $e_{y}$ 很小时，上式改写为:
$\tag{9} \dot{e}_{y} \approx-k e_{y}$
积分上式（一阶线性微分方程的求解），得：
$\tag{10} e_{y}(t)=e_{y}(0) \times e^{-k t}$
因此，当 $t\rightarrow\infty$ 时，横向误差以指数形式收敛于 0，参数 $k$ 决定了收敛速度。

1.4 算法伪代码

输入：当前车辆位置、航向角 $\psi$ 、速度 $v$ ?，当前目标路点和离车辆前轴中心最近目标路径点处的航向角 $\psi_t$
计算：
- 计算横向误差 $e_y$ （这一步很重要）
- 计算 $\delta = \psi_t-\psi+\arctan{\frac{ke_y}{v}}$
输出：前轮转角控制量 $\delta$

2. python代码实现

2.1 车辆模型

设车辆运动学模型为以后轴中心为车辆中心的单车运动学模型（具体可参考笔者之前的博客），即
$\left\{\begin{array}{l} \dot{x}=V \cos (\psi) \\ \dot{y}=V \sin (\psi) \\ \dot{\psi}=\frac{V}{L}\tan{\delta_f}\\ \dot{V}=a \end{array}\right.$
python实现代码如下。

import math
class KinematicModel_3:
  """假设控制量为转向角delta_f和加速度a
  """

  def __init__(self, x, y, psi, v, L, dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.L = L
    # 实现是离散的模型
    self.dt = dt

  def update_state(self, a, delta_f):
    self.x = self.x+self.v*math.cos(self.psi)*self.dt
    self.y = self.y+self.v*math.sin(self.psi)*self.dt
    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v

2.2 相关参数设置

k=0.2 # 增益系数
dt=0.1 # 时间间隔，单位：s
L=2 # 车辆轴距，单位：m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角

2.3 搜索目标临近点

def cal_target_index(robot_state, refer_path):
    """得到临近的路点

    Args:
        robot_state (_type_): 当前车辆位置
        refer_path (_type_): 参考轨迹（数组）

    Returns:
        _type_: 最近的路点的索引
    """
    dists = []
    for xy in refer_path:
        dis = np.linalg.norm(robot_state-xy)
        dists.append(dis)

    min_index = np.argmin(dists)
    return min_index

2.4 角度归一化

def normalize_angle(angle):
    """
    Normalize an angle to [-pi, pi].

    :param angle: (float)
    :return: (float) Angle in radian in [-pi, pi]
    copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html
    """
    while angle > np.pi:
        angle -= 2.0 * np.pi

    while angle < -np.pi:
        angle += 2.0 * np.pi

    return angle

2.5 Stanley 算法实现

def stanley_control(robot_state,refer_path, refer_path_psi):
    """stanley控制

    Args:
        robot_state (_type_): 机器人位姿，包括x,y,yaw,v
        refer_path (_type_): 参考轨迹的位置
        refer_path_psi (_type_): 参考轨迹上点的切线方向的角度
        last_target_index (_type_): 上一个目标临近点

    Returns:
        _type_: _description_
    """
    current_target_index = cal_target_index(robot_state[0:2],refer_path)

    
    # 当计算出来的目标临近点索引大于等于参考轨迹上的最后一个点索引时
    if current_target_index>=len(refer_path):  
        current_target_index=len(refer_path)-1 
        current_ref_point = refer_path[-1] 
        psi_t = refer_path_psi[-1]
    else:
        current_ref_point=refer_path[current_target_index]
        psi_t = refer_path_psi[current_target_index]
    
    # 计算横向误差e_y,参考自https://blog.csdn.net/renyushuai900/article/details/98460758
    if(robot_state[0]-current_ref_point[0])*psi_t-(robot_state[1]-current_ref_point[1])>0:

        e_y=np.linalg.norm(robot_state[0:2]-current_ref_point)
    else:
        e_y = -np.linalg.norm(robot_state[0:2]-current_ref_point)


    # 通过公式(5)计算转角,符号保持一致
    psi = robot_state[2]
    v = robot_state[3]
    # psi_t的计算我看还有直接这么计算的
    # psi_t = math.atan2(current_ref_point[1]-robot_state[1],current_ref_point[0]-robot_state[0])
    theta_e = psi_t-psi
    delta_e = math.atan2(k*e_y,v)
    delta = normalize_angle(theta_e+delta_e)
    return delta,current_target_index

2.6 主函数



from celluloid import Camera # 保存动图时用，pip install celluloid
# set reference trajectory
refer_path = np.zeros((1000, 2))
refer_path[:, 0] = np.linspace(0, 100, 1000)  # 直线
refer_path[:, 1] = 2*np.sin(refer_path[:, 0]/3.0) +  2.5*np.cos(refer_path[:, 0]/2.0)  # 生成正弦轨迹
refer_path_psi = [math.atan2(refer_path[i+1,1]-refer_path[i,1],refer_path[i+1,0]-refer_path[i,0]) for i in range(len(refer_path)-1)] # 参考轨迹上点的切线方向的角度,近似计算

# 运动学模型
ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)

x_ = []
y_ = []
fig = plt.figure(1)
# 保存动图用
camera = Camera(fig)
# plt.ylim([-3,3])
for i in range(500):
    robot_state = np.zeros(4)
    robot_state[0] = ugv.x
    robot_state[1] = ugv.y
    robot_state[2]=ugv.psi
    robot_state[3]=ugv.v


    delta,ind = stanley_control(robot_state,refer_path,refer_path_psi)

    ugv.update_state(0, delta)  # 加速度设为0，恒速

    x_.append(ugv.x)
    y_.append(ugv.y)

    # 显示动图
    plt.cla()
    plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)
    plt.plot(x_, y_, "-r", label="trajectory")
    plt.plot(refer_path[ind,0], refer_path[ind,1], "go", label="target")
    # plt.axis("equal")
    plt.grid(True)
    plt.pause(0.001)
#     camera.snap()
# animation = camera.animate()
# animation.save('trajectory.gif')
plt.figure(2)
plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)
plt.plot(x_, y_, 'r')
plt.show()

跟踪正弦曲线，效果如下：
在这里插入图片描述

跟踪非正弦曲线，效果如下：
在这里插入图片描述

可见效果不是很好，猜测原因有三：一个原因可能是控制增益没有调好；第二，也可能因为我计算目标路点的切线角度(航向角 $\psi_t$ )时用的是近似计算的；最后，有可能因为我计算横向误差时方式不太对，代码中的横向误差计算是参考别人的，目前也不是很理解那个判断是怎么来的，有知道怎么计算横向误差的朋友们可以在评论区解个惑呀。谢谢~~

跟踪直线的效果还是挺好的
在这里插入图片描述