基于多机器人协作的自主探索建图系统及探索建图方法

摘要

本发明涉及一种基于多机器人协作的自主探索建图系统及探索建图方法。本发明可以分为顶层服务器模块和底层机器人模块，其中顶层服务器模块由地图融合模块、未知边界点检测模块、边界点聚类滤波模块、探索目标点获取模块以及机器人任务分配模块五部分组成；底层机器人模块分为路径规划模块和局部地图构建模块。本方法实现了对未知环境的多机器人协作探索任务，能够自主完成栅格地图构建。本发明具有地图构建准确，探索效率高等优点，可以在很大程度上解放地图构建过程中的人力资源，具有较强的市场潜力和应用价值。

说明书

技术领域

本发明属于机器人协作领域，具体涉及一种基于多机器人协作的自主探索建图系统及探索建图方法。

背景技术

随着人工智能时代的到来，机器人技术、信息技术、通信技术和人工智能得到进一步融合。传统工业技术方面，控制器、伺服电机、以及减速器等逐步向人工智能领域的计算机视觉、自然语言处理和机器学习过渡；应用领域方面，机器人与人类社会联系更加密切，从工业和商业用户，逐步向家庭和个人用户渗透；人机交互方面，从单一开关操作到语义和手势理解，更加注重人机关系与协作。现实环境存在大量未知性的场景，大多数情况下移动机器人对于工作环境是不具备先验知识的，移动机器人执行复杂任务的前提是对周围环境进行准确的地图构建，并将任务转化到已知环境中处理。因此未知环境探索和区域地图构建是移动机器人研究领域的重要问题。传统探索建图任务大多是由单机器人执行的，多机器人协作探索建图与之相比，探索效率更高。同时机器人之间的协作通信使得整个系统具有更高的可靠性和稳定性。

发明内容

鉴于上述背景技术的不足，本发明的目的在于提供一种基于多机器人协作的自主探索建图系统，该系统通过SLAM地图构建、未知边界点检测、任务分配和栅格地图融合完成了对未知环境的探索和地图构建任务。很好地集成了激光建图、机器人导航和路径规划以及自动控制等技术。有效提升了未知场景地图构建的速度和准确性，同时减少了传统建图过程中所需要的人力投入，具有很好的工程意义。

基于多机器人协作的自主探索建图系统，该系统分为两部分：顶层服务器模块和底层机器人模块；顶层服务器模块包括五个部分：地图融合模块、未知边界点检测模块、边界点聚类滤波模块、探索目标点获取模块以及机器人任务分配模块；地图融合模块使用基于局部特征提取的方法进行栅格地图融合，将机器人构建的多张局部地图融合成全局地图；未知边界点检测模块根据接收到的全局地图使用快速扩展随机树算法进行边界点检索，同时改进边界点检索方式，若系统内机器人在设定时间内没有接收到目标点，则以系统内被阻塞的机器人所在的栅格为中心，遍历其周围的栅格，直到遍历到未知栅格为止，将该未知栅格在全局地图对应的坐标点作为新的未知边界点；边界点聚类滤波模块采用均值漂移聚类算法对未知边界点进行聚类滤波，只保留聚类中心点并将其发送给目标点获取模块，以过滤掉不符合要求的边界点；探索目标点获取模块通过深度优先搜索算法对边界点进一步处理，找到基于该边界点的一段连续的边界，最后求出边界的质心，作为移动机器人运动的目标点；机器人任务分配模块采用模拟市场竞标的策略对上文获取的目标点进行分配，将机器人看作竞拍者，探索目标点看作商品，通过计算机器人与目标点之间收益来决定目标点的分配结果；

底层机器人模块包括两个部分：路径规划模块和局部地图构建模块；路径规划模块接收到顶层服务器分配的目标点后使用基于ROS的move_base库控制移动小车导航；局部地图构建模块根据车载激光雷达上传的数据使用SLAM算法进行地图构建，并发送到顶层服务器的地图融合模块中去。

基于多机器人协作的自主探索建图系统的探索建图方法，该方法具体包括以下步骤：

步骤一：将整个系统部署在待建图区域中，顶层服务器与各个底层机器人连接到同一个网络下，完成多机通信和数据传输；

步骤二：未知边界点检测模块接收到初始全局地图，从起始点出发采用随机采样增加子节点的形式生成随机扩展树；若构成一段树枝的两个端点都在已知区域内，则说明随机树还需要继续向下生长寻找新的节点；若构成一段树枝的两个端点分别落在已知区域和未知区域时，说明树枝穿过了探索地图的边界，然后按照预设步长寻找树枝上的边界点；同时在边界点探索后期，如果整个系统内所有机器人都在预设时间内没有接收到目标点，则以系统内机器人所在的栅格为中心，遍历其周围的栅格，直到遍历到未知栅格为止，将该未知栅格在全局地图对应的坐标点作为新的未知边界点；

步骤三：为了过滤掉不符合要求的边界点，同时减轻后续模块的计算压力，边界点滤波模块接收到边界点之后，使用均值漂移聚类算法进行聚类滤波，删除已经被探索到的处于已知区域内的边界点，只保留聚类中心点发送给目标点获取模块；对于给定待建图区域内的n个未知边界点集X，空间中任意点x的均值漂移向量基本形式可以表示为式(1)，其中i＝1,...,n：

其中M

S

而漂移的过程，就是通过计算得漂移向量，然后更新球心y位置，使得球心一直处于力的平衡位置，具体实验时不断迭代上述过程，直到找到区域内密度最大的区域为一次聚类结束，此时的圆心便是一个聚类的中心点；

步骤四：探索目标点获取模块在接收到聚类滤波得到的边界点之后，根据深度优先搜索算法对边界点周围的栅格进一步遍历，搜索出一条连续的边界，再求出连续边界栅格的质心，最后找到距离质心最近的未知栅格，将其作为探索目标点，机器人对该点进行探索就可以一次性完成对整个边界的探索；

步骤五：任务分配模块采用模拟市场竞标的策略对目标点进行分配；将机器人看作市场内的竞拍者，探索目标点看作商品，通过计算机器人与目标点之间收益来决定目标点的分配结果；机器人竞拍总收益受两个参数影响，代价函数Cost和信息增益函数IG；第m个机器人到第n个目标点的总收益公式Revenue如下所示，其中λ为预设的为了保持信息增益与代价函数量级对等的权重因数；

Revenue (m,n)＝(λ×IG(n))-Cost(m,n) (3)

此场景中的代价函数为机器人到目标点的距离代价，用欧式距离表示；而机器人到达目标点所能探索的未知区域面积即为该点的信息增益，如式(4)所示，site(m)和site(n)分别为空间中两点的位置坐标，S(i,j)为坐标点(i,j)所在的栅格的面积，GridOccupy(i,j)表示坐标为(i,j)的未知区域uknown的栅格占用概率；InfoRadius为信息增益半径，信息增益函数一般约束在以目标点为圆心，激光雷达覆盖距离为半径的圆内；

同时在以上分配算法基础上引入打折函数，对于已经分配好的目标点，其他机器人计算该点附近目标点的信息增益时将两目标点重叠部分的面积减去；引入打折函数后的目标点最终信息增益FinalGain计算公式如下：

步骤六：系统中的机器人将各自构建的局部地图发送给地图融合模块进行两两地图融合；两个待融合局部地图记为A和B，通过刚体变换使得两个待融合局部地图准确拼接，地图融合模块通过构建如式(6)的优化问题求解局部地图之间的旋转矩阵R和平移向量t完成拼接融合：

式(6)中α为权重因数,|·|表示集合中的元素个数，ξ

为确保获得的拼接参数最优，采用迭代最近点算法来计算刚体变换，最终完成两个局部地图的融合；然后将融合的地图与下一个局部地图进行下一次融合，不断执行步骤六，直到系统内所有满足融合重叠条件的局部地图完成融合。

本发明的有益效果：本发明方法采用SLAM地图构建、未知边界点检测、任务分配和栅格地图融合等方式实现了对未知环境的探索和地图构建功能，集成了多个传感器的信息融合、完成了分布式的设计，具有执行能力强、功能模块分工明确。本发明可快速完成未知环境的自主地图构建任务，且具有地图构建准确，探索效率高的优点，具有可观的理论价值和实践价值。

具体实施方式

基于多机器人协作的自主探索建图系统，该系统分为两部分：顶层服务器模块和底层机器人模块；顶层服务器模块包括五个部分：地图融合模块、未知边界点检测模块、边界点聚类滤波模块、探索目标点获取模块以及机器人任务分配模块；地图融合模块使用基于局部特征提取的方法进行栅格地图融合，将机器人构建的多张局部地图融合成全局地图；未知边界点检测模块根据接收到的全局地图使用快速扩展随机树算法进行边界点检索，同时改进边界点检索方式，若系统内机器人在设定时间内没有接收到目标点，则以系统内被阻塞的机器人所在的栅格为中心，遍历其周围的栅格，直到遍历到未知栅格为止，将该未知栅格在全局地图对应的坐标点作为新的未知边界点；边界点聚类滤波模块采用均值漂移聚类算法对未知边界点进行聚类滤波，只保留聚类中心点并将其发送给目标点获取模块，以过滤掉不符合要求的边界点；探索目标点获取模块通过深度优先搜索算法对边界点进一步处理，找到基于该边界点的一段连续的边界，最后求出边界的质心，作为移动机器人运动的目标点；机器人任务分配模块采用模拟市场竞标的策略对上文获取的目标点进行分配，将机器人看作竞拍者，探索目标点看作商品，通过计算机器人与目标点之间收益来决定目标点的分配结果；

底层机器人模块包括两个部分：路径规划模块和局部地图构建模块；路径规划模块接收到顶层服务器分配的目标点后使用基于ROS的move_base库控制移动小车导航；局部地图构建模块根据车载激光雷达上传的数据使用SLAM算法进行地图构建，并发送到顶层服务器的地图融合模块中去。

基于多机器人协作的自主探索建图系统的探索建图方法，该方法具体包括以下步骤：

步骤一：将整个系统部署在待建图区域中，顶层服务器与各个底层机器人连接到同一个网络下，完成多机通信和数据传输；

步骤二：未知边界点检测模块接收到初始全局地图，从起始点出发采用随机采样增加子节点的形式生成随机扩展树；若构成一段树枝的两个端点都在已知区域内，则说明随机树还需要继续向下生长寻找新的节点；若构成一段树枝的两个端点分别落在已知区域和未知区域时，说明树枝穿过了探索地图的边界，然后按照预设步长寻找树枝上的边界点；同时在边界点探索后期，如果整个系统内所有机器人都在预设时间内没有接收到目标点，则以系统内机器人所在的栅格为中心，遍历其周围的栅格，直到遍历到未知栅格为止，将该未知栅格在全局地图对应的坐标点作为新的未知边界点；

步骤三：为了过滤掉不符合要求的边界点，同时减轻后续模块的计算压力，边界点滤波模块接收到边界点之后，使用均值漂移聚类算法进行聚类滤波，删除已经被探索到的处于已知区域内的边界点，只保留聚类中心点发送给目标点获取模块；对于给定待建图区域内的n个未知边界点集X，空间中任意点x的均值漂移向量基本形式可以表示为式(1)，其中i＝1,...,n：

其中M

S

而漂移的过程，就是通过计算得漂移向量，然后更新球心y位置，使得球心一直处于力的平衡位置，具体实验时不断迭代上述过程，直到找到区域内密度最大的区域为一次聚类结束，此时的圆心便是一个聚类的中心点；

步骤四：探索目标点获取模块在接收到聚类滤波得到的边界点之后，根据深度优先搜索算法对边界点周围的栅格进一步遍历，搜索出一条连续的边界，再求出连续边界栅格的质心，最后找到距离质心最近的未知栅格，将其作为探索目标点，机器人对该点进行探索就可以一次性完成对整个边界的探索；

步骤五：任务分配模块采用模拟市场竞标的策略对目标点进行分配；将机器人看作市场内的竞拍者，探索目标点看作商品，通过计算机器人与目标点之间收益来决定目标点的分配结果；机器人竞拍总收益受两个参数影响，代价函数Cost和信息增益函数IG；第m个机器人到第n个目标点的总收益公式Revenue如下所示，其中λ为预设的为了保持信息增益与代价函数量级对等的权重因数；

Revenue (m,n)＝(λ×IG(n))-Cost(m,n) (8)

此场景中的代价函数为机器人到目标点的距离代价，用欧式距离表示；而机器人到达目标点所能探索的未知区域面积即为该点的信息增益，如式(4)所示，site(m)和site(n)分别为空间中两点的位置坐标，S(i,j)为坐标点(i,j)所在的栅格的面积，GridOccupy(i,j)表示坐标为(i,j)的未知区域uknown的栅格占用概率；InfoRadius为信息增益半径，信息增益函数一般约束在以目标点为圆心，激光雷达覆盖距离为半径的圆内；

同时在以上分配算法基础上引入打折函数，对于已经分配好的目标点，其他机器人计算该点附近目标点的信息增益时将两目标点重叠部分的面积减去；引入打折函数后的目标点最终信息增益FinalGain计算公式如下：

步骤六：系统中的机器人将各自构建的局部地图发送给地图融合模块进行两两地图融合；两个待融合局部地图记为A和B，通过刚体变换使得两个待融合局部地图准确拼接，地图融合模块通过构建如式(6)的优化问题求解局部地图之间的旋转矩阵R和平移向量t完成拼接融合：

式(6)中α为权重因数,|·|表示集合中的元素个数，ξ

为确保获得的拼接参数最优，采用迭代最近点算法来计算刚体变换，最终完成两个局部地图的融合；然后将融合的地图与下一个局部地图进行下一次融合，不断执行步骤六，直到系统内所有满足融合重叠条件的局部地图完成融合。