CN110221614B - 一种基于快速探索随机树的多机器人地图探索方法 - Google Patents

Abstract

本发明涉及一种基于快速探索随机树的多机器人地图探索方法，包括以下步骤：步骤1：利用基于RRT的边界检测器，包括全局边界检测器和局部边界检测器，检测边界点；步骤2：对得到的边界点进行滤波，即剔除已被探测过的边界点并将一些密集的边界点进行聚类，再将滤波后的边界点存入边界点列表中；步骤3：使用基于市场经济的多机器人任务分配策略，根据各个边界点和机器人的位置，通过对成本和收益的计算，将特定的边界点分配给利润最高的机器人，从而建立机器人的局部地图；步骤4：通过网络通信，共享并融合各个机器人所得到的地图信息从而得到最终的地图。本发明能够克服现有技术的不足，引导多机器人自主探索未知环境并建立环境地图。

Description

一种基于快速探索随机树的多机器人地图探索方法

技术领域

本发明涉及工业移动机器人设计领域，特别是一种基于快速探索随机树的多机器人地图探索方法。

背景技术

机器人积极探索未知环境并构建环境地图，对于移动机器人实现自主导航具有重要意义。自主探索是将探索区域从环境中扩展出来，同时最大限度的提高地图精度。根据不同任务或可用硬件(如移动机器人平台)提出了不同的地图探索方法。移动机器人的探索运动可以通过使用预定路径计划，导航图或基于边界的探索来实现。地图构建的一个难点是在自动探索未知环境的过程中，必须要规划机器人的动作以收集更多环境信息并避免机器人与环境发生冲突。一些研究人员已经提出了一些方法来解决这个问题，但是这些方法中的大多数都只能被用于单个机器人地图探索。因此，机器人探索的效率很低，特别是在现实世界的环境中。本文重点介绍使用多个机器人自动探索环境。

在环境探索任务中，多机器人系统在单机器人系统中具有更好的鲁棒性，可靠性，探索速度和经济性。在环境探索期间，机器人一步一步地在环境中移动并通过多个传感器感知环境，感知到的传感器信息用于构建环境地图。因为目标点的选择决定了机器人的探索位置和探索顺序，所以为每个机器人选择和分配合适的目标点对于提高探索效率非常重要。此外，在探索过程中也应考虑避免碰撞。

对于多机器人系统，机器人要相互协作并自动探索环境并且避免与环境发生冲突。如何有效的控制多机器人系统中的每个机器人是另一个难点。在进行决策时，每个机器人既要考虑全局任务需求，又要考虑局部环境的约束。通常，多机器人系统的控制方法可以分为两种：集中式方法和分散式方法。集中式方法可以有效地消除多个机器人之间的冲突，但所有机器人都需要与中央控制器进行实时通信。当存在通信干扰或中央控制器故障时，集中式系统不稳定。在分散式方法中，由于每个机器人的决定完全基于来自其自身传感器的信息，因此只需要考虑自己的运动而不考虑其他机器人的运动。但是，机器人之间信息交换的延迟会对分散的网络产生严重影响。

发明内容

有鉴于此，本发明的目的是提出一种基于快速探索随机树的多机器人地图探索方法，能够克服现有技术的不足，引导多机器人自主探索未知环境并建立环境地图。

本发明采用以下方案实现：一种基于快速探索随机树的多机器人地图探索方法，具体包括以下步骤：

步骤S1：利用基于RRT的边界检测器检测边界点；

步骤S2：对步骤S1得到的边界点进行滤波，再将滤波后的边界点存入边界点列表中；

步骤S3：使用基于市场经济的多机器人任务分配策略，根据各个边界点和机器人的位置，考虑机器人探索每个边界点的收益和成本，选择利润最高的边界点并将其分配给机器人，从而建立机器人的局部地图；

步骤S4：通过网络通信，共享并融合各个机器人所得到的地图信息从而得到最终的地图。

进一步地，步骤S1具体为：使用占用栅格法来表示地图，根据每个栅格上的不同情况，将其分为空闲、占用和未知三种状态,如公式1所示：

利用基于RRT的边界点检测器，包括全局边界检测器和局部边界检测器，来检测边界点，检测器的本质是构建一个空间填充树随机搜索地图空间并实现边界检测，该树的扩展状态与Voronoi区域的大小成比例，由于最大的Voronoi区域代表搜索边界的状态，这意味着树优先延伸到大的未探测区域；当树同时落在空闲区域和未知区域时，连接未知区域的树顶点即为边界点；全局树从一个固定的点开始生长，不依赖于机器人的运动，而局部树则以机器人当前的位置为起点开始生长，当检测到新的边界点时局部树就会重置。

进一步地，如果直接将边界点发送到机器人任务分配器不仅会增加系统计算资源的负担，而且会因为边界点过于密集而导致机器人探索同一区域，因此步骤S2具体为：滤波器接收并处理由全局检测器和本地检测器检测到的边界点，对密集的边界点进行聚类，并删除已经探索过的边界点，由过滤器处理的边界点存储在不断更新的边界点列表中，并将该列表中的边界点发送到机器人任务分配器。

进一步地，步骤S3中，成本为机器人从当前位置前往边界点的轨迹长度T，采用了最短路径算法来规划机器人到边界点的轨迹；将收益分为两个部分，分别是边界点的信息收益I和预期轨迹的信息收益I_o；采用网格图分割地图，将收益值用未知区域的大小来表示，即未知区域所占网格的数量乘以单元栅格面积(可以根据地图分辨率计算)；边界点的信息收益是在以该边界点为圆心，半径为r的圆中，未知区域的面积大小，轨迹的收益信息是在机器人前往该边界点的预期路径上能够探索到的未知区域的面积，未知区域面积的计算半径也为r；其中，r设置为小于传感器感知半径的值。在理想的情况下，未知区域的计算半径r应该等于传感器的感知范围，但是该范围内可能存在的障碍物将会影响到传感器感知范围的实际大小。所以本发明将未知区域的计算半径r设置为一个小于传感器感知半径的值。

进一步地，步骤S3具体包括以下步骤：

步骤S31：将利润的计算转化为一个优化问题，约束条件如公式2所示：

式中，

代表了边界点的位置，

为边界点的信息收益，

是预期轨迹上的点，

为预期轨迹的信息收益，

为机器人前往该边界点的预期轨迹长度，x_r是当前机器人的位置；α和β是权重系数，在机器人探索环境时，认为边界点的信息收益比预期轨迹的信息收益更为重要，所以令α的值大于β的值；

步骤S32：通过将约束转化为代价函数，将上述问题转化为单目标函数优化问题，如公式3所示：

式中，λ是一个用户定义的常量参数；

步骤S33：采用基于市场经济的任务分配策略来协调机器人，将任务分配方案分为两种方式，分别对应机器人的空闲状态和忙碌状态；将边界点优先分配给空闲状态的机器人；对相关列表进行初始化之后，接收到滤波器发送的边界点，初始化列表如公式4所示：

式中，V是投标列表，R是机器人列表，C是边界点列表；将信息增益h_gain作为一个可调参数引入，当机器人与某个边界点的距离小于h_rad时，则将该边界点的收益值放大如公式5所示：

机器人将优先搜索附近的边界点；

步骤S34：当某一边界点被分配给指定机器人之后，放大在该边界点的一定范围内的其它边界点的收益值，以便让该机器人优先搜索该区域内的边界点，如公式6所示：

式中，

代表其它边界点的位置；同时，相应增加其它机器人前往该区域探索的成本，从而让所有机器人尽可能的往不同方向探索，避免重复探索，如公式7所示：

式中，x_rj是其它机器人的位置，h_cost是一个可调的成本放大系数，r_frontier是一个可调的范围阀值；当某一边界点被分配给机器人作为该机器人的目标点时，如果该边界点已经被其它机器人探索完成，并且预期轨迹的信息收益值小于h_info。则取消该目标点并且将该机器人置为空闲状态，等待下一个目标点的发布；其中，h_info表示一个信息收益的可调阀值。

进一步地，步骤S4中，要将所有的局部地图融合成一个全局地图，需要本地地图帧之间的帧转换；选择其中一个机器人的地图帧作为全局帧，并且假设其他机器人地图帧与全局帧的变换是刚性的(即，不会随着时间的变化而变化)；为了实现帧变换，对所有机器人的位姿进行初始化，而且机器人之间的相对位置和方向都是已知的。

较佳的，本发明采用multirobot_map_merge程序包来进行地图融合。

与现有技术相比，本发明有以下有益效果：本发明采用基于RRT的边界点检测器。其中，全局边界点检测器可以保证机器人完全发现和探索环境地图，局部边界点检测器可以快速探索到新的边界点，在边界点检测速度和探索环境的完整度上优于其他方法。本发明采用了基于市场经济的多机器人任务分配策略，根据各个边界点和机器人的位置，通过对成本(T)和收益(I+I_o)的计算，将特定的边界点分配给利润最高的机器人。该策略可以让各个机器人得到合适的目标点，避免重复探索，提高探索的效率。与集中式控制方法相比，采用的分散式控制方法降低多机器人通信的复杂度和信息量，提高了子系统的可靠性，降低了对管理计算机的要求。

附图说明

图1为本发明实施例的采用的全局边界检测器检测边界点的示意图。其中(a)表示全局树从固定点开始生长，落在空闲区域和未知区域上，(b)表示将连接未知区域和空闲区域的全局树的树顶点作为边界点。

图2为本发明实施例的采用的局部边界检测器检测边界点的示意图。其中(a)表示局部树从机器人当前位置开始生长，(b)表示将连接未知区域和空闲区域的局部树的树顶点作为边界点，(c)表示局部树在搜索到边界点之后开始重置，(d)表示机器人从当前机器人位置重新开始生长。

图3为本发明实施例的采用的机器人收益计算示意图。其中(a)表示在机器人预期路径上无未知区域时机器人的收益，(b)表示在机器人预期路径上存在未知区域时机器人的收益。

图4为本发明实施例的仿真环境和建立的地图示意图。其中(a)为机器人在gazebo中的仿真环境，(b)为探索仿真环境后生成的2D地图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图1所示，本实施例提供了一种基于快速探索随机树的多机器人地图探索方法，具体包括以下步骤：

步骤S1：利用基于RRT的边界检测器检测边界点；

步骤S2：对步骤S1得到的边界点进行滤波，再将滤波后的边界点存入边界点列表中；

步骤S3：使用基于市场经济的多机器人任务分配策略，根据各个边界点和机器人的位置，考虑机器人探索每个边界点的收益(I+I_o)和成本(T)，选择利润最高的边界点并将其分配给机器人，从而建立机器人的局部地图；

步骤S4：通过网络通信，共享并融合各个机器人所得到的地图信息从而得到最终的地图。

在本实施例中，步骤S1具体为：使用占用栅格法来表示地图，根据每个栅格上的不同情况，将其分为空闲(free)、占用(occupied)和未知(unknown)三种状态,如公式1所示：

利用基于RRT的边界点检测器，包括全局边界检测器和局部边界检测器，来检测边界点，检测器的本质是构建一个空间填充树随机搜索地图空间并实现边界检测，该树的扩展状态与Voronoi区域的大小成比例，由于最大的Voronoi区域代表搜索边界的状态，这意味着树优先延伸到大的未探测区域；当树同时落在空闲区域和未知区域时，连接未知区域的树顶点即为边界点；全局树从一个固定的点开始生长，不依赖于机器人的运动，如图1所示，而局部树则以机器人当前的位置为起点开始生长，当检测到新的边界点时局部树就会重置，如图2所示。

在本实施例中，如果直接将边界点发送到机器人任务分配器不仅会增加系统计算资源的负担，而且会因为边界点过于密集而导致机器人探索同一区域，因此步骤S2具体为：滤波器接收并处理由全局检测器和本地检测器检测到的边界点，对密集的边界点进行聚类，并删除已经探索过的边界点，由过滤器处理的边界点存储在不断更新的边界点列表中，并将该列表中的边界点发送到机器人任务分配器。

在本实施例中，步骤S3中，成本为机器人从当前位置前往边界点的轨迹长度T，采用了最短路径算法来规划机器人到边界点的轨迹；将收益分为两个部分，分别是边界点的信息收益I和预期轨迹的信息收益I_o；具体情况如图3所示。采用网格图分割地图，将收益值用未知区域的大小来表示，即未知区域所占网格的数量乘以单元栅格面积(可以根据地图分辨率计算)；边界点的信息收益是在以该边界点为圆心，半径为r的圆中，未知区域的面积大小，轨迹的收益信息是在机器人前往该边界点的预期路径上能够探索到的未知区域的面积，未知区域面积的计算半径也为r；其中，r设置为小于传感器感知半径的值。在理想的情况下，未知区域的计算半径r应该等于传感器的感知范围，但是该范围内可能存在的障碍物将会影响到传感器感知范围的实际大小。所以本发明将未知区域的计算半径r设置为一个小于传感器感知半径的值。

在本实施例中，步骤S3具体包括以下步骤：

步骤S31：将利润的计算转化为一个优化问题，约束条件如公式2所示：

式中，

代表了边界点的位置，

为边界点的信息收益，

是预期轨迹上的点，

为预期轨迹的信息收益，

为机器人前往该边界点的预期轨迹长度，x_r是当前机器人的位置；α和β是权重系数，在机器人探索环境时，认为边界点的信息收益比预期轨迹的信息收益更为重要，所以令α的值大于β的值；

步骤S32：通过将约束转化为代价函数，将上述问题转化为单目标函数优化问题，如公式3所示：

式中，λ是一个用户定义的常量参数；

步骤S33：采用基于市场经济的任务分配策略来协调机器人，将任务分配方案分为两种方式，分别对应机器人的空闲状态和忙碌状态；将边界点优先分配给空闲状态的机器人；对相关列表进行初始化之后，接收到滤波器发送的边界点，初始化列表如公式4所示：

式中，V是投标列表，R是机器人列表，C是边界点列表；将信息增益h_gain作为一个可调参数引入，当机器人与某个边界点的距离小于h_rad时，则将该边界点的收益值放大如公式5所示：

机器人将优先搜索附近的边界点；

步骤S34：当某一边界点被分配给指定机器人之后，放大在该边界点的一定范围内的其它边界点的收益值，以便让该机器人优先搜索该区域内的边界点，如公式6所示：

式中，

代表其它边界点的位置；同时，相应增加其它机器人前往该区域探索的成本，从而让所有机器人尽可能的往不同方向探索，避免重复探索，如公式7所示：

式中，x_rj是其它机器人的位置，h_cost是一个可调的成本放大系数，r_frontier是一个可调的范围阀值；当某一边界点被分配给机器人作为该机器人的目标点时，如果该边界点已经被其它机器人探索完成，并且预期轨迹的信息收益值小于h_info。则取消该目标点并且将该机器人置为空闲状态，等待下一个目标点的发布；其中，h_info表示一个信息收益的可调阀值。

在本实施例中，步骤S4中，要将所有的局部地图融合成一个全局地图，需要本地地图帧之间的帧转换；选择其中一个机器人的地图帧作为全局帧，并且假设其他机器人地图帧与全局帧的变换是刚性的(即，不会随着时间的变化而变化)；为了实现帧变换，对所有机器人的位姿进行初始化，而且机器人之间的相对位置和方向都是已知的。

较佳的，本实施例采用multirobot_map_merge程序包来进行地图融合。

具体的，本实施例的基于快速探索随机树的多机器人地图探索方法应用于工业移动机器人的地图探索领域，其目的在于克服现有技术的不足，引导多机器人自主探索未知环境并建立环境地图。具体设置如下：

机器人的速度和角速度分别被设置为0.6m/s和

我们以机器人1的位置为原点建立世界坐标系，机器人2的位置坐标为(3,0)，机器人3的位置坐标为(0,3)，仿真环境的面积大小约为270m²，具体如图4所示。全局边界点检测器的生长因子设置为5m，局部边界点检测器的生长因子设置为3m。其它相关参数如下：λ＝3，α＝1.2，β＝0.8，h_gain＝3，h_rad＝5cm，h_cost＝3，r_frontier＝5cm。

如图4所示，给出了机器人探索环境建立的地图。从图中可以看出，各个机器人协作完成了地图探索，建立了完整的环境地图。

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

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

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

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

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (4)

1.一种基于快速探索随机树的多机器人地图探索方法，其特征在于，包括以下步骤：

步骤S1：利用基于RRT的边界检测器检测边界点；

步骤S2：对步骤S1得到的边界点进行滤波，再将滤波后的边界点存入边界点列表中；

步骤S3：使用基于市场经济的多机器人任务分配策略，根据各个边界点和机器人的位置，考虑机器人探索每个边界点的收益和成本，选择利润最高的边界点并将其分配给机器人，从而建立机器人的局部地图；

步骤S4：通过网络通信，共享并融合各个机器人所得到的地图信息从而得到最终的地图；

其中，步骤S3中，成本为机器人从当前位置前往边界点的轨迹长度T，采用了最短路径算法来规划机器人到边界点的轨迹；将收益分为两个部分，分别是边界点的信息收益I和预期轨迹的信息收益I_o；采用网格图分割地图，将收益值用未知区域的大小来表示，即未知区域所占网格的数量乘以单元栅格面积；边界点的信息收益是在以该边界点为圆心，半径为r的圆中，未知区域的面积大小，轨迹的收益信息是在机器人前往该边界点的预期路径上能够探索到的未知区域的面积，未知区域面积的计算半径也为r；其中，r设置为小于传感器感知半径的值；

其中，步骤S3具体包括以下步骤：

步骤S31：将利润的计算转化为一个优化问题，约束条件如公式2所示：

式中，

代表了边界点的位置，

为边界点的信息收益，

是预期轨迹上的点，

为预期轨迹的信息收益，

为机器人前往该边界点的预期轨迹长度，x_r是当前机器人的位置；α和β是权重系数，在机器人探索环境时，认为边界点的信息收益比预期轨迹的信息收益更为重要，所以令α的值大于β的值；

步骤S32：通过将约束转化为代价函数，将上述问题转化为单目标函数优化问题，如公式3所示：

式中，λ是一个用户定义的常量参数；

步骤S33：采用基于市场经济的任务分配策略来协调机器人，将任务分配方案分为两种方式，分别对应机器人的空闲状态和忙碌状态；将边界点优先分配给空闲状态的机器人；对相关列表进行初始化之后，接收到滤波器发送的边界点，初始化列表如公式4所示：

式中，V是投标列表，R是机器人列表，C是边界点列表；将信息增益h_gain作为一个可调参数引入，当机器人与某个边界点的距离小于h_rad时，则将该边界点的收益值放大如公式5所示：

机器人将优先搜索附近的边界点；

步骤S34：当某一边界点被分配给指定机器人之后，放大在该边界点的一定范围内的其它边界点的收益值，以便让该机器人优先搜索该区域内的边界点，如公式6所示：

式中，

代表其它边界点的位置；同时，相应增加其它机器人前往该区域探索的成本，从而让所有机器人尽可能的往不同方向探索，避免重复探索，如公式7所示：

式中，x_rj是其它机器人的位置，

表示机器人从位置x_rj前往边界点位置x_fpj预期的轨迹长度，h_cost是一个可调的成本放大系数，r_frontier是一个可调的范围阀值；当某一边界点被分配给机器人作为该机器人的目标点时，如果该边界点已经被其它机器人探索完成，并且预期轨迹的信息收益值小于h_info；则取消该目标点并且将该机器人置为空闲状态，等待下一个目标点的发布；其中，h_info表示一个信息收益的可调阀值。

2.根据权利要求1所述的一种基于快速探索随机树的多机器人地图探索方法，其特征在于，步骤S1具体为：使用占用栅格法来表示地图，根据每个栅格上的不同情况，将其分为空闲、占用和未知三种状态,如公式1所示：

利用基于RRT的边界点检测器，包括全局边界检测器和局部边界检测器，来检测边界点，检测器的本质是构建一个空间填充树随机搜索地图空间并实现边界检测，该树的扩展状态与Voronoi区域的大小成比例，由于最大的Voronoi区域代表搜索边界的状态，这意味着树优先延伸到大的未探测区域；当树同时落在空闲区域和未知区域时，连接未知区域的树顶点即为边界点；全局树从一个固定的点开始生长，不依赖于机器人的运动，而局部树则以机器人当前的位置为起点开始生长，当检测到新的边界点时局部树就会重置。

3.根据权利要求1所述的一种基于快速探索随机树的多机器人地图探索方法，其特征在于，步骤S2具体为：滤波器接收并处理由全局检测器和本地检测器检测到的边界点，对密集的边界点进行聚类，并删除已经探索过的边界点，由过滤器处理的边界点存储在不断更新的边界点列表中，并将该列表中的边界点发送到机器人任务分配器。

4.根据权利要求1所述的一种基于快速探索随机树的多机器人地图探索方法，其特征在于，步骤S4中，要将所有的局部地图融合成一个全局地图，需要本地地图帧之间的帧转换；选择其中一个机器人的地图帧作为全局帧，并且假设其他机器人地图帧与全局帧的变换是刚性的；为了实现帧变换，对所有机器人的位姿进行初始化，而且机器人之间的相对位置和方向都是已知的。

Images