pure pursuit纯跟踪

115 阅读 0 评论 76 点赞

我是靠谱客的博主顺心月光，这篇文章主要介绍pure pursuit纯跟踪，现在分享给大家，希望可以做个参考。

Pure Pursuit是一种几何追踪方法,速度越小,performance越好;

$delta$ :汽车前轮转角

L:前后轮轴距(车长)

R:转弯半径

将车辆模型简化为自行车模型(这里默认左轮和右轮的旋转是一致的)!!!

bicycle model:

$tandelta =frac{L}{R }$

pure pursuit建立于自行车模型和阿克曼小车模型的基础上,goal point为距离后轴中心最近的点.

1、pure pursuit的公式推导:

$alpha$ :目标点方向和当前航向角方向夹角;

$l_{d}$ :前视距离;

$e_{ld}$ :横向误差；

$frac{l_{d}}{sin2alpha }=frac{R}{sin(frac{Pi }{2}-alpha )}$

$frac{l_{_d}}{2sinalpha cosalpha }=frac{R}{cosalpha }$

$R = frac{2l_{_d}}{sinalpha }$

$sinalpha =frac{e_{ld}}{l_{d}}$

联立 $R = frac{2l_{_d}}{sinalpha }$ 和 $tandelta =frac{L}{R }$ 可得：

$delta(t) = tan^{-1}(frac{2L}{l_{d}^2}e_{ld}(t))$

以上就是pure pursuit的相关公式，purepursuit是基于横向误差（cross track error）放大 $frac{2L}{l_{d}^2}$ 倍的比例控制器。

2、pure pursuit的实现步骤：

（1）确定车辆自身位置

（2）找到距离当前位置最近的点

（3）寻找目标点G，以车辆后轴为中心，Ld为半径画一个圆弧找到规划路径的交点

（4）转换到车身坐标系下

（5）用pure pursuit计算公式计算到达目标点所需的转向角

3、影响因素

由purepursuit公式可知，影响最大的就是 $l_{_{d}}$ 、 $l_{_{d}}$ 会影响(steering angle ) $delta$ 、进而影响车辆对轨迹的追踪效果；

$l_{_d}$ 小	pure pursuit performance 越好	稳定性越差	准确性越高
$l_{_d}$ 大	pure pursuit performance 越差	稳定性越好	准确性越低

4、改进

$l_{_d}$ 并没有和vehicle的velociety相关，并且(steering angle) $delta$ 并不能无限大无限小；

改进：对 $l_{_d}$ 和速度关联起来（pure_pursuit的特性是：长的平滑轨迹上越小的前视距离准确度越好），对 $delta$ 设定范围；

$delta (t)=tan^{-1}(frac{2Lsin(alpha(t))}{l_{d}})$

$delta(t)=tan^{-1}(frac{2Lsin(alpha )}{kv_{x}})$

$l_{_d} = KV_{_x}$ 将 $l_{_d}$ 与V关联起来，V正比于 $l_{_d}$ ；

K越小	稳定性越差
K越大	Acc越小

5、pure_pursuit的挑战

（1）如何选择一个合适的前视距离？

答： $l_{_d} = KV_{_x}$

（2）不要刻意的将pure_pursuit针对于某一特定的场景进行调整、因为会出现过拟合现象；

（3）当车辆还没有到预瞄点的时候就切换到下一个目标点，故无法对曲线达到100%的追踪，对于直线的效果很好；

复制代码

#!/usr/bin/env python

import os
import csv
import math

from geometry_msgs.msg import Quaternion, PoseStamped, TwistStamped, Twist

from styx_msgs.msg import Lane, Waypoint

from gazebo_msgs.msg import ModelStates

import tf
import rospy

HORIZON = 6.0

class PurePersuit:
	def __init__(self):
		rospy.init_node('pure_persuit', log_level=rospy.DEBUG)

rospy.Subscriber('/smart/rear_pose', PoseStamped, self.pose_cb, queue_size = 1)
		rospy.Subscriber('/smart/velocity', TwistStamped, self.vel_cb, queue_size = 1)
		rospy.Subscriber('/final_waypoints', Lane, self.lane_cb, queue_size = 1)

self.twist_pub = rospy.Publisher('/smart/cmd_vel', Twist, queue_size = 1)

self.currentPose = None
		self.currentVelocity = None
		self.currentWaypoints = None

self.loop()

def loop(self):
		rate = rospy.Rate(20)
		rospy.logwarn("pure persuit starts")
		while not rospy.is_shutdown():
			if self.currentPose and self.currentVelocity and self.currentWaypoints:
				twistCommand = self.calculateTwistCommand()
				self.twist_pub.publish(twistCommand)
			rate.sleep()

def pose_cb(self,data):
		self.currentPose = data

def vel_cb(self,data):
		self.currentVelocity = data

def lane_cb(self,data):
		self.currentWaypoints = data

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
		#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)
		yaw = euler[2]
		#get angle difference
		alpha = math.atan2(targetY - currentY, targetX - currentX) - yaw
		l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
		if(l > 0.5):
			theta = math.atan(2 * 1.868 * math.sin(alpha) / l)
			# #get twist command
			twistCmd = Twist()
			twistCmd.linear.x = targetSpeed
			twistCmd.angular.z = theta 
		else:
			twistCmd = Twist()
			twistCmd.linear.x = 0
			twistCmd.angular.z = 0

return twistCmd

if __name__ == '__main__':
    try:
        PurePersuit()
    except rospy.ROSInterruptException:
        rospy.logerr('Could not start motion control node.')

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
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
#!/usr/bin/env python

import os
import csv
import math

from geometry_msgs.msg import Quaternion, PoseStamped, TwistStamped, Twist

from styx_msgs.msg import Lane, Waypoint

from gazebo_msgs.msg import ModelStates

import tf
import rospy

HORIZON = 6.0

class PurePersuit:
	def __init__(self):
		rospy.init_node('pure_persuit', log_level=rospy.DEBUG)

		rospy.Subscriber('/smart/rear_pose', PoseStamped, self.pose_cb, queue_size = 1)
		rospy.Subscriber('/smart/velocity', TwistStamped, self.vel_cb, queue_size = 1)
		rospy.Subscriber('/final_waypoints', Lane, self.lane_cb, queue_size = 1)

		self.twist_pub = rospy.Publisher('/smart/cmd_vel', Twist, queue_size = 1)

		self.currentPose = None
		self.currentVelocity = None
		self.currentWaypoints = None

		self.loop()

	def loop(self):
		rate = rospy.Rate(20)
		rospy.logwarn("pure persuit starts")
		while not rospy.is_shutdown():
			if self.currentPose and self.currentVelocity and self.currentWaypoints:
				twistCommand = self.calculateTwistCommand()
				self.twist_pub.publish(twistCommand)
			rate.sleep()

	def pose_cb(self,data):
		self.currentPose = data

	def vel_cb(self,data):
		self.currentVelocity = data

	def lane_cb(self,data):
		self.currentWaypoints = data

	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
		#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)
		yaw = euler[2]
		#get angle difference
		alpha = math.atan2(targetY - currentY, targetX - currentX) - yaw
		l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
		if(l > 0.5):
			theta = math.atan(2 * 1.868 * math.sin(alpha) / l)
			# #get twist command
			twistCmd = Twist()
			twistCmd.linear.x = targetSpeed
			twistCmd.angular.z = theta 
		else:
			twistCmd = Twist()
			twistCmd.linear.x = 0
			twistCmd.angular.z = 0

		return twistCmd


if __name__ == '__main__':
    try:
        PurePersuit()
    except rospy.ROSInterruptException:
        rospy.logerr('Could not start motion control node.')