劉順衛(wèi) 何元烈

摘要：針對LVI—SAM算法中視覺慣性子系統(tǒng)容易因快速運動或明暗變化激烈導(dǎo)致算法回環(huán)檢測無效的問題，文章設(shè)計了一種結(jié)合 scan context 描述子的回環(huán)檢測系統(tǒng)的多傳感器融合算法。首先，在原有的LVI-SAM算法的激光慣性子系統(tǒng)中增加 scan context模塊，形成 scan context矩陣，用二進制矩陣來形成關(guān)鍵幀提取策略，以減少算法的計算冗余。同時，基于scan context描述子解決在光照環(huán)境不佳情況下的回環(huán)檢測問題。

關(guān)鍵詞：LVI-SAM;scan context 描述子;關(guān)鍵幀策略;回環(huán)檢測

中圖法分類號：TP391文獻標(biāo)識碼：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引言

近年來，同時定位和地圖構(gòu)建被廣泛引用到移動機器人導(dǎo)航的各個方面，如無人機、自動駕駛汽車、虛擬/增強現(xiàn)實中。然而，單傳感器在各種具有挑戰(zhàn)性的場景中進行實時狀態(tài)估計和地圖構(gòu)建，都各自存在缺點。將激光雷達和相機與慣性測量單元進行融合可以很好地克服單傳感器缺點，并有效提高系統(tǒng)的魯棒性和準(zhǔn)確性。

LVI?SAM[1]系統(tǒng)是多傳感器融合系統(tǒng)，由一個視覺慣性系統(tǒng)（VIS）和一個激光雷達慣性系統(tǒng)（LIS）組成。視覺里程計因子通過優(yōu)化視覺重投影和IMU測量的誤差得到，LIS系統(tǒng)提取激光雷達的邊緣和平面特征，并將它們與保持在滑動窗口中的特征圖相匹配，形成激光里程計因子，最后對以上兩個因子、回環(huán)檢測因子和IMU預(yù)積分因子進行了聯(lián)合優(yōu)化。盡管其有著優(yōu)秀的魯棒性，但LVI?SAM系統(tǒng)仍存在一定局限性。

LVI?SAM系統(tǒng)的回環(huán)檢測部分先由VIS系統(tǒng)識別，根據(jù)DBoW2[2]模型進行選取回環(huán)檢測幀。由于失敗檢測機制的存在，一旦出現(xiàn)光線強度變化過大導(dǎo)致回環(huán)檢測系統(tǒng)失效，無法選取回環(huán)幀。同時，在LIS系統(tǒng)中，當(dāng)姿態(tài)變化超過1m或旋轉(zhuǎn)角度超過10°時，當(dāng)前幀被選為新的關(guān)鍵幀，這種選幀策略會大大降低實時性能。本文在LVI?SAM的基礎(chǔ)上設(shè)計了一種結(jié)合scancontext[3]描述子的SC?LVI?SAM算法，優(yōu)化其激光慣性系統(tǒng)，以解決以上兩個問題。

2系統(tǒng)概述

SC?LVI?SAM系統(tǒng)由3D激光點云、單目圖像和IMU信息作為輸入。本文整個框架由兩個關(guān)鍵的子系統(tǒng)組成，即視覺慣性系統(tǒng)和激光雷達慣性系統(tǒng)。VIS系統(tǒng)接受圖像和IMU信息，同時可以借助激光雷達信息獲取深度。LIS統(tǒng)通過最小化IMU和視覺的測量殘差來得到視覺里程計。激光里程計通過最小化檢測到的線面特征到特征圖的距離得到。回環(huán)檢測由兩部分組成，即由VIS系統(tǒng)利用詞袋模型選取回環(huán)候選幀，同時在LIS系統(tǒng)中生成scancontext描述子選取回環(huán)候選幀，識別后由LIS系統(tǒng)進行ICP匹配得到回環(huán)約束因子。最后，由iSAM2來優(yōu)化因子圖中IMU預(yù)積分以及視覺里程計、激光里程計和閉環(huán)約束的殘差。

3激光里程計

3.1關(guān)鍵幀選取

當(dāng)接受新一幀雷達點云數(shù)據(jù)，系統(tǒng)使用來自IMU傳播的運動預(yù)測來獲得去除運動畸變的點云，然后評估整個局部點云的凹凸程度來提取邊特征點和面特征點。當(dāng)凹凸程度較大時，將這類點云分成邊特征點，反之則為面特征點[4～5];將從時刻i獲取的點云中提出的邊特征點和面特征點的集合表述為Fei和FPi，所以在本時刻提取的所有特征點構(gòu)成了雷達幀。

本文引用scancontext描述子來選取新的關(guān)鍵幀策略，一幀激光幀被分為Nr×Ns個區(qū)間，NS和Nr分別代表扇形和環(huán)形的數(shù)量。在本文算法中設(shè)扇形的中心角為6°，即Ns為20;取激光傳感器的掃描最大距離為Lmax，則環(huán)之間的徑向間隙為max。則設(shè)NR為60。如公式1所示，Aij則表示區(qū)間中第i個環(huán)形和第j個扇形重疊的點云集合。同時，將動態(tài)物體視為稀疏噪聲。

在點云分區(qū)完畢后，取點云的最大高度為點云賦值。對于掃描的點云P中的每個點Pk，Z（）是P中Z坐標(biāo)值的函數(shù)，如式2所示。因此，形成矩陣N為Ns?Nr矩陣為式3所示，該矩陣可以有效描述該激光點云幀幾何結(jié)構(gòu)信息。

為了保持選取關(guān)鍵幀的高效性，本文用一種粗略但有效的方法來快速索引對定義兩個矩陣之間的相似性。首先定義一個二進制矩陣I，如公式（4）（5）所示：

然后通過當(dāng)前幀Iq和候選幀Ic之間的元素異或運算來定義相似性，如公式7示：

其中|x|是x中元素的總數(shù)，XOR（x，y）是指矩陣x和y之間的元素異或運算。最后通過平移scancontext矩陣的列向量匹配到最優(yōu)二進制矩陣Iq。如果當(dāng)前幀與前一個關(guān)鍵幀的相似度小于閾值，則選取當(dāng)幀為新的一幀關(guān)鍵幀，加入局部地圖進行匹配。

3.2掃描匹配

使用新的關(guān)鍵幀策略選出新關(guān)鍵幀F(xiàn)i+1時，系統(tǒng)在前一幀關(guān)鍵幀一共提取n個子關(guān)鍵幀加入到一個固定滑動窗口用于估計相對轉(zhuǎn)換關(guān)系。子關(guān)鍵幀的集合表示為{Fi-n，…，F(xiàn)i}，對應(yīng)n+1個姿態(tài)估計表示為{Ti-n，…，Ti}，并將子關(guān)鍵幀從激光坐標(biāo)系B轉(zhuǎn)換到世界坐標(biāo)系W中，由此構(gòu)建了一個局部地圖Mi。

當(dāng)新關(guān)鍵幀加入到滑動窗口后，通過IMU的預(yù)積分得到當(dāng)前幀的姿態(tài)估計Ti+1，將激光雷達坐標(biāo)系下的特征點{BFi（e）+1，BFi（p）+1}轉(zhuǎn)換到全局坐標(biāo)系{WFi（e）+1，WFi（p）+1}上。對于新關(guān)鍵幀F(xiàn)i+1上的每個特征點j，可以在局部地圖中找到其對應(yīng)邊子地圖和面子地圖，通過式（7）和式（8）來計算j到其對應(yīng)的線和面之間的距離：

式中，k，u，v，w是對應(yīng)特征點在其對應(yīng)的集合中的索引，而后使用高斯?牛頓法求解，最后獲得當(dāng)前關(guān)鍵幀與局部地圖之間的相對變換，并將其作為激光雷達里程計因子添加到因子圖中。

4回環(huán)檢測

基于計算的激光描述子scancontext矩陣N，比較歷史關(guān)鍵幀的相似度來判斷本幀是否為回環(huán)幀。在用于檢測關(guān)鍵幀的方法中，公式（6）會產(chǎn)生過多回環(huán)候選幀。因此，對于回環(huán)檢測部分，本文用矩陣的余弦距離來判斷兩幀的相似度。

由于圓環(huán)表示的旋轉(zhuǎn)不變性，整個scancontext矩陣用列向量進行比較。所以，距離表示為同一索引下每列距離的和。而余弦距離用于計算同一索引下Cqj和Ccj兩個列向量的距離，最后歸一化處理。對于當(dāng)前幀Iq和待選候選幀Ic函數(shù)如公式（9）：

與選取關(guān)鍵幀相同，選取閉環(huán)幀也需要考慮掃描角度的變化，計算出最優(yōu)旋轉(zhuǎn)矩陣Nk（q）。最后，本文通過對比相似度與閾值來比較是否選取本幀為閉環(huán)幀。閾值設(shè)置為0.3，若檢測到該幀為閉環(huán)幀，可以對兩個匹配的點云進行配準(zhǔn)，得到相對位姿關(guān)系T，形成閉環(huán)約束因子，并插入到全局因子圖中。

5實驗與分析

urbanNav數(shù)據(jù)集將用于LVI?SAM算法與本文算法SC?LVI?SAM結(jié)果進行比較。UrbanNav數(shù)據(jù)集有2個子數(shù)據(jù)集，分別為UrbanNav?HK?Data2020314和UrbanNav?HK?Data20190428。

數(shù)據(jù)集UrbanNav?HK?Data2020314于下午采集中國香港九龍低城市化地區(qū)，光線充足。存在較少動態(tài)物體與高樓，全場1.21公里。圖1中的（a）和（b）分別表示LVI?SAM算法與SC?LVI?SAM算法在UrbanNav?HK?Data2020314數(shù)據(jù)集上與真實軌跡（虛線）的比較。SC?LVI?SAM算法與LVI?SAM算法在光照環(huán)境變化較少的情景下比較，加入激光描述子（scancontext）對定位精度基本沒有影響，但由于使用scancontext描述子改變其關(guān)鍵幀策略，LVI?SAM算法在本數(shù)據(jù)集選取了867個激光關(guān)鍵幀，而本文算法選取了709個激光關(guān)鍵幀，減少了約19%。即在不影響定位精度的情況下，SC?LVI?SAM算法減少了計算冗余。

子數(shù)據(jù)集UrbanNav?HK?Data20190428涉及眾多動態(tài)物體和高樓建筑遮擋，全長2.01公里。圖1中的（c）和（d）分別表示LVI?SAM算法與SC?LVI?SAM算法在UrbanNav?HK?Data2020314數(shù)據(jù)集上與真實軌跡的比較。由于測量車輛在接近回環(huán)時遇到紅綠燈車輛停止運動且處于立交橋下，導(dǎo)致LVI?SAM算法的視覺慣性系統(tǒng)失效，系統(tǒng)無法檢測回環(huán)幀導(dǎo)致無法添加閉環(huán)因子，整體測量軌跡重力矢量上存在明顯漂移。而加入激光描述子后，可正常檢測閉環(huán)幀并完成閉環(huán)檢測與優(yōu)化。由圖1擬合軌跡中可知，在UrbanNav?HK?Data2020314數(shù)據(jù)集上，SC?LVI?SAM算法的定位精度比LVI?SAM算法更優(yōu)秀。

6結(jié)論

本文結(jié)合scancontext描述子的方法，首先改進了LVI?SAM系統(tǒng)中的激光子系統(tǒng)的選取關(guān)鍵幀策略，然后把每幀激光幀轉(zhuǎn)換成scancontext矩陣，從而通過一個二進制法獲取到了有效的關(guān)鍵幀。此外，原算法中基于DBOW2的閉環(huán)檢測方法在光照強度變化大或運動激烈時并不適用，使用scancontext描述子檢測回環(huán)可以有效解決此問題。實驗表明，SC?LVI?SAM算法對比LVI?SAM算法在光照情況變換激烈時定位精度提高了52%，一定程度上改進了LVI?SAM算法。在關(guān)鍵幀選取上，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.

作者簡介：

劉順衛(wèi)（1996—），碩士，研究方向：多傳感器融合SLAM系統(tǒng)。