基于ICP—SVD的多移动机器人拓扑地图创建研究

王娜++王海艳++刘纪新++郑义

摘要：移动机器人要完成智能任务，前提是创建未知环境地图，本文利用图像配准方法，采用ICP算法结合奇异值分解算法（SVD），建立了地图融合的数学模型，解决了多移动机器人拓扑地图融合问题。实验结果表明，将ICP-SVD图像配准算法应用于机器人拓扑地图融合，算法简单，消耗时间短，且有效提高了地图的匹配效果。

关键词：多移动机器人；拓扑地图；图像配准；ICP-SVD

0 引言

多个移动机器人位于同一空间，协同工作，可以提高探索环境的效率；融合多个机器人的传感器数据，可以有效提高环境地图的精确性。拓扑地图重在描述环境的拓扑结构，占用存储空间少，计算时间短，所以，拓扑地图的相关计算效率较高。

现阶段的研究工作中，Dedeoglu解决了融合基于路标的地图，利用两个地图间的某一顶点匹配，估计一种转变方案，配准其他的顶点，产生统一的全局地图，但是，多机器人协作未得到一个完整的总体解决方案。Konolige采用一种决策框架理论方法，在框架范围内的局部地图中，实现了机器人相对定位，证明基于特征点的融合方法具有更高效率，缺点是特征点的提取过程较复杂。

本文中采用的ICP（Iterative Closest Point）图像配准方法，即反复迭代寻找最近点，是Besl在1992年提出的。文中的拓扑地图融合来自几何信息，如路径连通性，节点类型等。利用结构的图像匹配和几何特征的图像配准，依据匹配范围和误差的大小，保留最佳匹配方案的假设，融合两个地图。

1 地图融合的数学模型建立

首先对拓扑地图进行平面转换，采用线性变换中的刚体变换：

将平面中的一点用齐次坐标表示，向量 表示点 ， 代表旋转矩阵， 代表平移矩阵， 分别表示x方向平移，y方向平移和逆时针旋转角度。

令 是三个实数，则点p关于 转移关系为：

式（1）表示对点 进行角度为 的旋转变换，沿坐标轴方向在坐标平面内进行 的平移。

文中的拓扑地图融合，采用的是两两匹配方法。若两个局部地图已知，目的是搜索某一平面转换，此时对应的匹配范围应为最大。

将这两个地图记为MapA和MapB，EA、EB表示地图的两个矩阵，矩阵中的元素表示路径端点坐标。设路径数分别为n和m，则地图MapA、MapB间的相似度关系如下：

式中， 表示两个地图匹配的程度， 表示地图MapA中的路径i， 表示地图MapB中的路径j。

2 ICP-SVD算法解决拓扑地图融合

将ICP-SVD算法应用于拓扑地图融合，基本输入是两个地图的路径端点集合，搜索各个路径端点在固定地图上的最近点。

固定地图的路径端点集合记作M，坐标为： ；另一进行平面变换的地图路径端点集合记作D，坐标为： 。当计算到第k次迭代时， ，计算M与 间的变换矩阵，更新原变换，到两组数据间的距离最小为止，即经过迭代使如下优化函数最小。

ICP-SVD算法的每一次迭代包括以下四步：

Step1：初始化：求两组点集的均值， ， ；初始化旋转矩阵R= ，平移矩阵T= ，则 ；

Step2：采用奇异值分解算法（SVD）计算旋转矩阵和平移矩阵：

设d为点集D的质心，m为点集M的质心，将所有点的坐标减去质心坐标。则平移之后的点为 ， 。平移之后的总误差为：

最小化E等价于最大化

其中， 。

对H进行奇异值分解计算， ，令 ，若X的行列式det（X）=1，表明 和 是2个相互正交的矩阵，得到R=X，平移向量 ；

Step3：将R与T应用到点集D，求出新点集 ，并用它代替原有D；

Step4：计算新点集D与M间的目标函数 ，若两次迭代误差小于给定阈值 （本文实验中取值为 ），则迭代结束。

3 实验仿真结果及分析

本文使用机器人Pioneer3在固定的室内环境中进行实验。Pioneer3共配置了8个声纳传感器，每10cm停下测量一次数据，其路线能覆盖整个环境，这样进行实验检测到了多个数据样本。

为了验证文中ICP-SVD配准算法融合拓扑地图的可行性，利用MATLAB实验仿真。在实际环境中，移动机器人运动方向多样化，从不同角度探测环境，地图融合时，旋转角度 需考虑多种取值。

图1和图2所示为环境中待配准的原始地图，配准过程中，将地图MapA固定，视为“模板”，对地图MapB进行平面变换。图3所示为两张地图的最终配准结果，从图中可以看出，配准融合后得到的全局地图准确度大大提高。

图4所示为相邻两次的迭代误差差值变化曲线，横坐标是迭代次数，纵坐标是相邻两次的迭代误差差值。此实验中，当迭代11次，相邻两次的迭代误差值小于给定阈值0.1，此时迭代结束。

ICP-SVD配准算法融合拓扑地图的仿真实验结果表明，对于任意环境中的两张地图，配准融合后得到的地图效果很好。而且，算法实现简单，消耗时间短，很大程度提高了多移动机器人系统的工作效率，得到的环境地图更精确。

利用我校的机器人实验室条件，对本文中算法地图融合的准确性进行了验证（实验中均采用两个机器人工作）。

机器人实验室中的实验结果表明，本文中的算法用于实际环境中拓扑地图融合准确度较高，减少了时间损耗，满足实际要求。

4 结论

本文研究了多移动机器人的拓扑地图融合，在机器人相对位置未知的情况下，对于两个拓扑地图，采用图像配准方法中经典的ICP（Iterative Closest Point）算法结合奇异值分解算法（SVD），进行地图融合。在MATLAB环境下，针对不同的环境进行了仿真，在实验室中的实际环境中进行验证实验。结果表明，利用文中的ICP- SVD图像配准算法进行拓扑地图融合，能够达到理想的效果，而且算法较稳定，实现简单，消耗时间较短，很大程度上提高了多移动机器人系统的工作效率。

参考文献：

[1]赵翊捷，陈卫东.基于地图的移动机器人定位技术新进展[J].上海交通大学学报.Vol.36，No.10.2002，1435-1438.

[2]W.H.Huang，and K.R.Beevers.Topological map merging[C].The 7th Intl Symp on Distributed Autonomous Robotis Systems（DRAS 2004）.

[3]陈世欢.基于改进蚁群算法的改航路径规划[J].计算机技术与发展.2015（2）：52-54.

[4]易学渊.一种图形处理用的多格式定点运算器[J].计算机技术与发展.2014（10）：147-150.

[5]G.Dedeoglu and G.S.Sukhatme.Landmark-based matching algorithm for cooperative mapping by autonomous robots[C].In L.E.Parker，G.W.Bekey，and J.Barhen，editors，Distributed Autonomous Robotic Systems 4，pages 251–260.Springer-Verlag，2000.

[6]晁衍凯，徐昱琳.基于双目视觉的机器人目标定位与机械臂控制[J].计算机技术与发展[J].2013（7）：6-9.

[7]P.J.Besl and N.D.McKay.A method for registration of 3-D shapes[C].IEEE Transactions on Pattern Analysis and Machine Intelligence，14（2）：239–256，February 1992.

[8]王娜.移动机器人拓扑地图创建研究[D]：[硕士学位论文].山东大学，2009.

[9]王娜.基于声纳的移动机器人地图创建改进方法[J].科协论坛.2010，（6）：65-67.

基金项目：山东省高等学校科技计划项目（J14LB61）