CN115903781A - 基于ros2系统的移动机器人自主探索方法及系统 - Google Patents

Abstract

本发明涉及一种基于ROS2系统的移动机器人自主探索方法及系统，方法包括获取机器人所处位置的环境信息；将环境信息输入至cartographer建图算法进行地图的构建；在地图的空闲区域内生长两棵快速搜索随机树进行边界点提取；过滤和剔除无效边界点；为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点；利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，完成环境探索。本发明借助ROS2系统开发了基于快速搜索随机树的自主探索方法及系统，提高了自主探索系统在场景下工作的稳定性。

Description

基于ROS2系统的移动机器人自主探索方法及系统

技术领域

本发明涉及人工智能技术领域，尤其是指一种基于ROS2系统的移动机器人自主探索方法及系统。

背景技术

人工智能技术的突破给移动型服务机器人研究带来了巨大的机遇，智能公共服务机器人应用场景和服务模式不断拓展，带动服务机器人市场规模高速增长。移动机器人自主探索技术是移动机器人领域的一项关键技术，该技术在隧道勘探、水下探测和机器人搜救等领域均得到了广泛应用。移动机器人自主探索需要机器人在有限的时间内，通过自身移动来尽可能多的获取未知环境信息从而建立完整的环境地图。边界作为区分已探索区域与未知区域的分界线，它附近具有较多不确定的环境信息，因而基于边界的探索策略受到广大学者的青睐。

基于边界的探索策略可简要的概括为如下过程：首先机器人根据自身传感器更新的地图数据检测地图中的边界，然后在边界附近生成目标点，再结合评价函数选择最具有探索价值的目标点作为最佳的目标点，最后利用运动控制模块控制机器人向最佳的目标点运动，并更新环境地图直到探索完成。然而现有的探索方法虽然在仿真场景中表现相对较好，但是在实际的应用中还存在诸多问题。比如，现有的方法多是基于ROS1系统进行开发，而ROS1系统由于ROS Master的限制，当Master出现问题时，整个系统就会停止运动。而ROS2系统采用分布式通信，相对于ROS1系统更加灵活和稳定。此外，在实际应用中，随着建图过程的进行，地图信息的更新导致边界点的位置发生偏移，这些边界点在地图更新的时候应当被清除掉，否则将导致机器人难以到达，进而影响后面的探索任务。基于这些特点，基于ROS2系统面向实际应用的自主探索算法开发对移动型服务机器人具有重要意义。

发明内容

为此，本发明所要解决的技术问题在于克服现有技术中存在的基于ROS1系统的机器人探索系统无法稳定应用于ROS2系统以及探索效率低的技术缺陷。

为解决上述技术问题，本发明提供了一种基于ROS2系统的移动机器人自主探索方法，包括以下步骤：

S1：获取机器人所处位置的环境信息，其中所述环境信息包括激光雷达信息、IMU信息和里程计信息；

S2：将激光雷达信息、IMU信息和里程计信息输入至cartographer建图算法进行地图的构建，并更新一部分地图为已知的区域；

S3：在地图的空闲区域内生长两棵快速搜索随机树，使用两棵快速搜索随机树进行边界点的提取；

S4：过滤和剔除无效边界点；

S5：为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点；

S6：利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，以不断探索未知区域；

S7：重复步骤S2～S6，直至环境探索完成。

在本发明的一个实施例中，在S1中，利用机器人自身携带的传感器获取其所处位置的环境信息。

在本发明的一个实施例中，在S3中，两棵快速搜索随机树分为全局树和局部树，全局树和局部树提取边界点的方法相同，其中当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长，且每隔设定的时间，将机器人当前位置作为节点添加到局部树上。

在本发明的一个实施例中，在S3中，使用两棵快速搜索随机树进行边界点的提取，包括：

S31：设置快速搜索随机树采样区域为以初始位置为中心的1000m²的正方形区域，将起点添加到树结构中作为根节点；

S32：在地图区域内随机撒点作为候选点，若该候选点在已知区域内，则遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，将最邻近点到候选点的连线作为生长方向，若最邻近点与候选点的距离超过预设的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；若最邻近点与候选点的距离不超过步长，则该候选点作为生长点；若候选点在未知区域内，则先找到该候选点的最邻近点，最邻近点到候选点的连线作为生长方向，然后由最邻近点沿着生长方向向前生长，到达边界的地方作为边界点；

S33：将生长点和候选点的连线在地图上做碰撞检测，判断该条连线上所有栅格点的栅格状态，若栅格点的状态是被占据的，则返回步骤S32重新进行采样，反之则将该候选点与生长点的连线添加到树结构中。

在本发明的一个实施例中，在S4中，在过滤和剔除无效边界点时，使用mean-shift聚类算法对边界点进行聚类处理，得到其质心点。

在本发明的一个实施例中，在过滤和剔除无效边界点时，剔除信息增益少的边界点、未知区域内的边界点以及过滤太靠近障碍物或处于障碍物上的边界点。

在本发明的一个实施例中，在S5中，所述收益函数包括信息增益和路径成本，其公式如下：

R1_f＝w₁*I_f-w₂*N_f

其中，R1_f表示收益函数，I_f表示信息增益，N_f表示路径成本，w₁和w₂表示权重。

此外，本发明还提供一种基于ROS2系统的移动机器人自主探索系统，包括：

数据获取模块，其用于获取机器人所处位置的环境信息，其中所述环境信息包括激光雷达信息、IMU信息和里程计信息；

地图构建模块，其用于将激光雷达信息、IMU信息和里程计信息输入至cartographer建图算法进行地图的构建，并更新一部分地图为已知的区域；

边界点提取模块，其用于在地图的空闲区域内生长两棵快速搜索随机树，使用两棵快速搜索随机树进行边界点的提取；

边界点过滤模块，其用于过滤和剔除无效边界点；

探索导航模块，其用于为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点，利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，以不断探索未知区域。

在本发明的一个实施例中，在边界点提取模块中，两棵快速搜索随机树分为全局树和局部树，全局树和局部树提取边界点的方法相同，其中当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长，且每隔设定的时间，将机器人当前位置作为节点添加到局部树上。

在本发明的一个实施例中，在边界点过滤模块中，剔除信息增益少的边界点、未知区域内的边界点以及过滤太靠近障碍物或处于障碍物上的边界点。

本发明的上述技术方案相比现有技术具有以下优点：

1、本发明所述的一种基于ROS2系统的移动机器人自主探索方法及系统，其借助ROS2系统开发了基于快速搜索随机树的自主探索方法及系统，提高了自主探索系统在场景下工作的稳定性；

2、本发明所述的一种基于ROS2系统的移动机器人自主探索方法及系统，其采用cartographer建图算法进行未知环境的建模，提高了构建的环境地图的质量，并且增加了边界点的过滤条件使地图前后更新差别较大时，机器人探索任务仍能有序进行，提高了探索系统的适应性和效率。

附图说明

为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中

图1是本发明所提出的一种基于ROS2系统的移动机器人自主探索方法的流程示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

参照图1所示，本发明的一种基于ROS2系统的移动机器人自主探索方法，包括以下步骤：

S1：获取机器人所处位置的环境信息，其中所述环境信息包括激光雷达信息、IMU信息和里程计信息；

S2：将激光雷达信息、IMU信息和里程计信息输入至cartographer建图算法进行地图的构建，并更新一部分地图为已知的区域；

S3：在地图的空闲区域内生长两棵快速搜索随机树，使用两棵快速搜索随机树进行边界点的提取；

S4：过滤和剔除无效边界点；

S5：为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点；

S6：利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，以不断探索未知区域；

S7：重复步骤S2～S6，直至环境探索完成。

本发明所述的一种基于ROS2系统的移动机器人自主探索方法及系统，其借助ROS2系统开发了基于快速搜索随机树的自主探索方法及系统，提高了自主探索系统在场景下工作的稳定性。

其中，在S1中，机器人对于整个环境信息都是未知的，利用机器人自身携带的传感器获取其所处位置的环境信息。

其中，在S3中，使用两棵快速搜索随机树进行边界点的提取，包括：

S31：设置快速搜索随机树(RRT)采样区域为以初始位置为中心的1000m²的正方形区域，将起点添加到树结构中作为根节点，由于目前移动机器人的应用场景多是家居、商超、医院这样的场景，这些场景的平面楼层通常均小于1000m²，因此设置RRT采样区域为以初始位置为中心的1000m²的正方形区域；

S32：在地图区域内随机撒点作为候选点，若该候选点在已知区域内，则遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，将最邻近点到候选点的连线作为生长方向，若最邻近点与候选点的距离超过预设的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；若最邻近点与候选点的距离不超过步长，则该候选点作为生长点；若候选点在未知区域内，则先找到该候选点的最邻近点，最邻近点到候选点的连线作为生长方向，然后由最邻近点沿着生长方向向前生长，到达边界的地方作为边界点；

S33：将生长点和候选点的连线在地图上做碰撞检测，判断该条连线上所有栅格点的栅格状态，若栅格点的状态是被占据的，则返回步骤S32重新进行采样，反之则将该候选点与生长点的连线添加到树结构中。

上述使用两棵快速搜索随机树进行边界点提取的方法中，两棵快速搜索随机树分为全局树和局部树，全局树和局部树提取边界点的方法相同，不同的是当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长，且每隔设定的时间(例如1s)，将机器人当前位置作为节点添加到局部树上，添加原理同上述步骤S32所述，即在局部树上寻找最近的节点与该节点相连接。

其中，在S4中，在过滤和剔除无效边界点时，通过上述两棵快速搜索随机树得到的边界点通常会有很多，因此需要通过mean-shift聚类算法对这些边界点进行聚类处理得到他们的质心点。这些质心点可以很好地表征一块待探索的区域，同时减少计算量。

上述在过滤和剔除无效边界点时，剔除信息增益少的边界点、未知区域内的边界点以及过滤太靠近障碍物或处于障碍物上的边界点，具体剔除方法如下：

剔除信息增益较少的边界点：当机器人向着质心点所在的未知区域探索后，这些质心点所包含的信息增益将会大大减少，因此应将其剔除。本发明方法为检测该边界点的栅格状态以及在costmap(代价地图)中的值(costmap将每一栅格值划分在0～255之间，白色值为255，代表空闲状态；黑色值为0，代表障碍物；之间的值是灰色，代表未知的)，若该栅格状态是空闲的(表明该栅格点已被探索)或者costmap中值超过某一阈值，则表明该栅格点所在区域已被探索，该栅格点也作为无效点剔除。

剔除未知区域内的边界点：受机器人里程计漂移以及在狭窄的走廊场景中感知效果很差，导致某个时刻建图算法在更新匹配地图时有较大的调整，这时边界点的位置通常也会发生变动，原本处于地图边界的点变成了位于未知区域的点，这类点将导致机器人难以规划到达边界点的所在区域，因此也被剔除。剔除方法为统计以边界点为中心的一个信息增益半径的圆内未知状态的栅格数目，若未知状态的栅格数目低于一定的阈值，则表示边界点所在区域的信息增益较少，应被剔除。若圆内均是未知状态的栅格，则表示该点不处于边界点区域，这种点也会影响机器人的探索任务，因此也应被剔除。

剔除太靠近或处于障碍物上的边界点：同上述所述，地图更新同样会出现原本处于地图边界的点变成了距离太靠近或处于障碍物上的点，这些点导致机器人难以规划到达所在区域，因此也要剔除。剔除方法为统计以边界点为中心的一个信息增益半径的圆内占用的状态的栅格数目，若占用的状态的栅格数目超过一定的阈值，则表示边界点过于靠近障碍物，应被剔除。

其中，在S5中，所述收益函数包括信息增益和路径成本，其公式如下：

R1_f＝w₁*I_f-w₂*N_f

其中，R1_f表示收益函数，I_f表示信息增益，N_f表示路径成本，w₁和w₂表示权重。

上述信息增益I_f指在边界点的半径为r的圆内未知的栅格的个数，未知栅格的数目越多，到达该点收获的信息越多；其中还包含一个折扣过程，机器人当前位置为圆心、半径为r的圆与以边界点为圆心、半径r的圆内重复的未知栅格会被剔除，此过程使机器人越靠近该边界点，信息增益I_f越小，可以有效避免机器人原地停留的情况。

上述路径成本N_f指机器人当前位置与边界点位置的欧式距离，此距离越大，机器人到该边界点消耗的成本越高。

基于上述所述，ROS1系统由于ROS Master的限制，当Master出现问题时，将影响整个系统的正常运行，因此基于ROS2系统更有利于自主探索算法开发应用的稳定性。

本发明在自主探索建图框架中采用cartographer建图算法，能够进行回环检测；累计误差低,能天然的输出协方差矩阵,后端优化的输入项；能够对未知环境进行三维建图。

通过在地图上点四个点来确定RRT采样区域的这种方式会导致地图在扩张后RRT无法继续探测边界点，探索任务终止。因此本发明结合移动机器人的应用场景都是家居、商超、医院这样的场景(正常平面楼层均小于100m²)，可以直接将RRT采样区域设置为以初始位置为中心的1000m²的正方形区域，这样可以保证RRT可以在实际应用中持续的探测边界点，进而使机器人完成整个环境的建模。

在局部探索树上，因为某些场景导致树无法及时生长到未知区域，因此本发明每隔1s，将机器人所在位置直接作为局部树的节点，这样能够保证局部树一直跟随着机器人生长，进而提高边界点的探索效率。

在过滤和剔除无效边界点时，有时因为机器人里程计的漂移或者如狭窄的走廊这样的环境中，机器人感知效果较差时前后构建的地图可能会发生较大的更新，这时边界点的位置也会发生变动，原本处于地图边界的点变成了位于未知区域的点或者太靠近障碍物的点，这类点将导致机器人难以规划到达边界点的所在区域，因此需要增加额外的条件进行剔除。

本发明借助ROS2系统开发了基于快速搜索随机树的自主探索方法及系统，提高了自主探索系统在场景下工作的稳定性。采用cartographer建图算法进行未知环境的建模，提高了构建的环境地图的质量。在边界点提取阶段，采用默认设置的采样区域保证RRT可以在工作场景中持续地提取边界点；之后将机器人当前位置定期添加到局部树的节点上，提高了边界点的提取效率；最后增加了边界点的过滤条件使地图前后更新差别较大时，机器人探索任务仍能有序进行，提高了探索系统的适应性和效率。

下面对本发明实施例公开的一种基于ROS2系统的移动机器人自主探索系统进行介绍，下文描述的一种基于ROS2系统的移动机器人自主探索系统与上文描述的一种基于ROS2系统的移动机器人自主探索方法可相互对应参照。

本发明实施例还提供了本发明还提供一种基于ROS2系统的移动机器人自主探索系统，包括：

数据获取模块，其用于获取机器人所处位置的环境信息，其中所述环境信息包括激光雷达信息、IMU信息和里程计信息；

地图构建模块，其用于将激光雷达信息、IMU信息和里程计信息输入至cartographer建图算法进行地图的构建，并更新一部分地图为已知的区域；

边界点提取模块，其用于在地图的空闲区域内生长两棵快速搜索随机树，使用两棵快速搜索随机树进行边界点的提取；

边界点过滤模块，其用于过滤和剔除无效边界点；

探索导航模块，其用于为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点，利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，以不断探索未知区域。

在本发明的一个实施例中，在边界点提取模块中，两棵快速搜索随机树分为全局树和局部树，全局树和局部树提取边界点的方法相同，其中当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长，且每隔设定的时间，将机器人当前位置作为节点添加到局部树上。

在本发明的一个实施例中，在边界点过滤模块中，剔除信息增益少的边界点、未知区域内的边界点以及过滤太靠近障碍物或处于障碍物上的边界点。

本实施例的基于ROS2系统的移动机器人自主探索系统用于实现前述的基于ROS2系统的移动机器人自主探索方法，因此该系统的具体实施方式可见前文中的基于ROS2系统的移动机器人自主探索方法的实施例部分，所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再展开介绍。

另外，由于本实施例的基于ROS2系统的移动机器人自主探索系统用于实现前述的基于ROS2系统的移动机器人自主探索方法，因此其作用与上述方法的作用相对应，这里不再赘述。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.一种基于ROS2系统的移动机器人自主探索方法，其特征在于：包括以下步骤：

S1：获取机器人所处位置的环境信息，其中所述环境信息包括激光雷达信息、IMU信息和里程计信息；

S2：将激光雷达信息、IMU信息和里程计信息输入至cartographer建图算法进行地图的构建，并更新一部分地图为已知的区域；

S3：在地图的空闲区域内生长两棵快速搜索随机树，使用两棵快速搜索随机树进行边界点的提取；

S4：过滤和剔除无效边界点；

S5：为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点；

S6：利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，以不断探索未知区域；

S7：重复步骤S2～S6，直至环境探索完成。

2.根据权利要求1所述的基于ROS2系统的移动机器人自主探索方法，其特征在于：在S1中，利用机器人自身携带的传感器获取其所处位置的环境信息。

3.根据权利要求1所述的基于ROS2系统的移动机器人自主探索方法，其特征在于：在S3中，两棵快速搜索随机树分为全局树和局部树，全局树和局部树提取边界点的方法相同，其中当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长，且每隔设定的时间，将机器人当前位置作为节点添加到局部树上。

4.根据权利要求1或3所述的基于ROS2系统的移动机器人自主探索方法，其特征在于：在S3中，使用两棵快速搜索随机树进行边界点的提取，包括：m²

S31：设置快速搜索随机树采样区域为以初始位置为中心的1000m²的正方形区域，将起点添加到树结构中作为根节点；

S32：在地图区域内随机撒点作为候选点，若该候选点在已知区域内，则遍历树结构上的所有已有的节点，选取距离候选点最近的节点作为最邻近点，将最邻近点到候选点的连线作为生长方向，若最邻近点与候选点的距离超过预设的步长，则由最邻近点沿着生长方向生长一个步长，到达的点作为生长点；若最邻近点与候选点的距离不超过步长，则该候选点作为生长点；若候选点在未知区域内，则先找到该候选点的最邻近点，最邻近点到候选点的连线作为生长方向，然后由最邻近点沿着生长方向向前生长，到达边界的地方作为边界点；

S33：将生长点和候选点的连线在地图上做碰撞检测，判断该条连线上所有栅格点的栅格状态，若栅格点的状态是被占据的，则返回步骤S32重新进行采样，反之则将该候选点与生长点的连线添加到树结构中。

5.根据权利要求1所述的基于ROS2系统的移动机器人自主探索方法，其特征在于：在S4中，在过滤和剔除无效边界点时，使用mean-shift聚类算法对边界点进行聚类处理，得到其质心点。

6.根据权利要求5所述的基于ROS2系统的移动机器人自主探索方法，其特征在于：在过滤和剔除无效边界点时，剔除信息增益少的边界点、未知区域内的边界点以及过滤太靠近障碍物或处于障碍物上的边界点。

7.根据权利要求1所述的基于ROS2系统的移动机器人自主探索方法，其特征在于：在S5中，所述收益函数包括信息增益和路径成本，其公式如下：

R1_f＝w₁*I_f-w₂*N_f

其中，R1_f表示收益函数，I_f表示信息增益，N_f表示路径成本，w₁和w₂表示权重。

8.一种基于ROS2系统的移动机器人自主探索系统，其特征在于：包括：

数据获取模块，其用于获取机器人所处位置的环境信息，其中所述环境信息包括激光雷达信息、IMU信息和里程计信息；

地图构建模块，其用于将激光雷达信息、IMU信息和里程计信息输入至cartographer建图算法进行地图的构建，并更新一部分地图为已知的区域；

边界点提取模块，其用于在地图的空闲区域内生长两棵快速搜索随机树，使用两棵快速搜索随机树进行边界点的提取；

边界点过滤模块，其用于过滤和剔除无效边界点；

探索导航模块，其用于为过滤后的边界点设计收益函数，计算每一个质心点的收益值，选择收益值最大的质心点作为目标点，利用ROS2系统下的导航框架Navigation2对机器人向目标点进行导航，以不断探索未知区域。

9.根据权利要求8所述的基于ROS2系统的移动机器人自主探索系统，其特征在于：在边界点提取模块中，两棵快速搜索随机树分为全局树和局部树，全局树和局部树提取边界点的方法相同，其中当局部树探测到边界点后，局部树会被清除并在机器人当前位置重新生长，且每隔设定的时间，将机器人当前位置作为节点添加到局部树上。

10.根据权利要求8所述的基于ROS2系统的移动机器人自主探索系统，其特征在于：在边界点过滤模块中，剔除信息增益少的边界点、未知区域内的边界点以及过滤太靠近障碍物或处于障碍物上的边界点。

Images