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