ROS局部路径规划器插件teb_local_planner流程梳理(上)

news2025/1/20 1:38:07

在这里插入图片描述


   在我之前的文章《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》中已经介绍过Move_base节点调用局部路径规划器插件的接口函数是computeVelocityCommands,接下来,我们就从这个函数入手梳理一下teb_local_planner功能包的工作流程。

   ☆注:因篇幅较长,本部分内容分成了上和下两篇文章,阅读完成本文的内容后,想要阅读后续内容,请前往下,继续阅读。

   下篇文章链接如下:

   《ROS局部路径规划器插件teb_local_planner流程梳理(下)》


   一、规划前的准备工作

   1、判断插件是否已完成初始化,若没有则返回错误信息


if(!initialized_)

   2、获取机器人当前位姿及速度信息

costmap_ros_->getRobotPose(robot_pose);
odom_helper_.getRobotVel(robot_vel_tf);
robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);

   3、对走过的全局路径部分进行修剪

   由于机器人是实时运动的,所以需要对已经走过的全局路径裁减处理,通过调用pruneGlobalPlan函数完成裁剪处理,其实现思路是从全局路径规划容器的起点开始一个个遍历,计算其与机器人当前点的距离差,直到找到距离小于我们设定的阈值的点,将该点之前的全局路径点全部删除,这样就可以将位于机器人后方距离超过一定阈值的点删除掉。

   上述判断用的阈值使用的是外部参数:global_plan_prune_distance。

  //修改全局路径规划,将已经走过的路径删除,保留机器人前方的路径
  pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);

   4、 将全局路径转换到局部代价地图的坐标系下,并从中选择合适的点作为局部路径点

   调用transformGlobalPlan函数,通过tf 树将全局路径信息转换到局部代价地图的坐标系下,值得注意的是,在这个函数中并不仅仅进行了坐标系的变换,还对全局路径信息点进行了一些处理,从中选出合适的点作为局部路径规划点。

   其具体过程是依次对全局路径进行坐标变换,同时计算其与当前位置的距离,并统计已转换的全局路径点之间的距离和(即已转换的全局路径的长度),直到超过设定的长度阈值或者与当前点的距离大于设定的距离阈值,则丢弃剩余的路径点。

   其中,长度阈值由外部参数max_global_plan_lookahead_dist决定,距离阈值由局部代价地图的大小width和height以及系数0.85有关

transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, 
                           transformed_plan, &goal_idx, &tf_plan_to_global)

   附:距离阈值sq_dist_threshold 计算过程:

double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
double sq_dist_threshold = dist_threshold * dist_threshold;

   概括一下,经过本步之后从全局路径信息中,选出的局部路径点是在以机器人当前位置为圆心距离阈值sq_dist_threshold 半径的圆内的路径点,且累积路径长度小于路径长度阈值的点的集合。


   5、 对得到的局部路径规划点进行进一步的筛选

   调用updateViaPointsContainer函数,对上一步得到的局部路径规划点进行进一步的筛选,其实现思路是,循环遍历局部路径规划点,从中选出距离上一个被选中的点的距离大于设定的阈值的点(局部路径规划点中的第一个点为本步筛选中第一个被选中的点)

   这样处理可以将上一步得到的局部路径点稀疏化,从而减少局部路径点的个数,阈值由外部参数global_plan_viapoint_sep决定,若该参数设置为负数,则表示跳过本步处理,不进行筛选

updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);


   第3~5步过程动态演示如下:

ROS的TEB局部路径规划器全局路径处理过程示意


   6、 判断机器人是否到达目标点

   得到局部路径的状态点后,先检查是否已经到达了全局目标点,具体要检查以下内容:

   (1)当前点距离目标点的距离是否小于由外部参数设定的阈值xy_goal_tolerance

   (2)当前点的姿态角与目标姿态角是否小于由外部参数设定的阈值yaw_goal_tolerance

   (3)判断机器人是否完全到位,由参数complete_global_plan以及via_points_.size()影响,其中complete_global_plan主要作用是防止机器人在越过终点时提前结束路径

   (4)有些版本中还会检测判断机器人当前速度是否停止,跟参数theta_stopped_vel、trans_stopped_vel、free_goal_vel有关,其中free_goal_vel由外部参数设定是否允许机器人以最大速度驶向目的地,即是否允许机器人达到目标点的速度不为零,参数heta_stopped_vel和trans_stopped_vel在melodic版本的包中并没有。

if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
    && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
    && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))

   7、 判断是否进入恢复行为模式

   调用configureBackupModes函数判断是否进入恢复模式:

   (1)短时间内规划失败:若机器人规划失败时,没有到达目标点,但是距离上次局部路径规划成功的时间间隔很小,算法会缩小规划范围,对局部路径点进行裁减,将远处的点先裁减掉,以更近的点作为规划目标,尝试重新规划出可行路径。

   (2)长时间的规划失败:多次短期规划都失败后,算法判断机器人是否长时间在某个地方发生振荡,其具体思路是:首先将机器人当前速度与角速度归一化到[0,1],然后对线速度与角速度进行判断,若机器人在一段时间内的平均线速度与角速度都小于阈值,并且前后两次采样过程中机器人的角速度相反,则认为机器人陷入了振荡状态(算法中创建了容器buffer_。用于面存储了一段时间内的机器人线速度与角速度)

   当机器人处于振荡状态,同时如果这个时间超过阈值oscillation_recovery_min_duration则机器人会进行旋转,旋转方向由机器人当前角速度反方向决定,如果没有超过阈值则以当前机器人角速度方向进行旋转。

configureBackupModes(transformed_plan, goal_idx);

   8、调整局部目标点的角度

   根据外部参数 global_plan_overwrite_orientation决定是否调整局部目标点的角度(也就是经过上述处理后的路径的最后一个点的角度),调整是通过调用estimateLocalGoalOrientation函数来实现的

if (cfg_.trajectory.global_plan_overwrite_orientation)
robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);

   当需要修改时,会判断当前局部路径规划的终点与全局路径规划的终点是否接近,如果两个点比较接近(就是同一个点或者下一个点就是终点)则直接使用当前局部路径规划中终点的方向作为规划的方向。如果当前局部路径规划的终点跟全局目标点之间还间隔很多个点的话,会使用当前局部路径的终点以及向后两个点的角度进行平滑处理,得到一个新的角度值。

   当不需要修改时,采用局部目标点的原有角度。


   9、将机器人当前的位姿插入到局部路径点中

   上一步完成了局部规划的目标点的确认,这一步要完成局部规划的起始点的确认,,前面虽然根据全局路径规划中截取了局部路径规划的点,但是这个容器的起点不一定是是机器人当前时刻所处的点,因此,需要将机器人当前位姿插入到前面得到的局部路径点中作为规划的起点。

  transformed_plan.front() = robot_pose; // update start  更新起始点

   10、更新障碍物信息

   根据外部参数 costmap_converter决定如何更新障碍物信息,若该参数不为空,则调用costmap_converter插件进行地图信息更新,若为空直接使用costmap的信息进行更新。

   分别对应下面那个函数:

updateObstacleContainerWithCostmapConverter
updateObstacleContainerWithCostmap

   若使用costmap_converter插件进行更新,则在updateObstacleContainerWithCostmapConverter函数中调用了getObstacles()函数从代价地图转换器获得障碍物,并对障碍物根据其形状(圆、点、线、多边形)进行分类,将其转换成相应的障碍物类型,然后将障碍物坐标存储到obstacles_容器中,针对移动的障碍物还会设定其速度。

   若使用costmap的信息进行更新,根据外部参数 include_costmap_obstacles决定是否考虑costmap中的障碍物,若考虑则遍历整个代价地图,对于每一个代价地图单元格,如果该单元格的代价为LETHAL_OBSTACLE(代价值为100),则将其视为障碍物,并检测该障碍物是否足够引人注意(例如,离机器人不远),如果障碍物在机器人后面且距离机器人的距离超过了外部参数决定的阈值 costmap_obstacles_behind_robot_dist,则忽略该障碍物。最后将得到的障碍物添加到障碍物容器中


   除此之外,还要从ROS消息中获取障碍物信息,然后将这些障碍物添加到当前的障碍物容器中。该过程通过函数updateObstacleContainerWithCustomObstacles完成

  // also consider custom obstacles (must be called after other updates, since the container is not cleared)
  // 还要考虑custom障碍(必须在其他更新之后调用,因为障碍容器没有被清除)
  updateObstacleContainerWithCustomObstacles();

   消息来源于:

custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

   至此,规划前的准备工作完成,下面正式开始规划进行局部路径的规划


   二、调用核心函数plan进行局部路径规划

  bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);

   需要注意的是存在三个名为plan的函数,根据传入的形参及其含义可知此处调用的是如下函数:

bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)

   1、根据teb_.isInit()判断是否初始化从起点到目标点的路径

   若是,则调用initTrajectoryToGoal函数完成该操作,若不是则采用热启动的方式,此时需要进一步判断,如果当前目标与上一个目标的位置差小于阈值force_reinit_new_goal_dist,姿态角度差小于阈值force_reinit_new_goal_angular,则调用updateAndPruneTEB函数清理路径上已经走过的点,若不满足上述阈值,则认为当前目标点与上一个目标点之间的距离足够远了,需要重新调用initTrajectoryToGoal函数进行路径的初始化。

   其中,updateAndPruneTEB函数的具体实现思路是,首先在局部路径规划点中寻找距离当前位置最近的点,然后从局部路径规划点的第一个点开始往后搜索直至找到这个最近的点。然后删除这个最近的点之前的那些点。

   调用initTrajectoryToGoal函数的语句如下:

 teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
      cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);

   需要注意的是存在两个名为initTrajectoryToGoal的函数,根据传入的形参及其含义可知此处调用的是如下函数:

bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)

   其具体实现思路如下:

   (1)检查是否已经初始化路径,如果没有,则添加起始位置处的姿态点(包括机器人的位置和朝向)到局部路径点的容器中,并设置起点优化中为固定不变的。

   (2)设定后退标志位,若外部参数 allow_init_with_backwards_motion为真,即否允许在开始时想后退来执行轨迹,且目标点在起始点的后方,则将后退标志位backwards标志设置为true,否则为false。

   (3)从起始点开始遍历局部路径点容器中的每个点,若外部参数 global_plan_overwrite_orientation为真,即否允覆盖全局路径中局部路径点的朝向,若允许,则根据相邻两个局部路径点之间的连线的朝向利用atan2函数重新计算每个局部路径点的朝向,否则,保持每个局部路径姿态点的原有朝向不变。

   在上述遍历中,还会利用外部参数 max_vel_x和max_vel_theta,即设定的最大线速度和角速度来预估相邻两个局部路径点之间要花费的时间dt,即两点间位置差除以max_vel_x,得到线运动时间,两点间角度差除以max_vel_theta得到角运动时间,然后取这两个时间中的较大值作为两点之间小路径的预估时间dt。

   (4)判断是否需要强制添加局部路径点,若当前的局部路径点的数量小于由外部参数设定的最小样本数 min_samples-1,为了保证生成的路径点数量足够多,则会强制添加一些局部路径点,具体方式是不断在当前点与目标点之间取中点添加到局部路径点中(当前点会变化),直至局部路径点的数量等于min_samples-1。

  static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
  {
    return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
  }

   (5)将目标点添加到局部路径点的末尾处,并将其设为固定,即在优化中不能被改变。

   至此,局部路径的初始化完成,可以这样理解以上过程,当我们接收到新的目标点时(新的目标点与上一个目标点的距离或姿态差大于设定的阈值)需要重新调用initTrajectoryToGoal函数进行路径的初始化,若目标点没有变化,或者变化小于阈值,则认为是机器人移动过程中针对同一目标点的多次规划,此时只需要调用updateAndPruneTEB函数对已经走过的部分的路径点进行裁剪就可以了,不需要重新进行路径初始化,也就是所谓的热启动模式。


   ☆注:因篇幅较长,本部分内容分成了上和下两篇文章,阅读完成本文的内容后,想要阅读后续内容,请前往下,继续阅读。

   下篇文章链接如下:

   《ROS局部路径规划器插件teb_local_planner流程梳理(下)》



本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/895835.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

Vue使用Element的表格Table显示树形数据,多选框全选无法选中全部节点

使用Element的组件Table表格&#xff0c;当使用树形数据再配合上多选框&#xff0c;如下&#xff1a; 会出现一种问题&#xff0c;点击左上方全选&#xff0c;只能够选中一级树节点&#xff0c;子节点无法被选中&#xff0c;如图所示&#xff1a; 想要实现点击全选就选中所有的…

强强联手:Eyeshot 2023.2 集成 CAD Exchanger SDK 3.21 Crack

Lab 中的新绘图模式和网格抽取、Eyeshot 集成以及从 CAD Exchanger 3.21.0 中的 CATIA 导入图形 PMI 通过探索新的绘图模式和可调整的网格抽取&#xff0c;更深入地了解实验室&#xff0c;见证与 devDept Eyeshot 的无缝集成&#xff0c;并直接从 CATIA 导入图形 PMI。 今年 8…

阿里云100元预算可选的云服务器配置2核2G3M带宽

阿里云服务器100元可以买到哪些配置&#xff1f;如果是一年时长&#xff0c;轻量应用服务器2核2G3M带宽一年108元&#xff0c;系统盘为50GB高效云盘。以前阿里云服务器ECS卖过35元一年、69元、88元、89元和99元的都有过&#xff0c;但是现在整体费用上涨&#xff0c;入门级云服…

超实用的40道JAVA经典算法题(含答案)

作为一名Java程序员&#xff0c;想要拿到一份满意的offer&#xff0c;就必须做好充足的准备。众所周知&#xff0c;算法可以说是大厂面试Java程序员的必问题。好的算法可以让性能得到万倍提升&#xff0c;做到毫秒级处理千万数据的程度。因此&#xff0c;算法的重要性不言而喻&…

【云原生,k8s】Helm应用包管理器介绍

目录 一、为什么需要Helm&#xff1f; &#xff08;一&#xff09;Helm介绍 &#xff08;二&#xff09;Helm有3个重要概念&#xff1a; &#xff08;三&#xff09;Helm特点 二、Helm V3变化 &#xff08;一&#xff09;架构变化 &#xff08;二&#xff09;自动创建名…

ICCV23 | Ada3D:利用动态推理挖掘3D感知任务中数据冗余性

​ 论文地址&#xff1a;https://arxiv.org/abs/2307.08209 项目主页&#xff1a;https://a-suozhang.xyz/ada3d.github.io/ 01. 背景与动因 3D检测(3D Detection)任务是自动驾驶任务中的重要任务。由于自动驾驶任务的安全性至关重要(safety-critic)&#xff0c;对感知算法的延…

CodeSite for .NET Crack

CodeSite for .NET Crack CodeSite for.NET与Visual Studio集成&#xff0c;通过实时查看器日志记录系统提供对代码执行的更深入了解&#xff0c;该系统有助于在本地或远程执行代码时快速查找问题。超越传统的断点调试&#xff0c;在应用程序继续运行时记录应用程序的执行&…

强训第34天

选择 A B D 由协议处理粘包问题 C C C host字段表示要访问的IP地址 Connection为Close&#xff0c;为短连接 A RTT报文往返时间&#xff0c;根据窗口大小和拥塞控制可以得知发了1 2 4 8 总共16个接收缓存并且没由进行数据提取&#xff0c;则剩余接收量为16-151&#xff0c;会经…

k8s简介、虚拟机快速搭建k8s集群、集群管理方式及K8S工作原理和组件介绍

文章目录 1、k8s简介1.1、部署方式的变迁1.2、定义1.3、Kubernetes提供的功能 2、虚拟机快速搭建k8s集群2.1、虚拟机配置&#xff08;centos7 2G内存2个处理器&#xff09;2.2、基础环境准备2.3、docker安装&#xff08;易踩坑&#xff09;2.4、安装k8s组件2.5、master节点部署…

Power BI 之 初相识

一、简介 传统数据分析工具入Excel等分析大数据量存在一些弊端&#xff0c;一是无法应对多数据源&#xff0c;二是数据量大容易卡死&#xff0c;三是无法根据数据源动态变化&#xff0c;导致每天重复分析数据&#xff0c;无法关注数据价值本身。 Power BI&#xff08;简写PBI…

隧道代理技术解析:为批量数据采集提供强大支持

嘿&#xff01;作为一名专业的爬虫程序员&#xff0c;我今天要和大家分享一个强大的技术&#xff0c;它能够为批量数据采集提供强大的支持——隧道代理技术。如果你在进行大规模数据采集任务时遇到了IP封禁和限制的问题&#xff0c;那么这项技术将是你的救星。废话不多说&#…

案例-基于MVC和三层架构实现商品表的增删改查

文章目录 0. 项目介绍1. 环境准备2. 查看所有2.1 编写BrandMapper接口2.2 编写服务类&#xff0c;创建BrandService&#xff0c;用于调用该方法2.5 编写Servlet2.4 编写brand.jsp页面2.5 测试 3.添加3.1 编写BrandMapper接口 添加方法3.2 编写服务3.3 改写Brand.jsp页面&#x…

【使用群晖远程链接drive挂载电脑硬盘】

文章目录 前言1.群晖Synology Drive套件的安装1.1 安装Synology Drive套件1.2 设置Synology Drive套件1.3 局域网内电脑测试和使用 2.使用cpolar远程访问内网Synology Drive2.1 Cpolar云端设置2.2 Cpolar本地设置2.3 测试和使用 3. 结语 前言 群晖作为专业的数据存储中心&…

用pytorch实现AlexNet

AlexNet经典网络由Alex Krizhevsky、Hinton等人在2012年提出&#xff0c;发表在NIPS&#xff0c;论文名为《ImageNet Classification with Deep Convolutional Neural Networks》&#xff0c;论文见&#xff1a;http://www.cs.toronto.edu/~hinton/absps/imagenet.pdf &#xf…

原型设计必备,10个简单好用的界面工具推荐

在数字产品和应用设计过程中,使用简单实用的界面原型设计工具至关重要。它可以将设计理念快速转换为交互原型,便于团队成员之间的沟通和协作。对设计新人来说,选择一个上手快速、功能强大的界面原型设计工具尤其关键。本文将推荐10款常用的界面原型设计工具,帮助初学者找到适合…

解决C/C++/QT url中含有特殊字符转码的问题

一、http请求链接中的特殊字符处理 解决包括Postman 中的post、get等链接请求中的特殊字符(如:*,#,{},[],^,%,(,?,!..等等)的情况 1、场景: 特殊字符存在,报400错误 2、原因 如果请求的参数带有一些特殊字符,如 +,%,=等,如果没有进行URL编码可能在发送请求的时候,后…

WinPlan经营大脑垂直大模型行业报告

一、引言 在当前高度信息化的时代,企业经营管理决策的重要性已经得到了广泛的认可。然而,在实际操作中,许多企业仍然在凭经验、拍脑袋进行经营决策,缺乏数据工具与专职分析团队,导致决策难、效率低等问题。针对这一问题,近年来,一种名为“WinPlan”的经营决策产品逐渐崭…

NVIDIA vGPU License许可服务器高可用全套部署秘籍

第1章 前言 近期遇到比较多的场景使用vGPU&#xff0c;比如Citrix 3D场景、Horizon 3D场景&#xff0c;还有AI等&#xff0c;都需要使用显卡设计研发等&#xff0c;此时许可服务器尤为重要&#xff0c;许可断掉会出现掉帧等情况&#xff0c;我们此次教大家部署HA许可服务器。 …

视频汇聚/视频云存储/视频监控管理平台EasyCVR添加萤石云设备详细操作来啦!

安防视频监控/视频集中存储/云存储/磁盘阵列EasyCVR平台可拓展性强、视频能力灵活、部署轻快&#xff0c;可支持的主流标准协议有国标GB28181、RTSP/Onvif、RTMP等&#xff0c;以及支持厂家私有协议与SDK接入&#xff0c;包括海康Ehome、海大宇等设备的SDK等。平台既具备传统安…

解决Fastjson2 oom(Out Of Memory),支持大对象(LargeObject 1G)json操作

在使用Fastjson中的 JSON.toJSONString时,如果对象数据太大&#xff08;>64M&#xff09;会出现Out Of Memory,查看源码发现为JSONWriter中的判断代码 其中maxArraySize默认最大为64M,如果超过了就会抛出oom错误 如果fastjson过多的使用内存,也可能导致java堆内存溢出,所以这…