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

基于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配准算法融合拓扑地图的仿真实验结果表明，对于任意环境中的两张地图，配