文章目录

参考资料
1. Stanley算法
- 1.1 算法思想
- 1.2 公式推导
- 1.3 横向误差变化率
- 1.4 算法伪代码
2. python代码实现
- 2.1 车辆模型
- 2.2 相关参数设置
- 2.3 搜索目标临近点
- 2.4 角度归一化
- 2.5 Stanley 算法实现
- 2.6 主函数
3. 小结：Pure pursuit与Stanley 算法简单对比

参考资料

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

1. Stanley算法

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

1.1 算法思想

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

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

1.2 公式推导

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

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

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

显然，δe\delta_eδe满足
δe=arctan⁡eyd=arcsin⁡keyv(2)\tag{2} \delta_e= \arctan{\frac{e_y}{d}}= \arcsin{\frac{ke_y}{v}} δe=arctandey=arcsinvkey(2)
其中，ddd是与车速vvv相关的，即
d=vk(3)\tag{3} d=\frac{v}{k} d=kv(3)

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

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

1.3 横向误差变化率

横向误差的变化率为
e˙y=−vsin⁡δe(6)\tag{6} \dot e_y = -v\sin{\delta_e} e˙y=−vsinδe(6)

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

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

故有:
e˙y=−kveyv2+(key)2=−key1+(keyv)2(8)\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=v2+(key)2−kvey=1+(vkey)2−key(8)
当横向跟踪误差 eye_{y}ey 很小时，上式改写为:
e˙y≈−key(9)\tag{9} \dot{e}_{y} \approx-k e_{y} e˙y≈−key(9)
积分上式（一阶线性微分方程的求解），得：
ey(t)=ey(0)×e−kt(10)\tag{10} e_{y}(t)=e_{y}(0) \times e^{-k t} ey(t)=ey(0)×e−kt(10)
因此，当t→∞t\rightarrow\inftyt→∞时，横向误差以指数形式收敛于 0，参数 kkk 决定了收敛速度。

1.4 算法伪代码

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

2. python代码实现

2.1 车辆模型

设车辆运动学模型为以后轴中心为车辆中心的单车运动学模型（具体可参考笔者之前的博客），即
{x˙=Vcos⁡(ψ)y˙=Vsin⁡(ψ)ψ˙=VLtan⁡δfV˙=a\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. ⎩⎨⎧x˙=Vcos(ψ)y˙=Vsin(ψ)ψ˙=LVtanδfV˙=a
python实现代码如下。

import math
class KinematicModel_3:"""假设控制量为转向角delta_f和加速度a"""def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = L# 实现是离散的模型self.dt = dtdef update_state(self, a, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v+a*self.dtdef 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.piwhile angle < -np.pi:angle += 2.0 * np.pireturn angle

2.5 Stanley 算法实现

def stanley_control(robot_state,refer_path, refer_path_psi):"""stanley控制Args:robot_state (_type_): 机器人位姿，包括x,y,yaw,vrefer_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/98460758if(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-psidelta_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.xrobot_state[1] = ugv.yrobot_state[2]=ugv.psirobot_state[3]=ugv.vdelta,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()

跟踪正弦曲线，效果如下：

跟踪非正弦曲线，效果如下：

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

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

完整python代码文件见github仓库

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

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

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

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

【自动驾驶】Stanley（前轮反馈）实现轨迹跟踪相关推荐

自动驾驶（七）---------初探轨迹规划
自动驾驶中轨迹规划是最常见的问题,本文分以下几个场景设计轨迹规划: 1. 带约束的多项式拟合算法.2. 贝赛尔曲线拟合. 3. 三次样条差值. 1. 带约束的多项式拟合算法自动驾驶中经常遇到一类问题 ...
自动驾驶中的轨迹预测数据集汇总！
作者 | 冯偲编辑 | 汽车人原文链接:https://zhuanlan.zhihu.com/p/555618753 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷 ...
自动驾驶轨迹预测任务浅述
注:本文只用来记录我实习过程中积累的粗浅理解,平时抽空写写,不一定正确和全面.随着认识的增多我会不断更新修正本文内容,也欢迎大家帮我指正问题. 目录 1. 自动驾驶任务浅述 2. 轨迹预测任务 2.1 ...
stanley 轨迹跟踪算法
一:简单了解简介:Stanley是一种轨迹跟踪算法: Stanley control low: ComputerControlcmd函数,根据Stanley算法的公式进行代码的编写,所以需要调用接下 ...
【自动驾驶轨迹预测】一文熟悉自动驾驶轨迹预测发展现状！
来源 | 自动驾驶之心来源 | https://zhuanlan.zhihu.com/p/365881810 知圈 | 进"滑板底盘群"请加微yanzhi-6,备注底盘目录何 ...
综述：城市道路自动驾驶车辆规划与控制技术
作者 | 深蓝学院编辑 | 深蓝AI 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[规划控制]技术交流群后台回复[规划控制综述 ...
自动驾驶之路已走了多远？一文读懂研究现状
作者:Claudine Badue等编译:机器之心 Panda 摘要:自动驾驶载具或将引爆人类的下一次出行方式革命,而我们目前又已经走到了哪一步?近日,巴西圣埃斯皮里图联邦大学的研究者在 arXiv ...
Mark # 自动驾驶之路已走了多远？一文读懂研究现状
自动驾驶载具或将引爆人类的下一次出行方式革命,而我们目前又已经走到了哪一步?近日,巴西圣埃斯皮里图联邦大学的研究者在 arXiv 上发布了一篇自动驾驶汽车研究情况总结,并简单梳理了产业界的进展.本文摘 ...
自动驾驶规划术语与搜索空间的几种方法
1. 导读目前,自动驾驶或自动驾驶汽车是学术界和汽车界研究的核心,因为它具有多方面的优势,包括提高安全性.减少拥堵.降低排放和提高机动性.其实软件是支持自动驾驶的关键驱动因素,在将乘客或货物从指定的 ...
仿真环境跟车2分钟，就让自动驾驶系统撞上马路牙子，攻破率超90%，多传感器融合系统都失效...
鱼羊萧萧发自凹非寺量子位报道 | 公众号 QbitAI 自动驾驶领域目前最强的MSF(多传感器融合)定位算法,再次被攻破了. 攻击之下,平均30秒内,正常行驶中的自动驾驶汽车就撞上了马路牙子 ...

【自动驾驶】Stanley（前轮反馈）实现轨迹跟踪