49.在ROS中实现local planner(2)- 实现Purepersuit(纯跟踪)算法

这篇具有很好参考价值的文章主要介绍了49.在ROS中实现local planner(2)- 实现Purepersuit(纯跟踪)算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

48.在ROS中实现local planner(1)- 实现一个可以用的模板实现了一个模板,接下来我们将实现一个简单的纯跟踪控制,也就是沿着固定的路径运动,全局规划已经规划出路径点,基于该路径输出相应的控制速度

1. Pure Pursuit

Pure Pursuit路径跟随便是基于受约束移动机器人圆周运动的特性所开发出来的运动控制方式。原理十分简单,如图所示,移动机器人有一个前视的搜索半径,与机器人规划的路径有一个焦点,假设机器人从当前位置到路径焦点的运动为圆周运动。其中的前视距离便是图1中的L。根据几何关系便可以计算机器人的运动半径。
49.在ROS中实现local planner(2)- 实现Purepersuit(纯跟踪)算法

受约束的机器人模型(不能横向运动)可由两个控制量组成,即运动参考点的线速度v与角速度w。在极短的运行周期中,机器人都是以固定的线速度与角速度运动。因此机器人的运动可以视为圆周运动(w=0时为直线运动)。

2. 运动半径推算

如图所示的机器人便是绕着一个旋转中心进行圆周运动,于是移动机器人的运动控制可视为求解其在运动过程中的实时旋转半径。图中,r为移动机器人的旋转半径。

我们以base坐标系为例,及当前机器人为坐标原点,x轴为前方,y轴为左方,即ROS的坐标系,(x,y)为目标点, L为距离目标点的距离(前视距离),如下计算,容易求得旋转半径r

由图可得 d = r − y d = {r - y} d=ry
d 2 + x 2 = r 2 d^2+x^2 = r^2 d2+x2=r2
即: r 2 − 2 r y + y 2 + x 2 = r 2 r^2-2ry+y^2+x^2 = r^2 r22ry+y2+x2=r2
即: x 2 + y 2 = 2 r y x^2+y^2 = 2ry x2+y2=2ry
即: L 2 = 2 r y L^2 = 2ry L2=2ry
即: r = L 2 / 2 y r = L^2/2y r=L2/2y

运动半径=前视距离的平方/两倍的y

我们知道r=v/w 即我们只需要给定v/w为固定的值即可

因v与L相关 我们取一次关系
v=aL a为长数项
可得w=v/r=a*2y/L

3. 坐标转换

我们知道setPlan下发的坐标一般使用的是map坐标系,我们计算的时候需要转换为base坐标系

我们可以使用init接口提供的tf::TransformListener即可完成, (1.14.0版本后接口更新,使用新的接口)


geometry_msgs::PoseStamped PurepursuitPlanner::goalToBaseFrame(const geometry_msgs::PoseStamped& goal_pose_msg) {

#if ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, 1, 14, 0)

  geometry_msgs::PoseStamped goal_pose, base_pose_msg;

  goal_pose = goal_pose_msg;

  goal_pose.header.stamp = ros::Time(0.0);

  try {

    base_pose_msg = tf_->transform(goal_pose, "base_link");

  } catch (tf2::TransformException& ex) {

    ROS_WARN("transform err");

    return base_pose_msg;

  }

#else

  geometry_msgs::PoseStamped base_pose_msg;

  tf::Stamped<tf::Pose> goal_pose, base_pose;

  poseStampedMsgToTF(goal_pose_msg, goal_pose);

  goal_pose.stamp_ = ros::Time();

  try {

    tf_->transformPose(costmap_ros_->getBaseFrameID(), goal_pose, base_pose);

  } catch (tf::TransformException& ex) {

    ROS_WARN("transform err");

    return base_pose_msg;

  }

  tf::poseStampedTFToMsg(base_pose, base_pose_msg);

#endif

  return base_pose_msg;

}

4. 前视距离

我们不断根据当前位置,更新前视距离,通过前面的接算,给定速度

4.1 前视距离大小设置

前世距离可以根据V我们预设速度相关

  • 如果前世距离较大,相当于路径采样间隔较大,跟踪路径与规划路径的偏差会大。

  • 如果前世距离较小,机器人容易抖动

4.2 前视距离更新策略

如果当前距离路径中前视距离的点后的n个点的距离小于前世距离,则更新前视距离
即如果当前前视距离的点在路径索引为n,则判断n+m索引距离当前点位置是否小于预设前视距离值

5. 速度限制

一般机器人小车,线速度是>0的即,只能前进,无法后退。这就需要我们新增当前前视点角度判断, 如果角度超过90,即在车的后方。可以对速度修正强制旋转

  auto target_yaw_diff = atan2(goal.pose.position.y, goal.pose.position.x);  // 当前目标点相对机器人的角度
  
.... // 计算半径 速度

    // 当前目标点相对机器人的角度 相差较大(即目标点在机器人后面), 需要直发角速度(即原地旋转), 转向目标点
    if (target_yaw_diff > PI*0.5) {
      cmd_vel.linear.x = 0;
      cmd_vel.angular.z = 0.8;
    } else if (target_yaw_diff < -PI*0.5) {
      cmd_vel.linear.x = 0;
      cmd_vel.angular.z = -0.8;
    }

6. 完成判断

我们在前视点到达规划路径的最后一个时,且当前点与该最后一点距离差小于预设的容忍差,强制输出0速度

  if (got && l < GOAL_TOERANCE_XY) {
    goal_reached_ = true;
    cmd_vel.angular.z = 0;
    cmd_vel.linear.x = 0;
  }

7. 测试

  • 修改move_base配置文件move_base_params.yaml
# base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_local_planner: pure_pursuit_local_planner/PurepursuitPlanner

dwa_local_planner/DWAPlannerROS—>pure_pursuit_local_planner/PurepursuitPlanner

  • 启动模拟器
pibot_simulator
  • 启动rviz
pibot_view

49.在ROS中实现local planner(2)- 实现Purepersuit(纯跟踪)算法文章来源地址https://www.toymoban.com/news/detail-441567.html

到了这里,关于49.在ROS中实现local planner(2)- 实现Purepersuit(纯跟踪)算法的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 运行teb_local_planner/TebLocalPlannerROS时报错

    在navigation时,运行pnc导航,报了Failed to create the teb_local_planner/TebLocalPlannerROS的错误。 解决办法: 在你的工作空间catkin_ws/src下进行teb源码安装 完成后回到工作空间运行以下命令: 进行编译: 此时再运行导航即可。 大功告成!!!

    2024年02月11日
    浏览(43)
  • 【运动规划算法项目实战】如何使用MPC算法进行路径跟踪(附ROS C++代码)

    自动驾驶和机器人领域中,路径跟踪是一项关键技术,它使车辆或机器人能够沿着预定轨迹行驶或移动。传统的控制方法往往难以应对复杂的动态环境和非线性特性,而模型预测控制(Model Pr

    2024年02月12日
    浏览(39)
  • 【运动规划算法项目实战】如何使用Pure Pursuit算法进行路径跟踪(附ROS C++代码)

    在自动驾驶和机器人导航领域,路径跟踪是一项关键技术,它使车辆或机器人能够按照预定的路径规划进行移动。路径跟踪算法的选择对于系统性能和安全性至关重要。本文将介绍一种常用且有效的路径跟踪算法——Pure Pursuit算法。我们将详细讨论Pure Pursuit算法的原理、

    2024年02月10日
    浏览(71)
  • 轨迹规划 | 图解路径跟踪PID算法(附ROS C++/Python/Matlab仿真)

    🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。 🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法 PID控制 是一种常用的

    2024年02月07日
    浏览(51)
  • ros中实现全局/局部避障算法的添加与rviz中规划路径的显示(上)

    目录 前言 一、命令行 二、所用到的launch文件、yaml文件等 1.map1_mrobot_laser_nav_gazebo.launch 2.gmapping_demo.launch 3.gmapping.launch 4.move_base.launch 5.nav03_map_server.launch 6.mrobot_teleop.launch 三、rviz中添加path插件 总结 最近在做ros相关的作业,故写下本文留做参考以便日后再次使用或理解,如有

    2024年02月01日
    浏览(39)
  • yolov8跟踪模式部署Ros系统上,跟踪鼠标选择的目标,实时发布目标的坐标信息(python实现)

    鼠标不点击目标,不发送任何信息,图像显示的是yolov8检测目标的所有结果 鼠标点击后,跟踪鼠标选择的目标并实时循环发布目标的坐标信息,图像显示的是目标的坐标框 若选择的目标丢失在摄像头内,暂停发送坐标信息,且图像显示的是yolov8检测目标的所有结果,等待鼠

    2024年04月27日
    浏览(41)
  • ROS中实现A*路径规划

    学习A*路径规划算法,优化启发函数,并在ROS中进行测试。 A star算法教程 源码地址:https://gitcode.net/VOR234/aster 安装Linux系统,建议Ubuntu18.04; 安装ROS环境并学习其基本操作; 查找A 路径规划资料,学习并熟知A 路径规划算法; 对比赛中所提供A*算法的启发函数AstarPathFinder::ge

    2024年01月16日
    浏览(33)
  • 利用OpenCV光流算法实现视频特征点跟踪

            光流(optical flow)是运动物体在观察成像平面上的像素运动的瞬时速度。光流法是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系,从而计算出相邻帧之间物体的运动信息的一种方法。通常将二维图像平

    2024年02月12日
    浏览(35)
  • UKF跟踪算法原理及C++代码实现

    UKF跟踪算法是一种常用的非线性滤波算法,用于在不确定度较高的情况下估计系统的状态。其原理是通过对观测到的状态信息进行加权处理来计算系统的实际状态值。以下是UKF跟踪算法的原理和cpp代码实现的详细说明。 UKF跟踪算法原理 UKF跟踪算法是一种基于卡尔曼滤波算法

    2024年02月06日
    浏览(76)
  • 【FPGA目标跟踪】基于FPGA的帧差法和SAD匹配算法的目标跟踪实现

    quartusii12.1 FPGA整体的算法流程如下图所示:  FPGA的模块主要包括如下几大模块: 摄像机驱动程序,SDRAM控制程序,显示屏驱动程序,中值滤波程序,帧差法模块,SAD模板匹配模块,跟踪定位模块等等。 跟踪模块顶层程序 帧差法模块 SAD模板匹配模块,R通道 SAD模板匹配模块,

    2024年02月04日
    浏览(43)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包