CN108256563A - 基于距离度量的视觉词典闭环检测方法与装置 - Google Patents

Abstract

本发明揭示了一种基于距离度量的视觉词典闭环检测方法与装置，其中，基于距离度量的视觉词典闭环检测方法包括：确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，所述历史帧图片为与所述当前帧图片的相似度达到第一预设阈值的帧图片；若所述最短空间距离小于第二预设阈值，则将所述历史帧图片纳入候选帧图片集合，所述第二预设阈值为预先获取到的所述当前帧与其邻近帧的不确定度的预设倍数；将所述候选帧图片与所述当前帧图片进行闭环检测。本发明结合最短空间距离约束视觉词典中相似图片的选择范围，以便找到合适的相似图片进行闭环检测，有效减少视觉歧义，提高闭环检测的准确性，增加机器人的定位精度。

Description

基于距离度量的视觉词典闭环检测方法与装置

技术领域

本发明涉及到机器人领域，特别是涉及到基于距离度量的视觉词典闭环检测方法与装置。

背景技术

同步定位与地图构建是移动机器人导航领域的基本问题与研究热点，是否具备同步定位与地图构建的能力被许多人认为是机器人是否能够实现自主导航的关键的前提条件。SLAM(Simultaneous Localization and Mapping，定位与地图构建)过程中机器人实现自定位的同时构建环境地图，由于先验知识的匮乏和环境的不确定性，机器人在行走的过程中需要判断当前位置是否处于已经访问过的环境区域，并以此作为判断是否需要环境更新的依据，即闭环检测问题。由于视觉传感器观测的范围有限，单目视觉SLAM闭环检测面临很多问题，包括机器人运动的不确定性和误差会导致数据关联错误、视觉特征如何检测、视觉场景模型如何表征等等。其中如何准确建立场景模型是视觉SLAM闭环检测的关键，目前大部分基于视觉的场景模型通过直接获得的环境外观特征来描述。BoVW(Bag of VisualWords)算法是一种有效的图像特征建模方法，被广泛用于视觉SLAM闭环检测。该方法首先用SURF(Speed-up robust features，加速健壮特征)或SIFT(Scale-invariant featuretransform，尺度不变特征转换)算子提取图像的局部特征，然后将特征进行分类构建视觉词典，基于创建的视觉词典，任何一幅图像都可以用视觉词典中的视觉单词集合进行表征。然而，由于视觉扫地机视野范围小，难免会出现不在同一地方拍摄到的两帧图像具有很相似的特征，这时单纯利用视觉词典匹配得到的闭环很可能代表错误的信息，引入错误的信息就会导致扫地机定位出现错误，进而影响整个系统的稳定性。

因此，现有技术还有待改进。

发明内容

本发明的主要目的为提供一种基于距离度量的视觉词典闭环检测方法，旨在解决单纯利用视觉词典匹配的闭环信息不准确而导致扫地机定位错误的技术问题。

本发明第一方面提出一种基于距离度量的视觉词典闭环检测方法，包括：

确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，所述历史帧图片为与所述当前帧图片的相似度达到第一预设阈值的帧图片；

若所述最短空间距离小于第二预设阈值，则将所述历史帧图片纳入候选帧图片集合，所述第二预设阈值为预先获取到的所述当前帧与其邻近帧的不确定度的预设倍数；

将上述候选帧图片与上述当前帧图片进行闭环检测。

优选地，上述确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离的步骤，包括：

根据移动过程中形成的位姿空间约束关系确定上述第一位姿与上述第二位姿之间的不确定性；

根据上述不确定性通过Dijkstra最短路径算法寻找上述第一位姿与上述第二位姿之间的最短距离；

将上述最短距离设定为上述最短空间距离。

优选地，上述根据移动过程中形成的位姿空间约束关系确定上述第一位姿与上述第二位姿之间的不确定性的步骤，包括：

计算上述第一位姿与上述第二位姿之间的协方差矩阵；

根据预设标量值度量上述协方差矩阵对应的不确定性。

优选地，上述根据上述不确定性通过Dijkstra最短路径算法寻找上述第一位姿与上述第二位姿之间的最短距离的步骤，包括：

标记第一位姿为第一源节点，并将第一源节点纳入距离最小点集合，上述位姿空间约束关系中剩余节点为第一未标记状态节点；

分别计算与上述第一源节点直接连接的各上述第一未标记状态节点到上述第一源节点的第一类距离；

根据上述第一类距离选择距离最小的第一距离对应的第一节点，并将上述第一节点放入上述距离最小点集合；

标记上述第一节点为第二源节点，标记上述距离最小点集合之外的节点为第二未标记状态节点；

分别计算与上述第二源节点直接连接的各上述第二未标记状态节点到上述第二源节点的第二类距离；

根据上述第二类距离选择距离最小的第二距离对应的第二节点，并将上述第二节点放入上述距离最小点集合，直至上述位姿空间约束关系中所有节点均位于上述上述距离最小点集合中；

根据将节点纳入上述距离最小点集合的次序，形成上述第一位姿与上述第二位姿之间的最短路径，以上述最短路径的距离为上述第一位姿与上述第二位姿之间的最短距离。

优选地，上述确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离的步骤之前，包括：

根据上述当前帧图片的视觉词袋特征，筛选与上述当前帧图片具有指定数量范围的匹配层点对数的相似图片作为上述历史帧图片。

优选地，上述根据上述当前帧图片的视觉词袋特征，筛选与上述当前帧图片具有指定数量范围的匹配层点对数的相似图片作为上述历史帧图片的步骤，包括：

获取上述当前帧图片的第一视觉词袋特征和各图片的第二视觉词袋特征；

判断上述第一视觉词袋特征与上述第二视觉词袋特征的层点对数的匹配量是否大于预设值；

若大于，则判定该上述第二视觉词袋特征所对应的图片为上述相似图片。

本发明第二方面提供了一种基于距离度量的视觉词典闭环检测装置，该装置被配置实现上述第一方面或第一方面任一可选的实现方式所提供的方法的功能，由软件实现，其软件包括与上述功能相应的模块，例如，确定模块、纳入模块以及进行模块，其中：

确定模块，用于确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，上述历史帧图片为与上述当前帧图片的相似度达到第一预设阈值的帧图片；

纳入模块，用于若所述最短空间距离小于第二预设阈值，则将所述历史帧图片纳入候选帧图片集合，所述第二预设阈值为预先获取到的所述当前帧与其邻近帧的不确定度的预设倍数；

进行模块，用于将上述候选帧图片与上述当前帧图片进行闭环检测。

本发明有益技术效果：本发明结合最短空间距离约束视觉词典中相似图片的选择范围，并通过视觉词袋特征，找到合适的相似图片，以便进行闭环检测，能有效减少视觉歧义的现象，提高闭环检测的准确性，增加机器人的定位精度。

附图说明

图1本发明一实施例的基于距离度量的视觉词典闭环检测方法流程示意图；

图2本发明一实施例步骤S1的流程示意图；

图3本发明一实施例步骤S10的流程示意图；

图4本发明一实施例步骤S11的流程示意图；

图5本发明一实施例步骤S4的流程示意图；

图6本发明一实施例的机器人运行路径中的约束关系转换为路径树状图的示意图；

图7本发明一实施例寻找最短路径的节点约束关系示意图；

图8本发明一实施例的基于距离度量的视觉词典闭环检测装置的结构示意图；

图9本发明一实施例确定模块的结构示意图；

图10本发明一实施例确定单元的结构示意图；

图11本发明一实施例寻找单元的结构示意图；

图12本发明一实施例的基于距离度量的视觉词典闭环检测装置的优化结构示意图；

图13本发明一实施例筛选模块的结构示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参照图1，本发明一实施例的基于距离度量的视觉词典闭环检测方法，包括：

S1：确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，上述历史帧图片为与上述当前帧图片的相似度达到第一预设阈值的帧图片。

本步骤中的空间距离不仅包括相距位置的欧氏距离，还包括拍摄角度的相差量。本实施例的位姿指机器人所处的位置和姿态；历史帧图片指机器人历史时刻拍摄到的图片中满足与当前帧图片相似度在第一预设阈值的帧图片。

S2:若上述最短空间距离小于第二预设阈值，则将上述历史帧图片纳入候选帧图片集合，上述第二预设阈值为预先获取到的上述当前帧与其邻近帧的不确定度的预设倍数。

本步骤中的最短空间距离范围设置为小于当前帧与邻近帧的不确定度的3倍值。比如：历史帧图片为与当前帧相似的3个帧图片，且相应的最短空间距离为：S1、S2以及S3，那么首先需要分别比较S1、S2以及S3与当前帧与邻近帧的不确定度的预设倍数(如3倍)之间的大小关系，如果S1小于当前帧与邻近帧的不确定度的预设倍数，则将S1范围内的历史帧图片纳入候选帧图片，同理，S2、S3与S1类似。本发明实施例在选定相似图片的基础上，进一步利用最短空间距离约束视觉词典中相似图片的选择范围，只选择距离当前时刻的位置最近的相似图片，作为闭环候选帧图片。

S3:将上述候选帧图片与上述当前帧图片进行闭环检测。

本实施例将距离度量的最短空间距离结合在视觉词典闭环检测中，能有效减少视觉歧义的现象，提高闭环检测的准确性，增加机器人的定位精度。

参照图2，本发明一实施例中，步骤S1，包括：

S10：根据移动过程中形成的位姿空间约束关系确定上述第一位姿与上述第二位姿之间的不确定性。

如附图6所示，图(a)表示机器人运行的一段路径，节点表示机器人的姿态数据，实线和虚线连接的边都表示节点之间的约束关系，其中连续节点之间边的约束为里程计约束，不连续节点边的约束为闭环约束，两个节点之间的约束关系为位姿的不确定性。比如，以节点4作为源节点，与其他节点形成了约束关系图，虚线表示两个节点之间当前路径的不确定度大于其他路径，例如由于节点4-7的姿态不确定度大于节点7-6-4的，所以4-7之间边的连接以虚线表示。

S11：根据上述不确定性通过Dijkstra最短路径算法寻找上述第一位姿与上述第二位姿之间的最短距离。

本步骤通过Dijkstra算法，计算位姿节点间的最短路径。首先以第一位姿对应的节点为中心向外层层扩展，直到扩展到第二位姿所对应的节点为止，每次扩展一个距离最短的点，更新与其相邻点的距离，当所有边权值都为正时，由于不会存在一个距离更短的未扩展过的点，所以保证了最短距离的正确性，以进一步提高筛选有效相似图片的准确度。

S12：将上述最短距离设定为上述最短空间距离。

参照图3，本发明一实施例中，步骤S10，包括：

S101：计算上述第一位姿与上述第二位姿之间的协方差矩阵。

本实施例用位姿之间的协方差矩阵来描述空间约束关系的不确定性。

S102：根据预设标量值度量上述协方差矩阵对应的不确定性。

在三维坐标系统中，每一个节点表示的姿态有六个自由度，可以描述为代表位置的平移变量x,y,z，和代表方向的角度变量φ,θ,ψ，则节点x_i可表示为：

节点x_i处的期望和方差可表示为

将所有节点位姿用状态向量表示为则状态向量均值和协方差可表示如下：

这里C(x_j,x_i)＝C(x_i,x_j)^T。

由(1.3)可得，两个节点姿态之间的协方差矩阵可表示如下：

这里选用一个标量值ρ来度量不确定度，两个节点姿态x_i，x_j之间的不确定度可表示为：

ρ_ij＝det(C^-1(x_ij)) (1.5)

若存在三个相邻的节点，为x_j,x_i,x_k，定义复合运算为：

则x_j,x_k之间的协方差矩阵可表示为

同(1.5)可得，其不确定度为：

ρ_jk＝det(C^-1(x_jk)) (1.8)

其中R1表示节点x_j的旋转矩阵：

K₁,K₂,M值分别为：

参照图4，本发明一实施例中，步骤S11，包括：

S111：标记第一位姿为第一源节点，并将第一源节点纳入距离最小点集合，上述位姿空间约束关系中剩余节点为第一未标记状态节点。

S112：分别计算与上述第一源节点直接连接的各上述第一未标记状态节点到上述第一源节点的第一类距离。

S113：根据上述第一类距离选择距离最小的第一距离对应的第一节点，并将上述第一节点放入上述距离最小点集合。

S114：标记上述第一节点为第二源节点，标记上述距离最小点集合之外的节点为第二未标记状态节点。

S115：分别计算与上述第二源节点直接连接的各上述第二未标记状态节点到上述第二源节点的第二类距离。

S116：根据上述第二类距离选择距离最小的第二距离对应的第二节点，并将上述第二节点放入上述距离最小点集合，直至上述位姿空间约束关系中所有节点均位于上述上述距离最小点集合中。

S117：根据将节点纳入上述距离最小点集合的次序，形成上述第一位姿与上述第二位姿之间的最短路径，以上述最短路径的距离为上述第一位姿与上述第二位姿之间的最短距离。

本实施例用位姿图表示机器人的运动轨迹，位姿图中的节点表示机器人的位姿，节点之间的边表示位姿之间的空间约束关系。节点之间的约束关系分为里程计约束和闭环约束，里程计约束代表机器人不同时刻之间的相对位置关系，闭环约束代表检测到闭环后产生的连接关系。节点之间边的存在会使位姿图中存在很多封闭的环状结构，导致从一个节点到另一个节点之间的路径有很多条。当所建的图越大时，节点数目也会越来越多，边的约束关系也会越来越多。为了找到一个节点到另一个节点的最佳路径，本实施例通过Dijkstra(狄克斯特拉算法)最短路径算法来寻找两个节点之间的最短距离，以便更精准地确定两节点之间的最短距离，以进一步提高寻找合适相似图片的匹配准确度。步骤如下：

如附图7所示，首先设第一位姿为第一源节点A，求第一源节点到其他未标定的各节点(比如B、C、D、E、F)的最短路径。线上所标注为相邻线段之间的距离，即权值(位姿不确定性)。分为距离最小点集合S和剩余节点集合U，初始状态S＝{A}，U＝{B,C,D,E,F}，以节点A为源节点，从节点A开始找；发现A→B＝6,A→C＝3,则节点A到节点C距离为最短，将节点C加入距离最小点集合S中；然后以节点C为第二源节点，以A→C路径开始找，A→C→B＝5，A→C→D＝6，A→C→E＝7,发现A→C→B＝5距离最短，则将节点B加入集合S中；如此操作直到找到最终节点，即第二位姿所对应的节点，以便确定第一位姿与上述第二位姿之间的最短路径的距离。

进一步地，本实施例中，步骤S1之前，包括：

S4：根据上述当前帧图片的视觉词袋特征，筛选与上述当前帧图片具有指定数量范围的匹配层点对数的相似图片作为上述历史帧图片。

本实施例通过在所有图片集合中结合视觉词袋特征，以找到合适的相似图片，以便进行闭环检测。本实施例的视觉词袋特征是指，根据数据集选取特征，然后进行描述以形成特征数据，如检测图片中的Sift Keypoints,然后计算Keypoints Descriptors,生成128-D的特征向量；利用处理好的特征数据全部合并，再用聚类的方法把特征词分为若干类，每一个类相当于一个视觉词；利用视觉词袋量化图片特征，每一张图片由很多视觉词汇组成，利用统计的词频直方图，来表示图片属于哪一类。闭环检测是为了判断机器人是否回到之前访问过的场景。闭环检测需要将当前帧与过去帧进行匹配，如果当前的观测值与过去某一观测值有非常大的相似性，比如与上述当前帧图片具有指定数量范围的匹配层点对数，则认为有非常大的相似性，就可以计算观测值之间的转换关系以加入新的约束，以便逐渐减少地图的累积误差。

参照图5，本发明一实施例中，步骤S4，包括：

S40：获取上述当前帧图片的第一视觉词袋特征和各图片的第二视觉词袋特征。

本步骤通过机器人前进过程中利用自身携带的单目或双目摄像机获取当前帧图片的第一视觉词袋特征和各图片的第二视觉词袋特征。

S41：判断上述第一视觉词袋特征与上述第二视觉词袋特征的层点对数的匹配量是否大于预设值。

层点对数匹配量越多，两帧图片越相似，比如，预设值为层点对数为100。

S42：若大于，则判定该上述第二视觉词袋特征所对应的图片为上述相似图片。

参照图8，本发明一实施例的基于距离度量的视觉词典闭环检测装置，包括：

确定模块1，用于确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，上述历史帧图片为与上述当前帧图片的相似度达到第一预设阈值的帧图片。

本实施例中的空间距离不仅包括相距位置的欧氏距离，还包括拍摄角度的相差量。本实施例的位姿指机器人所处的位置和姿态；历史帧图片指机器人历史时刻拍摄到的图片中满足与当前帧图片相似度在第一预设阈值的帧图片。

纳入模块2，用于若上述最短空间距离小于第二预设阈值，则将上述历史帧图片纳入候选帧图片集合，上述第二预设阈值为预先获取到的上述当前帧与其邻近帧的不确定度的预设倍数。

本实施例中的最短空间距离范围设置为小于当前帧与邻近帧的不确定度的3倍值。比如：历史帧图片为与当前帧相似的3个帧图片，且相应的最短空间距离为：S1、S2以及S3，那么首先需要分别比较S1、S2以及S3与当前帧与邻近帧的不确定度的预设倍数(如3倍)之间的大小关系，如果S1小于当前帧与邻近帧的不确定度的预设倍数，则将S1范围内的历史帧图片纳入候选帧图片，同理，S2、S3与S1类似。本发明实施例在选定相似图片的基础上，进一步利用最短空间距离约束视觉词典中相似图片的选择范围，只选择距离当前时刻的位置最近的相似图片，作为闭环候选帧图片。

进行模块3，用于将上述候选帧图片与上述当前帧图片进行闭环检测。

本实施例将距离度量的最短空间距离结合在视觉词典闭环检测中，能有效减少视觉歧义的现象，提高闭环检测的准确性，增加机器人的定位精度。

参照图9，本发明一实施例中，确定模块1，包括：

确定单元10，用于根据移动过程中形成的位姿空间约束关系确定上述第一位姿与上述第二位姿之间的不确定性。

如附图6所示，图(a)表示机器人运行的一段路径，节点表示机器人的姿态数据，实线和虚线连接的边都表示节点之间的约束关系，其中连续节点之间边的约束为里程计约束，不连续节点边的约束为闭环约束，两个节点之间的约束关系为位姿的不确定性。比如，以节点4作为源节点，与其他节点形成了约束关系图，虚线表示两个节点之间当前路径的不确定度大于其他路径，例如由于节点4-7的姿态不确定度大于节点7-6-4的，所以4-7之间边的连接以虚线表示。

寻找单元11，用于根据上述不确定性通过Dijkstra最短路径算法寻找上述第一位姿与上述第二位姿之间的最短距离。

本实施例通过寻找单元11执行Dijkstra算法，寻找位姿节点间的最短路径。首先以第一位姿对应的节点为中心向外层层扩展，直到扩展到第二位姿所对应的节点为止，每次扩展一个距离最短的点，更新与其相邻点的距离，当所有边权值都为正时，由于不会存在一个距离更短的未扩展过的点，所以保证了最短距离的正确性，以进一步提高筛选有效相似图片的准确度。

设定单元12，用于将上述最短距离设定为上述最短空间距离。

参照图10，本发明一实施例中，确定单元10，包括：

计算子单元101，用于计算上述第一位姿与上述第二位姿之间的协方差矩阵。

本实施例通过计算子单元101计算位姿之间的协方差矩阵来描述空间约束关系的不确定性。

度量子单元102，用于根据预设标量值度量上述协方差矩阵对应的不确定性。

具体实现手段在前述方法实施例中已有说明，此处不再赘述。

参照图11，本发明一实施例中，寻找单元11，包括：

第一标记子单元111，用于标记第一位姿为第一源节点，并将第一源节点纳入距离最小点集合，上述位姿空间约束关系中剩余节点为第一未标记状态节点。

第一计算子单元112，用于分别计算与上述第一源节点直接连接的各上述第一未标记状态节点到上述第一源节点的第一类距离。

第一选择子单元113，用于根据上述第一类距离选择距离最小的第一距离对应的第一节点，并将上述第一节点放入上述距离最小点集合。

第二标记子单元114，用于标记上述第一节点为第二源节点，标记上述距离最小点集合之外的节点为第二未标记状态节点。

第二计算子单元115，用于分别计算与上述第二源节点直接连接的各上述第二未标记状态节点到上述第二源节点的第二类距离。

第二选择子单元116，用于根据上述第二类距离选择距离最小的第二距离对应的第二节点，并将上述第二节点放入上述距离最小点集合，直至上述位姿空间约束关系中所有节点均位于上述上述距离最小点集合中。

形成子单元117，用于根据将节点纳入上述距离最小点集合的次序，形成上述第一位姿与上述第二位姿之间的最短路径，以上述最短路径的距离为上述第一位姿与上述第二位姿之间的最短距离。

本实施例用位姿图表示机器人的运动轨迹，位姿图中的节点表示机器人的位姿，节点之间的边表示位姿之间的空间约束关系。节点之间的约束关系分为里程计约束和闭环约束，里程计约束代表机器人不同时刻之间的相对位置关系，闭环约束代表检测到闭环后产生的连接关系。节点之间边的存在会使位姿图中存在很多封闭的环状结构，导致从一个节点到另一个节点之间的路径有很多条。当所建的图越大时，节点数目也会越来越多，边的约束关系也会越来越多。为了找到一个节点到另一个节点的最佳路径，本实施例通过Dijkstra最短路径算法来寻找两个节点之间的最短距离，以便更精准地确定两节点之间的最短距离，以进一步提高寻找合适相似图片的匹配准确度。步骤如下：

如附图7所示，首先通过第一标记子单元111标记第一位姿为第一源节点A，通过第一计算子单元112分别计算第一源节点A到其他未标定的各节点(比如B、C、D、E、F)的距离，并确定最短路径。线上所标注为相邻线段之间的距离，即权值(位姿不确定性)。本实施例将节点集合分为距离最小点集合S和剩余节点集合U，初始状态S＝{A}，U＝{B,C,D,E,F}，以节点A为源节点，从节点A开始找；发现A→B＝6,A→C＝3,则节点A到节点C距离为最短，通过第一选择子单元113将节点C加入距离最小点集合S中；然后通过第二标记子单元114标记节点C为第二源节点，以A→C路径开始找，通过第二计算子单元115分别计算得到：A→C→B＝5，A→C→D＝6，A→C→E＝7,发现A→C→B＝5距离最短，则通过第二选择子单元116将节点B加入集合S中；如此操作直到找到最终节点，即第二位姿所对应的节点，以便通过形成子单元117确定第一位姿与上述第二位姿之间的最短路径的距离。

参照图12，进一步地，本发明一实施例中，包括：

筛选模块4，用于根据上述当前帧图片的视觉词袋特征，筛选与上述当前帧图片具有指定数量范围的匹配层点对数的相似图片作为上述历史帧图片。

本实施例通过筛选模块4在所有图片集合中结合视觉词袋特征，以找到合适的相似图片，以便进行闭环检测。本实施例的视觉词袋特征是指，根据数据集选取特征，然后进行描述以形成特征数据，如检测图片中的Sift Keypoints,然后计算KeypointsDescriptors,生成128-D的特征向量；利用处理好的特征数据全部合并，再用聚类的方法把特征词分为若干类，每一个类相当于一个视觉词；利用视觉词袋量化图片特征，每一张图片由很多视觉词汇组成，利用统计的词频直方图，来表示图片属于哪一类。闭环检测是为了判断机器人是否回到之前访问过的场景。闭环检测需要将当前帧与过去帧进行匹配，如果当前的观测值与过去某一观测值有非常大的相似性，比如与上述当前帧图片具有指定数量范围的匹配层点对数，则认为有非常大的相似性，就可以计算观测值之间的转换关系以加入新的约束，以便逐渐减少地图的累积误差。

参照图13，本发明一实施例中，筛选模块4，包括：

获取单元40，用于获取上述当前帧图片的第一视觉词袋特征和各图片的第二视觉词袋特征。

本实施例通过机器人前进过程中利用获取单元40携带的单目或双目摄像机获取当前帧图片的第一视觉词袋特征和各图片的第二视觉词袋特征。

判断单元41，用于判断上述第一视觉词袋特征与上述第二视觉词袋特征的层点对数的匹配量是否大于预设值。

层点对数匹配量越多，两帧图片越相似，比如，预设值为层点对数为100。

判定单元42，用于若大于，则判定该上述第二视觉词袋特征所对应的候选帧图片为上述相似图片。

本发明实施例结合最短空间距离约束视觉词典中相似图片的选择范围，只选择距离当前时刻位置最近的相似图片作为闭环候选帧图片，并结合视觉词袋特征，找到合适的相似图片，以便进行闭环检测，能有效减少视觉歧义的现象，提高闭环检测的准确性，增加机器人的定位精度。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种基于距离度量的视觉词典闭环检测方法，其特征在于，包括：

确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，所述历史帧图片为与所述当前帧图片的相似度达到第一预设阈值的帧图片；

若所述最短空间距离小于第二预设阈值，则将所述历史帧图片纳入候选帧图片集合，所述第二预设阈值为预先获取到的所述当前帧与其邻近帧的不确定度的预设倍数；

将所述候选帧图片与所述当前帧图片进行闭环检测。

2.根据权利要求1所述的基于距离度量的视觉词典闭环检测方法，其特征在于，所述确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离的步骤，包括：

根据移动过程中形成的位姿空间约束关系确定所述第一位姿与所述第二位姿之间的不确定性；

根据所述不确定性通过Di jkstra最短路径算法寻找所述第一位姿与所述第二位姿之间的最短距离；

将所述最短距离设定为所述最短空间距离。

3.根据权利要求2所述的基于距离度量的视觉词典闭环检测方法，其特征在于，所述根据移动过程中形成的位姿空间约束关系确定所述第一位姿与所述第二位姿之间的不确定性的步骤，包括：

计算所述第一位姿与所述第二位姿之间的协方差矩阵；

根据预设标量值度量所述协方差矩阵对应的不确定性。

4.根据权利要求2所述的基于距离度量的视觉词典闭环检测方法，其特征在于，所述根据所述不确定性通过D i jkstra最短路径算法寻找所述第一位姿与所述第二位姿之间的最短距离的步骤，包括：

标记第一位姿为第一源节点，并将第一源节点纳入距离最小点集合，所述位姿空间约束关系中剩余节点为第一未标记状态节点；

分别计算与所述第一源节点直接连接的各所述第一未标记状态节点到所述第一源节点的第一类距离；

根据所述第一类距离选择距离最小的第一距离对应的第一节点，并将所述第一节点放入所述距离最小点集合；

标记所述第一节点为第二源节点，标记所述距离最小点集合之外的节点为第二未标记状态节点；

分别计算与所述第二源节点直接连接的各所述第二未标记状态节点到所述第二源节点的第二类距离；

根据所述第二类距离选择距离最小的第二距离对应的第二节点，并将所述第二节点放入所述距离最小点集合，直至所述位姿空间约束关系中所有节点均位于所述所述距离最小点集合中；

根据将节点纳入所述距离最小点集合的次序，形成所述第一位姿与所述第二位姿之间的最短路径，以所述最短路径的距离为所述第一位姿与所述第二位姿之间的最短距离。

5.根据权利要求1所述的基于距离度量的视觉词典闭环检测方法，其特征在于，所述确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离的步骤之前，包括：

根据所述当前帧图片的视觉词袋特征，筛选与所述当前帧图片具有指定数量范围的匹配层点对数的相似图片作为所述历史帧图片。

6.一种基于距离度量的视觉词典闭环检测装置，其特征在于，包括：

确定模块，用于确定当前帧图片对应的第一位姿与历史帧图片对应的第二位姿之间的最短空间距离，所述历史帧图片为与所述当前帧图片的相似度达到第一预设阈值的帧图片；

纳入模块，用于若所述最短空间距离小于第二预设阈值，则将所述历史帧图片纳入候选帧图片集合，所述第二预设阈值为预先获取到的所述当前帧与其邻近帧的不确定度的预设倍数；

进行模块，用于将所述候选帧图片与所述当前帧图片进行闭环检测。

7.根据权利要求6所述的基于距离度量的视觉词典闭环检测装置，其特征在于，所述确定模块，包括：

确定单元，用于根据移动过程中形成的位姿空间约束关系确定所述第一位姿与所述第二位姿之间的不确定性；

寻找单元，用于根据所述不确定性通过Di jkstra最短路径算法寻找所述第一位姿与所述第二位姿之间的最短距离；

设定单元，用于将所述最短距离设定为所述最短空间距离。

8.根据权利要求7所述的基于距离度量的视觉词典闭环检测装置，其特征在于，所述确定单元，包括：

计算子单元，用于计算所述第一位姿与所述第二位姿之间的协方差矩阵；

度量子单元，用于根据预设标量值度量所述协方差矩阵对应的不确定性。

9.根据权利要求7所述的基于距离度量的视觉词典闭环检测装置，其特征在于，所述寻找单元，包括：

第一标记子单元，用于标记第一位姿为第一源节点，并将第一源节点纳入距离最小点集合，所述位姿空间约束关系中剩余节点为第一未标记状态节点；

第一计算子单元，用于分别计算与所述第一源节点直接连接的各所述第一未标记状态节点到所述第一源节点的第一类距离；

第一选择子单元，用于根据所述第一类距离选择距离最小的第一距离对应的第一节点，并将所述第一节点放入所述距离最小点集合；

第二标记子单元，用于标记所述第一节点为第二源节点，标记所述距离最小点集合之外的节点为第二未标记状态节点；

第二计算子单元，用于分别计算与所述第二源节点直接连接的各所述第二未标记状态节点到所述第二源节点的第二类距离；

第二选择子单元，用于根据所述第二类距离选择距离最小的第二距离对应的第二节点，并将所述第二节点放入所述距离最小点集合，直至所述位姿空间约束关系中所有节点均位于所述所述距离最小点集合中；

形成子单元，用于根据将节点纳入所述距离最小点集合的次序，形成所述第一位姿与所述第二位姿之间的最短路径，以所述最短路径的距离为所述第一位姿与所述第二位姿之间的最短距离。

10.根据权利要求6所述的基于距离度量的视觉词典闭环检测装置，其特征在于，包括：

筛选模块，用于根据所述当前帧图片的视觉词袋特征，筛选与所述当前帧图片具有指定数量范围的匹配层点对数的相似图片作为所述历史帧图片。