参考链接：
https://blog.csdn.net/renyushuai900/article/details/98460758
论文链接：
http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf
Github链接：
https://github.com/chanchanchan97/ROS
1. 算法简介
Stanley方法是一种基于横向跟踪误差（cross-track error：
e
e
e为前轴中心到最近路径点(
p
x
p_x
px,
p
y
p_y
py)的距离）的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量，其中包含横向误差e和航向误差
θ
e
θ_e
θe。
δ
(
t
)
=
δ
e
(
t
)
+
δ
θ
e
(
t
)
δ(t)=δ_e(t)+δ_{\theta_e}(t)
δ(t)=δe(t)+δθe(t)在不考虑横向跟踪误差
e
e
e的情况下，前轮偏角和给定路径切线方向一致，如图所示。其中
θ
e
θ_e
θe表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向与所在路径点的方向相同：
δ
θ
e
(
t
)
=
θ
e
(
t
)
δ_{\theta_e}(t)=\theta_e(t)
δθe(t)=θe(t)在不考虑航向跟踪误差
θ
e
θ_e
θe的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预期轨迹在距离前轮
d
(
t
)
d(t)
d(t)处与给定路径上最近点切线相交，根据几何关系得出如下非线性比例函数：
δ
e
(
t
)
=
a
r
c
t
a
n
e
(
t
)
d
(
t
)
=
a
r
c
t
a
n
k
e
(
t
)
v
(
t
)
δ_e(t)=arctan\frac{e(t)}{d(t)}=arctan\frac{ke(t)}{v(t)}
δe(t)=arctand(t)e(t)=arctanv(t)ke(t)
其中
d
(
t
)
d(t)
d(t)与车速相关，最后用车速
v
(
t
)
v(t)
v(t)，增益参数k表示。随着横向误差的增加，arctan函数产生一个直接指向期望路径的前轮偏角，并且收敛受车速
v
(
t
)
v(t)
v(t)限制。
综合两方面控制因素，基本转向角控制率如下：
δ
(
t
)
=
θ
e
(
t
)
+
a
r
c
t
a
n
k
e
(
t
)
v
(
t
)
δ(t)=\theta_e(t)+arctan\frac{ke(t)}{v(t)}
δ(t)=θe(t)+arctanv(t)ke(t)使用线性自行车运动模型，可以得到横向误差的变化率：
e
˙
(
t
)
=
−
v
(
t
)
s
i
n
δ
e
(
t
)
\dot{e}(t)=-v(t)sinδ_e(t)
e˙(t)=−v(t)sinδe(t)其中
s
i
n
δ
e
(
t
)
sinδ_e(t)
sinδe(t)根据几何关系可知：
s
i
n
δ
e
(
t
)
=
e
(
t
)
d
(
t
)
2
+
(
e
(
t
)
)
2
=
k
e
(
t
)
v
(
t
)
2
+
(
k
e
(
t
)
)
2
sinδ_e(t)=\frac{e(t)}{\sqrt{d(t)^2+(e(t))^2}}=\frac{ke(t)}{\sqrt{v(t)^2+(ke(t))^2}}
sinδe(t)=d(t)2+(e(t))2e(t)=v(t)2+(ke(t))2ke(t)
e
˙
(
t
)
=
−
v
(
t
)
k
e
(
t
)
v
(
t
)
2
+
(
k
e
(
t
)
)
2
=
−
k
e
(
t
)
1
+
(
k
e
(
t
)
v
(
t
)
)
2
\dot{e}(t)=\frac{-v(t)ke(t)}{\sqrt{v(t)^2+(ke(t))^2}}=\frac{-ke(t)}{\sqrt{1+(\frac{ke(t)}{v(t)})^2}}
e˙(t)=v(t)2+(ke(t))2−v(t)ke(t)=1+(v(t)ke(t))2−ke(t)当横向跟踪误差
e
(
t
)
e(t)
e(t)很小时，
(
k
e
(
t
)
v
(
t
)
)
2
→
0
(\frac{ke(t)}{v(t)})^{2}→0
(v(t)ke(t))2→0:
e
˙
(
t
)
≈
−
k
e
(
t
)
\dot{e}(t)\approx-ke(t)
e˙(t)≈−ke(t)通过积分上式:
e
(
t
)
=
e
(
0
)
×
e
−
k
t
e(t)=e(0)×e^{-kt}
e(t)=e(0)×e−kt因此横向误差指数收敛于e(t)=0，参数k决定了收敛速度。对于任意横向误差，微分方程都单调的收敛到0。
2. 源码解析
Note: 由于该部分代码与Pure_Persuit算法的代码有较大相似之处，因此这里只讲解核心部分calculateTwistCommand()函数及其不同之处。
(1) calculateTwistCommand()函数part.1
lad = 0.0 #look ahead distance accumulator
targetIndex = len(self.currentWaypoints.waypoints) - 1
for i in range(len(self.currentWaypoints.waypoints)):
if((i+1) < len(self.currentWaypoints.waypoints)):
this_x = self.currentWaypoints.waypoints[i].pose.pose.position.x
this_y = self.currentWaypoints.waypoints[i].pose.pose.position.y
next_x = self.currentWaypoints.waypoints[i+1].pose.pose.position.x
next_y = self.currentWaypoints.waypoints[i+1].pose.pose.position.y
lad = lad + math.hypot(next_x - this_x, next_y - this_y)
if(lad > HORIZON):
targetIndex = i+1
break
targetWaypoint = self.currentWaypoints.waypoints[targetIndex]
targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x
targetX = targetWaypoint.pose.pose.position.x
targetY = targetWaypoint.pose.pose.position.y
currentX = self.currentPose.pose.position.x
currentY = self.currentPose.pose.position.y
#print '[',targetX, targetY, ']'
#print targetWaypoint.pose.pose.orientation
print self.currentVelocity
#get waypoint yaw angle
waypoint_quanternion = (targetWaypoint.pose.pose.orientation.x, targetWaypoint.pose.pose.orientation.y, targetWaypoint.pose.pose.orientation.z, targetWaypoint.pose.pose.orientation.w)
waypoint_euler = tf.transformations.euler_from_quaternion(waypoint_quanternion)
TargetYaw = waypoint_euler[2]
print 'TargetYaw = ', TargetYaw
#get vehicle yaw angle
quanternion = (self.currentPose.pose.orientation.x, self.currentPose.pose.orientation.y, self.currentPose.pose.orientation.z, self.currentPose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quanternion)
CurrentYaw = euler[2]
print 'CurrentYaw = ', CurrentYaw
说明：
- 这部分与Pure_Persuit算法基本相同，大致分为三个小部分，因此只简述其中每一小部分的作用。
- 第一部分的主要作用是找到距离当前位置最近的局部路径点的横、纵坐标。
- 第二部分的主要作用是获取该局部路径点对应的目标航向角。
- 第三部分的主要作用是获取小车当前的航向角。
(2) calculateTwistCommand()函数part.2
#get the difference between yaw angle of vehicle and tangent line of waypoint
if TargetYaw >= 0:
alpha = TargetYaw - CurrentYaw
flag_alpha = 1
else:
if TargetYaw * CurrentYaw < 0:
if (TargetYaw < 0) and (CurrentYaw > 0):
alpha = (math.pi - CurrentYaw) + (math.pi + TargetYaw)
flag_alpha = 2
else:
alpha = -((math.pi + CurrentYaw) + (math.pi - TargetYaw))
flag_alpha = 3
else:
alpha = TargetYaw - CurrentYaw
flag_alpha = 4
print 'flag_alpha = ', flag_alpha
print '(', currentX, currentY, ')'
print '(', targetX, targetY, ')'
print 'alpha = ', alpha
print 'x = ', targetX - currentX
print 'y = ', targetY - current
说明：
- 该部分的主要作用是计算得到目标航向角与当前实际航向角之间的航向误差alpha。
(3) calculateTwistCommand()函数part.3
#get the error between target position and current position
if alpha >= 0:
error = abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
else:
error = -abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
print 'error = ', error
说明：
- 该部分的主要作用是计算得到小车前轮中心轴位置与目标路径点之间的横向误差error。
(4) calculateTwistCommand()函数part.4
#get the velocity of vehicle
vel = math.sqrt(math.pow(self.currentVelocity.twist.linear.x, 2) + math.pow(self.currentVelocity.twist.linear.y, 2))
print 'vel = ', vel
说明：
- 该部分的主要作用是获取小车在当前航向角方向上的速度值。
(5) calculateTwistCommand()函数part.5
#get the nonlinear proportional function from geometry of Vehicular mobility model
if vel < 0.001:
delta = 0
else:
delta = math.atan(k * error / vel)
print 'delta = ', delta
说明：
- 该部分的主要作用是根据几何关系得出一个非线性比例函数，从而产生一个直接指向期望路径的前轮偏角delta，并且收敛受车速v(t)限制。
(6) calculateTwistCommand()函数part.6
#get the distance between final position and current position
l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
l2 = math.sqrt(math.pow(currentX - final_targetX, 2) + math.pow(currentY - final_targetY, 2))
if l > 0.001:
if l2 > 0.1:
theta = alpha + delta
print 'theta = ', theta
#self.ackermann_steering_control(targetSpeed, theta)
self.ackermann_steering_control(15, -theta)
#get twist command
left_steer = Float64()
right_steer = Float64()
left_vel = Float64()
right_vel = Float64()
left_steer.data = left_angle
right_steer.data = right_angle
left_vel.data = left_speed
right_vel.data = right_speed
flag = 1
else:
left_steer = Float64()
right_steer = Float64()
left_vel = Float64()
right_vel = Float64()
left_steer.data = 0
right_steer.data = 0
left_vel.data = 0
right_vel.data = 0
flag = 2
else:
left_steer = Float64()
right_steer = Float64()
left_vel = Float64()
right_vel = Float64()
left_steer.data = 0
right_steer.data = 0
left_vel.data = 0
right_vel.data = 0
flag = 3
print 'flag = ', flag
return left_vel, right_vel, left_steer, right_steer
说明：
- 该部分的主要作用是获取小车当前位置与目标位置之间的距离，若小车当前位置与终点位置(0,4)的距离小于0.1m，则小车到达终点，此时停止运动。
3. 效果演示
ROS｜平平无奇路径跟踪仿真（Stanley）
1769
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
