pure pursuit纯跟踪

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

:汽车前轮转角

L:前后轮轴距(车长)

R:转弯半径

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

bicycle model:

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

1、pure pursuit的公式推导:

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

:前视距离;

:横向误差；

   

联立和可得：

以上就是pure pursuit的相关公式，purepursuit是基于横向误差（cross track error）放大倍的比例控制器。

2、pure pursuit的实现步骤：

（1）确定车辆自身位置

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

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

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

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

3、影响因素

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

小	pure pursuit performance 越好	稳定性越差	准确性越高
大	pure pursuit performance 越差	稳定性越好	准确性越低

 4、改进

并没有和vehicle的velociety相关，并且(steering angle)并不能无限大无限小；

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

 

将与V关联起来，V正比于；

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

5、pure_pursuit的挑战

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

答：

（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.')