参考资料

• 控制算法-Stanley前轮反馈控制法
• 使用Stanley method实现无人车轨迹追踪

1. Stanley算法

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

1.1 算法思想

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

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

1.2 公式推导

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

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

根据上图展示的几何关系，易得
$$\delta ={\theta }_e+{\delta }_e\text{\hspace{1em}(1)}$$

显然，${\delta }_e$满足
$${\delta }_e=\arctan \frac{e_y}{d}=\arcsin \frac{k{e}_y}{v}\text{\hspace{1em}(2)}$$
其中，$d$是与车速$v$正相关的，即
$$d=\frac{v}{k}\text{\hspace{1em}(3)}$$
$k$为正系数。

另外，根据几何关系，${\theta }_e$满足
$${\theta }_e={\psi }_t-\psi \text{\hspace{1em}(4)}$$
其中，$\psi$表示车辆航向角，${\psi }_t$表示离车辆前轴中心最近目标路径点处的航向角。

结合等式(1)(3)(4)，得前轮转角控制量$\delta$的最终表达式为
$$\delta ={\psi }_t-\psi +\arctan \frac{k{e}_y}{v}\text{\hspace{1em}(5)}$$

1.3 横向误差变化率

横向误差的变化率为
$${\dot{e}}_y=-v\sin {\delta }_e\text{\hspace{1em}(6)}$$

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

其中 $\sin {\delta }_e$ 根据几何关系可知：
$$\sin {\delta }_e=\frac{e_y}{\sqrt{d(t{)}^2+e_y^2}}\text{\hspace{1em}(7-1)}$$
根据公式(3)，进一步也有
$$\sin {\delta }_e=\frac{k{e}_y}{\sqrt{v^2+{\left(k{e}_y\right)}^2}}\text{\hspace{1em}(7-2)}$$

故有:
$${\dot{e}}_y=\frac{-kv{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}}\text{\hspace{1em}(8)}$$
当横向跟踪误差 $e_y$ 很小时，上式改写为:
$${\dot{e}}_y\approx -k{e}_y\text{\hspace{1em}(9)}$$
积分上式（一阶线性微分方程的求解），得：
$$e_y(t)=e_y(0)\times e^{-kt}\text{\hspace{1em}(10)}$$
因此，当$t\to \infty$时，横向误差以指数形式收敛于 0，参数 $k$ 决定了收敛速度。

1.4 算法伪代码

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

2. python代码实现

2.1 车辆模型

设车辆运动学模型为以后轴中心为车辆中心的单车运动学模型（具体可参考笔者之前的博客），即
$$\{\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}$$
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$)时用的是近似计算的；最后，有可能因为我计算横向误差时方式不太对，代码中的横向误差计算是参考别人的，目前也不是很理解那个判断是怎么来的，有知道怎么计算横向误差的朋友们可以在评论区解个惑呀。谢谢~~

跟踪直线的效果还是挺好的

完整python代码文件见github仓库

3. 小结：Pure pursuit与Stanley 算法简单对比

Pure pursuit 与 Stanley 两个方法都是基于对前轮转角进行控制来消除横向误差。

Pure pursuit算法的关键在于预瞄点的选取：其距离越短，控制精度越高，但可能会产生震荡；预瞄距离越长，控制效果趋于平滑，震荡减弱。实际调试只需根据上述规则以及应用需求调整预瞄系数即可。

Stanley 算法的控制效果取决于控制增益，它缺少Pure pursuit算法的规律性，调试时需要花一定精力去寻找合适的控制增益值。

4. 后记——c++代码实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边不做相关代码解释。完整代码详见另一个github仓库。