CN107886129B - 一种基于视觉词袋的移动机器人地图闭环检测方法 - Google Patents

Abstract

本发明公开了一种基于视觉词袋的移动机器人地图闭环检测方法，该方法提出基于视觉字典的图像相似性检测算法，并将该算法作为闭环检测的前端，即通过图像相似性检测可判断候选闭环节点，然后采用时间约束和空间位置验证的方法进一步确定闭环节点；经过大量实验证明，本发明提出的闭环检测方法能够对各种不同的闭环进行准确的检测，且算法计算速度快，可适应SLAM对闭环检测部分较高的实时性要求。

Description

一种基于视觉词袋的移动机器人地图闭环检测方法

技术领域

本发明属于地图创建领域，特别涉及一种基于视觉词袋的移动机器人地图闭环检测方法。

背景技术

在同步定位与地图创建SLAM中，闭环检测是指根据传感器获得的信息判断当前时刻机器人是否处在之前访问过的某个区域，或者说机器人当前所处的位置是否在已经创建的地图中有了相应的描述。在基于图优化的SLAM方法中，闭环检测是非常关键的一个环节，正确的闭环检测有助于修正里程计误差，从而得到误差小、全局信息一致的地图，但错误的闭环检测反而会加大误差甚至会毁坏整个地图。

闭环检测不仅是未知环境中地图创建的一个重点，同时也是一个难点，主要体现在如下几个方面：

(1)SLAM一般运行在相似的场景中，而相似的场景不一定来自于同一场景，即产生了感知歧义，这种情况最终会导致错误的闭环检测结果。

(2)在进行闭环检测时，需要将当前观测数据与已处理且存储的信息作比较，从而确定它们是否处于相同场景下，而需要处理和存储的数据随着SLAM系统运行时间的增长而加大，因此，数据规模大也是闭环检测面临的一个难点问题。

(3)图优化的前提是建立完整的闭环约束，对闭环进行优化后可以减小累积误差，但是错误的闭环结果对最终的计算精度会产生很大影响，严重时甚至导致整个地图严重失真，由此可见，判断闭环检测准确性显得尤为重要。一般地，评价闭环检测方法的一个重要指标是准确率-召回率曲线，其中，准确性描述了正确检测出来的闭环数占总的检测个数的百分比；而召回率则描述了正确检测出来的闭环数占真实闭环数的百分比。实际上，SLAM要求闭环检测的正确率达到接近100％，同时具有较高的召回率，这样才能通过闭环约束对运动轨迹进行有效地矫正。

发明内容

本发明针对现有的SLAM对闭环检测较高的实时性要求，提出了一种基于视觉词袋的移动机器人地图闭环检测方法，通过图像相似性检测判断候选闭环节点，采用时间约束和空间位置验证的方法进一步确定闭环节点，能够对各种不同的闭环进行准确的检测，且算法计算速度快。

一种基于视觉词袋的移动机器人地图闭环检测方法，包括以下步骤：

步骤1：利用场景中的多幅图像中的ORB特征点，构建场景图像视觉字典模型；

步骤2：在移动机器人前进过程中，通过自身的摄像头实时采集当前场景图像序列；

步骤3：利用场景图像视觉字典模型，依次提取当前场景图像序列中每一帧图像对应的字典向量；

步骤4：基于字典向量间的相似性以及图像的相对运动估计，提取当前场景图像序列中的关键帧，并将提取关键帧按提取顺序进行存储；

若当前待判断图像满足以下公式，则判定为关键帧：

S≤S_kf∪R≥R_kf∪T≥T_kf

其中，S、R、t依次表示当前待判断图像与上一次获得的关键帧图像之间的视觉字典相似性、旋转分量、平移分量，S_kf表示第一相似性阈值，R_kf表示旋转分量阈值，T_kf表示平移分量阈值；

以第一帧待判断图像作为第一帧关键帧；

步骤5：基于最新提取的关键帧和关键帧存储序列中的关键帧，进行闭环检测；

步骤5.1：以关键帧存储序列中未被选取的第一帧关键帧作为待比较关键帧I_k；

步骤5.2：将最新提取关键帧I_t与待比较关键帧的时间间隔进行判断，若t-k≥ε_t，则进入步骤5.3，否则，判断所有关键帧是否被选取完，若是，则返回步骤4，否则，选取关键帧存储序列中未被选取的下一帧关键帧作为待比较关键帧，重复步骤5.2；

步骤5.3：若

则最新提取关键帧I_t作为候选闭环节点，同时进入步骤5.4，否则，选取关键帧存储序列中未被选取的下一帧关键帧作为待比较关键帧，重复步骤5.2；

表示I_t和I_k对应的视觉向量的相似性；

步骤5.4：若I_t和I_k之间的相对运动估计T_t,k对应的欧氏距离D_t,k不大于设定的距离阈值ε_d，则I_t和I_k形成闭环，否则，选取关键帧存储序列中未被选取的下一帧关键帧作为待比较关键帧，重复步骤5.2，直到关键帧存储序列中所有关键帧均被选取完毕，进入步骤3，对场景图像序列帧下一帧图像是否为关键真的判断；

其中，ε_t表示时间间隔阈值，ε_S表示第二相似性阈值，ε_d表示距离阈值。

进一步地，所述场景图像视觉字典模型的构建过程如下：

步骤A：设定字典树的分支因子k和树的层数L，k^L大于f﹒m；

其中，f为构建视觉词典的图像帧数和m为单幅图像中包含的最大的ORB特征点数量；

步骤B：对所有图像序列的ORB特征点集进行K均值聚类，得到分成k个分支，然后对每个分支再进行递归的K均值聚类，直到达到L层，进入步骤C；

步骤C：将每个分支的聚类中心作为字典树的节点，提取叶子节点并将其组合为向量，该向量即为视觉字典。

进一步地，所述K均值聚类中使用的聚类中心采用以下步骤获得：

1)从用于构建视觉字典的所有图像对应的ORB特征点数据集中随机选取某个点作为种子点；

2)对于数据样本中的每个点x，计算它与最近聚类中心之间的距离D(x)；

3)随机选取一个新的点作为聚类中心，样本中某点被选取作为聚类中心的概率正比于D(x)²；

4)重复步骤2)与步骤3)直到k个聚类中心被选取出来。

进一步地，所述图像的视觉字典向量为：

W(I)＝[ω₁(I),ω₂(I),…,ω_C(I)]

其中，wi表示图像I在视觉字典第i个叶子节点对应单词处的得分，C表示图像中单词总量；C_i表示在图像I中第i个叶子节点对应单词出现的次数；N为参与训练的图片总量；N_i表示在N中具有第i个叶子节点对应单词的图像总数。

进一步地，所述两幅图像对应的视觉字典向量间的相似性按照以下公式计算：

其中，v_i和v_j分别表示两幅图像对应的视觉字典向量。

有益效果

本发明提供了一种基于视觉词袋的移动机器人地图闭环检测方法，该方法提出基于视觉字典的图像相似性检测算法，并将该算法作为闭环检测的前端，即通过图像相似性检测可判断候选闭环节点，然后采用时间约束和空间位置验证的方法进一步确定闭环节点；经过大量实验证明，本发明提出的闭环检测方法能够对各种不同的闭环进行准确的检测，且算法计算速度快，可适应SLAM对闭环检测部分较高的实时性要求。

附图说明

图1为图像视觉字典的表示方法图；

图2为视觉字典的生成过程示意图；

图3为图像与视觉间所对应的逆向数据库示意图；

图4为空间位置的验证方法示意图；

图5为图像间相似性验证示意图，

图6为场景中单闭环路径以及多闭环路径进行闭环检测的两条路径对应的真实轨迹；

图7为闭环检测的准确率和召回率曲线；

图8为单闭环路径对应的闭环检测结果；

图9多闭环路径对应的闭环检测结果。

具体实施方式

下面将结合附图和实施例对本发明做进一步的说明

一种基于视觉词袋的移动机器人地图闭环检测方法，包括以下步骤：

步骤1：视觉字典模型的建立；

如图1所示，首先采用离线训练的方式对场景中多幅图像提取ORB特征点，然后训练成树状的视觉字典，而在SLAM运行过程中会实时地检测闭环，获得机器人当前观测到的图像后，通过查询已建立的视觉字典来获取对应的字典向量，该字典向量即对应了该图像所在位置处的场景描述。

在图像特征的分层量化过程中，词汇的生成是在对特征的分层K-Means聚类的基础上而建立的，如图2所示，视觉字典树生成过程主要包括图像特征提取、分层K均值聚类、字典生成三个过程。在图像特征提取方面，由于本发明选择ORB算子则作为特征提取算法，并且使用BRIEF作为特征描述算子，计算高效是其最大的优势，其优势从运动估计求解过程直接延续到闭环检测环节；特征的分层量化是指对待训练的特征集进行循环的K均值聚类，从而生成树状的数据存储结构。该环节执行过程如下：

(1)首先定义字典树的分支因子k和树的层数L。

(2)对原始特征集进行一次K均值聚类，从而将特征集分成k个分支，然后对每个分支再进行递归的K均值聚类，直到达到L层为止。

(3)将每个分支的聚类中心作为树的节点，最终提取叶子节点并将其组合为向量，该向量即为视觉字典。

由此可知，字典树节点个数的总数为(k^L+1-k)/(k-1)个，而视觉单词个数(即叶子结点)为k^L，因此具有一个若干层和若干分支的树能够轻易地产生上万个视觉单词，同样的计算复杂度，该方式对图像的表征能力远大于线性结构的模型，并且图像特征在查询视觉字典时，通过树形结构的搜索，每层只需执行k次线性比较，这种最近邻搜索方式明显优于平面字典结构的纯线性的搜索方式。因而该视觉字典树模型在视觉单词的表征能力和计算效率方面具有明显优势。

传统的K-Means算法存在两个明显的缺陷：

1)需要预先指定K值(即聚类中心的个数)但实际上数据集在处理之前很难确定应该分成几类；2)种子点是随机选取的，而初始点选择的不同对最终的聚类结果有很大影响；这两个缺陷有可能造成最终的聚类结果与数据的实际分布有很大差距。对于图像局部特征的聚类而言，在K值较小时，第一种缺陷对最终结果并无太大影响，而第二种缺陷会产生明显影响，为了解决这种情况带来的影响，本发明所述方法选择K-Means++算法替代传统的K-Means算法，该算法执行过程如下：

1)从数据集中随机选取某个点作为种子点；

2)对于数据样本中的每个点x，计算它与最近聚类中心之间的距离D(x)；

3)随机选取一个新的点作为聚类中心，样本中某点被选取作为聚类中心的概率正比于D(x)²；

4)重复步骤2)与步骤3)直到k个聚类中心被选取出来；

5)利用以上步骤选取的k个聚类中心执行标准的K-Means算法。

步骤2:关键帧提取；

本发明对获取的所有图像进行关键帧提取，使用关键帧表示场景信息。场景中某一区域可以用一帧图像表示，那么该帧可称为该区域的关键帧。

由此可见，关键帧与该区域中其它帧应该具有较多的相似性特征；相反，不同关键帧之间存在较少的相似性特征。

本发明采用基于视觉字典的相似性检测方法外加旋转、平移量信息来提取关键帧，即当前图像与前一帧关键帧之间满足以下任何一项则认为是关键帧：

1)相对运动估计中平移分量T达到设定的阈值；

2)相对运动估计中旋转分量R达到设定的阈值；

3)两幅图像间的相似性得分S小于设定的阈值；其数学描述如下：

S≤S_kf∪R≥R_kf∪T≥T_kf

其中，S_kf、R_kf、T_kf分别表示判断关键帧时设定的阈值。

基于字典模型的图像相似性度量是建立视觉字典后，为了求解图像间的相似性，对照视觉字典对图像进行查询，即对图像进行深度最近邻搜索，为每个特征寻找与之最近的单词，最终形成图像对应的视觉字典向量，最后采用相似性得分准则确定图像间的相似性。

本发明采用TF-IDF熵作为图像在该视觉单词处的得分权重。设图像I在视觉字典第i个叶子结点对应单词处的得分为：

其中，C表示图像中单词总量；C_i表示在图像I中第i个叶子节点对应单词出现的次数；N为参与训练的图片总量；N_i表示在N中具有第i个叶子节点对应单词的图像总数。则图像I在视觉字典中的得分向量(即对应的字典向量)为：

W(I)＝[ω₁(I),ω₂(I),…,ω_C(I)]

由此可知，某个单词在训练数据集中出现次数越少，它所对应的得分权重越大，同时说明该单词的表征能力越强。

在检测闭环时，获取当前帧的视觉字典向量后，就可以与之前已获取的观测信息所对应的字典向量进行比较，以求出它们之间的相似性。假设当前帧所对应的字典向量为v_i，场景中已获取的某帧图像所对应的字典向量为v_j，图像间的相似性度量标准如下：

由上式可知，图像间的相似性η(v_i,v_j)∈[0,1]，其值为1则表示两幅图像完全一样，其为0时表示两幅图像完全没有共同特征。

当从图像数据库中查询与当前图像相似的图像时，不同图像以及视觉字典向量中视觉单词分布的不同将会使得相似性得分的差别很大，为了在查询相似图像时使相似性得分更加规范化，图像间的相似性度量标准可改为如下形式：

其中，v_i、v_i-1以及v_j分别为数据库中第i、i-1以及j时刻获取的图像所对应的视觉字典向量。在图像数据库中，第i时刻获取的图像与第i-1时刻(i时刻的前一时刻)获取的图像之间具有最高的相似性，将之作为基准值，将使得所有图像在一个共同的标准下进行相似性度量，因而使得相似性得分具有参考意义。

由于SLAM要求系统能够实时检测闭环，因此在查询相似图像时，对数据查询效率要求的很高。鉴于此，这里对于所有已获取的图像在求取视觉向量后为之建立逆向索引数据库，以方便快速地进行数据查询，其结构如图3所示。

步骤3：获取当前图像帧I_c，查询视觉字典并求取其对应的视觉字典向量V_t，检测当前帧与已访问过的前一关键帧I_p之间的相似性

若

大于阈值S_kf，或当前帧与上一关键帧之间的旋转量R和平移量T中某一个大于阈值，即S≤S_kf∪R≥R_kf∪T≥T_kf，则将该帧图像设为关键帧I_t，否则转到下一帧继续判断。

(2)对于已访问过的关键帧{I₁,I₂…I_t-1}，取其中某帧I_k，当I_k与当前关键帧I_t之间获取的时间间隔大于阈值ε_t时，即t-k≥ε_t，则执行后续操作，否则继续按顺序从{I₁,I₂…I_t-1}中取关键帧进行时间约束的验证，当所有关键帧都比较完时转到步骤(1)。

(3)将符合时间约束的关键帧I_k与当前关键帧I_t进行相似性比较，若相似性得分

大于阈值ε_S，则将该关键帧作为候选闭环节点，否则转到步骤(2)。

(4)对于候选闭环节点，需要求取它与当前关键帧之间的相对运动估计T_t,k，当两帧间的欧式距离D_t,k小于闭环约束所要求的距离阈值ε_d时，即可确定该候选关键帧与当前关键帧之间形成闭环<I_t,I_k>，否则转到步骤(2)。

时间约束验证：

本发明在闭环检测过程中，设置一定的时间间隔限制，使得当前关键帧只跟那些与其获取时间具有一定时间间隔的关键帧进行闭环检测，设当前关键帧对应的获取时间为t_c，之前的某个关键帧的获取时间为t_p，则仅当t_c-t_p≥ε_t时，才对这两幅图像执行后续闭环检测流程(相似性检测和空间距离验证)。

空间距离验证：

本发明首先对提取的关键帧采用基于视觉词典方法求取相似性图像，将这些图像作为候选闭环节点，然后采用两幅图像间的空间位置关系作进一步验证，最终判断候选节点是否能够形成闭环。当确定当前关键帧I_t与之前已访问过的某个关键帧I_k为相似帧时，求解这两幅图像间的相对运动估计T_t,k，当且仅当两个节点间的距离在一定范围内时才确定为闭环节点，即D_t,k≤ε_d，其中ε_d为设定的距离阈值，如图4所示。

本发明采用某除冰实验场景作为实验场景，对提出的闭环检测方案进行验证。首先在离线的情况下对在该场景采集到的443帧图片进行训练形成视觉字典，其中字典树的分支因子k取3，层数L取8，然后分别对基于视觉字典模型的图像相似性检测以及上文所述的闭环检测方案进行实验验证。

图像相似性验证：

取实验场景中6帧连续的图像以及另外1帧来源于其他场景的图像进行相似性验证，其中图像间的相似性用

表示，其中i∈{2,3,4,5,6,7}，分别表示第一帧与其他帧之间的相似性，结果如图5所示；

从上面结果可以直观看出图像间的相似性随着图像间的重叠度的减少而减小，由此可见，视觉字典向量可以有效的对图像进行描述；另外，对比图①和图⑥可知，在图像间几乎无重叠区的情况下，仍然计算出了一定的相似性值；而对比图①和图⑦可知，这两幅完全不同的图像之间的相似性几乎为0，由此可见相同场景中的不同位置处的图像有时会存在一定的感知歧义，因此单纯采用基于外观相似的方式检测闭环是不可靠的。

闭环检测效果验证：

本发明闭环检测方案中需要设定多个重要的参数，其中关键帧选取过程中的参数可以根据实际需要进行设定，且对阈值选取的要求并不是十分严格，可根据计算机的处理速度的和存储量大小进行合理选择。但对于闭环检测中相似性阈值的选取而言就显得至关重要了，不同阈值会对后续优化结果产生很大的影响，因此，首先分别对场景中单闭环路径以及多闭环路径采用不同的相似性阈值进行闭环检测，以选取最佳的相似性阈值α。两条路径下所使用的采样频率约为30HZ，单闭环路径下共采集到320帧图片，多闭环路径下采集到430帧图片。这两条路径的真实轨迹如图6所示，将闭环检测结果与真实轨迹对比，可以得出闭环检测的正确率与召回率。实验中设定的时间间隔约束为10s，根据运动估计算法的实际性能将距离约束设定为0.3m，当相似性阈值α∈[0.5,0.8]时，检测结果所对应的准确率和召回率曲线如图7所示。由图7可知，综合单闭环和多闭环路径下闭环检测的结果，当二者的闭环检测的准确率均保持在100％时，可达到的最大召回率为62％，此时对应的相似性阈值α为6.5。正确的闭环有助于减小累积误差，而错误的闭环有可能会破坏整个位姿图，因此本发明将优先考虑闭环检测的准确性，即保持正确率为100％的情况下使召回率达到最高，本实验将该场景中的相似性阈值设为0.65，其闭环检测后对应的轨迹分别如图8和图9所示。图中“*”所示的点为关键帧所对应的位置。

现有的闭环检测的主要方法有基于距离的方法以及基于外观相似的方法等，为了对比本发明提出的闭环检测算法与这些常见算法的各项性能指标，分别就多单环场景下闭环检测的准确率、召回率以及闭环区域内的图像进行闭环检测所需的平均时间这三个指标进行对比，其中本发明提出的闭环检测算法所需时间包括生成字典向量、图像相似性检测以及几何验证几个部分所需时间之和，性能对比如表1所示。

表1各种闭环检测方法性能对比

综合上述实验结果可知，采用基于距离的方法具有较高的时间复杂度，不利于SLAM系统的实时运行；当采用外观相似的方法时，由于实验场景存在视觉混淆，从而具有一定的感知歧义，若不加以限制直接将该方法用于闭环检测将会产生错误的检测结果，而加入时间约束和空间位置验证后可以保证闭环检测的正确率达到100％；另一方面，本发明提出的闭环检测方法在保证结果正确率为100％的情况下仍然能获得较高的召回率(其中单闭环可达70％，多闭环可达62％)，不仅能够对简单的闭环进行高效的检测，同时也能够保证在多闭环复杂系统中有效运行。

实验证明，本发明提出的算法能够对各种不同的闭环进行准确的检测，且算法计算速度快，可适应SLAM对闭环检测部分较高的实时性要求。

Claims (5)

1.一种基于视觉词袋的移动机器人地图闭环检测方法，其特征在于，包括以下步骤：

步骤1：利用场景中的多幅图像中的ORB特征点，构建场景图像视觉字典模型；

步骤2：在移动机器人前进过程中，通过自身的摄像头实时采集当前场景图像序列；

步骤3：利用场景图像视觉字典模型，提取当前场景图像序列中当前图像帧I_c对应的字典向量；

步骤4：基于字典向量间的相似性以及图像的相对运动估计，提取当前场景图像序列中的关键帧I_t，并将提取关键帧按提取顺序进行存储，形成已访问过的关键帧存储序列{I₁,I₂…I_t-1}；

若当前待判断图像帧I_c满足以下公式，则判定为关键帧I_t：

S≤S_kf或R≥R_kf或T≥T_kf

其中，S、R、T依次表示当前待判断图像与上一次获得的关键帧图像之间的视觉字典相似性、旋转分量、平移分量，S_kf表示第一相似性阈值，R_kf表示旋转分量阈值，T_kf表示平移分量阈值；

以第一帧待判断图像作为第一帧关键帧；

步骤5：基于最新提取的关键帧和关键帧存储序列中的关键帧，进行闭环检测；

步骤5.1：以关键帧存储序列中未被选取的第一帧关键帧作为待比较关键帧I_k；

步骤5.2：将最新提取关键帧I_t与待比较关键帧的时间间隔进行判断，若t-k≥ε_t，则进入步骤5.3，否则，判断所有关键帧是否被选取完，若是，则返回步骤4，否则，选取关键帧存储序列中未被选取的下一帧关键帧作为待比较关键帧，重复步骤5.2；

步骤5.3：若

则最新提取关键帧I_t作为候选闭环节点，同时进入步骤5.4，否则，选取关键帧存储序列中未被选取的下一帧关键帧作为待比较关键帧，重复步骤5.2；

表示I_t和I_k对应的视觉向量的相似性；

步骤5.4：若I_t和I_k之间的相对运动估计T_t,k对应的欧氏距离D_t,k不大于设定的距离阈值ε_d，则I_t和I_k形成闭环，否则，选取关键帧存储序列中未被选取的下一帧关键帧作为待比较关键帧，重复步骤5.2，直到关键帧存储序列中所有关键帧均被选取完毕，提取场景图像序列帧下一帧图像作为当前图像帧，进入步骤3；

其中，ε_t表示时间间隔阈值，ε_S表示第二相似性阈值，ε_d表示距离阈值。

2.根据权利要求1所述的方法，其特征在于，所述场景图像视觉字典模型的构建过程如下：

步骤A：设定字典树的分支因子k和树的层数L，k^L大于f﹒m；

其中，f为构建视觉词典的图像帧数和m为单幅图像中包含的最大的ORB特征点数量；

步骤B：对所有图像序列的ORB特征点集进行K均值聚类，得到分成k个分支，然后对每个分支再进行递归的K均值聚类，直到达到L层，进入步骤C；

步骤C：将每个分支的聚类中心作为字典树的节点，提取叶子节点并将其组合为向量，该向量即为视觉字典。

3.根据权利要求2所述的方法，其特征在于，所述K均值聚类中使用的聚类中心采用以下步骤获得：

1)从用于构建视觉字典的所有图像对应的ORB特征点数据集中随机选取某个点作为种子点；

2)对于数据样本中的每个点x，计算它与最近聚类中心之间的距离D(x)；

3)随机选取一个新的点作为聚类中心，样本中某点被选取作为聚类中心的概率正比于D(x)²；

4)重复步骤2)与步骤3)直到k个聚类中心被选取出来。

4.根据权利要求1-3任一项所述的方法，其特征在于，所述图像的视觉字典向量为：

W(I)＝[ω₁(I),ω₂(I),…,ω_C(I)]

其中，wi表示图像I在视觉字典第i个叶子节点对应单词处的得分，C表示图像中单词总量；C_i表示在图像I中第i个叶子节点对应单词出现的次数；N为参与训练的图片总量；N_i表示在N中具有第i个叶子节点对应单词的图像总数。

5.根据权利要求4所述的方法，其特征在于，两幅图像对应的视觉字典向量间的相似性按照以下公式计算：

其中，v_i和v_j分别表示两幅图像对应的视觉字典向量。

Images