基于scancontext描述子的LVI?SAM算法改進(jìn)研究

劉順衛(wèi) 何元烈

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

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

中圖法分類(lèi)號(hào)：TP391文獻(xiàn)標(biāo)識(shí)碼：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引言

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

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

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

2系統(tǒng)概述

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

3激光里程計(jì)

3.1關(guān)鍵幀選取

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

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

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

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

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

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

3.2掃描匹配

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

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

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

4回環(huán)檢測(cè)

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

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

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

5實(shí)驗(yàn)與分析

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

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

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

6結(jié)論

本文結(jié)合scancontext描述子的方法，首先改進(jìn)了LVI?SAM系統(tǒng)中的激光子系統(tǒng)的選取關(guān)鍵幀策略，然后把每幀激光幀轉(zhuǎn)換成scancontext矩陣，從而通過(guò)一個(gè)二進(jìn)制法獲取到了有效的關(guān)鍵幀。此外，原算法中基于DBOW2的閉環(huán)檢測(cè)方法在光照強(qiáng)度變化大或運(yùn)動(dòng)激烈時(shí)并不適用，使用scancontext描述子檢測(cè)回環(huán)可以有效解決此問(wèn)題。實(shí)驗(yàn)表明，SC?LVI?SAM算法對(duì)比LVI?SAM算法在光照情況變換激烈時(shí)定位精度提高了52%，一定程度上改進(jìn)了LVI?SAM算法。在關(guān)鍵幀選取上，SC?LVI?SAM算法比原方法選幀減少19%，但是定位精度不變，可以有效減少計(jì)算冗余。接下來(lái)，對(duì)SC?LVI?SAM算法而言，主要將進(jìn)行對(duì)scancontext矩陣匹配精度問(wèn)題的解決。

參考文獻(xiàn)：

[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.

作者簡(jiǎn)介：

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