路径跟踪——Stanley算法

发表于 | 分类于

1.算法简介

相关链接：
https://blog.csdn.net/renyushuai900/article/details/98460758
论文链接：
http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf

1.1算法推断

Stanley方法是一种基于横向跟踪误差（cross-track error：e为前轴中心到最近路径点(px​,py​)的距离）的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量，其中包含横向误差e和航向误差θe​。

●在不考虑横向跟踪误差e的情况下，前轮偏角和给定路径切线方向一致，如图所示。其中θe​表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向与所在路径点的方向相同：

●在不考虑航向跟踪误差θe​的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预期轨迹在距离前轮d(t)处与给定路径上最近点切线相交，根据几何关系得出如下非线性比例函数：

其中d(t)与车速相关，最后用车速v(t)，增益参数k表示。随着横向误差的增加，arctan函数产生一个直接指向期望路径的前轮偏角，并且收敛受车速v(t)限制。
综合两方面控制因素，基本转向角控制率如下：

●使用线性自行车运动模型，可以得到横向误差的变化率：

●其中sinδe​(t)根据几何关系可知：

●当横向跟踪误差e(t)很小时，:

●通过积分上式:

因此横向误差指数收敛于e(t)=0，参数k决定了收敛速度。对于任意横向误差，微分方程都单调的收敛到0。

2.源码解析

Note：由于该部分代码与Pure_Persuit算法的代码有较大相似之处，因此这里只讲解其中的不同以及核心部分。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
def calculateTwistCommand(self):
	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算法基本相同，大致分为三个小部分，因此只简述其中每一小部分的作用。
●第一部分的主要作用是找到距离当前位置最近的局部路径点的横、纵坐标。
●第二部分的主要作用是获取该局部路径点对应的目标航向角。
●第三部分的主要作用是获取小车当前的航向角。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#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。
1
2
3
4
5
6
#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。
1
2
3
#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

说明：
●该部分的主要作用是获取小车在当前航向角方向上的速度值。
1
2
3
4
5
6
#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)限制。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#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.效果演示