专利名称：基于分层拓扑结构的多机器人系统地图拼接方法
技术领域：
本发明属于智能移动机器人领域，涉及一种基于分层拓扑结构的未知环境下多机器人系统地图拼接方法。
背景技术：
近年来，移动机器人的应用越来越普遍，各种具有较高智能的移动机器人系统在不同场合正在发挥重要的作用。但随着人类社会活动领域的不断延伸以及机器人学研究的不断发展，单个移动机器人系统在代替人完成包括大范围灾难救援、科学考察等任务以及战场环境监察等军事使命时面临着诸多的困难，如:可靠性差、作业范围小、任务完成效率低等。而相关研究表明，由多个移动机器人组成的多机器人系统通过协调、合作恰恰可以解决这些问题。因此，多机器人系统被认为具有广阔的应用前景。地图拼接作为多机器人系统导航领域中的一项关键性技术，已引起了众多学者的关注。在机器人初始位姿是已知的假设下，多机器人之间的地图拼接问题可转化为单机器人同步定位与地图创建(Simultaneous Localization and Mapping, SLAM)问题的直接扩展，然而针对大规模的未知环境，该假设存在着较大的局限性；在机器人必须相遇条件下，可借助于内部传感器信息计算地图变换矩阵进而实现地图拼接，该方法需对传感器信息不确定性进行分析与优化，且拼接地图效果受限于传感器检测精度。考虑到算法的通用性和鲁棒性，解决机器人间相对位姿未知情况下的地图拼接问题有着重要的意义。目前有学者利用相关扫描匹配策略搜索地图公共区域来实现地图拼接，取得了一些成果，解决方案是将局部地图与局部地图的数据关联问题转化为点与局部地图的匹配问题，大大节省了搜索空间。Yan Yu等提出一种基于线段几何特征的扫描匹配方法，在结构化环境中取 得较好拼接结果。Zhou和Roumeliotis基于最邻近搜索算法建立目标函数，提出一种路标匹配方法实现特征地图融合。Lee H C和Lee B H.于2011年在《Advanced Robotics》杂志上发表的论文 “ Improved feature map merging usingVirtual Supporting Lines for mult1-Robot systems”,通过提取环境中的VSLs(VirtualSupporting Lines)非迭代特征进行匹配来实现拼接,保证了地图精度的同时降低算法复杂度。然而上述方法在缺少空间约束的环境中并不适用，且在大规模未知场景下，相关特征的错误关联会引入拼接误差。随着vSLAM(Visual SLAM)算法的日益成熟，一些学者尝试将视觉特征融入系统以解决地图拼接准确性问题，但视觉特征提取实时性问题又是一个难题。
发明内容
为了克服上述地图拼接方法中存在的上述问题，本发明基于分层拓扑结构提供了一种视觉特征关联与局部扫描匹配相结合的多机器人系统地图拼接方法。本发明采用的技术方案如下:在机器人间相对位姿未知条件下，针对大规模的未知环境建立一种分层拓扑结构(Hierarchical Topology Map, HTM),利用基于视觉特征的拓扑地图节点匹配方法,结合局部扫描匹配策略，提出一种快速鲁棒的多机器人系统地图拼接方法。该方法主要包括以下内容:第一，提出融合可通行空间树(Admissible Space Tree, AST)和尺度不变特征(Scale-1nvariant Feature Transform, SIFT)的分层拓扑结构，并利用一种改进的同时路径规划与地图创建(Simultaneous Path Planning and Topological Mapping, SP2ATM)方法建立未知环境的拓扑结构并增量式地创建地图；第二，采取节点匹配策略关联场景信息，利用SIFT匹配度作为地图拼接的先验信息，并在递增生成的拓扑节点处设定关键帧(KeyFrame)，以降低匹配过程的计算复杂度；第三，在机器人间相对位姿未知情况下，结合迭代最近点(Iterative Closest Point, ICP)扫描匹配算法求取局部地图间的变换矩阵，当由ICP算法计算得到的地图间相似度大于给定阈值时，完成多机器人系统的地图拼接。基于分层拓扑结构的多机器人系统地图拼接方法，包括以下步骤:步骤一，获取可通行空间树。为建立环境的拓扑结构并增量式地创建地图，本发明首先根据传感器信息建立环境中的可通行空间树结构，并实时获取 机器人探索过程中的可能目标点，进而完成路径规划与地图创建任务。可通行空间建立方法如下:利用获取的激光数据信息，将距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧线，称之为扇区。为确定在扇区中是否存在可能目标点，本方法假设存在一个矩形通道，将大于机器人宽度的给定阈值w作为矩形的宽，并使该矩形以机器人当前位置为中心进行旋转，在每个扇区中寻找可通行角度区间。在已建立可通行空间树结构中，为确定机器人的前进方向和目标点，选取最外层扇区中的弧线中间点，将其作为可能目标点。这样，机器人通过目标点的实时更新完成环境探索任务。步骤二，建立分层拓扑结构。在可通行空间树的基础上建立一种融合混合特征的分层拓扑地图结构。在分层拓扑结构中维护了两层地图:第一层地图是当前节点的可通行空间树的集合A ;第二层是已生成拓扑节点集合T，称为拓扑层。同时，为获取更丰富的环境信息，本发明在拓扑节点处提取比例不变特征(Scale-1nvariant Feature Transform, SIFT),并存储于分层拓扑地图结构当中。机器人探索过程中，若已生成的可能目标点与已生成拓扑节点或者可能点间距离过于接近时，需利用节点更新规则对分层拓扑结构中的节点不断进行添加、绑定与删除等动态维护操作。步骤三，创建全局拓扑地图。本发明基于上述的可能目标点提取方法，不断被提取新的目标点，通过拓扑节点的实时更新来完成全局拓扑地图的创建。探索过程中，每个机器人建立自身的拓扑树型结构。机器人进入未探索环境中，利用更新规则判断当前的可通行区域，若机器人当前不存在任何可能目标点时，为完成对环境的遍历，机器人必须回溯到距离机器人最近且存在未探索可能目标点的拓扑节点处，并将此节点设置为回溯目标。同时，为避免多机器人探索同一局部区域，每个机器人根据自身的分层拓扑地图结构和节点更新规则动态维护结构中所有节点，防止机器人间的重复访问，以充分提高地图创建效率。步骤四，提取SIFT特征并进行特征匹配。SIFT特征对图像的比例缩放、旋转有着较好的不变性，并且对于三维视角、噪声、光照的变化也保持着较好的可匹配性。因此，本发明通过提取环境中的SIFT特征角点来描述未知环境，并将节点间的SIFT匹配度作为地图拼接的先验信息。本发明在拓扑节点提取环境中的SIFT特征点，包括特征点检测和特征点描述两个部分。采用基于欧氏距离的最邻近法进行特征点匹配，利用K-D树的数据结构进行搜索，根据最近两个特征点的距离比确定是否接受这一匹配对。步骤五，基于ICP扫描匹配进行地图拼接。本发明基于上述分层拓扑结构完成机器人地图创建，并对不同机器人在拓扑节点处提取的SIFT特征进行匹配，若匹配度满足阈值要求，则对当前不同机器人所生成的地图使用ICP扫描匹配的方法来实现地图拼接，无需考虑机器人相对位姿信息。拼接过程中，考虑ICP方法的匹配误差，提高地图拼接的准确度。本发明的有益效果是:在机器人间相对位姿未知情况下，提出一种融合SIFT特征的分层拓扑结构，并增量式地创建全局拓扑地图，根据节点间的SIFT匹配信息，结合扫描匹配的方法实现大规模未知环境下的多机器人系统地图拼接。有效地解决了地图创建效率问题，提高了拼接地图的准确度。
图1为本发明所涉及的方法流程图；图2为可通行空间树示意图；图3为分层拓扑结构示意图；图4为路径探索方法流程图；图5为应用本发明地图拼接场景的实际环境示意图:(a)为实验真实场景，(b)为实验环境的二维几何示意图；图6为应用本发明实现地图拼接的结果示意图。
具体实施例方式下面结合附图和具体实施方式
对本发明作进一步的详细说明。本发明所述方法的流程图如附图1所示，包括以下几个步骤:步骤一，获取可通行空间树，。可通行空间树由可通行空间以及可能目标点组成，其具体建立方法如下: (1)犹取扇区利用获取的激光数据信息，将距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧线，弧线所围成的区域称之为扇区。(2)判断可通行角度区间假设存在一个矩形通道，设定阈值w大于机器人宽度，并将w作为矩形的宽。使该矩形以机器人当前位置为中心进行旋转，在每个扇区中寻找可通行角度区间。(3)选取可能目标点为确定机器人的前进方向和目标点，选取最外层扇区中的可见点，将其作为当前目标点K，则机器人的可能目标集合描述为:
权利要求
1.一种基于分层拓扑结构的多机器人系统地图拼接方法，其特征在于包括以下几个步骤: 步骤一，获取可通行空间树，方法如下: (1)获取扇区 利用获取的激光数据信息，将距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧线，弧线围成一个个扇区； (2)判断可通行角度区间 假设存在一个矩形通道，设定阈值w大于机器人宽度，并将w作为矩形的宽；使该矩形以机器人当前位置为中心进行旋转，在每个扇区中寻找可通行角度区间； (3)选取可能目标点 为确定机器人的前进方向和目标点，选取最外层扇区中的可见点，将其作为当前目标点P;，则机器人的可能目标集合描述为: 式中，F是当前可能目标点隼P1是该点集中的第j个可能目标点； 步骤二，建立分层拓扑结构，力法卯下: 在可通行空间树的基础上建立一种融合混合特征的分层拓扑地图结构；在分层拓扑结构H中维护两层地图:第一层地图是当前节点的可通行空间树的集合A ;第二层是已生成拓扑节点的集合T，即拓扑层，分层拓扑结构H描述为:H = {A, T} 在拓扑节点处提取比例不变特征(SIFT)，并融合到分层拓扑地图结构当中；拓扑节点集合Ti可描述为:Ti = {s，A，p} 式中，s是Ti处所提取的SIFT特征向量,A为Ti处建立的可能目标点的集合，p表示机器人当前位姿； 假设S为已生成的拓扑节点，其可能目标点集合为(Q1, Q2,..., Qj,..., Qn);当前拓扑节点为T，可能目标点集合为(P1, P2,..., Pi,..., Pm)，其中N、M分别为S、T所含通路点个数；用d(P，Q)表示节点P与Q之间的距离；fflC(P，Q)表示节点P与Q之间的连通关系，其值为TRUE,表示P与Q连通；其值为FLASE，则表示P与Q间存在障碍；S = S/Qj表示将从拓扑节点S中删除，T = T U Qj表示将添加到拓扑节点S中；具体动态维护规则如下: (I)拓扑节点T所属区域\内的可能目标点Pi对机器人探索的作用不大，删除Pi ;即: 若(KPiJ)彡 Vr，则 T = IyPi ； (2把落在已生成的拓扑节点S所属区域1，且？1与S间存在可通行路径，删除Pi ;即:若 d (Pi, S)彡 Vr，且 C (Pi, S) = TRUE,则 T = TZPi ； (3)若相邻的通路点PpQj间不存在障碍物，且距离在阈值0内，保留其中最有利于机器人探索的环境通路点；即:在(!(Pi, Qj) ( O 的情况下，若 d (Pi, T)彡 d(Qj，S)，则 T = TA3i ;否则 S = S/Qj ； (4)机器人探索过程中，需要对某些满足条件的通路点重排列以保证有效的环境探索；即:若 d (Qj, T)≤ d (Qj, S)，且 C (Qj, T) = TRUE,则 S = S/Qj, T = T U Qj ； (5)若机器人运动到附近，且％与T间不存在障碍物，表明所代表未知区域已被探索，直接删除％;即:若 d (Qj, T)≤ Vr，且 C (Qj, T) = TRUE,则 S = S/Qj ； 步骤三，创建全局拓扑地图，方法如下: (1)探索开始时，首先获取激光传感器信息，根据激光数据对可能目标点进行搜索； (2)在整个环境中均不存在尚未探索的可能目标点的情况下，若d(T，0)( 0，探索终止；否则，机器人回溯到初始点；其中T为当前拓扑节点，O为初始点，O为距离阈值； (3)当前时刻提取的可能目标点可能会有多个，采用效用函数U(P)这一评价标准，计算各个通路点的效用值，并选取效用函数值最大者作为即时目标点，同时添加当前拓扑节点T并更新拓扑地图；U(P)的表达式为:
全文摘要
本发明属于智能移动机器人领域，公开了一种基于分层拓扑结构的未知环境下多机器人系统地图拼接方法。解决了在相对位姿未知情况下的多机器人系统地图拼接问题。该方法包括获取可通行空间树，建立分层拓扑结构，创建全局拓扑地图，提取SIFT特征并进行特征匹配，基于ICP扫描匹配进行地图拼接。本发明在机器人间相对位姿未知的情况下，提出一种融合SIFT特征的分层拓扑结构，并增量式地创建全局拓扑地图，根据节点间的SIFT匹配信息，结合扫描匹配的方法实现大规模未知环境下的多机器人系统地图拼接。有效地提高了拼接的准确性和实时性。该方法可适用于与地图创建与地图拼接有关的智能移动机器人领域。
文档编号G06T5/50GK103247040SQ20131017539
公开日2013年8月14日 申请日期2013年5月13日 优先权日2013年5月13日
发明者贾松敏, 李雨晨, 王可, 李秀智 申请人:北京工业大学