基于scancontext描述子的LVI?SAM算法改进研究

刘顺卫 何元烈

摘要：针对LVI—SAM算法中视觉惯性子系统容易因快速运动或明暗变化激烈导致算法回环检测无效的问题，文章设计了一种结合 scan context 描述子的回环检测系统的多传感器融合算法。首先，在原有的LVI-SAM算法的激光惯性子系统中增加 scan context模块，形成 scan context矩阵，用二进制矩阵来形成关键帧提取策略，以减少算法的计算冗余。同时，基于scan context描述子解决在光照环境不佳情况下的回环检测问题。

关键词：LVI-SAM;scan context 描述子;关键帧策略;回环检测

中图法分类号：TP391文献标识码：A

Research on improvement of LVI-SAM algorithm based on scan context descriptor

LIU Shunwei，HEYuanlie

（Guangdong University of Technology，Guangzhou 510000，China）

Abstract：Aiming at the problem that the visual-inertial subsystem in the LVI-SAM algorithm is easy to make the loop closure detection of the algorithm invalid due to rapid motion or large illumination changes， a multi-sensor fusion algorithm of the loop closure detection system combined with the scan context descriptor is designed. First， a scan context module is added to the laser inertial subsystem of the original LVI-SAM algorithm to form a scan context matrix， and a binary matrix is used to form a key frame extraction strategy to reduce the computational redundancy of the algorithm.At the same time， based on the scan context descriptor， the loop closure detection problem in the case of poor lighting environment is solved.

Key words： LVI-SAM， scan context， keyframe strategy，loop detection

1引言

近年來，同时定位和地图构建被广泛引用到移动机器人导航的各个方面，如无人机、自动驾驶汽车、虚拟/增强现实中。然而，单传感器在各种具有挑战性的场景中进行实时状态估计和地图构建，都各自存在缺点。将激光雷达和相机与惯性测量单元进行融合可以很好地克服单传感器缺点，并有效提高系统的鲁棒性和准确性。

LVI?SAM[1]系统是多传感器融合系统，由一个视觉惯性系统（VIS）和一个激光雷达惯性系统（LIS）组成。视觉里程计因子通过优化视觉重投影和IMU测量的误差得到，LIS系统提取激光雷达的边缘和平面特征，并将它们与保持在滑动窗口中的特征图相匹配，形成激光里程计因子，最后对以上两个因子、回环检测因子和IMU预积分因子进行了联合优化。尽管其有着优秀的鲁棒性，但LVI?SAM系统仍存在一定局限性。

LVI?SAM系统的回环检测部分先由VIS系统识别，根据DBoW2[2]模型进行选取回环检测帧。由于失败检测机制的存在，一旦出现光线强度变化过大导致回环检测系统失效，无法选取回环帧。同时，在LIS系统中，当姿态变化超过1m或旋转角度超过10°时，当前帧被选为新的关键帧，这种选帧策略会大大降低实时性能。本文在LVI?SAM的基础上设计了一种结合scancontext[3]描述子的SC?LVI?SAM算法，优化其激光惯性系统，以解决以上两个问题。

2系统概述

SC?LVI?SAM系统由3D激光点云、单目图像和IMU信息作为输入。本文整个框架由两个关键的子系统组成，即视觉惯性系统和激光雷达惯性系统。VIS系统接受图像和IMU信息，同时可以借助激光雷达信息获取深度。LIS统通过最小化IMU和视觉的测量残差来得到视觉里程计。激光里程计通过最小化检测到的线面特征到特征图的距离得到。回环检测由两部分组成，即由VIS系统利用词袋模型选取回环候选帧，同时在LIS系统中生成scancontext描述子选取回环候选帧，识别后由LIS系统进行ICP匹配得到回环约束因子。最后，由iSAM2来优化因子图中IMU预积分以及视觉里程计、激光里程计和闭环约束的残差。

3激光里程计

3.1关键帧选取

当接受新一帧雷达点云数据，系统使用来自IMU传播的运动预测来获得去除运动畸变的点云，然后评估整个局部点云的凹凸程度来提取边特征点和面特征点。当凹凸程度较大时，将这类点云分成边特征点，反之则为面特征点[4～5];将从时刻i获取的点云中提出的边特征点和面特征点的集合表述为Fei和FPi，所以在本时刻提取的所有特征点构成了雷达帧。

本文引用scancontext描述子来选取新的关键帧策略，一帧激光帧被分为Nr×Ns个区间，NS和Nr分别代表扇形和环形的数量。在本文算法中设扇形的中心角为6°，即Ns为20;取激光传感器的扫描最大距离为Lmax，则环之间的径向间隙为max。则设NR为60。如公式1所示，Aij则表示区间中第i个环形和第j个扇形重叠的点云集合。同时，将动态物体视为稀疏噪声。

在点云分区完毕后，取点云的最大高度为点云赋值。对于扫描的点云P中的每个点Pk，Z（）是P中Z坐标值的函数，如式2所示。因此，形成矩阵N为Ns?Nr矩阵为式3所示，该矩阵可以有效描述该激光点云帧几何结构信息。

为了保持选取关键帧的高效性，本文用一种粗略但有效的方法来快速索引对定义两个矩阵之间的相似性。首先定义一个二进制矩阵I，如公式（4）（5）所示：

然后通过当前帧Iq和候选帧Ic之间的元素异或运算来定义相似性，如公式7示：

其中|x|是x中元素的总数，XOR（x，y）是指矩阵x和y之间的元素异或运算。最后通过平移scancontext矩阵的列向量匹配到最优二进制矩阵Iq。如果当前帧与前一个关键帧的相似度小于阈值，则选取当帧为新的一帧关键帧，加入局部地图进行匹配。

3.2扫描匹配

使用新的关键帧策略选出新关键帧Fi+1时，系统在前一帧关键帧一共提取n个子关键帧加入到一个固定滑动窗口用于估计相对转换关系。子关键帧的集合表示为{Fi-n，…，Fi}，对应n+1个姿态估计表示为{Ti-n，…，Ti}，并将子关键帧从激光坐标系B转换到世界坐标系W中，由此构建了一个局部地图Mi。

当新关键帧加入到滑动窗口后，通过IMU的预积分得到当前帧的姿态估计Ti+1，将激光雷达坐标系下的特征点{BFi（e）+1，BFi（p）+1}转换到全局坐标系{WFi（e）+1，WFi（p）+1}上。对于新关键帧Fi+1上的每个特征点j，可以在局部地图中找到其对应边子地图和面子地图，通过式（7）和式（8）来计算j到其对应的线和面之间的距离：

式中，k，u，v，w是对应特征点在其对应的集合中的索引，而后使用高斯?牛顿法求解，最后获得当前关键帧与局部地图之间的相对变换，并将其作为激光雷达里程计因子添加到因子图中。

4回环检测

基于计算的激光描述子scancontext矩阵N，比较历史关键帧的相似度来判断本帧是否为回环帧。在用于检测关键帧的方法中，公式（6）会产生过多回环候选帧。因此，对于回环检测部分，本文用矩阵的余弦距离来判断两帧的相似度。

由于圆环表示的旋转不变性，整个scancontext矩阵用列向量进行比较。所以，距离表示为同一索引下每列距离的和。而余弦距离用于计算同一索引下Cqj和Ccj两个列向量的距离，最后归一化处理。对于当前帧Iq和待选候选帧Ic函数如公式（9）：

与选取关键帧相同，选取闭环帧也需要考虑扫描角度的变化，计算出最优旋转矩阵Nk（q）。最后，本文通过对比相似度与阈值来比较是否选取本帧为闭环帧。阈值设置为0.3，若检测到该帧为闭环帧，可以对两个匹配的点云进行配准，得到相对位姿关系T，形成闭环约束因子，并插入到全局因子图中。

5实验与分析

urbanNav数据集将用于LVI?SAM算法与本文算法SC?LVI?SAM结果进行比较。UrbanNav数据集有2个子数据集，分别为UrbanNav?HK?Data2020314和UrbanNav?HK?Data20190428。

数据集UrbanNav?HK?Data2020314于下午采集中国香港九龙低城市化地区，光线充足。存在较少动态物体与高楼，全场1.21公里。图1中的（a）和（b）分别表示LVI?SAM算法与SC?LVI?SAM算法在UrbanNav?HK?Data2020314数据集上与真实轨迹（虚线）的比较。SC?LVI?SAM算法与LVI?SAM算法在光照环境变化较少的情景下比较，加入激光描述子（scancontext）对定位精度基本没有影响，但由于使用scancontext描述子改变其关键帧策略，LVI?SAM算法在本数据集选取了867个激光关键帧，而本文算法选取了709个激光关键帧，减少了约19%。即在不影響定位精度的情况下，SC?LVI?SAM算法减少了计算冗余。

子数据集UrbanNav?HK?Data20190428涉及众多动态物体和高楼建筑遮挡，全长2.01公里。图1中的（c）和（d）分别表示LVI?SAM算法与SC?LVI?SAM算法在UrbanNav?HK?Data2020314数据集上与真实轨迹的比较。由于测量车辆在接近回环时遇到红绿灯车辆停止运动且处于立交桥下，导致LVI?SAM算法的视觉惯性系统失效，系统无法检测回环帧导致无法添加闭环因子，整体测量轨迹重力矢量上存在明显漂移。而加入激光描述子后，可正常检测闭环帧并完成闭环检测与优化。由图1拟合轨迹中可知，在UrbanNav?HK?Data2020314数据集上，SC?LVI?SAM算法的定位精度比LVI?SAM算法更优秀。

6结论

本文结合scancontext描述子的方法，首先改进了LVI?SAM系统中的激光子系统的选取关键帧策略，然后把每帧激光帧转换成scancontext矩阵，从而通过一个二进制法获取到了有效的关键帧。此外，原算法中基于DBOW2的闭环检测方法在光照强度变化大或运动激烈时并不适用，使用scancontext描述子检测回环可以有效解决此问题。实验表明，SC?LVI?SAM算法对比LVI?SAM算法在光照情况变换激烈时定位精度提高了52%，一定程度上改进了LVI?SAM算法。在关键帧选取上，SC?LVI?SAM算法比原方法选帧减少19%，但是定位精度不变，可以有效减少计算冗余。接下来，对SC?LVI?SAM算法而言，主要将进行对scancontext矩阵匹配精度问题的解决。

参考文献：

[1]KaessM，JohannssonH，RobertsR，etal.iSAM2：IncrementasmoothingandmappingusingtheBayestree[J].TheInternationalJournalofRoboticsResearch，2011，31（2）：216?235.

[2]ShanT，EnglotB，RattiC，etal.LVI?SAM：Tightly?coupledLidar?Visual?InertialOdometryviaSmoothingandMapping[J/OL].http：∥arxiv.org/abs/2104.10831.

[3]ZhangJ，SinghS.Low?driftandReal?timeLidarOdometryandMapping[J].AutonomousRobots，2017，41（2）：401?416.

[4]ZhangJ，SinghS.LOAM：LidarOdometryandMappinginReal?time[C]∥Robotics：ScienceandSystemsConference.2014.

[5]Galvez?LpezD，TardosJD.BagsofBinaryWordsforFastPlaceRecognitioninImageSequences[J].IEEETransactionsonRobotics，2012，28（5）：1188?1197.

作者简介：

刘顺卫（1996—），硕士，研究方向：多传感器融合SLAM系统。