基于激光雷達(dá)的詞袋回環(huán)檢測(cè)算法研究

作者簡(jiǎn)介：陳宇航（1998— ），男，重慶人，碩士；研究方向：多傳感器融合建圖與定位。

摘要：回環(huán)檢測(cè)作為同步建圖與定位（Simulation Localization and Mapping， SLAM）算法中的基本組成部分，能有效關(guān)聯(lián)相同場(chǎng)景之間的特征信息，提供全局一致性的位姿估計(jì)。基于詞袋（Bag of Words， BoW）模型的回環(huán)檢測(cè)算法在視覺SLAM領(lǐng)域有著顯著成效，但對(duì)于激光雷達(dá)SLAM算法，主流的方法無(wú)法實(shí)時(shí)有效地識(shí)別回環(huán)場(chǎng)景，且通常無(wú)法校正完整的六自由度（6 Degree of Freedom，6-DOF）環(huán)路姿態(tài)。針對(duì)以上問題，文章提出了一種基于線性關(guān)鍵點(diǎn)特征表示的詞袋模型，用于激光雷達(dá)SLAM中的實(shí)時(shí)回環(huán)檢測(cè)。該詞袋模型計(jì)算性能高效，可滿足自動(dòng)駕駛實(shí)時(shí)性要求。同時(shí)，算法具有穩(wěn)定的姿態(tài)校正能力，可用于精確的點(diǎn)對(duì)點(diǎn)匹配。在公開數(shù)據(jù)集上，將文章提出的方法嵌入激光SLAM算法中進(jìn)行閉環(huán)性能評(píng)估。結(jié)果表明，基于詞袋模型的回環(huán)檢測(cè)算法在激光SLAM領(lǐng)域優(yōu)于現(xiàn)有的主流方法。

關(guān)鍵詞：詞袋；回環(huán)檢測(cè)；線性關(guān)鍵點(diǎn)特征；實(shí)時(shí)

中圖分類號(hào)：TN958.98" 文獻(xiàn)標(biāo)志碼：A

0" 引言

高效且魯棒的閉環(huán)是長(zhǎng)期同步建圖與定位的基本要求之一。經(jīng)過長(zhǎng)時(shí)間的里程計(jì)估計(jì)，位姿將產(chǎn)生較大的漂移，而標(biāo)準(zhǔn)的幀間匹配算法難以實(shí)現(xiàn)當(dāng)前觀測(cè)值與回環(huán)時(shí)的位置對(duì)齊。回環(huán)檢測(cè)算法可以提供正確的數(shù)據(jù)關(guān)聯(lián)和全局一致性的位姿估計(jì)以避免漂移現(xiàn)象的產(chǎn)生。在視覺SLAM領(lǐng)域，許多算法基于詞袋模型比較圖像的2D特征并進(jìn)行匹配。詞袋模型實(shí)現(xiàn)了非常高效的圖像檢索能力，推動(dòng)了視覺SLAM的發(fā)展。FABMap［1］使用樹形結(jié)構(gòu)來學(xué)習(xí)離線單詞的共視概率，但由于依賴Surf［2］，導(dǎo)致其在特征提取上花費(fèi)大量時(shí)間。DBoW2［3］使用基于Brief描述符［4］的二進(jìn)制詞袋以及高效的Fast特征檢測(cè)器，極大提高了重復(fù)檢索的效率。對(duì)于使用3D點(diǎn)云的方法，周治國(guó)等［5］采用強(qiáng)度掃描上下文的全局描述符進(jìn)行回環(huán)檢測(cè)，集成了點(diǎn)云的幾何和強(qiáng)度特征以減少漂移誤差。M2DP［6］將3D點(diǎn)云投影到多個(gè)二維平面，并為每個(gè)平面中的點(diǎn)生成密度簽名。使用簽名的奇異值分解分量來計(jì)算全局描述符，該描述符對(duì)噪聲影響和分辨率變化更具魯棒性。Scan-Context［7］將點(diǎn)云掃描轉(zhuǎn)換為可見空間，并使用相似度分?jǐn)?shù)來計(jì)算2個(gè)掃描上下文之間的距離。然而，該方法只能提供環(huán)路中幀間的相對(duì)偏航角估計(jì)，無(wú)法校正環(huán)路完整的6-DoF位姿。因此，本文提出了一種新穎的詞袋模型，基于3D點(diǎn)云特征構(gòu)建線性關(guān)鍵點(diǎn)表示的特征數(shù)據(jù)庫(kù)，實(shí)時(shí)且有效地識(shí)別回環(huán)場(chǎng)景。為了實(shí)現(xiàn)快速檢索，采用哈希表作為數(shù)據(jù)庫(kù)的基本結(jié)構(gòu)，實(shí)現(xiàn)在線回環(huán)檢索和更新。同時(shí)，所提出的詞袋模型可實(shí)時(shí)校正完整的6-DoF回環(huán)位姿，并用于位姿圖優(yōu)化過程以實(shí)現(xiàn)魯棒的回環(huán)約束。

1" 算法研究

本文提出的基于詞袋模型的回環(huán)檢測(cè)算法整體流程如圖1所示。首先從原始點(diǎn)云中提取線性關(guān)鍵點(diǎn)表示的3D特征，并嵌入LeGO-LOAM［8］算法中進(jìn)行里程計(jì)位姿估計(jì)；然后使用提出的詞袋模型檢測(cè)閉環(huán)并修正位姿，若檢測(cè)到閉環(huán)且優(yōu)化了相應(yīng)環(huán)路，則向LeGO-LOAM算法提供反饋并更新局部地圖。該方法為后續(xù)位姿圖優(yōu)化過程提供更準(zhǔn)確的位姿估計(jì)。

1.1" 特征提取

采用線性關(guān)鍵點(diǎn)作為描述符表示相應(yīng)區(qū)域中的特定關(guān)鍵點(diǎn)，該方法無(wú)需通過離線方式構(gòu)建樹形結(jié)構(gòu)的單詞來轉(zhuǎn)換特征。線性關(guān)鍵點(diǎn)描述符的核心思想是使用鄰域信息來表示當(dāng)前關(guān)鍵點(diǎn)，由邊緣關(guān)鍵點(diǎn)提取和邊緣關(guān)鍵點(diǎn)聚合兩部分組成。在關(guān)鍵點(diǎn)提取中，將激光雷達(dá)點(diǎn)云大致分為兩種類型：邊緣點(diǎn)和平面點(diǎn)。根據(jù)點(diǎn)在局部平面的平滑程度區(qū)分兩種類型關(guān)鍵點(diǎn)。給定一幀3D點(diǎn)云Pl，令i為Pl中的一點(diǎn)，PS是與點(diǎn)i在同一掃描線上的一組連續(xù)點(diǎn)，并均勻分布在i的兩側(cè)。則當(dāng)前點(diǎn)i的平滑項(xiàng)定義如下：

i=1|S|‖∑j∈Ps，j≠i（pj-pi）‖2（1）

式中，pi和pj表示點(diǎn)i和j的坐標(biāo)。|S|表示PS的基數(shù)。

得到邊緣點(diǎn)之后，有很多高于Th的不穩(wěn)定點(diǎn)。這些點(diǎn)通常是分散的，如果利用這些點(diǎn)進(jìn)行匹配可能會(huì)導(dǎo)致匹配失效。因此，有必要將其過濾掉并找到真正有效的邊緣關(guān)鍵點(diǎn)。本文基于同一垂直邊緣的每個(gè)點(diǎn)具有大致相同的水平角，將以激光雷達(dá)坐標(biāo)系為中心的平面均勻劃分為N個(gè)扇形區(qū)域，然后僅對(duì)每個(gè)扇形區(qū)域內(nèi)的點(diǎn)進(jìn)行聚類，提升了算法效率。

1.2" 回環(huán)檢索

詞袋模型檢索線性關(guān)鍵點(diǎn)表示的單詞并統(tǒng)計(jì)其在每幀點(diǎn)云中出現(xiàn)的頻率。如果某位置的最高頻率大于頻率閾值Thf，則該位置被認(rèn)為是候選位置。為了快速檢索，使用類似于逆文檔頻率的比率因子來衡量當(dāng)前集合與所有集合中的平均值之間的差異，定義如下：

ratio=Nset/（Nnw）（2）

式中，Nset表示當(dāng)前集合中的位置數(shù)，Nnw表示一組點(diǎn)數(shù)量的平均值。

1.3" 回環(huán)校正

回環(huán)校正為后端位姿圖優(yōu)化提供相應(yīng)約束。首先根據(jù)線性關(guān)鍵點(diǎn)的匹配結(jié)果計(jì)算環(huán)路的觀測(cè)約束，然后使用RANSAC方法去除未匹配的點(diǎn)，從而計(jì)算回環(huán)幀l和當(dāng)前幀c之間的位姿估計(jì)。給定當(dāng)前幀點(diǎn)集{S}c和回環(huán)幀的匹配點(diǎn)集{S}l，最小化代價(jià)函數(shù)：

rl，c（Rl，c，tl，c）=12∑ni=1‖sil-（Rl，csic+tl，c）‖2（3）

式中，sil∈{S}l和sic∈{S}c是對(duì)應(yīng)的匹配點(diǎn)。Rl，c和tl，c分別是變換矩陣Tl，c的旋轉(zhuǎn)和平移。

1.4" 回環(huán)優(yōu)化

經(jīng)過回環(huán)校正后的位姿用于構(gòu)建回環(huán)幀的位姿圖供后續(xù)優(yōu)化過程使用。其中，位姿圖的頂點(diǎn)由待優(yōu)化的全局位姿構(gòu)成，位姿圖的邊表示觀測(cè)約束，由連續(xù)幀之間的相對(duì)位姿和校正后的回環(huán)位姿組成。將第i幀和第j幀之間的殘差定義為：

ri，j（Tw，i，Tw，i）=ln（T-1i，jT-1w，iTw，j）∨（4）

位姿圖通過最小化以下代價(jià)函數(shù)來優(yōu)化：

minT∑（i，j）∈S‖ri，j‖2+∑（i，j）∈L‖ri，j‖2（5）

式中，S是所有順序邊的集合，L是所有回環(huán)邊的集合。

經(jīng)過位姿圖優(yōu)化后，根據(jù)優(yōu)化后的全局位姿對(duì)局部地圖中的點(diǎn)進(jìn)行更新，實(shí)現(xiàn)全局一致性的位姿估計(jì)。

2" 實(shí)驗(yàn)結(jié)果及分析

本文采用KITTI數(shù)據(jù)集來驗(yàn)證詞袋模型回環(huán)檢測(cè)算法精度。該數(shù)據(jù)集包含22個(gè)不同室外環(huán)境的數(shù)據(jù)序列。其中，11個(gè)序列（00-10）提供了由GPS獲取的真值軌跡，可用于精度評(píng)價(jià)工作。本研究將回環(huán)檢測(cè)算法嵌入LeGO-LOAM算法中，與主流Scan-Context回環(huán)檢測(cè)算法進(jìn)行對(duì)比。為了驗(yàn)證整個(gè)軌跡的準(zhǔn)確性，使用均方根誤差（Root Mean Square Error， RMSE）進(jìn)行評(píng)估，其定義如下：

RMSE=（1m∑mi=1‖trans（Q-1iPi）‖2）12（6）

式中，Qi表示軌跡真值，Pi表示估計(jì)位姿。

實(shí)驗(yàn)結(jié)果如圖2所示，圖2（a）為構(gòu)建的全局點(diǎn)云地圖，圖2（b）中矩形框?yàn)镾can-Context回環(huán)檢測(cè)結(jié)果，橢圓框表示本文提出的詞袋模型回環(huán)檢測(cè)算法結(jié)果。結(jié)果表明，本文提出的回環(huán)檢測(cè)算法在回環(huán)場(chǎng)景中的表現(xiàn)優(yōu)于Scan-Context算法，在回環(huán)場(chǎng)景中能夠正確關(guān)聯(lián)相似特征。

采用2種回環(huán)檢測(cè)算法分別對(duì)位姿進(jìn)行優(yōu)化，結(jié)果和真值軌跡進(jìn)行對(duì)比，如圖3所示。同時(shí)，分析該序列下使用2種回環(huán)檢測(cè)算法時(shí)的旋轉(zhuǎn)誤差、平移誤差以及均方根誤差，結(jié)果如表1所示，本文提出的算法在3項(xiàng)評(píng)價(jià)指標(biāo)上均優(yōu)于Scan-Context方法。

3" 結(jié)語(yǔ)

本文提出了一種基于線性關(guān)鍵點(diǎn)特征表示的詞袋模型回環(huán)檢測(cè)算法，由回環(huán)檢索、更新和優(yōu)化3個(gè)部分組成。在回環(huán)場(chǎng)景中，本算法有效關(guān)聯(lián)了點(diǎn)云之間的特征信息，提供了全局一致性的位姿估計(jì)。同時(shí)，本文算法具有較高的實(shí)時(shí)性和穩(wěn)定的姿態(tài)校正能力，優(yōu)于主流的回環(huán)檢測(cè)算法。算法可拓展于激光雷達(dá)SLAM領(lǐng)域，以提高系統(tǒng)的效率和魯棒性。

參考文獻(xiàn)

［1］CUMMINS M， NEWMAN P. Appearance-only SLAM at large scale with FAB-MAP 2.0［J］. The International Journal of Robotics Research， 2011（9）： 1100-1123.

［2］BAY H， TUYTELAARS T， GOOL L. Surf： speeded up robust features： European Conference on Computer Vision， May 07-13， 2006［C］. Berlin： Springer， 2006.

［3］GLVEZ-LPEZ D， TARDOS J D. Bags of binary words for fast place recognition in image sequences［J］. IEEE Transactions on Robotics， 2012（5）： 1188-1197.

［4］CALONDER M， LEPETIT V， STRECHA C， et al. Brief： binary robust independent elementary features： European Conference on Computer Vision， September 05-11， 2010［C］.Berlin： Springer， 2010.

［5］周治國(guó)，邸順帆.基于強(qiáng)度掃描上下文回環(huán)檢測(cè)的激光雷達(dá)SLAM算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2022（6）： 738-745.

［6］HE L， WANG X， ZHANG H. M2DP： a novel 3D point cloud descriptor and its application in loop closure detection： in 2016 IEEE International Conference on Intelligent Robots and Systems （IROS）， October 09-14， 2016［C］. Daejeon： IEEE， 2016.

［7］KIM G， KIM A. Scan context： egocentric spatial descriptor for place recognition within 3D point cloud map： in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）， October 01-05， 2018［C］. Madrid： IEEE， 2018.

［8］SHAN T， ENGLOT B. LeGO-LOAM： lightweight and ground-optimized lidar odometry and mapping on variable terrain： in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）， October 01-05， 2018［C］. Madrid： IEEE， 2018.

（編輯" 王永超）

Research on Bag of Words loop detection algorithm based on lidar

Chen" Yuhang

（Chongqing Jiaotong University， Chongqing 400074， China）

Abstract：" As a basic component of the Simulation Localization and Mapping （SLAM） algorithm， loop detection can effectively correlate feature information between the same scenes and provide globally consistent pose estimation. The loop detection algorithm based on the Bag of Words （BoW） model has achieved remarkable results in the field of visual SLAM. However， for the lidar SLAM algorithm， the mainstream method cannot effectively identify the loop scene in real time， and usually cannot correct the complete six degrees of freedom loop stance. In response to the above problems， this paper proposes a bag-of-word model based on linear key point feature representation for real-time loop detection in lidar SLAM. The bag-of-words model has efficient computing performance and can meet the real-time requirements of autonomous driving. At the same time， the algorithm has stable attitude correction capabilities and can be used for accurate point-to-point matching. On public data sets， the method proposed in this article is embedded into the laser SLAM algorithm for closed-loop performance evaluation. The results show that the loop detection algorithm based on the bag-of-words model is superior to the existing mainstream methods in the field of laser SLAM.

Key words： Bag of Words; loop detection; linear key point features; real-time