CN110472585A - 一种基于惯导姿态轨迹信息辅助的vi-slam闭环检测方法 - Google Patents

Abstract

本发明提供一种基于惯导姿态轨迹信息辅助的VI‑SLAM闭环检测方法，通过惯导提供移动对象的姿态，解算每帧图像相对于前一帧图像的旋转角度，基于旋转角度获取关键帧；然后基于惯导轨迹，在关键帧的基础上进行粒子滤波，获得条件较为宽松的候选闭环帧；创建词袋模型，利用视觉词典树对候选闭环帧和当前帧进行特征量化；最后使用改进的金字塔TF‑IDF得分匹配方法对当前帧数据和候选闭环帧进行逐帧匹配，进而提取闭环。应用本发明的方法，效果是：与传统的词袋模型方法相比，既能有效提高正确率，又能提高计算效率。

Description

一种基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法

技术领域

本发明涉及定位技术领域，具体涉及一种基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法。

背景技术

VISLAM技术对无人驾驶、AR等热门应用的进步起着重要的推动作用。闭环检测是VISLAM中实时检验移动对象行进轨迹是否与之前所到地点出现重合的方法,它起到纠正移动对象前行过程中位姿和地图创建累积误差的作用，是VISLAM必不可少的重要环节。

闭环检测应具备准确性、高效性和实时性的特点,同时面临着关键帧的合理选择、重复场景导致图像的感知混淆、闭环检测方法的选取、闭环检测结果的鲁棒性检验等关键问题。

闭环检测作为VISLAM技术的重要环节，一直受研究人员的关注，现阶段研究人员主要是从关键帧的选择、闭环检测的方法等方面进行改进与突破，详情如下：

(1)基于词袋模型的闭环检测方法，此方法是通过提取图像特征，建立视觉字典，量化特征，最后进行相似度测量，判定是否形成闭环。该方法的缺点：该方法闭环检测过程中会将当前帧与历史帧逐一进行匹配，这样会造成计算量极大的问题，而且对***的硬件要求较高。

(2)基于高效回环检测和重定位的VISLAM算法，具体提出了一种基于旋转度的关键帧提取算法和一种基于历史模型的差异性回环检测算法，通过上述两种算法，提高了闭环检测的效率。该方法的缺点是：基于旋转度的关键帧提取算法中，其旋转度是由图像间的李代数欧氏距离表示，欧式距离的计算会给算法带来大量的计算，导致效率降低。基于历史模型的差异性回环检测算法中假定历史数据集的前半段为低概率闭环区，进行跳跃式检测，这一假定没有任何依据，会给检测带来大量的误差。

(3)一种基于历史模型集的改进闭环检测算法，具体是：首先，在基于Kinect传感器的帧到模型配准模型的基础上，增加特征描述向量并使用加权方法对其进行更新，从而构建历史模型集，并利用视觉词典树对历史模型集和当前帧数据进行场景描述；其次，以反比例函数代替最小值函数，使两幅图像在单个节点的相似性得分函数得以优化，从而得到改进的金字塔TF-IDF(词频－逆向文件频率)得分匹配方法。再次，使用改进的金字塔TF-IDF得分匹配方法对当前帧数据和候选历史模型集所包含的关键帧进行相似性分析，进而提取候选闭环；最后，从时间连续性和对极几何约束两个方面剔除误正闭环。该方法对闭环检测的效率有一定的提升，但是还是达不到实时性的这样一个要求，因为该算法首先需要进行从属判断，需要大量时间，然后再与从属的图像帧进行逐一匹配，这也要消耗大量时间。

综上，开发一种能兼顾正确率和计算效率的闭环检测方法具有重要意义。

发明内容

本发明的目的在于提供一种能兼顾正确率(解决难以区分相似性场景的问题)和计算效率的闭环检测方法。具体技术方案如下：

一种基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，包括以下步骤：

旋转角度的获得，具体是：通过惯导提供移动对象的姿态，解算每帧图像相对于前一帧图像的移动对象旋转角度；

获取关键帧，具体是：基于旋转角度获取关键帧；

获取候选闭环帧，具体是：基于惯导轨迹，在关键帧的基础上进行粒子滤波，获得条件较为宽松的候选闭环帧；

获取候选闭环帧的数据和当前帧的数据，具体是：创建词袋模型，利用视觉词典树对候选闭环帧和当前帧进行特征量化，得到候选闭环帧的数据和当前帧的数据；

提取闭环，具体是：使用改进的金字塔TF-IDF得分匹配方法对当前帧的数据和候选闭环帧的数据进行逐帧匹配，进而提取闭环。

以上技术方案中优选的，利用惯导提供移动对象相邻帧的姿态，来解算旋转角度采用表达式1)

Δθ₁₂＝|θ₁-θ₂|1)；

其中：θ₁和θ₂为相邻帧图像中惯导提供的移动对象的航向角度，Δθ₁₂为相邻帧移动对象的旋转角度；

相邻两帧图像之间的旋转角度Δθ_ij是以从第i到第j之间相邻图像帧旋转角度的和表示，见表达式2)：

以上技术方案中优选的，基于旋转角度获取关键帧具体是：选取匹配点为Q且累积旋转角度大于等于P或自上个关键帧过去R帧的图像帧作为关键帧，其中：Q为起始位置， P取值为0.05，R取值为25-100。

以上技术方案中优选的，粒子滤波过程采用表达式3)：

其中：x(t)为t时刻状态，u(t)为控制量，w(t)和v(t)分别为模型噪声和观测噪声，前一个方程是状态转移方程，后一个方程是观测方程；

粒子滤波包括初始状态、预测阶段、预测阶段、重采样和滤波五个步骤，初始状态：用大量粒子模拟x(t)，粒子在空间内均匀分布；预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；滤波：将重采样后的粒子代入状态转移方程得到新的预测粒子；

基于粒子滤波的闭环检测算法中，每一个粒子代表地图中的一个节点，粒子集表示为表达式4)：

其中，代表t时刻的第j^th个粒子，M代表粒子总数；粒子分布表征了当前有效节点的概率分布；伴随着越来越多的节点加入到地图中去，当某个节点领域内分布有较为密集的粒子时，实现闭环检测，将此节点作为候选闭环帧。

以上技术方案中优选的，特征量化后还包括相似性计算，具体采用反比例函数来优化两幅图像在单个节点的相似性得分函数，具体是：使用反比例函数代替图像X和Y在单个节点O_il的相似性得分函数见表达式11)：

其中：表示参考图特征向量和实时图特征向量的相似度，表示参考图特征向量，表示实时图特征向量；

基于表达式11)，定义图像在第l层的相似性为表达式12)：

其中：k^l表示的是l层中的k组图像，S^l(X,Y)为图像在第l层的相似性；

使用自下而上计算图像间相似性增量的方法能有效避免重复累计相似性，定义第l层的相似性得分增量△S^l为表达式13)：

其中：L表示视觉词典树的总层数。

以上技术方案中优选的，获取候选闭环帧的数据和当前帧的数据过程中定义金字塔匹配核为表达式14)：

其中：η_l表示视觉词典树第l层的匹配强度系数；

取η_l＝1/k^L-1来抑制不同层次的匹配差异，最终表达式14)式重写为表达式15)：

表达式15)为改进的视觉词典树金字塔TF-IDF得分匹配公式，对当前帧数据和候选闭环帧的数据进行逐帧匹配，取匹配得分大于阈值的图像帧为最终的候选闭环。

以上技术方案中优选的，通过时间连续来对所提取的闭环进行筛选，当所提取的闭环不满足时间连续性要求，则认为该闭环为误正闭环，将其删除；当所提取的闭环满足时间连续性要求，则认为该闭环为正确闭环，将其保留。

本发明方法先通过惯导(惯性导航***)提供移动对象的姿态，解算每帧图像相对于前一帧图像的移动对象旋转角度，基于旋转角度获取关键帧；然后基于惯导轨迹，在关键帧的基础上进行粒子滤波，获得条件较为宽松的候选闭环帧；创建词袋模型，利用视觉词典树对候选闭环帧和当前帧进行特征量化；最后使用改进的金字塔TF-IDF得分匹配方法对当前帧数据和候选闭环帧进行逐帧匹配，进而提取闭环。应用本发明的方法，效果是：与传统的词袋模型方法相比，既能提高正确率，又能提高计算效率。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照附图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本实施例中基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法的流程图；

图2是本实施例中关键帧选取流程图；

图3是本实施例中基于粒子滤波的闭环检测算法流程图；

图4是本实施例中闭环检测示意图；

图5是本实施例中视觉词典树的示意图；

图6是本实施例闭环检测方法和传统词袋模型闭环检测方法的正确率对比图；

图7是本实施例闭环检测方法和传统词袋模型闭环检测方法的所需时间对比图。

具体实施方式

以下结合附图对本发明的实施例进行详细说明，但是本发明可以根据权利要求限定和覆盖的多种不同方式实施。

实施例：

本实施例相关名称解释如下：

闭环检测：是指机器人识别曾到达某场景，使得地图闭环的能力。说的简单点，就是机器人在左转一下，右转一下建图的时候能意识到某个地方是“我”曾经来过的，然后把此刻生成的地图与刚刚生成的地图做匹配。

TF-IDF：是一种统计方法，用以评估一个字词对于一个文件集或一个语料库中的其中一份文件的重要程度。字词的重要性随着它在文件中出现的次数成正比增加，但同时会随着它在语料库中出现的频率成反比下降。

SLAM：即时定位与地图构建，或并发建图与定位。问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图，所谓完全的地图是指不受障碍行进到房间可进入的每个角落。

VISLAM：视觉惯性融合的及时定位与建图。

词袋模型：是个在自然语言处理和信息检索下被简化的表达模型。

Kinect传感器：一种视觉传感器，用于采集图像数据。

聚类：将物理或抽象对象的集合分成由类似的对象组成的多个类的过程被称为聚类。由聚类所生成的簇是一组数据对象的集合，这些对象与同一个簇中的对象彼此相似，与其他簇中的对象相异。

误正闭环：指已经成为候选闭环，但实际上是错误闭环的图像帧。

本实施例公开一种基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，通过惯导提供移动对象的姿态，解算每帧图像相对于前一帧图像的移动对象旋转角度，基于旋转角度获取关键帧；然后基于惯导轨迹，在关键帧的基础上进行粒子滤波，获得条件较为宽松的候选闭环帧；创建词袋模型，利用视觉词典树对候选闭环帧和当前帧进行特征量化；最后使用改进的金字塔TF-IDF得分匹配方法对当前帧数据和候选闭环帧进行逐帧匹配，进而提取闭环。具体流程详见图1，包括：输入图像帧；基于旋转角度进行关键帧选取；基于轨迹进行粒子滤波，获取候选闭环帧；创建词袋模型，提取关键帧特征点，并基于视觉词典树量化特征；相似性计算，剔除误正闭环。详情如下：

第一步、旋转角度的获得，通过惯导提供移动对象的姿态，解算每帧图像相对于前一帧图像的移动对象旋转角度。

关键帧的选取是闭环检测过程中的一个重要部分，传统方法一般不进行关键帧选取，这会使得计算量大且数据冗余，通过关键帧选取可以有效降低数据量，加快计算。一般认为，闭环存在的地方其旋转角度肯定大于零，如果旋转角度等于零，则是一直做直线运动。基于旋转角度的关键帧选取策略，可以获取旋转角度大于阈值的关键帧，对于移动对象快速运动、旋转等情况下具有鲁棒性。为了算法能够准确的跟踪旋转较大的图像帧，利用惯导提供移动对象相邻帧的姿态，来解算旋转角度采用表达式1)：

Δθ₁₂＝|θ₁-θ₂| 1)；

其中：θ₁和θ₂为相邻帧图像中惯导提供的移动对象的航向角度，Δθ₁₂为相邻帧移动对象的旋转角度。

在采集数据期间，可能会有相邻两帧航向角度相似，但期间包含大量旋转的情况，为避免这种情况给提取关键帧带来误差，本实施例规定相邻两帧图像之间的旋转角度Δθ_ij是以从第i到第j之间相邻图像帧旋转角度的和表示，见表达式2)：

第二步、获取关键帧，具体是：基于旋转角度获取关键帧，详见图2。关键帧的选取对于匹配点个数有一定的要求，匹配点太少，匹配会失败，所以有第一个限制条件匹配点大于15个；然后选取累积旋转角度大于等于P或自上个关键帧过去25-100帧的图像帧，这样就不会丢失较多的关键帧数据，因此，基于旋转角度(又叫旋转度)获取关键帧的核心是：选取匹配点数为Q(任意位置，可定义为起始位置)且累积旋转角度大于等于P(P 表示的累积旋转角度，描述物体的转弯情况，本实施例中相邻帧之间取0.05度，且满足连续相邻帧大于等于0.05度)或自上个关键帧过去R帧的图像帧作为关键帧(本实施例中：转弯部位取值为25-50，直线部分取值为50-100)。

第三步、获取候选闭环帧，具体是：基于惯导轨迹，在关键帧的基础上进行粒子滤波，获得条件较为宽松(根据粒子滤波的结果：具体表现为待确定的闭环约束位置之间的距离，本实施例中距离取0.5m)的候选闭环帧，得到候选闭环帧集合，然后进行逐帧匹配，详情如下：

粒子滤波是贝叶斯滤波的实现方法之一，采用非线性方程表达式3)表示：

其中：x(t)为t时刻状态，u(t)为控制量，w(t)和v(t)分别为模型噪声和观测噪声，前一个方程是状态转移方程，后一个方程是观测方程。

粒子滤波的五个步骤是：初始状态：用大量粒子模拟x(t)，粒子在空间内均匀分布；预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子(粒子滤波根据粒子与真实位置的距离来确定比重，然后根据所撒粒子总数进行权重归一化计算，本文所用的粒子数目为50个，所以每一个粒子的权重均值为0.02，而权重大的粒子直的是0.02 以上的粒子，而权重小的粒子指的是部分权重在0.01以下的粒子；本实施例中：权重大的粒子占60-80％，其余为权重小的粒子)；滤波：将重采样后的粒子代入状态转移方程得到新的预测粒子。

基于粒子滤波的闭环检测算法中，每一个粒子代表地图中的一个节点，粒子集表示为表达式4)：

其中，代表t时刻的第j^th个粒子，M代表粒子总数；粒子分布表征了当前有效节点的概率分布；伴随着越来越多的节点加入到地图中去，当某个节点领域内分布有较为密集的粒子时(即超过一定阈值，本实施例阀值为20)，实现闭环检测，将此节点作为候选闭环帧。

基于粒子滤波的闭环检测算法流程详见图3(即基于蒙特卡洛的闭环检测算法流程)，其中：N为当前节点总数，代表了当前节点的位置，后续如果该节点被选取为候选闭环，将用N值来提取该候选闭环；t为时刻；Z_t为预测粒子；χ_t为经过采样随机化后的粒子；为新的预测粒子。散布粒子部分进行粒子的传播，包括状态转移和粒子权重分配；重采样部分是对粒子依据权重进行重采样，重采样之后，所有的粒子具有相同的权重；随机化部分是将随机样本加入粒子群，以保持粒子群的多样性，避免粒子退化。在该方法中，具有较大数目(或较高密集度)粒子的节点被认为是检测到了闭环，作为候选闭环约束。如图 4所示，圆圈代表位姿节点，实心圆代表当前节点，图中获得了三个候选节点构成闭环约束。当以较高的可信度确定检测到候选闭环时，再将当前帧与候选闭环帧进行逐帧匹配。

第四步、获取候选闭环帧的数据和当前帧的数据，具体是：创建词袋模型，利用视觉词典树对候选闭环帧和当前帧进行特征量化，得到候选闭环帧的数据和当前帧的数据，详情如下：

4.1、特征提取，特征提取过程我们选择的是SIFT算法。尺度不变特征转换即SIFT(Scale-Invariant Feature Transform)是一种计算机视觉的算法。它用来侦测与描述影像中的局部性特征，它在空间尺度中寻找极值点，并提取出其位置、尺度、旋转不变量。

SIFT特征提取主要包括尺度空间构建和空间极值点检测(关键点的初步查探)两个步骤，具体是：

尺度空间构建的基础是DOG金字塔，DOG金字塔构建的基础是高斯金字塔。高斯尺度空间可由图像和不同的高斯卷积得到如表达式5)：

L(x,y,σ)＝G(x,y,σ)×I(x,y)5)；

其中：二维高斯函数如表达式6)：

其中：x和y表示点的坐标，σ代表高斯正态分布的方差。

空间极值点检测(关键点的初步查探)具体是：对于二维图像I(x,y)，在不同尺度下的尺度空间表示L(x,y,σ)可由图像I(x,y)与高斯核G(x,y,σ)的卷积得到。在建立图像的DOG金字塔基础上,在DOG尺度空间中的26个邻域中检测极值，D(x,y,σ)是两个相邻尺度图像之差，即见表达式7)：

其中：k表示尺度系数，本实施例算法中初始值取1.6；

一个点如果与周围八个点以及上下层的十八个邻域点共26个邻域中是最大值或最小值,就确定该点是图像在该尺度下的一个特征点。

为了寻找DOG函数的极值点，每一个像素、点要和它所有的相邻点比较，看其是否比它的图像域和尺度空间域的相邻点大或者小。

在二维图像空间，中心点与它3×3邻域内的8个点做比较，在同一组内的尺度空间上，中心点和上下相邻的两层图像的2×9个点作比较，这样则可以保证检测到的关键点在尺度空间和二维图像空间上都是局部极值点。

4.2、视觉词典树，视觉词典树的建立参照二叉树搜索原理，可以解决平面结构特征查询效率低的问题。

假设树的分支数为k，层数为L，对每一分支递归调用K-means算法，从而得到k个子分支，直到第L层，如图5所示。将每个分支特征的聚类中心作为该分支的节点，其描述向量作为视觉单词，从而建立视觉词典树。

K-means聚类的一般过程如下：(1)在所有数据中任意选取k个簇(聚类集合)中心；(2)对所有数据分别比较其与各个簇中心的距离，将其加入最小距离所对应的簇；(3) 对同类簇中的数据通过求平均值的方法计算它们的簇中心，直到该簇类中心收敛不变而结束该算法处理过程，否则重复第(2)步。

聚类方法应用在图像特征点上，针对的数据是特征描述子。经过聚类后，同类特征被归类到同一个视觉单词(簇中心)。k-means聚类简单快速，对大规模数据集处理有较高效率。

4.3、量化特征，量化特征是通过对图像帧提取的特征进行量化，来表达图像特性的过程。

一棵L层k分支视觉词典树的单词空间表征能力为表达式8)：

假设从图像X中提取n个d维特征：X＝{x₁,x₂,…,x_n},x_i∈R^d，按照图5中方法构建视觉词典树，将图像在各个树节点的TF-IDF熵作为图像在该视觉单词的得分权重。定义 TF-IDF熵为表达式9)：

其中，ω′_i(X)表示图像X在视觉词典树的第l层的第i个节点处的投影得分，N表示待处理图像总数，n_i表示图像X投影到节点i的特征数，N_i表示至少有一个特征投影到节点i的图像数。

记图像X量化后的特征向量为：

W(X)＝(W¹(X),W²(X),…,W^L(X)) 10)；

其中：表示图像X在视觉词典树的第l层的得分向量。

4.4、相似性计算，闭环检测的难点之一在于相似性场景会产生误正闭环，从而导致检测错误。针对实际环境中存在的相似性场景问题，采用反比例函数代替最小值函数以优化两幅图像在单个节点的相似性得分函数，有效减少了误正闭环的产生。

使用反比例函数代替图像X和Y在单个节点的相似性得分函数见表达式11)：

其中：表示参考图特征向量和实时图特征向量的相似度，表示参考图特征向量，表示实时图特征向量。

基于表达式11)，定义图像在第l层的相似性为表达式12)：

其中：k^l表示的是l层中的k组图像，S^l(X,Y)为图像在第l层的相似性。

使用自下而上计算图像间相似性增量的方法可有效避免重复累计相似性，因此，本实施例定义第l层的相似性得分增量△S^l为表达式13)：

其中：L表示视觉词典树的总层数。

定义金字塔匹配核为表达式14)：

其中：η_l表示视觉词典树第l层的匹配强度系数；

取η_l＝1/k^L-1来抑制不同层次的匹配差异，最终表达式14)式重写为表达式15)：

表达式15)为改进的视觉词典树金字塔TF-IDF得分匹配公式，对当前帧数据和候选闭环帧的数据进行逐帧匹配，选取匹配得分大于阈值的图像帧为最终的候选闭环。本实施例阈值选择为0.6，即相似度大于0.6即为有效的闭环检测区域。

剔除误正闭环，具体是：移动对象在进行SLAM时所获得的场景图像具有时间连续性，所以通常情况下闭环会在连续几帧数据中发生，因此，通过时间连续来对所提取的闭环进行筛选，当所提取的闭环不满足时间连续性要求(闭环信息在短段时间内是连续出现的，若出现孤立的闭环信息，则不满足时间连续性)，则认为该闭环为误正闭环，将其删除；当所提取的闭环满足时间连续性要求，则认为该闭环为正确闭环，将其保留。

将本实施例闭环检测方法和传统词袋模型闭环检测方法进行对比，在同一场景模式下，从闭环检测正确率和计算效率两方面进行比较，详见图6和图7，从图6中可以看出：在同一场景模式的条件下，显示了本实施例方法(实线示意)和传统方法的闭环检测正确率。本实验一共分为10组，每组进行10次闭环检测实验，得到每组的实验正确率。结果表明，本专利方法相较于传统的词袋模型方法在正确率方面有所提高，得益于利用视觉词典树进行量化特征，并用TF-IDF得分匹配方法进行相似性分析，减少感知歧义。从图7 中可以看出：在同一场景模式的条件下，显示了本实施例方法(虚线示意)和传统方法的闭环检测计算所需时间。本实验一共分为10组，每组进行10次闭环检测实验，得到每组每次检测的平均检测时间。结果表明，本专利方法相较于传统的词袋模型方法在计算效率方面有所提高，得益于利用惯导获得的旋转角度进行关键帧的选取，避免逐帧匹配，然后基于惯导提供的轨迹，进行粒子滤波，获取候选闭环，进一步加快闭环检测效率。

应用本实施例的技术方案，效果是：

(1)本发明方法提出使用惯导提供移动对象姿态信息，解算相邻帧的旋转角度，根据旋转角度获取初步的关键帧数据。这样就使得***构建局部地图时在相机快速运动、旋转、外界条件困难的情况下也可以鲁棒的运行，而在后端的优化中又不至于计算量十分巨大。

(2)本发明方法提出利用惯导提供的移动对象轨迹，进行粒子滤波，获取条件较为宽松的候选闭环帧，最后再进行逐帧匹配，剔除误正闭环。通过淘汰部分关键帧，进一步减少了匹配次数，从而加快了算法计算速度。

(3)本发明方法基于视觉词典树对图像进行描述，量化特征，不受视觉单词个数的限制。同时用改进的金字塔TF-IDF得分匹配方法进行相似度测量，有效减少了闭环检测的感知歧义。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：包括以下步骤：

旋转角度的获得，具体是：通过惯导提供移动对象的姿态，解算每帧图像相对于前一帧图像的旋转角度；

获取关键帧，具体是：基于旋转角度获取关键帧；

获取候选闭环帧，具体是：基于惯导轨迹，在关键帧的基础上进行粒子滤波，获得条件较为宽松的候选闭环帧；

获取候选闭环帧的数据和当前帧的数据，具体是：创建词袋模型，利用视觉词典树对候选闭环帧和当前帧进行特征量化，得到候选闭环帧的数据和当前帧的数据；

提取闭环，具体是：使用改进的金字塔TF-IDF得分匹配方法对当前帧的数据和候选闭环帧的数据进行逐帧匹配，进而提取闭环。

2.根据权利要求1所述的基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：利用惯导提供移动对象相邻帧的姿态，来解算旋转角度采用表达式1)：

Δθ₁₂＝|θ₁-θ₂| 1)；

其中：θ₁和θ₂为相邻帧图像中惯导提供的移动对象的航向角度，Δθ₁₂为相邻帧移动对象的旋转角度；

相邻两帧图像之间的旋转角度Δθ_ij是以从第i到第j之间相邻图像帧旋转角度的和表示，见表达式2)：

3.根据权利要求2所述的基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：基于旋转角度获取关键帧具体是：选取匹配点为Q且累积旋转角度大于等于P或自上个关键帧过去R帧的图像帧作为关键帧，其中：Q为起始位置，P取值为0.05，R取值为25-100。

4.根据权利要求3所述的基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：粒子滤波过程采用表达式3)：

其中：x(t)为t时刻状态，u(t)为控制量，w(t)和v(t)分别为模型噪声和观测噪声，前一个方程是状态转移方程，后一个方程是观测方程；

粒子滤波包括初始状态、预测阶段、预测阶段、重采样和滤波五个步骤，初始状态：用大量粒子模拟x(t)，粒子在空间内均匀分布；预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；滤波：将重采样后的粒子代入状态转移方程得到新的预测粒子；

基于粒子滤波的闭环检测算法中，每一个粒子代表地图中的一个节点，粒子集表示为表达式4)：

其中，代表t时刻的第j^th个粒子，M代表粒子总数；粒子分布表征了当前有效节点的概率分布；伴随着越来越多的节点加入到地图中去，当某个节点领域内分布有较为密集的粒子时，实现闭环检测，将此节点作为候选闭环帧。

5.根据权利要求4所述的基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：特征量化后还包括相似性计算，具体采用反比例函数来优化两幅图像在单个节点的相似性得分函数，具体是：使用反比例函数代替图像X和Y在单个节点O_il的相似性得分函数见表达式11)：

其中：表示参考图特征向量和实时图特征向量的相似度，表示参考图特征向量，表示实时图特征向量；

基于表达式11)，定义图像在第l层的相似性为表达式12)：

其中：k^l表示的是l层中的k组图像，S^l(X,Y)为图像在第l层的相似性；

使用自下而上计算图像间相似性增量的方法能有效避免重复累计相似性，定义第l层的相似性得分增量△S^l为表达式13)：

其中：L表示视觉词典树的总层数。

6.根据权利要求5所述的基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：获取候选闭环帧的数据和当前帧的数据过程中定义金字塔匹配核为表达式14)：

其中：η_l表示视觉词典树第l层的匹配强度系数；

取η_l＝1/k^L-1来抑制不同层次的匹配差异，最终表达式14)式重写为表达式15)：

表达式15)为改进的视觉词典树金字塔TF-IDF得分匹配公式，对当前帧数据和候选闭环帧的数据进行逐帧匹配，选取匹配得分大于阈值的图像帧为最终的候选闭环。

7.根据权利要求6所述的基于惯导姿态轨迹信息辅助的VI-SLAM闭环检测方法，其特征在于：通过时间连续来对所提取的闭环进行筛选，当所提取的闭环不满足时间连续性要求，则认为该闭环为误正闭环，将其删除；当所提取的闭环满足时间连续性要求，则认为该闭环为正确闭环，将其保留。