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 pythonimport os
import csv
import mathfrom geometry_msgs.msg import Quaternion, PoseStamped, TwistStamped, Twistfrom styx_msgs.msg import Lane, Waypointfrom gazebo_msgs.msg import ModelStatesimport tf
import rospyHORIZON = 6.0class 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 = Noneself.currentVelocity = Noneself.currentWaypoints = Noneself.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 = datadef vel_cb(self,data):self.currentVelocity = datadef lane_cb(self,data):self.currentWaypoints = datadef calculateTwistCommand(self):lad = 0.0 #look ahead distance accumulatortargetIndex = len(self.currentWaypoints.waypoints) - 1for i in range(len(self.currentWaypoints.waypoints)):if((i+1) < len(self.currentWaypoints.waypoints)):this_x = self.currentWaypoints.waypoints[i].pose.pose.position.xthis_y = self.currentWaypoints.waypoints[i].pose.pose.position.ynext_x = self.currentWaypoints.waypoints[i+1].pose.pose.position.xnext_y = self.currentWaypoints.waypoints[i+1].pose.pose.position.ylad = lad + math.hypot(next_x - this_x, next_y - this_y)if(lad > HORIZON):targetIndex = i+1breaktargetWaypoint = self.currentWaypoints.waypoints[targetIndex]targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.xtargetX = targetWaypoint.pose.pose.position.xtargetY = targetWaypoint.pose.pose.position.y       currentX = self.currentPose.pose.position.xcurrentY = self.currentPose.pose.position.y#get vehicle yaw anglequanternion = (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 differencealpha = math.atan2(targetY - currentY, targetX - currentX) - yawl = 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 commandtwistCmd = Twist()twistCmd.linear.x = targetSpeedtwistCmd.angular.z = theta else:twistCmd = Twist()twistCmd.linear.x = 0twistCmd.angular.z = 0return twistCmdif __name__ == '__main__':try:PurePersuit()except rospy.ROSInterruptException:rospy.logerr('Could not start motion control node.')

pure pursuit纯跟踪相关推荐

Pure Pursuit纯跟踪算法Python/Matlab算法实现
本文的python源代码来自: https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit ...
[运动控制算法]Pure Pursuit纯路径跟踪算法
纯路径跟踪 Pure Pursuit是一种用于路径跟踪的控制算法.它通过计算角速度控制机器人从当前位置移到机器人前方的某个预瞄点.假定线速度是恒定的,当然可以随意更改机器人的线速度.该算法会根据机器人 ...
matlab实现纯跟踪算法
对<Pure Pursuit纯跟踪算法Python/Matlab算法实现>该博客matlab算法提升: 该博客链接:https://blog.csdn.net/gophae/article ...
自动驾驶纯跟踪算法仿真基于Carsim-ros-simulink联合仿真平台的pp算法仿真
自动驾驶纯跟踪算法仿真基于Carsim-ros-simulink联合仿真平台的pp算法仿真 pure pursuit纯跟踪算法轨迹跟踪仿真可用于两个PC端或者虚拟机支持Carsim2018和m ...
无人车系统（五）：轨迹跟踪Pure Pursuit方法
今天介绍一种基于几何追踪的无人车轨迹跟踪方法--Pure Pursuit(纯跟踪)方法. 1. 阿克曼转向几何模型在无人车系统(一):运动学模型及其线性化一文中,里面介绍无人车的运动学模型为阿克曼转 ...
自动驾驶笔记-轨迹跟踪之①纯跟踪算法（Pure Pursuit）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.阿克曼转向模型 1.1 模型理解 1.2 模型表达二.纯跟踪算法(Pure Pursuit) 2.1 算法理解 ...
基于车辆运动学模型的纯跟踪(Pure Pursuit)法
一.定义及概论纯跟踪控制算法(Pure Pursuit)是一种典型的横向控制方法,最早由 R. Wallace 在1985年提出,该方法对外界的鲁棒性较好. 该算法的思想:基于当前车辆后轮中心位置, ...
路径跟踪之Pure Pursuit控制算法
Pure Pursuit是一种几何跟踪控制算法,也被称为纯跟踪控制算法.该算法最早由R. Wallace在1985年提出,其思想是基于当前车辆的后轮中心位置(车辆质心),在参考路径上向(称为前视距离) ...
无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪
无人驾驶汽车系统入门(十八)--使用pure pursuit实现无人车轨迹追踪对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位 ...

pure pursuit纯跟踪

pure pursuit纯跟踪相关推荐

最新文章

热门文章