基于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系統。