一种未知环境下的多移动机器人协同探索方法

摘要

本发明公开了一种未知环境下的多移动机器人协同探索方法，包括以下步骤：N个移动机器人分散在未知环境的不同区域内协同工作，相互之间进行数据共享，采用分割策略快速遍历未知环境地图，完成探索；各移动机器人确定探索目标点之后，以移动机器人所处的当前位置为起点，探索目标点为终点采用A*算法完成各移动机器人到探索目标点的全局最短路径规划；本发明采用分割策略将每个移动机器人分配到不同的区域进行探索，避免机器人聚集在同一区域，当某一区域完成探索后，其区域机器人会被分配到探索较少的区域，进一步划分区域探索，直到未知环境区域探索完成，提高了效率；另外，不同机器人之间进行数据共享，可以更有效的完成未知未知环境探索。

说明书

技术领域

本发明涉及机器人技术，具体地说，涉及一种未知环境下的多移动机器人协同探索方法。

背景技术

随着机器人技术的不断更新和应用领域需求的不断拓展，对机器人的工作能力、稳定性、鲁棒性以及效率都提出了更高的要求。单机器人是感知、处理、控制、执行的综合个体，但是面对一些复杂、协作、高效率需求的任务时却难以胜任。多机器人系统相比单个机器人系统具备许多优势特点，多机器人系统通过协作的方式提高了执行复杂任务的能力，能够提供更有效的解决方案；多机器人系统通常可以在更短的时间内完成任务，提升了系统性能；多机器人系统在面对一些单点故障时有较高的容错性和鲁棒性；多机器人系统在面对新的功能需求时具有更好的可扩展性。

自主机器人探索是机器人领域的重要研究课题。而对于机器人自主探索目的的差异，不同方向的探索策略也相继被提出。为了在未知环境中完成指定的任务，需要移动机器人具有自主搜寻目标物体的能力，因此，机器人不仅需要路径规划、避障、自身定位等功能，同时还要具有对全区域进行搜索遍历及在线检测识别目标物体的能力。

发明内容

发明目的：本发明的目的是提供一种未知环境下的多移动机器人协同探索方法，以解决上述背景技术中提出的问题。

技术方案：本发明公开了一种未知环境下的多移动机器人协同探索方法，包括以下步骤：

N个移动机器人分散在未知环境的不同区域内协同工作，相互之间进行数据共享，采用分割策略快速遍历未知环境地图，完成探索；各移动机器人确定探索目标点之后，以移动机器人所处的当前位置为起点，探索目标点为终点采用A*算法完成各移动机器人到探索目标点的全局最短路径规划；

其中，采用分割策略快速遍历未知环境地图，完成探索的方法为：每台移动机器人扫描周围环境，构建当前环境的局部栅格地图，并将局部栅格地图传递给控制器数据库，控制器同时将各移动机器人的局部栅格地图同步传输给其他移动机器人；同时，在移动机器人构建局部栅格地图过程中，控制器通过定位算法实现对各移动机器人的定位；控制器根据N个移动机器人构建的N个局部栅格地图进行映射环境，得到未知环境地图；控制器将未知环境区域划分为N个区域，每个移动机器人对应一个区域；各移动机器人采用RRT探索算法对其所在区域进行探索，并将探索结果实时发送至控制器，且各移动机器人之间进行数据共享；若某一区域探索完成，则控制器根据各区域探索情况，分配完成区域的移动机器人前往未探索的区域，并重新对未探索的区域按照所述区域划分方法进一步划分子区域，移动机器人对各自所在子区域继续采用RRT探索算法进行探索，直至所有区域探索完成，即实现了未知环境的探索，获得未知环境地图。

进一步的，采用Gmapping算法构建当前环境的局部栅格地图。

进一步的，控制器采用基于KL距离算法自适应调整粒子样本数量的自适应蒙特卡罗定位AMCL算法，来实现机器人的定位；

其中，AMCL算法在MCL算法的基础上进行改进，包括：AMCL算法根据长期估计权重w

其中，w

AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量表示为：

其中，ε表示真实分布与估计分布之间的误差，z

进一步的，RRT探索算法具体为：

(1)设定初始点xinit与目标点xgoal自行设定状态采样空间M；

(2)进行随机采样得到采样点xrand，如果采样点xrand在障碍物内，则重新随机采样；

(3)若不在障碍物内，计算该采样点xrand与已经生成的节点集合τ中的所有节点之间的距离，得到离得最近的节点xnear，再从节点xnear以步长StepSize走向节点xrand，生成一个新的节点xnew，若xnear与xnew的连线Ei经过障碍物，则重新随机采样；其中，0

(4)经过反复迭代，生成一个随机扩展树，当随机扩展树中的子节点进入规定的目标区域，便在随机扩展树中完成对未知区域的探索。

进一步的，采用A*算法完成移动机器人到目标点的全局最短路径规划的方法为：

A*算法结合了Dijkstra算法的快速性和宽度优先搜索BFS算法的优点；A*算法的代价函数如下式所示：

f(n)＝g(n)+h(n)

其中，f(n)表示从起始点A经过结点n(x,y)到目标点的总代价，g(n)表示从起始点A到该结点n(x,y)的移动代价，计算公式如下式：

其中，parent(n)为n的父结点，dist(.,n)为起始点A与n节点的距离；h(n)表示从起始点A到目标点的代价估计，h(n)的存在使得算法在进行搜索时具有方向性，采用欧几里德距离，如下式所示：

h(n)＝dist(n,B)

其中，B点为目标点，A*算法的实现需要维护两个集合，分别为集合OPEN和集合

CLOSE集合OPEN中存放所有已经生成而未访问过的点，集合CLOSE中存放已经访问过的点；在栅格地图中，A*算法采用八邻域法向周围进行搜索，找出可行的邻近区域中代价函数最小的节点，以此节点再向周围的可行区域进行搜索，直到目标节点在集合CLOSE中或者OPEN集合为空为止；机器人沿着生成的路径移动至目标位置。

进一步的，移动机器人为两轮的差速移动机器人，其上搭载了激光雷达和支持ROS操作系统的PC电脑，激光雷达用于扫描周围环境，PC电脑用于实现对应移动机器人与控制器之间的信息交互；控制器与各移动机器人的PC电脑之间，均基于ROS节点间的通讯方式，通过无线网络连接。

在本发明的另一实施例中，本发明的一种未知环境下的多移动机器人协同探索系统，包括：

探索模块，用于实现N个移动机器人分散在未知环境的不同区域内协同工作，相互之间进行数据共享，采用分割策略快速遍历未知环境地图，完成探索；包括：每台移动机器人扫描周围环境，构建当前环境的局部栅格地图，并将局部栅格地图传递给控制器数据库，同时将各自的局部栅格地图同步传输给其他移动机器人；同时，在移动机器人构建局部栅格地图过程中，控制器通过定位算法实现对各移动机器人的定位；控制器根据N个移动机器人构建的N个局部栅格地图进行映射环境，得到未知环境地图；各移动机器人的定位将未知环境区域划分为N个区域，未知区域的栅格按照栅格数均分为N份，每个移动机器人对应一个区域；各移动机器人采用RRT探索算法对其所在区域进行探索，并将探索结果实时发送至控制器，且各移动机器人之间进行数据共享；若某一区域探索完成，则控制器根据各区域探索情况，分配该区域的移动机器人前往探索较少的区域，并重新对该探索较少的区域按照所述区域划分方法进一步划分子区域，移动机器人对各自所在子区域继续采用RRT探索算法进行探索，直至所有区域探索完成，即实现了未知环境的探索，获得未知环境地图；

路径规划模块，采用A*算法完成移动机器人到目标点的全局最短路径规划。

优选的，多个移动机器人为两轮的差速移动机器人，其上搭载激光雷达和PC电脑，PC电脑支持ROS操作系统。

本发明的再一实施例中，一种装置设备，包括存储器和处理器，其中：

存储器，用于存储能够在处理器上运行的计算机程序；

处理器，用于在运行所述计算机程序时，执行如上述一种未知环境下的多移动机器人协同探索方法的步骤。

本发明的又一实施例中，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如上述一种未知环境下的多移动机器人协同探索方法的步骤。

有益效果：与现有技术相比，本发明的显著技术效果为：1、使用分割策略将多个机器人分布在不同的区域，避免机器人聚集在同一区域，可以有效地减少机器人探索所有区域的时间，提高了探索的效率；2、探索效率提高进一步优化了能源消耗，因为当时间减少时，它意味着机器人需要更少的时开机时间，这导致他们使用更少的能源；3、使用多移动机器人，且多移动机器人之间进行数据共享，提高了工作效率，节点式的分布方式，以多台较低性能的机器人，完成较复杂的任务，节省任务成本。

附图说明

图1是本发明方法流程图；

图2是本发明的移动机器人示意图；

图3是本发明的系统结构示意图；

图4是本发明的移动机器人定位流程图；

图5是本发明的分割策略算法流程图；

图6是本发明的移动机器人搜索RRT算法流程图。

具体实施方式

以下将参照附图，通过具体实施例对本发明进行详细地描述。在此需要说明的是，对于这些实施例方式的说明用于帮助理解本发明，但并不构成对本发明的限定。

本发明的一种未知环境下的多移动机器人协同探索方法，采用循环分割策略快速遍历未知环境地图，完成探索；具体为：每台移动机器人扫描周围环境，构建当前环境的局部栅格地图，并将局部栅格地图传递给控制器数据库，同时将各自的局部栅格地图同步传输给其他移动机器人；同时，在移动机器人构建局部栅格地图过程中，控制器通过定位算法实现对各移动机器人的定位；控制器根据N个移动机器人构建的N个局部栅格地图进行映射环境，得到未知环境地图；控制器将未知环境区域划分为N个区域，未知区域的栅格按照栅格数均分为N份，每个移动机器人对应一个区域；各移动机器人采用RRT探索算法对其所在区域进行探索，并将探索结果实时发送至控制器，且各移动机器人之间进行数据共享；若某一区域探索完成，则控制器根据各区域探索情况，分配完成区域的移动机器人前往未探索的区域，并重新对该未探索的区域按照所述区域划分方法进一步划分子区域，移动机器人对各自所在子区域继续采用RRT探索算法进行探索，直至所有区域探索完成，即实现了未知环境的探索，获得未知环境地图。并采用A*算法完成移动机器人到目标点的全局最短路径规划。如图1所示，具体包括以下步骤：

步骤1、构建地图；

如图2和图3所示，系统中包括若干辆移动机器人和控制器，移动机器人为两轮差速移动机器人，其上搭载了激光雷达，以及支持ROS操作系统的PC电脑，整个系统框架中控制器与各移动机器人的pc电脑之间，基于ROS节点间的通讯方式，通过无线网络连接。

分散在未知环境不同区域的N个移动机器人，在控制器端完成参数配置并运行Gmapping功能包，各移动机器人使用其各自搭载的激光雷达扫描周围环境，在自身pc电脑中构建当前环境的局部栅格地图，并将局部栅格地图传递给控制器，同时将各自的局部栅格地图同步传输给其他移动机器人。Gmapping是一个基于2D激光雷达使用RBPF算法完成二维栅格地图构建的SLAM算法。Gmapping功能包订阅机器人的深度信息、IMU信息和里程计信息，同时完成一些必要参数的配置，即可创建并输出基于概率的二维栅格地图，即局部栅格地图。

步骤2、机器人定位，在移动机器人移动过程中，控制器采用基于KL距离(KLD)算法自适应调整粒子样本数量的自适应蒙特卡罗定位(AMCL)算法，实现移动机器人相对世界坐标系的位姿，即实现移动机器人的定位；

局部栅格地图上的定位包括状态空间中机器人位置(x,y)和朝向角θ。MCL算法根据现有的数据估计机器人位姿的后验信息，可用的数据有两种类型，分别为测量数据和里程数据。采用里程计运动模型表示机器人运动p(x

η

其中，

AMCL算法是在MCL的基础上改进而来的，它根据长期估计权重w

其中，w

AMCl重采样阶段采用KLD算法自适应调整粒子数量，粒子数量可表示为：

其中，ε表示真实分布与估计分布之间的误差，z

机器人定位的步骤如图4所示：

2.1初始化粒子：将移动机器人的初始位置作为粒子集合的中心，随机生成一定数量的粒子，每个粒子的位置和方向都是随机的；

2.2运动模型：根据移动机器人的运动数据，更新粒子的位置和方向。运动模型可以是简单的模型，如直线运动或旋转运动，也可以是复杂的模型，如运动学模型或动力学模型。

2.3计算粒子权重：根据机器人传感器的测量数据，计算每个粒子的权重；

2.4KLD重采样：为了控制上述粒子数冗余而设计的。比如在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。对粒子集合进行重采样，重采样后的粒子集合更加接近。

2.5更新机器人位置：根据KLD重采样后的粒子集合，计算机器人在地图上的位置和方向。

步骤3、未知区域探索，机器人采用分割策略的搜索算法完成对未知区域的探索。

为高效的机器人探索区域，机器人在自身pc电脑中加载局部环境地图之后，将局部环境地图上传至控制器，控制器根据各移动机器人的局部地图，尝试映射环境，构建出全局环境地图，控制器对全局环境地图分割成N块，未知区域的栅格按照栅格数均分为N份，每块区域前往一个机器人进行探索，每个区域的位置机器人间彼此共享；当一区域机器人完成探索时，控制器重新将未探索区域分割，分配机器人前往最近的未知区域探索，前往的机器人使用边界探索算法完成对未知区域的探索；每个机器人之间的信息共享，方便机器人了解所探测的区域；当有机器人完成未知区域探索时，控制器再次进行未知区域分割，分派最近的移动机器人进行探索，一直循环完成对未知区域的探索。

如图5所示，系统首先尝试映射环境，这种映射基于最初步骤1中移动机器人生成的局部栅格地图；即控制器根据N个移动机器人构建的N个局部栅格地图进行映射环境，得到未知环境地图；而在探索过程中，任何一个移动机器人都接收控制器发送的其他移动机器人完成探测区域的信息。通过控制器和移动机器人之间的数据传输过程，可以帮助所有的移动机器人了解所探测的区域。

机器人使用RRT算法完成对未知区域的探索，RRT的思想是快速扩张一群像树一样的路径以探索空间的大部分区域，找到可行的路径。RRT算法是一种对状态空间随机采样的算法，通过对采样点进行碰撞检测，避免了对空间的精确建模带来的大计算量，能够有效地解决高维空间和复杂约束的路径规划问题。

RRT算法的流程如图6所示：

(1)设定初始点x

(2)进行随机采样得到采样点x

(3)若不在障碍物内，计算该采样点x

(4)经过反复迭代，生成一个随机扩展树，当随机扩展树中的子节点进入了我们规定的目标区域，便可以在随机扩展树中完成对未知区域的探索；

控制器循环进行未知地图分割和分配移动机器人进行未知区域探索。这个过程可以一直进行到未知区域的宽度小于机器人的感知范围为止。

步骤4、规划最优路径，地图探索构建完成后，各机器人得到目标点任务后，采用A*算法解决机器人在静态环境中全局最短路径问题，求解最优控制问题，使其符合机器人的运动学模型，机器人沿着生成的路径移动至目标位置。

机器人确定探索目标点之后，以机器人所处的当前位置为起点，探索目标点为终点规划机器人的运动路径，本发明采取A*算法对机器人的运动路径进行规划。A*算法在求解静态环境中的最短路径方面有良好的效果，它结合了Dijkstra算法的快速性和宽度优先搜索(Breadth First Search,BFS)算法的优点，在栅格结构的环境中的表现尤为突出。A*算法的代价函数如下式所示：

f(n)＝g(n)+h(n)

f(n)表示从起始点A经过结点n(x,y)到目标点的总代价，g(n)表示从起始点到该结点的移动代价，它的计算公式如下式：

其中parent(n)为n的父结点，dist(A,n)为A节点与n节点的距离。h(n)表示从起始点A到目标点的代价估计，h(n)的存在使得算法在进行搜索时具有方向性，采用欧几里德距离，如下式所示：

h(n)＝dist(n,B)

其中B点为目标点。A*算法的实现需要维护两个集合，分别为集合OPEN和集合

CLOSE集合OPEN中存放所有已经生成而未访问过的点，集合CLOSE中存放已经访问过的点。在栅格地图中，A*算法采用八邻域法向周围进行搜索，找出可行的邻近区域中代价函数最小的节点，以此节点再向周围的可行区域进行搜索，直到目标节点在集合CLOSE中或者OPEN集合为空为止。机器人沿着生成的路径移动至目标位置。

在本发明的另一实施例中，本发明的一种未知环境下的多移动机器人协同探索系统，包括：

探索模块，用于实现N个移动机器人分散在未知环境的不同区域内协同工作，相互之间进行数据共享，采用分割策略快速遍历未知环境地图，完成探索；包括：每台移动机器人扫描周围环境，构建当前环境的局部栅格地图，并将局部栅格地图传递给控制器数据库，同时将各自的局部栅格地图同步传输给其他移动机器人；同时，在移动机器人构建局部栅格地图过程中，控制器通过定位算法实现对各移动机器人的定位；控制器根据N个移动机器人构建的N个局部栅格地图进行映射环境，得到未知环境地图；各移动机器人的定位将未知环境区域划分为N个区域，未知区域的栅格按照栅格数均分为N份，每个移动机器人对应一个区域；各移动机器人采用RRT探索算法对其所在区域进行探索，并将探索结果实时发送至控制器，且各移动机器人之间进行数据共享；若某一区域探索完成，则控制器根据各区域探索情况，分配该区域的移动机器人前往探索较少的区域，并重新对该探索较少的区域按照所述区域划分方法进一步划分子区域，移动机器人对各自所在子区域继续采用RRT探索算法进行探索，直至所有区域探索完成，即实现了未知环境的探索，获得未知环境地图；

路径规划模块，采用A*算法完成移动机器人到目标点的全局最短路径规划。

优选的，多个移动机器人为两轮的差速移动机器人，其上搭载激光雷达和PC电脑，PC电脑支持ROS操作系统。

本发明的再一实施例中，一种装置设备，包括存储器和处理器，其中：

存储器，用于存储能够在处理器上运行的计算机程序；

处理器，用于在运行所述计算机程序时，执行如上述一种未知环境下的多移动机器人协同探索方法的步骤，并能达到上述方法一致的技术效果。

本发明的又一实施例中，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如上述一种未知环境下的多移动机器人协同探索方法的步骤，并能达到上述方法一致的技术效果。