CN103247040B - 基于分层拓扑结构的多机器人系统地图拼接方法 - Google Patents

Abstract

本发明属于智能移动机器人领域，公开了一种基于分层拓扑结构的未知环境下多机器人系统地图拼接方法。解决了在相对位姿未知情况下的多机器人系统地图拼接问题。该方法包括：获取可通行空间树，建立分层拓扑结构，创建全局拓扑地图，提取SIFT特征并进行特征匹配，基于ICP扫描匹配进行地图拼接。本发明在机器人间相对位姿未知的情况下，提出一种融合SIFT特征的分层拓扑结构，并增量式地创建全局拓扑地图，根据节点间的SIFT匹配信息，结合扫描匹配的方法实现大规模未知环境下的多机器人系统地图拼接。有效地提高了拼接的准确性和实时性。该方法可适用于与地图创建与地图拼接有关的智能移动机器人领域。

Description

基于分层拓扑结构的多机器人系统地图拼接方法

技术领域

本发明属于智能移动机器人领域，涉及一种基于分层拓扑结构的未知环境下多机器人系统地图拼接方法。

背景技术

近年来，移动机器人的应用越来越普遍，各种具有较高智能的移动机器人系统在不同场合正在发挥重要的作用。但随着人类社会活动领域的不断延伸以及机器人学研究的不断发展，单个移动机器人系统在代替人完成包括大范围灾难救援、科学考察等任务以及战场环境监察等军事使命时面临着诸多的困难，如：可靠性差、作业范围小、任务完成效率低等。而相关研究表明，由多个移动机器人组成的多机器人系统通过协调、合作恰恰可以解决这些问题。因此，多机器人系统被认为具有广阔的应用前景。

地图拼接作为多机器人系统导航领域中的一项关键性技术，已引起了众多学者的关注。在机器人初始位姿是已知的假设下，多机器人之间的地图拼接问题可转化为单机器人同步定位与地图创建(SimultaneousLocalizationandMapping，SLAM)问题的直接扩展，然而针对大规模的未知环境，该假设存在着较大的局限性；在机器人必须相遇条件下，可借助于内部传感器信息计算地图变换矩阵进而实现地图拼接，该方法需对传感器信息不确定性进行分析与优化，且拼接地图效果受限于传感器检测精度。

考虑到算法的通用性和鲁棒性，解决机器人间相对位姿未知情况下的地图拼接问题有着重要的意义。目前有学者利用相关扫描匹配策略搜索地图公共区域来实现地图拼接，取得了一些成果，解决方案是将局部地图与局部地图的数据关联问题转化为点与局部地图的匹配问题，大大节省了搜索空间。YanYu等提出一种基于线段几何特征的扫描匹配方法，在结构化环境中取得较好拼接结果。Zhou和Roumeliotis基于最邻近搜索算法建立目标函数，提出一种路标匹配方法实现特征地图融合。LeeHC和LeeBH.于2011年在《AdvancedRobotics》杂志上发表的论文“ImprovedfeaturemapmergingusingVirtualSupportingLinesformulti-Robotsystems”，通过提取环境中的VSLs(VirtualSupportingLines)非迭代特征进行匹配来实现拼接，保证了地图精度的同时降低算法复杂度。然而上述方法在缺少空间约束的环境中并不适用，且在大规模未知场景下，相关特征的错误关联会引入拼接误差。随着vSLAM(VisualSLAM)算法的日益成熟，一些学者尝试将视觉特征融入系统以解决地图拼接准确性问题，但视觉特征提取实时性问题又是一个难题。

发明内容

为了克服上述地图拼接方法中存在的上述问题，本发明基于分层拓扑结构提供了一种视觉特征关联与局部扫描匹配相结合的多机器人系统地图拼接方法。

本发明采用的技术方案如下：

在机器人间相对位姿未知条件下，针对大规模的未知环境建立一种分层拓扑结构(HierarchicalTopologyMap，HTM)，利用基于视觉特征的拓扑地图节点匹配方法，结合局部扫描匹配策略，提出一种快速鲁棒的多机器人系统地图拼接方法。该方法主要包括以下内容：第一，提出融合可通行空间树(AdmissibleSpaceTree，AST)和尺度不变特征(Scale-InvariantFeatureTransform，SIFT)的分层拓扑结构，并利用一种改进的同时路径规划与地图创建(SimultaneousPathPlanningandTopologicalMapping，SP²ATM)方法建立未知环境的拓扑结构并增量式地创建地图；第二，采取节点匹配策略关联场景信息，利用SIFT匹配度作为地图拼接的先验信息，并在递增生成的拓扑节点处设定关键帧(KeyFrame)，以降低匹配过程的计算复杂度；第三，在机器人间相对位姿未知情况下，结合迭代最近点(IterativeClosestPoint，ICP)扫描匹配算法求取局部地图间的变换矩阵，当由ICP算法计算得到的地图间相似度大于给定阈值时，完成多机器人系统的地图拼接。

基于分层拓扑结构的多机器人系统地图拼接方法，包括以下步骤：

步骤一，获取可通行空间树。

为建立环境的拓扑结构并增量式地创建地图，本发明首先根据传感器信息建立环境中的可通行空间树结构，并实时获取机器人探索过程中的可能目标点，进而完成路径规划与地图创建任务。可通行空间建立方法如下：利用获取的激光数据信息，将距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧线，称之为扇区。为确定在扇区中是否存在可能目标点，本方法假设存在一个矩形通道，将大于机器人宽度的给定阈值w作为矩形的宽，并使该矩形以机器人当前位置为中心进行旋转，在每个扇区中寻找可通行角度区间。在已建立可通行空间树结构中，为确定机器人的前进方向和目标点，选取最外层扇区中的弧线中间点，将其作为可能目标点。这样，机器人通过目标点的实时更新完成环境探索任务。

步骤二，建立分层拓扑结构。

在可通行空间树的基础上建立一种融合混合特征的分层拓扑地图结构。在分层拓扑结构中维护了两层地图：第一层地图是当前节点的可通行空间树的集合A；第二层是已生成拓扑节点集合T，称为拓扑层。同时，为获取更丰富的环境信息，本发明在拓扑节点处提取比例不变特征(Scale-InvariantFeatureTransform，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)选取可能目标点

为确定机器人的前进方向和目标点，选取最外层扇区中的可见点，将其作为当前目标点则机器人的可能目标集合描述为：

$P^c=\left\{P_j^c\right\}$

式中，P^c是当前可能目标点集，是该点集中的第j个可能目标点。

本发明中可通行空间树结构如图2(a)所示所示。图2(b)显示了可通行角度区间的搜索方法，其中w为大于机器人宽度的阈值。

步骤二，建立分层拓扑结构。

本发明在可通行空间树的基础上建立一种融合混合特征的分层拓扑地图结构。在分层拓扑结构H中维护了两层地图：第一层地图是当前节点的可通行空间树的集合A；第二层是已生成拓扑节点集合T_g，称为拓扑层，分层拓扑结构H描述为：

H＝{A,T_g}

为使拓扑节点描述更丰富的环境信息，本发明在拓扑节点处提取比例不变特征(SIFT)，并融合到分层拓扑地图结构当中。拓扑节点集合T_i可描述为：

T_i＝{s,A_i,p}

式中，s是T_i处所提取的SIFT特征向量，A_i为T_i处提取的可通行空间树的集合，p表示机器人当前位姿。

机器人探索过程中，随着分层拓扑结构的递增式创建，可能存在已生成的可能目标点与已生成拓扑节点或者可能目标点间距离过于接近的情况，此时需利用节点更新规则对分层拓扑结构中的节点不断进行添加、绑定与删除等动态维护操作，以提高探索效率。假设S为已生成的拓扑节点，其可能目标点集合为(Q₁,Q₂,...,Q_j,...,Q_N)；当前拓扑节点为T，可能目标点集合为(P₁,P₂,...,P_i,...,P_M)，其中N、M分别为S、T所含通路点个数。用d(P,Q)表示节点P与Q之间的距离；用C(P,Q)表示节点P与Q之间的连通关系，其值为TRUE，表示P与Q连通；其值为FALSE，则表示P与Q间存在障碍。S＝S/Q_j表示将Q_j从拓扑节点S中删除，S＝S∪Q_j表示将Q_j添加到拓扑节点S中。具体动态维护规则如下：

(1)拓扑节点T所属区域V_r内的可能目标点P_i对机器人探索的作用不大，删除P_i。即：若d(P_i,T)≤V_r，则T＝T/P_i；

(2)P_i落在已生成的拓扑节点S所属区域V_r，且P_i与S间存在可通行路径，删除P_i。即：若d(P_i,S)≤V_r，且C(P_i,S)＝TRUE，则T＝T/P_i；

(3)若相邻的通路点P_i、Q_j间不存在障碍物，且距离在阈值σ内，保留其中最有利于机器人探索的环境通路点。即：

在d(P_i,Q_j)≤σ的情况下，若d(P_i,T)≥d(Q_j,S)，则T＝T/P_i；否则S＝S/Q_j；

(4)机器人探索过程中，需要对某些满足条件的通路点重排列以保证有效的环境探索。即：

若d(Q_j,T)≤d(Q_j,S)，且C(Q_j,T)＝TRUE，则S＝S/Q_j，T＝T∪Q_j；

(5)若机器人运动到Q_j附近，且Q_j与T间不存在障碍物，表明Q_j所代表未知区域已被探索，直接删除Q_j。即：

若d(Q_j,T)≤V_r，且C(Q_j,T)＝TRUE，则S＝S/Q_j。

本发明中分层拓扑结构如图3所示，其中包含拓扑层、可通行空间树及SIFT特征。

步骤三，创建全局拓扑地图。

本发明基于上述的可能目标点提取方法，不断提取新的目标点，通过拓扑节点的实时更新来完成全局拓扑地图的创建，其方法流程如图4所示。探索过程中，每个机器人建立自身的拓扑树型结构。机器人进入未探索环境中，需要根据激光传感器信息确定每步的最优目标点，并利用更新规则判断当前的可通行区域，实时确定机器人每次运动规划的当前目标点，以保证机器人顺利完成全局遍历。本发明中路径探索的具体步骤如下：

(1)探索开始时，首先获取激光传感器信息，根据激光数据对可能目标点进行搜索。

(2)在整个环境中均不存在尚未探索的可能目标点的情况下，若d(T,O)≤σ，探索终止；否则，机器人回溯到初始点。其中，T为当前拓扑节点，O为初始点，σ为距离阈值。

(3)当前时刻提取的可能目标点可能会有多个，采用效用函数U(P)这一评价标准，计算各个通路点的效用值，并选取效用函数值最大者作为即时目标点，同时添加当前拓扑节点T并更新拓扑地图。U(P)的表达式为：

$U\left(P\right)=\frac{G\left(P\right)\×L\left(P\right)\×e^{-\mathrm{\Δ}\mathrm{\θ}\left(P\right)}}{D\left(P\right)}$

式中，P为候选可能目标点；G(P)表示P处机器人所能观测到的未知环境面积大小；D(P)为机器人当前位置与P的距离；L(P)为P处相邻障碍物之间机器人可通行路径的宽度；Δθ(P)为P所指示的未探索区域方向θ_P与当前机器人朝向角θ_R之间差值的绝对值。

(4)当前拓扑节点T不存在可能目标点的情况下，搜索是否存在未探索的拓扑节点，若存在，则机器人回溯到最近的尚未探索的拓扑节点S处；否则返回步骤三(2)。若该拓扑节点存在至少一个可能目标点，返回步骤三(3)。

若机器人当前不存在任何可能目标点时，为完成对环境的遍历，机器人必须回溯到距离机器人最近且存在未探索可能目标点的拓扑节点处，并将此节点设置为回溯目标。并利用Bellman-Ford算法选择一条到达该目标的最短路径。Bellman-Ford算法描述为：

d_k(j)＝min{d_k-1(j),d_k-1(i)+d(i,j)}

式中，d_k(j)表示当前位置与第j个节点的距离，d(i,j)是第i个节点与第j个节点的距离。

回溯过程中，机器人将最远的可见节点T_v作为目标，若探索到新的可能点，机器人将沿此可能点方向继续遍历环境。

$d(p_r,p_{T_v})=\underset{r\≤k\≤v}{max}\left\{d(p_r,p_{T_k})\right\}$

式中，d(i,j)表示i、j两点之间的欧氏距离，p_r表示机器人当前位姿，表示拓扑节点v处位姿。

(5)重复步骤三(1)～(4)的方法，不断提取新的目标点，通过拓扑节点的实时更新来完成全局拓扑地图的创建。当不存在任何可能节点时，系统将停止工作。

本发明为避免多机器人探索同一局部区域，每个机器人通过自身的分层拓扑地图更新拓扑结构，并依据节点更新规则检查结构中所有节点，防止重复访问，以充分提高地图创建效率。

步骤四，提取SIFT特征并进行特征匹配。

本发明通过提取环境中的SIFT特征角点来描述未知环境，并将节点间的SIFT匹配度作为地图拼接的先验信息。本发明在拓扑节点提取环境中的SIFT特征点，提取与匹配的方法如下：

(1)获取高斯差分图像

SIFT特征角点提取首先采用高斯函数来建立尺度空间，高斯函数为：

$G(x,y,\mathrm{\σ})=\frac{1}{2{\mathrm{\π\σ}}^2}{e}^{-(x^2+y^2)/2{\mathrm{\σ}}^2}$

尺度空间的定义为：

L(x,y,σ)＝G(x,y,σ)*I(x,y)

式中，(x,y)表示图像点的坐标，I(x,y)为图像点(x,y)处的灰度值，σ表示尺度空间参数，随着σ的变化建立起不同的尺度空间，称之为图像金字塔。*代表卷积运算。

在尺度空间建立完毕后，为了能够找到稳定的关键点，采用高斯差分的方法来检测在局部位置的极值点，采用两个相邻尺度中的图像相减获取高斯差分图像(difference-of-Gaussian,DoG)D(x,y,σ)，即：

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)

＝L(x,y,kσ)-L(x,y,σ)

其中，k为比例系数；

(2)检测特征点

构建高斯差分图像完毕之后，为确保同时在尺度空间和二维图像空间检测局部极值，对于图像I中某点X＝(x,y)，将该点和它周围的8个像素点，以及与其相邻两层的尺度空间中对应位置的18个像素点进行比较，以确定该点是否为极大值或者极小值。

(3)特征点定位

为提高特征点的稳定性，需要对特征点进行过滤，去除边缘点和低对比度的点。

去除低对比度点：需要对DoG函数进行曲线拟合，利用DoG函数在尺度空间上的泰勒展开式：

$D\left(X\right)=D+{\frac{\∂D}{\∂X}}^{T}X+\frac{1}{2}{X}^T\frac{{\∂}^{2}D}{\∂X^2}X---\left(1\right)$

式中：D是DoG计算的结果，X＝(x,y,σ)是候选关键点之一。

求解上式得到修正值：

$\hat{X}=-\frac{\∂D^T}{\∂X}{\left(\frac{{\∂}^{2}D}{\∂X^2}\right)}^{-1}$

将带入泰勒展开式(1)中，若得到则该点是低对比度点，去除该点。

去除边缘点：DoG函数在图像边缘有较强的边缘响应，因此还需要排除边缘响应。需要计算该点的Hessian矩阵得到主曲率。Hessian矩阵H为：

$H=\left[\begin{array}{cc}{D}_{xx}& D_{xy}\\ D_{xy}& D_{yy}\end{array}\right]$

导数由采样点相邻差来估计得到，D的主曲率和H的特征值成正比，为了避免直接的计算这些特征值，而只是考虑它们的之间的比率。令α为最大特征值，β为最小的特征值，则矩阵H的迹Tr和行列式Det为：

Tr(H)＝D_xx+D_yy＝α+β

Det(H)＝D_xx×D_yy-D_xy×D_xy＝α×β

令α＝rβ，得：

$\frac{Tr{\left(H\right)}^2}{Det\left(H\right)}=\frac{{\left(\mathrm{\α}+\mathrm{\β}\right)}^2}{\mathrm{\α}\mathrm{\β}}=\frac{{(r+1)}^2}{r}$

(r+1)²/r在两特征值相等时，其值达最小，随r的增长而增长。检测主曲率，若在某阈值r下满足：则保留点(r取10)；反之去除该点。

(4)确定特征点方向

利用特征点邻域像素的梯度分布特征，可以为每个特征点指定参数方向，从而使特征点对图像旋转具有不变性。

梯度幅值： $m(x,y)=\sqrt{{\left(L\right(x+1,y)-L(x-1,y\left)\right)}^2+{\left(L\right(x,y+1)-L(x,y-1\left)\right)}^2}$

梯度方向： $\mathrm{\θ}(x,y)={\tan }^{-1}\[\frac{L(x,y+1)-L(x,y-1)}{L(x+1,y)-L(x-1,y)}\]$

式中，L所用尺度为每个特征点所在的尺度。实际计算中，在以特征点为中心的邻域窗口内采样，并用直方图统计邻域像素的梯度方向。

(5)生成特征点描述子

在得到邻域内点的主方向后，将特征点描述成特征向量，使关键点数值化。

为了确保旋转不变性，首先将坐标轴旋转为关键点的方向。以一个关键点为中心，取8×8的窗口，将该窗口切成2×2的子窗口，统计每个子窗口中的方向直方图。每一个子窗口的方向由其上4×4区域的方向来决定。每个关键点方向由2×2共4个种子点的方向决定，一个种子点有8个方向的信息，则每个关键点就有4×8＝32维。在实际计算过程中，为了增强匹配的稳健性，对每个关键点采用4×4共16个种子点来描述，这样一个关键点就有16×8＝128维的数据，即形成128维的SIFT特征向量，该向量具有旋转、尺度不变性，再进行归一化后，具有光照不变性。至此，得到了描述特征点的特征向量。

(6)进行特征匹配

采用基于欧氏距离的最近邻搜索方法，利用K-D树在待匹配的图像中搜索，找到与基准图像中的特征点欧式距离最近的前两个特征点，如果最近距离除以次近距离得到的值小于设定的比例阈值(0.7)，则接受这一对匹配点。

步骤五，基于ICP扫描匹配进行地图拼接。

本发明基于上述分层拓扑结构完成机器人地图创建，并对不同机器人在拓扑节点处提取的SIFT特征进行匹配，若匹配度满足阈值要求，则对当前不同机器人所生成的地图使用ICP扫描匹配的方法来实现地图拼接，无需考虑机器人相对位姿信息。拼接过程中，考虑ICP方法的匹配误差，提高地图拼接的准确度。具体方法如下：

(1)拓扑节点处提取SIFT特征

首先，每个机器人均采用前述改进方法完成拓扑地图的创建。本发明利用SIFT特征关联不同拓扑节点，每个机器人将获得SIFT特征向量集其中表示机器人i在拓扑节点j处提取的SIFT特征。为克服特征提取与匹配的实时性问题，本发明运用关键帧的思想，在地图创建过程中，视觉系统仅在拓扑节点处采集图像信息，并将图像的SIFT特征向量存储于当前节点信息当中，一定程度上提高了算法的实时性。探索过程中，当机器人i获得某一节点的信息后，将与其他机器人已生成进行匹配。若特征向量间的匹配程度满足阈值条件(阈值设定为0.7)，则表明两节点间具有很高的相似度。

(2)地图拼接

在节点特征匹配成功后，对两个机器人已生成的地图使用ICP算法进行点集匹配，来计算出点集间的旋转平移矩阵X，进而实现地图的拼接。

ICP算法的评估函数如下：

$\begin{array}{c}{E}_{dist}(\mathrm{\α},T)=\underset{R_{\mathrm{\α}},T,j\∈\{1,2,...,n\}}{\min }\left(\underset{i=1}{\overset{m}{\Σ}}\left|\right|\left(R_{\mathrm{\α}}{p}_i+T\right)-q_j|{|}_2^2\right)\\ s.t.R_{\mathrm{\α}}^T{R}_{\mathrm{\α}}=I_l,\det \left(R_{\mathrm{\α}}\right)=1\end{array}$

式中，R_α是旋转变换矩阵，α是旋转角，I_l为单位矩阵，T是平移向量，p_i和q_j分别为两点集中的数据点。即通过最小化误差和E_dist求两个点集间的相对坐标变换X＝(α，T)。本发明利用ICP算法来计算两个局部地图之间的相对坐标变换矩阵来更新机器人精确位姿，并实现地图拼接。

为降低SIFT特征的误匹配率，将ICP匹配误差作为反馈值来提高地图拼接的准确率，机器人i在当前位姿所生成的局部地图与全局地图的相似度P为：

$P\left(L_{X_{M_i}}\right(k)|L_{X_{ri}}\left(k\right),G_{X_{M_k}}\left(k\right))\≤\mathrm{\λ}$

若满足阈值λ要求，表明地图拼接成功；否则，重新进行SIFT匹配。

(3)位姿更新

完成拼接后，将此时机器人k的位姿机器人i在节点j处的位姿进行坐标转换，进而根据下式更新机器人i的位姿：

$p_k=\left[\begin{array}{cc}{\mathrm{cos\θ}}_j^{ik}& -{\mathrm{sin\θ}}_j^{ik}\\ {\mathrm{sin\θ}}_j^{ik}& {\mathrm{cos\θ}}_j^{ik}\end{array}\right]p_j^i+\left[\begin{array}{c}{x}_j^{ik}\\ y_j^{ik}\end{array}\right]$

$p_i=\left[\begin{array}{cc}{\mathrm{cos\θ}}_j^i& -{\mathrm{sin\θ}}_j^i\\ {\mathrm{sin\θ}}_j^i& {\mathrm{cos\θ}}_j^i\end{array}\right]p_j^i+\left[\begin{array}{c}{x}_j^i\\ y_j^i\end{array}\right]$

式中，是由激光采集的局部坐标点，p_i和p_k是机器人i和k的全局坐标点，为节点j处机器人k的位姿角，为节点j处机器人i的位姿角，相对坐标变换矩阵[R,T]由ICP算法求得。

下面给出应用本发明所述方法在室内真实环境下进行地图拼接的一个实验实例。

实验选用Pioneer3-DX作为机器人平台，机器人配备激光测距仪和相机。系统选用URG-04LX型激光测距仪。室内环境如图5所示，图5(a)为实验的真实场景，图5(b)为实验环境的二维几何示意图。两台机器人的线速度为0.5m/s，角速度为1rad/s，激光测距仪测量距离的范围为(0，5]m，夹角范围为[-π/2,π/2]。使用机器人配备相机获取环境中SIFT特征信息。两台机器人在相对位姿未知情况下，分别从不同起始位置探索未知环境，采用本文所提方法进行地图拼接实验。图6是地图拼接结果示意图。

由图6中的地图拼接结果可以看出，两台机器人各自生成的拓扑地图按本发明所述方法拼接后的地图真实的反映了实验环境，拼接结果与真实环境相一致，这表明，本发明所述方法可用于未知环境下多机器人系统地图拼接，具有较高的准确度。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于分层拓扑结构的多机器人系统地图拼接方法，其特征在于包括以下几个步骤：

步骤一，获取可通行空间树，方法如下：

(1)获取扇区

利用获取的激光数据信息，将距离机器人最近障碍物到激光最大量程的扫描区域划分为若干层，每层包含若干段弧线，弧线围成一个个扇区；

(2)判断可通行角度区间

假设存在一个矩形通道，设定阈值w大于机器人宽度，并将w作为矩形的宽；使该矩形以机器人当前位置为中心进行旋转，在每个扇区中寻找可通行角度区间；

(3)选取可能目标点

为确定机器人的前进方向和目标点，选取最外层扇区中的可见点，将其作为当前目标点则机器人的可能目标集合描述为：

$P^c=\left\{P_j^c\right\}$

式中，P^c是当前可能目标点集，是该点集中的第j个可能目标点；

步骤二，建立分层拓扑结构，方法如下：

在可通行空间树的基础上建立一种融合混合特征的分层拓扑地图结构；在分层拓扑结构H中维护两层地图：第一层地图是当前节点的可通行空间树的集合A；第二层是已生成拓扑节点的集合T_g，即拓扑层，分层拓扑结构H描述为：

H＝{A,T_g}

在拓扑节点处提取比例不变特征(SIFT)，并融合到分层拓扑地图结构当中；拓扑节点集合T_i可描述为：

T_i＝{s,A_i,p}

式中，s是T_i处所提取的SIFT特征向量，A_i为T_i处提取的可通行空间树的集合，p表示机器人当前位姿；

假设S为已生成的拓扑节点，其可能目标点集合为(Q₁,Q₂,...,Q_j,...,Q_N)；当前拓扑节点为T，可能目标点集合为(P₁,P₂,...,P_i,...,P_M)，其中N、M分别为S、T所含通路点个数；用d(P,Q)表示节点P与Q之间的距离；用C(P,Q)表示节点P与Q之间的连通关系，其值为TRUE，表示P与Q连通；其值为FALSE，则表示P与Q间存在障碍；S＝S/Q_j表示将Q_j从拓扑节点S中删除，S＝S∪Q_j表示将Q_j添加到拓扑节点S中；具体动态维护规则如下：

(1)拓扑节点T所属区域V_r内的可能目标点P_i对机器人探索的作用不大，删除P_i；即：

若d(P_i,T)≤V_r，则T＝T/P_i；

(2)P_i落在已生成的拓扑节点S所属区域V_r，且P_i与S间存在可通行路径，删除P_i；即：

若d(P_i,S)≤V_r，且C(P_i,S)＝TRUE，则T＝T/P_i；

(3)若相邻的通路点P_i、Q_j间不存在障碍物，且距离在阈值σ内，保留其中最有利于机器人探索的环境通路点；即：

在d(P_i,Q_j)≤σ的情况下，若d(P_i,T)≥d(Q_j,S)，则T＝T/P_i；否则S＝S/Q_j；

(4)机器人探索过程中，需要对某些满足条件的通路点重排列以保证有效的环境探索；即：

若d(Q_j,T)≤d(Q_j,S)，且C(Q_j,T)＝TRUE，则S＝S/Q_j，T＝T∪Q_j；

(5)若机器人运动到Q_j附近，且Q_j与T间不存在障碍物，表明Q_j所代表未知区域已被探索，直接删除Q_j；即：

若d(Q_j,T)≤V_r，且C(Q_j,T)＝TRUE，则S＝S/Q_j；

步骤三，创建全局拓扑地图，方法如下：

(1)探索开始时，首先获取激光传感器信息，根据激光数据对可能目标点进行搜索；

(2)在整个环境中均不存在尚未探索的可能目标点的情况下，若d(T,O)≤σ，探索终止；否则，机器人回溯到初始点；其中T为当前拓扑节点，O为初始点，σ为距离阈值；

(3)当前时刻提取的可能目标点可能会有多个，采用效用函数U(P)这一评价标准，计算各个通路点的效用值，并选取效用函数值最大者作为即时目标点，同时添加当前拓扑节点T并更新拓扑地图；U(P)的表达式为：

$U\left(P\right)=\frac{G\left(P\right)\×L\left(P\right)\×e^{-\mathrm{\Δ}\mathrm{\θ}\left(P\right)}}{D\left(P\right)}$

式中，P为候选可能目标点；G(P)表示P处机器人所能观测到的未知环境面积大小；D(P)为机器人当前位置与P的距离；L(P)为P处相邻障碍物之间机器人可通行路径的宽度；Δθ(P)为P所指示的未探索区域方向θ_P与当前机器人朝向角θ_R之间差值的绝对值；

(4)当前拓扑节点T不存在可能目标点的情况下，搜索是否存在未探索的拓扑节点，若存在，则机器人回溯到最近的尚未探索的拓扑节点S处；否则返回步骤三(2)；若该拓扑节点存在至少一个可能目标点，返回步骤三(3)；

若机器人当前不存在任何可能目标点，为完成对环境的遍历，机器人回溯到距离机器人最近且存在未探索可能目标点的拓扑节点处，并将此节点设置为回溯目标；并利用Bellman-Ford算法选择一条到达该目标的最短路径；Bellman-Ford算法描述为：

d_k(j)＝min{d_k-1(j),d_k-1(i)+d(i,j)}

式中，d_k(j)表示当前位置与第j个节点的距离，d(i,j)是第i个节点与第j个节点的距离；

回溯过程中，机器人将最远的可见节点T_v作为目标，若探索到新的可能点，机器人将沿此可能点方向继续遍历环境：

$d(p_r,p_{T_v})=\underset{r\≤k\≤v}{max}\left\{d(p_r,p_{T_k})\right\}$

式中，d(i,j)表示i、j两点之间的欧氏距离，p_r表示机器人当前位姿，表示拓扑节点v处位姿；

(5)重复步骤三(1)～(4)，不断提取新的目标点，通过拓扑节点的实时更新来完成全局拓扑地图的创建；当不存在任何可能节点时，系统将停止工作；

步骤四，提取SIFT特征并进行特征匹配，方法如下

(1)获取高斯差分图像

SIFT特征角点提取首先采用高斯函数来建立尺度空间，高斯函数为：

$G(x,y,\mathrm{\σ})=\frac{1}{2{\mathrm{\π\σ}}^2}{e}^{-(x^2+y^2)/2{\mathrm{\σ}}^2}$

尺度空间的定义为：

L(x,y,σ)＝G(x,y,σ)*I(x,y)

式中，(x,y)表示图像点的坐标，I(x,y)为图像点(x,y)处的灰度值，σ表示尺度空间参数，随着σ的变化建立起不同的尺度空间，称之为图像金字塔；*代表卷积运算；

采用两个相邻尺度中的图像相减获取高斯差分图像(difference-of-Gaussian,DoG)D(x,y,σ)，即：

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)

＝L(x,y,kσ)-L(x,y,σ)

其中，k为比例系数；

(2)检测特征点

对于图像I中某点X＝(x,y)，将该点和它周围的8个像素点，以及与其相邻两层的尺度空间中对应位置的18个像素点进行比较，以确定该点是否为极大值或者极小值；

(3)特征点定位

对特征点进行过滤，去除边缘点和低对比度的点；

去除低对比度点：需要对DoG函数进行曲线拟合，利用DoG函数在尺度空间上的泰勒展开式：

$D\left(X\right)=D+{\frac{\∂D}{\∂X}}^{T}X+\frac{1}{2}{X}^T\frac{{\∂}^{2}D}{\∂X^2}X---\left(1\right)$

式中：D是DoG计算的结果，X＝(x,y,σ)是候选关键点之一；

求解上式得到修正值：

$\hat{X}=-{\frac{\∂D}{\∂X}}^T{\left(\frac{{\∂}^{2}D}{\∂X^2}\right)}^{-1}$

将带入泰勒展开式(1)中，若得到则该点是低对比度点，去除该点；

去除边缘点：计算边缘点的Hessian矩阵H：

$H=\left[\begin{array}{cc}{D}_{xx}& D_{xy}\\ D_{xy}& D_{yy}\end{array}\right]$

由采样点相邻差来估计导数，D的主曲率和H的特征值成正比，为避免直接地计算这些特征值，只考虑它们的之间的比率；令α为最大特征值，β为最小的特征值，则矩阵H的迹Tr和行列式Det为：

Tr(H)＝D_xx+D_yy＝α+β

Det(H)＝D_xx×D_yy-D_xy×D_xy＝α×β

令α＝rβ，得：

$\frac{Tr{\left(H\right)}^2}{Det\left(H\right)}=\frac{{(\mathrm{\α}+\mathrm{\β})}^2}{\mathrm{\α}\mathrm{\β}}=\frac{{(r+1)}^2}{r}$

(r+1)²/r在两特征值相等时，其值最小，随r的增长而增长；检测主曲率，若在某阈值r下满足：则保留点，r取10；反之去除该点；

(4)确定特征点方向

用特征点邻域像素的梯度分布特征为每个特征点指定参数方向，从而使特征点对图像旋转具有不变性；

梯度幅值： $m(x,y)=\sqrt{{\left(L\right(x+1,y)-L(x-1,y\left)\right)}^2+{\left(L\right(x,y+1)-L(x,y-1\left)\right)}^2}$

梯度方向： $\mathrm{\θ}(x,y)={\tan }^{-1}\[\frac{L(x,y+1)-L(x,y-1)}{L(x+1,y)-L(x-1,y)}\]$

式中，L所用尺度为每个特征点所在的尺度；实际计算中，在以特征点为中心的邻域窗口内采样，并用直方图统计邻域像素的梯度方向；

(5)生成特征点描述子

为了确保旋转不变性，首先将坐标轴旋转为关键点的方向；在实际计算过程中，为了增强匹配的稳健性，对每个关键点采用4×4共16个种子点来描述，这样一个关键点就有16×8＝128维的数据，即形成128维的SIFT特征向量，该向量具有旋转、尺度不变性，再进行归一化后，具有光照不变性；至此，得到了描述特征点的特征向量；

(6)进行特征匹配

采用基于欧氏距离的最近邻搜索方法，利用K-D树在待匹配的图像中搜索，找到与基准图像中的特征点欧式距离最近的前两个特征点，如果最近距离除以次近距离得到的值小于设定的比例阈值，则接受这一对匹配点；

步骤五，基于ICP扫描匹配进行地图拼接，方法如下：

(1)拓扑节点处提取SIFT特征

每个机器人均采用前述改进方法完成拓扑地图的创建；利用SIFT特征关联不同拓扑节点，每个机器人将获得SIFT特征向量集 表示机器人i在拓扑节点j处提取的SIFT特征；为克服特征提取与匹配的实时性问题，运用关键帧的思想，在地图创建过程中，视觉系统仅在拓扑节点处采集图像信息，并将图像的SIFT特征向量存储于当前节点信息当中；探索过程中，当机器人i获得某一节点的信息后，将与其他机器人已生成进行匹配；若特征向量间的匹配程度满足阈值条件，则表明两节点间具有很高的相似度；

(2)进行地图拼接

在节点特征匹配成功后，对两个机器人已生成的地图使用ICP算法进行点集匹配，来计算出点集间的旋转平移矩阵X，进而实现地图的拼接；

ICP算法的评估函数如下：

$E_{dist}(\mathrm{\α},T)=\underset{R_{\mathrm{\α}},T,j\∈\{1,2,...,n\}}{min}\left(\underset{i=1}{\overset{m}{\Σ}}\right|\left|\right(R_{\mathrm{\α}}{p}_i+T)-q_j|{|}_2^2)$

$s.t.R_{\mathrm{\α}}^T{R}_{\mathrm{\α}}=I_l,\det \left(R_{\mathrm{\α}}\right)=1$

式中，R_α是旋转变换矩阵，α是旋转角，I_l为单位矩阵，T是平移向量，p_i和q_j分别为两点集中的数据点；即通过最小化误差和E_dist求两个点集间的相对坐标变换X＝(α，T)；利用ICP算法来计算两个局部地图之间的相对坐标变换矩阵来更新机器人精确位姿，并实现地图拼接；

为降低SIFT特征的误匹配率，将ICP匹配误差作为反馈值来提高地图拼接的准确率，机器人i在当前位姿所生成的局部地图与全局地图的相似度P为：

$P\left(L_{X_{M_i}}\right(k\left)\right|L_{X_{ri}}\left(k\right),G_{X_{M_k}}\left(k\right))\≤\mathrm{\λ}$

若满足阈值λ要求，表明地图拼接成功；否则，重新进行SIFT匹配；

(3)位姿更新

完成拼接后，将此时机器人k的位姿与机器人i在节点j处的位姿进行坐标转换，进而根据下式更新机器人i的位姿：

$p_k=\left[\begin{array}{cc}{\mathrm{cos\θ}}_j^{ik}& -{\mathrm{sin\θ}}_j^{ik}\\ {\mathrm{sin\θ}}_j^{ik}& {\mathrm{cos\θ}}_j^{ik}\end{array}\right]p_j^i+\left[\begin{array}{c}{x}_j^{ik}\\ y_j^{ik}\end{array}\right]$

$p_i=\left[\begin{array}{cc}{\mathrm{cos\θ}}_j^i& -{\mathrm{sin\θ}}_j^i\\ {\mathrm{sin\θ}}_j^i& {\mathrm{cos\θ}}_j^i\end{array}\right]p_j^i+\left[\begin{array}{c}{x}_j^i\\ y_j^i\end{array}\right]$

式中，是由激光采集的局部坐标点，p_i和p_k是机器人i和k的全局坐标点，为节点j处机器人k的位姿角，为节点j处机器人i的位姿角，相对坐标变换矩阵[R,T]由ICP算法求得。