【自动驾驶】Stanley实现轨迹跟踪

参考资料

1. Stanley算法

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

1.1 算法思想

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

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

1.2 公式推导

前轮转角控制变量【自动驾驶】Stanley实现轨迹跟踪由两部分构成:

  • 一部分是航向误差引起的转角,即当前车身方向与参考轨迹最近点的切线方向的夹角【自动驾驶】Stanley实现轨迹跟踪;
  • 另一部分是横向误差引起的转角,即前轮速度方向与参考轨迹最近点的切线方向所成的夹角【自动驾驶】Stanley实现轨迹跟踪

根据上图展示的几何关系,易得
【自动驾驶】Stanley实现轨迹跟踪

显然,【自动驾驶】Stanley实现轨迹跟踪满足
【自动驾驶】Stanley实现轨迹跟踪
其中,【自动驾驶】Stanley实现轨迹跟踪是与车速【自动驾驶】Stanley实现轨迹跟踪相关的。

这里值得注意的是,【自动驾驶】Stanley实现轨迹跟踪在许多论文中都是表示成
【自动驾驶】Stanley实现轨迹跟踪
猜测的一个可能原因是小角度假设,当【自动驾驶】Stanley实现轨迹跟踪很小时公式(3)是成立的;另外一个可能原因是因为增益系数【自动驾驶】Stanley实现轨迹跟踪可以任意选择,当【自动驾驶】Stanley实现轨迹跟踪选择恰当时,公式(3)是可以成立的。后面最终表达式也选用公式(3)的形式。

另外,根据几何关系,【自动驾驶】Stanley实现轨迹跟踪满足
【自动驾驶】Stanley实现轨迹跟踪
其中,【自动驾驶】Stanley实现轨迹跟踪表示车辆航向角,【自动驾驶】Stanley实现轨迹跟踪表示离车辆前轴中心最近目标路径点处的航向角。

结合等式(1)(3)(4),得前轮转角控制量【自动驾驶】Stanley实现轨迹跟踪的最终表达式
【自动驾驶】Stanley实现轨迹跟踪

1.3 横向误差变化率

横向误差的变化率为
【自动驾驶】Stanley实现轨迹跟踪

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

其中 【自动驾驶】Stanley实现轨迹跟踪 根据几何关系可知:
【自动驾驶】Stanley实现轨迹跟踪
根据公式(3),进一步也有
【自动驾驶】Stanley实现轨迹跟踪

故有:
【自动驾驶】Stanley实现轨迹跟踪
当横向跟踪误差 【自动驾驶】Stanley实现轨迹跟踪 很小时,上式改写为:
【自动驾驶】Stanley实现轨迹跟踪
积分上式(一阶线性微分方程的求解),得:
【自动驾驶】Stanley实现轨迹跟踪
因此,当【自动驾驶】Stanley实现轨迹跟踪时,横向误差以指数形式收敛于 0,参数 【自动驾驶】Stanley实现轨迹跟踪 决定了收敛速度。

1.4 算法伪代码

  • 输入:当前车辆位置、航向角【自动驾驶】Stanley实现轨迹跟踪、速度【自动驾驶】Stanley实现轨迹跟踪​,当前目标路点和离车辆前轴中心最近目标路径点处的航向角【自动驾驶】Stanley实现轨迹跟踪
  • 计算:
    • 计算横向误差【自动驾驶】Stanley实现轨迹跟踪(这一步很重要)
    • 计算【自动驾驶】Stanley实现轨迹跟踪
  • 输出:前轮转角控制量【自动驾驶】Stanley实现轨迹跟踪

2. python代码实现

2.1 车辆模型

车辆运动学模型以后轴中心为车辆中心的单车运动学模型(具体可参考笔者之前的博客),即
【自动驾驶】Stanley实现轨迹跟踪
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()


跟踪正弦曲线,效果如下:

跟踪非正弦曲线,效果如下:

可见效果不是很好,猜测原因有三:一个原因可能是控制增益没有调好;第二,也可能因为我计算目标路点的切线角度(航向角【自动驾驶】Stanley实现轨迹跟踪)时用的是近似计算的;最后,有可能因为我计算横向误差时方式不太对,代码中的横向误差计算是参考别人的,目前也不是很理解那个判断是怎么来的,有知道怎么计算横向误差的朋友们可以在评论区解个惑呀。谢谢~~

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

完整python代码文件见github仓库

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

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

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

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

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
上一篇 2022年5月25日 下午2:17
下一篇 2022年5月25日 下午2:21

相关推荐

本站注重文章个人版权,不会主动收集付费或者带有商业版权的文章,如果出现侵权情况只可能是作者后期更改了版权声明,如果出现这种情况请主动联系我们,我们看到会在第一时间删除!本站专注于人工智能高质量优质文章收集,方便各位学者快速找到学习资源,本站收集的文章都会附上文章出处,如果不愿意分享到本平台,我们会第一时间删除!