rtm 框架下基于分层拓扑结构的多机器人系统地图拼接

本文档下载自文库下载网，内容可能不完整，您可以点击以下网址继续阅读或下载HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLRTM框架下基于分层拓扑结构的多机器人系统地图拼接1机器人论文第35卷第3期2013年5月DOI103724/SPJ1218201300292机器人ROBOTVOL35,NO3MAY,2013RTM框架下基于分层拓扑结构的多机器人系统地图拼接贾松敏，李雨晨，王可，李秀智，郭兵（北京工业大学电子信息与控制工程学院，北京100124）摘要针对大规模的未知环境，在机器人中间件技术（RTM）框架下，提出了一种基于视觉特征的拓扑地图节点匹配方法，并结合局部扫描匹配策略，实现多机器人系统地图拼接该方法首先建立主辅结构的多机器人模型，利用改进的SP2ATM算法建立未知环境的拓扑结构并增量式地创建地图在此基础上，提出了融合尺度不变（SIFT）特征的分层拓扑地图结构，并结合迭代最近点算法来实现多机器人系统地图拼接本文以RTM作为通讯平台，使系统具有较高的实时性、灵活性和鲁棒性USARSIM仿真平台与真实环境下的实验结果验证了所提方法的可行性与有效性关键词多机器人系统；机器人中间件技术；地图拼接；尺度不变特征；迭代最近点算法中图分类号TP24文献标识码A文章编号10020446201303029207MAPMERGINGFORMULTIROBOTSYSTEMSBASEDONHIERARCHICALTOPOLOGYSTRUCTUREUNDERRTMFRAMEWORKJIASONGMIN，LIYUCHEN，WANGKE，LIXIUZHI，GUOBINGCOLLEGEOFELECTRONICINFORMATIONRTMROBOTTECHNOLOGYMIDDLEWAREMAPMERGINGSIFTSCALEINVARIANTFEATURETRANSFORMICPITERATIVECLOSESTPOINT1引言（INTRODUCTION）地图拼接作为多机器人系统（MULTIROBOTSYS12TEM，MRS）导航领域中的关键性技术，近年来引起了众多学者的关注HHTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLOWARD3提出将RBPF（RAOBLACKWELLIZEDPARTICLELTER）算法4扩展应用于多机器人SLAM，初步解决了初始位姿未知情况下的拼接问题在此基础上，ZHOU和ROUMELIOTIS在机器人必须相遇条件下5，借助于内部传感器信息来计算地图变换矩阵，该方法需对传感器信息不确定性进行分析与优化，且拼接地图效果受限于传感器检测精度鉴于此，有学者提出利用相关扫描匹配策略搜索地图公共区域来实现地图拼接YU等6提出一种基于线段几何特征（LINESEGMENTGEOMETRICFEATURE）的扫描匹配方法，该方法在结构化环境中取得了较好的拼接结果文5基于最邻近搜索算法（NEARESTNEIGHBORALGORITHM）建立目标函数，提出一种路标匹配方法实现特征地图融合为在保证地图精度的同时降低算法复杂度，文7通过提取环境中的VSL（VIRTUALSUPPORTINGLINE）非迭代特征进行匹配来实现拼接然而上述方法在缺少空间约束的环境中并不适用，且在大规模未知场景下，相关特征的错误关联会引入拼接误差随着VSLAM（VISUALSLAM）算法的日益成熟，一些学者尝试将基金项目北京市自然科学基金重点资助项目（KZ201110005004）；国家自然科学基金资助项目（61175087，61105033）；国家教育部留学回国人员科研启动基金资助项目通讯作者贾松敏，JSMBJUTEDUCN收稿录用修回20121022/20130112/20130325视觉特征融入系统8以解决地图拼接准确性问题，但视觉特征提取的实时性问题又是一个难题为解决上述研究中的问题，针对大规模未知环境，本文提出视觉特征关联与局部扫描匹配相结合的地图拼接方法首先提出一种融合混合特征的分层拓扑结构，在此基础上运用关键帧（KEYFRAME）的思想，采取拓扑节点匹配策略关联场景信息以降低匹配的计算复杂度；其次，结合迭代最近点（ITERATIVECLOSESTPOINT）扫描匹配算法求取局部地图变换矩阵，在主HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTML辅结构的多机器人模型下校正位姿估计误差，同时为确保地图信息的全局一致性，引入一种反馈机制解决拼接中误匹配问题；在地图融合过程中，无需考虑机器人相对位姿信息，以地图间相似度作为度量标准，搜索各局部地图间重叠部分，进而予以拼接此外，本文在机器人中间件技术（RTM）911框架下实现地图拼接系统模块化设计，从而进一步提高了地图拼接系统的灵活性、可靠性及实时性2地图创建（MAPBUILDING）针对多机器人系统的地图创建问题，本文基于SP2ATM（SIMULTANEOUSPATHPLANNINGANDTOPOLOGICALMAPPING）算法提出一种改进的路径规划算法，该算法通过对未知环境中拓扑结构的建立来实现移动机器人路径探索并增量式地创建连续地图，算法流程如图1所示算法中建立一种分层拓扑地图结构，该结构中包含SIFT角点、拓扑节点、场景信息等混合特征的精确描述，同时依据节点更新规则，避免多机器人间重复探索，有效提高地图创建效率21AST建立AST（ADMISSIBLESPACETREE）1是用来描述当前局部地图中的机器人可通行空间及可能目标点的集合AST建立方法如下利用获取的激光数据信息，将距离机器人最近的障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧线，称之为SECTION为确定在SECTION中是否存在可能目标点，本文假设存在一个矩形通道，将大于机器人宽度的给定阈值W作为矩形的宽，并使该矩形以机器人当前位置为中心进行旋转，在每个SECTION中寻找可通行角度区间图2表示对可能目标点的搜索方法，其中W为所选择的阈值，红线之间表示可通行的角度区间在已建立的AST中，为确定机器人的前进方向和目标点，选取最外层SECTION中的可见点，将其作图1路径规划算法流程FIG1FLOWCHARTOFPATHPLANNINGALGORITHM图2当前可能目标点搜索方法FIG2SEARCHINGFORTHHTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLECURRENTPOSSIBLEGOAL为当前目标点PJC，则机器人的可能目标集合描述为PCPCJ其中，PC是当前可能目标点集，PCJ是该点集中的第J个可能目标点图3为机器人在仿真环境中所生成的SECTION和可能目标点以及其角度区间假设第J个节点的可通行空间集合为SI，则此时AST所表示的集合AI可描述为AISI,PI图3SECTION及可能目标点的生成FIG3THEGENERATIONOFSECTIONSANDTHEPOSSIBLEGOALS22分层拓扑结构为实现环境探索，在AST的基础上建立一种融合混合特征的分层拓扑地图（HIERARCHICALTOPOLOGYMAP，HTM1）结构在HTM分层结构中维护了两层地图第一层地图是当前节点的AST集合A；第二层是已生成拓扑节点集合T，这里称为拓扑层这样HTM可以描述为MA,T其中TTI，AAITI是集合T中的第I个拓扑节点为描述更丰富的环境信息，将比例不变特征SIFT）12融合到分层拓扑地图结构当中，如图4所示拓扑节点集合TI可描述为TIS,A,P其中，S是TI处所提取的SIFT特征向量，A为TI处建立的AST集合，P表示机器人当前位姿LAYER2ASTSIFT图4分层拓扑结构FIG4HIERARCHICALTOPOLOGYSTRUCTURE在机器人自主探索过程中，为保证可能目标点提取的全局最优性，需根据已建立HTM结构中节点的全局信息，对可能点进行添加、绑定与删除等动态更新操作其中，假设T为已生成的拓扑节点，其可能点集HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTML合为Q1,Q2,QJ；TPR为当前节点，其可能点集合为P1,P2,PI；DPI,T表示PI与T间距离，CPI,T表示PI与T是否存在可通行通路，VR为节点间距离阈值；TT/PI表明将PI从T的可能点集合中删除，TTPI表明将PI与T绑定具体节点更新规则如下（见图5）RULE1IFDPI,TPRVR,THENTPRTPR/PI（图5A）RULE2IFDPI,TVR,THENTPRTPR/PI（图5B）RULE3IFDPI,QJVR,THENTPRTPR/PI（图5C）RULE4IFDPI,TVRTHENTANDCPI,TFALSE,PRTPRPI（图5D）RULE5IFDQJ,TPRDQJ,TANDCQJ,TPRTRUE,THENTT/QTJ,PRTPRQJ（图5E）CDE图5节点更新规则FIG5NODEUPDATINGRULE若机器人当前不存在任何可能目标点，为完成对环境的遍历，机器人必须回溯到距离机器人最近且存在未访问可能目标点的节点处，将此节点设置为目标，并利用BELLMANFORD算法选择一条到达该目标的最短路径BELLMANFORD算法描述为DKJMINDK1J,DK1IDI,J1式中DKJ表示当前位置与第J个节点的距离，DI,J是第I个节点与第J个节点的距离回溯过程中，机器人将最远的可见节点TV（见式2）作为目标，若探索到新HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTML的可能点，机器人将沿此可能点方向继续遍历环境DPR,PTVRMAXKVDPR,PTK2式中，DI,J表示I、J两点之间的欧氏距离，PR表示机器人当前位姿，PTV表示拓扑节点V处位姿23地图创建本文基于上述路径规划算法完成多机器人系统地图创建多机系统包含类似HTM结构的两类地图一类是存储着所有节点的拓扑地图；另一类是在拓扑节点处，记录所有可能节点的局部地图HTM结构不仅有着存储空间小、易于实现路径规划的优（势，并且基于HTM结构的节点更新规则提供了一种全局最优的路径规划策略，可进一步提高探索效率同时，为避免多机器人间的重复探索，每个机器人通过自身的HTM地图实时更新拓扑结构，并依据节点更新规则检查所有节点，防止节点的重复访问，以充分提高地图创建效率探索过程中，每个机器人建立自身的拓扑树型结构，并利用节点更新规则判断当前AST，若不存在任何可能节点，系统将停止工作单机器人地图创建效果如图6所示图6单机器人地图创建FIG6MAPBUILDINGOFASINGLEROBOT3多机系统地图拼接（MAPMERGINGFORMULTIROBOTSYSTEM）本文利用上述路径规划算法创建增量地图，建立一种包含SIFT特征的分层拓扑地图结构，并结合ICP扫描匹配算法降低误匹配率，最终完成局部地图拼接31比例不变特征变换比例不变特征变换，简称为SIFT12SIFT特征对图像的尺度缩放、旋转、3维视角变化具有较好的不变性，并且对噪声、光照影响不敏感因此，本文通过提取环境中的SIFT特征角点来描述未知环境，并将节点间的SIFT匹配度作为地图拼接的先验信息首先，在尺度空间中利用高斯模糊差分图像DX,Y,（见式3）的极值点检测方法来计算SIFT特征DX,Y,HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLGX,Y,KGX,Y,IX,YLX,Y,KLX,Y,3其中符号代表卷积运算，为比例因子MX,YLX1,YLX1,Y2LX,Y1LX,Y12X,YTAN1LX,Y1LX,Y1LX1,YLX1,Y4通过式4来计算图像中像素点的梯度大小MX,Y及方向X,Y在图像中的44区域上建立包含8个方向的梯度方向直方图，这样，每个特征点对应一个448128维特征向量描述器提取特征后，采用基于KD树的最邻近点搜索算法进行特征匹配，如果相似度满足阈值条件，则认为匹配成功应用SIFT特征的难题在于特征提取和匹配的实时性问题特征提取的时间主要消耗在高斯模糊差分图像计算上，虽然降低图像比例在一定程度上可以减小运算复杂度，却提高了误匹配率针对此问题，本文运用关键帧的思想，将SIFT特征融入拓扑地图结构在地图创建过程中，视觉系统仅在拓扑节点处提取SIFT特征来描述环境信息，并将特征向量存储于当前节点信息当中，一定程度上提高了算法的实时性32迭代最近点算法ICP算法13于1992年由BESL和MCKAY提出，用来在同一坐标系中匹配两个给定的点阵ICP算法是一个快速、精确的算法，该算法已广泛地应用于移动机器人定位与地图创建中ICP算法的评估函数如式5所示14EDIST,TRMINMRQ2P,T,J1,2,NITJI125STRTRIL,DETR1/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLPAR其中，R是旋转变换矩阵，是旋转角，T是平移向量即通过最小化误差和EDIST求两个点集间的相对坐标变换X,T本文利用ICP算法实现地图拼接，并通过计算两个局部地图之间的相对坐标变换矩阵来更新机器人精确位姿33地图拼接本文以多机器人在未知环境中进行地图拼接为研究背景，建立主辅结构的多机器人模型，如图7所示YLLLL图7主辅多机器人模型FIG7MAINAUXILIARYMULTIROBOTMODEL图7中，RI（I1,2,M）表示M个移动机器人，其中RL定义为主机器人；其余RI为辅机器人，与主机器人之间相对位姿未知系统状态表示为XSTGTTTTXR1T,GXM1T,GXL2T,GXLMT,LT,LTTX2T,XMT6其中，GXGXTR1TRT,GYRT,GRT，表示主机器人在全局坐标系中的位姿，GXM1T表示主机器人的全局地图；GXLIT（I2,3,M）表示辅机器人I的局部坐标系L在全局坐标系G中的位姿；LXLTTXTITRIT,LXMIT（I2,3,M），为局部坐标系下的系统状态；LXRTLXRT,LYRT,LRTT，表示辅机器人在各自局部地图中的位姿；LXMIT中表示辅机器人I的局部地图辅机器人I的局部地图与全局地图之间的映射是通过GXLIT实现的探索过程中，每个机器人均采用前述改进算法建立和维护增量地图GXM1T和LXMIT本文利用SIFT特征关联不同拓扑节点，每个机器人将获得SIFT特征向量集S12JI,HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTML,SI，其中SJI,SI表示机器人I在拓扑节点J处提取的SIFT特征当机器人I获得某一节点的SJI后，将与其他机器人已生成SNK进行匹配若特征向量匹配程度满足阈值条件，则表明两节点间具有很高的相似度，因此，对局部地图LXMIT与全局地图GXMKT使用ICP算法进行点集间匹配来实现拼接为降低SIFT特征的误匹配率，将ICP匹配误差作为反馈值来提高地图拼接的准确率PLXMIT|LXRIT,GXMKT7式7表示机器人I在当前位姿所生成的局部地图与全局地图的相似度，若满足阈值要求，即实现拼接，否则重新进行SIFT匹配地图拼接后，将此时机器人K的位姿LXRKT与机器人I在节点J处位姿LXRIT进行相对坐标转换，进而更新机器人I的位姿，如图8所示图8位姿更新模型FIG8POSEUPDATINGMODEL图8中，PIJ是由激光采集的局部坐标点，PI和PK是机器人I和K的全局坐标点，相对坐标变换矩阵R,T由ICP算法求得4基于RTM的模块化实现（COMPONENTSIMPLEMENTATIONBASEDONRTM）41机器人模块化技术概述RTM最早是由日本国家高级产业科学技术研究院（AIST）开发的它所依托的OPENRTMAIST技术采用了公共对象请求代理体系结构（CORBA），能够加快实现机器人系统在组件层面上的集成RTM的核心优势在于，它给机器人的硬件及软件开发层提供了标准平台，可以提高软件及功能构件的可重用性，避免大量重复工作，缩短开发周期，降低开发成本；不仅如此，RTM技术还可以使系统的各个中间件通过通信网络将各种机器人要素进行自由组合从而构成多样的机器人系统，提高现有软件资源和通用功能构件的再利用性，提高构筑机器人系统的效率42模块化设计与实现本文从功能构件具有最小机能、传输数HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTML据具有通用性、尽量减少各功能构件间的依存性、尽可能减少输入输出数据文件等基本原则出发，将系统最底层的功能构件分为电机，驱动器，基于视觉的图像处理，识别，基于激光测距的地图生成模块等将不同的底层构件组合实现更高级的模块，如地图创建模块、定位模块及导航模块等构件实现的系统结构图如图9所示基于RTM的地图拼接系统有着良好的灵活性、可靠性及实时性图9多机器人系统的地图拼接构件结构FIG9THECOMPONENTSSTRUCTUREOFMAPMERGINGFORMULTIROBOTSYSTEMABFIG10图10地图拼接结果SOMERESULTSOFMAPMERGINGUSINGTWOROBOTS5实验结果（EXPERIMENTALRESULTS）51仿真结果为了验证方法的可行性，使用USARSIM作为多机器人系统地图拼接的仿真平台仿真中选用PIONEER3AT作为机器人平台，机器人配备激光测距仪和相机系统选用SICK激光测距仪，测量角范围是180，角精度是05仿真环境为20M20M的区域，两台机器人的线速度为05M/S，角速度为1RAD/S，激光测距仪测量距离的范围为0,10M，夹角范围为/2,/2使用机器人配备相机获取环境中SIFT特征信息两台机器人在相对位姿未知情况下，分别从不同起始位置探索未知环境，采用本文方法进行地图拼接实验图10A中一系列连续点代表两台机器人所生成的拓扑节点，蓝色细线表示机器人可通行的方向，在标记的节点处，机器人车载视觉系统提取的SIFT特征角点匹配成功，此时采用ICP算法将两台机器人的局部地图进行拼接，拼接结果如图10A所示实验结果证明了本文方法的有效性52实机实验为验证方法在实际环境下的可行性以及不同环境规模下HTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTML的适用性，利用本文所提方法在真实环境下进行地图拼接实验，拼接结果如图10B所示同时，为验证本文基于关键帧理论提取SIFT特征方法的优势，在上述实验环境下，针对基于关键帧理论提取SIFT特征（KEYFRAME）与实时提取SIFT特征（FRAME）两种算法的实时性进行对比分析，结果如图11所示图中利用CPU使用率衡量算法的实时性，可以看出，基于FRAME的特征提取方法的CPU使用率始终保持较高状态，而基于KEYFRAME的SIFT特征提取方法仅在拓扑节点处CPU使用率上升，因此本文算法具有较高的实时性/UPC图11基于KEYFRAME与FRAME的特征提取方法实时性对比FIG11COMPARISONOFTHEREALTIMEPERFORMANCEBETWEENKEYFRAMEBASEDANDFRAMEBASEDFEATUREEXTRACTIONMETHODS6结论（CONCLUSIONS）本文系统地研究了未知环境下多机器人系统的地图拼接问题，引入机器人中间件技术作为系统的通讯平台，借助于拓扑地图节点匹配，利用一种基于SP2ATM的改进算法完成局部地图创建，在此基础上，提出了一种融合混合特征的分层拓扑地图结构，并结合ICP扫描匹配算法实现多机器人在未知环境下的地图拼接，同时校准机器人位姿实验结果验证了本文方法的可行性与有效性对目前算法的改进及多机器人间的协作和通讯策略将是后续研究的重点参考文献（REFERENCES）1GEASS,ZHANGQ,ABRAHAMAT,ETALSIMULTANEOUSPATHPLANNINGANDTOPOLOGICALMAPPINGSP2ATMFORENVIRONMENTEXPLORATIONANDGOALORIENTEDNAVIGATIONJROBOTICSANDAUTONOMOUSSYSTEMS,2011,593/42282422/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLRABRAHAMAT,GESS,TAOPYATOPOLOGICALAPPROACHOFPATHPLANNINGFORAUTONOMOUSROBOTNAVIGATIONINDYNAMICENVIRONMENTSC/IEEE/RSJINTERNATIONALCONFERENCEONINTELLIGENTROBOTSANDSYSTEMSPISCATAWAY,NJ,USAIEEE,2009490749123HOWARDAMULTIROBOTSIMULTANEOUSLOCALIZATIONANDMAPPINGUSINGPARTICLELTERSJINTERNATIONALJOURNALOFROBOTICSRESEARCH,2006,2512124312564HAHNELD,BURGARDW,FOXD,ETALANEFCIENTFASTSLAMALGORITHMFORGENERATINGMAPSOFLARGESCALECYCLICENVIRONMENTSFROMRAWLASERRANGEMEASUREMENTSC/IEEE/RSJINTERNATIONALCONFERENCEONINTELLIGENTROBOTSANDSYSTEMSPISCATAWAY,NJ,USAIEEE,20032062115ZHOUXS,ROUMELIOTISSIMULTIROBOTSLAMWITHUNKNOWNINITIALCORRESPONDENCETHEROBOTRENDEZVOUSCASEC/IEEE/RSJINTERNATIONALCONFERENCEONINTELLIGENTROBOTSANDSYSTEMSPISCATAWAY,NJ,USAIEEE,2006178517926YUY,WANGN,LIANGA,ETALINTEGRATINGLINESEGMENTBASEDMAPSINMULTIROHTTP/WWWWENKUXIAZAICOM/DOC/FB866D6127284B73F24250CEHTMLBOTSEXPLORATIONC/2NDINTERNATIONALCONFERENCEONFUTUREINFORMATIONTECHNOLOGYANDMANAGEMENTENGINEERINGPISCATAWAY,NJ,USAIEEE,20092302337LEEHC,LEEBHIMPROVEDFEATUREMAPMERGINGUSINGVIRTUALSUPPORTINGLINESFORMULTIROBOTSYSTEMSJADVANCEDROBOTICS,2011,2513ZKUCURNE,AKINHLCOOPERATIVEMULTIROBOTMAPMERGINGUSINGFASTSLAMC/LECTURENOTESINARTICIALINTELLIGENCEVOL5949BERLIN,GERMANYSPRINGERVERLAG,20104494609JIASM,MURAKAMIT,CHUGOD,ETALINTERACTIVEROBOTSYSTEMFORSUPPORTINGOBJECTACQUISITIONBASEDONROBOTTECHNOLOGYMIDDLEWAREC/IEEEINTERNATIONALCO