基于快速回環(huán)檢測(cè)的室外環(huán)境下激光雷達(dá)SLAM 算法

徐曉蘇，李 諾，姚逸卿

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping,SLAM）是移動(dòng)機(jī)器人在未知環(huán)境中實(shí)現(xiàn)自主導(dǎo)航[1]的一項(xiàng)基本要求。激光雷達(dá)SLAM 是利用激光雷達(dá)傳感器進(jìn)行自身位姿估計(jì)和構(gòu)建周圍環(huán)境地圖的一項(xiàng)技術(shù)。由于激光雷達(dá)具有測(cè)距精度高、視角廣的優(yōu)勢(shì)，并且對(duì)光照不敏感等特點(diǎn)，激光雷達(dá)SLAM 在自動(dòng)駕駛、無(wú)人自主導(dǎo)航領(lǐng)域已取得較大的發(fā)展。作為激光雷達(dá)SLAM 的關(guān)鍵環(huán)節(jié)，回環(huán)檢測(cè)通過(guò)計(jì)算幀間點(diǎn)云相似性判斷移動(dòng)機(jī)器人是否經(jīng)過(guò)同一位置，并在后端位姿圖中增加閉環(huán)位姿，最終利用位姿圖優(yōu)化校正位姿并生成全局一致的點(diǎn)云地圖。尤其是在室外環(huán)境下，沒有回環(huán)檢測(cè)會(huì)導(dǎo)致激光雷達(dá)SLAM 位姿誤差隨時(shí)間累積，從而影響SLAM 后端建圖的精度和效果。

針對(duì)回環(huán)檢測(cè)，Shan 等人于2016 年提出了LeGO-LOAM[2]，該算法引入地面優(yōu)化進(jìn)行位姿估計(jì)，并集成了回環(huán)檢測(cè)校正軌跡漂移，但是存在回環(huán)檢測(cè)失敗或漏檢的情況；Kim 等人于2018 年提出了一種用于回環(huán)檢測(cè)的掃描上下文Scan Context[3]，該算法不依賴直方圖[8]和先驗(yàn)信息實(shí)現(xiàn)回環(huán)幀的搜索和匹配，但是該算法僅有最大高度信息被編碼在圖像中，會(huì)丟失大量信息，并且采用了暴力匹配方案，耗時(shí)增加；Lin等人[4]提出了一種基于2D 直方圖的回環(huán)檢測(cè)算法，該算法采用2D 直方圖的相似性作為當(dāng)前關(guān)鍵幀與地圖中關(guān)鍵幀的相似性度量，該方案克服了旋轉(zhuǎn)的影響，產(chǎn)生了可靠和準(zhǔn)確的回環(huán)檢測(cè)；Shi 等人[5]在Scan Context 的基礎(chǔ)上加以改進(jìn)，通過(guò)建立局部參考幀，使其對(duì)視點(diǎn)變化更具魯棒性，并提出了一種三階段匹配算法進(jìn)行有效的回環(huán)檢測(cè)，有效地提升了回環(huán)檢測(cè)的精度和速度；Intensity SLAM[6]在Scan Context 基礎(chǔ)上提出了強(qiáng)度掃描上下文描述符用于回環(huán)檢測(cè)，并采用基于強(qiáng)度圖和幾何特征的方式進(jìn)行回環(huán)幀間配準(zhǔn)，有效提高了定位精度和建圖準(zhǔn)確性；仉等人[7]于2019 年提出了一種基于四元數(shù)卷積神經(jīng)網(wǎng)絡(luò)的閉環(huán)檢測(cè)算法，利用超像素分割和四元數(shù)CNN 對(duì)圖像進(jìn)行處理，有效減少了視覺里程計(jì)的累計(jì)誤差；Jiang 等人[8]于2020 年提出了一種基于特征的回環(huán)檢測(cè)算法，采用平面特征點(diǎn)的局部分布來(lái)計(jì)算特征方向，構(gòu)建點(diǎn)云子地圖的描述符，通過(guò)過(guò)濾并增強(qiáng)結(jié)構(gòu)化環(huán)境中的垂直障礙物權(quán)重，有效地提高了回環(huán)檢測(cè)算法的效率；鄭等人[9]于2020年提出了一種基于慣導(dǎo)和激光雷達(dá)的采煤機(jī)定位算法，該算法以激光雷達(dá)為輔助，基于組合導(dǎo)航系統(tǒng)卡爾曼濾波模型，有效地提高了采煤機(jī)的定位精度；Jiang 等人于2020 年提出了基于點(diǎn)云平面的回環(huán)檢測(cè)算法LiPMatch[10]，該算法使用幾何約束來(lái)進(jìn)行相似性評(píng)估，利用平面的法向量約束檢查符合幾何約束的候選關(guān)鍵幀，最后通過(guò)改進(jìn)的迭代最近點(diǎn)方法驗(yàn)證；Wang 等人[11]于2020 年提出了一種新的全局描述符Lidar Iris，該算法采用濾波和閾值計(jì)算在由Lidar Iris 表示的圖像上獲取特征圖像，通過(guò)計(jì)算兩幀點(diǎn)云之間的特征圖像的漢明距離計(jì)算相似度，實(shí)現(xiàn)了快速和精確的閉環(huán)檢測(cè)；Kim 等人于2021 年在Scan Context 的基礎(chǔ)上提出了魯棒性的回環(huán)檢測(cè)描述子Scan Context++[12]，該算法增加了對(duì)橫向和旋轉(zhuǎn)變化的魯棒性，并改進(jìn)了暴力匹配效率低下的問(wèn)題。

本文針對(duì)現(xiàn)有的激光雷達(dá)SLAM 方法在室外環(huán)境下位姿誤差隨時(shí)間積累導(dǎo)致定位精度低、回環(huán)檢測(cè)速度慢、建圖準(zhǔn)確度不高等問(wèn)題，提出用掃描上下文（Scan Context）提取全局特征，用Kd-tree 加速搜索獲取候選的回環(huán)幀，并結(jié)合激光雷達(dá)虹膜（Lidar Iris）計(jì)算回環(huán)幀間的相似度得分，通過(guò)基于殘差項(xiàng)加權(quán)的特征匹配獲取回環(huán)幀間的位姿變換，有效提升回環(huán)檢測(cè)的速度，最終生成全局一致的點(diǎn)云地圖。

1 A-LOAM 算法

1.1 點(diǎn)云預(yù)處理

A-LOAM 算法流程框圖如圖1 所示，從激光雷達(dá)采集的原始點(diǎn)云數(shù)據(jù)量大，在進(jìn)行激光里程計(jì)位姿估計(jì)前需要對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理，主要包含點(diǎn)云濾波和特征點(diǎn)云提取。

圖1 A-LOAM 算法流程框圖Fig.1 A-LOAM algorithm flow diagram

首先要濾除原始點(diǎn)云中的不可靠點(diǎn)云，這些點(diǎn)云最主要是因?yàn)榫嚯x激光雷達(dá)太近或太遠(yuǎn)，使得激光雷達(dá)返回信號(hào)變?nèi)跎踔羴G失，導(dǎo)致數(shù)據(jù)出現(xiàn)丟失。因此，本文利用點(diǎn)云PCL 庫(kù)中的無(wú)效點(diǎn)去除函數(shù)將這些點(diǎn)云作為噪聲點(diǎn)去除。進(jìn)行點(diǎn)云濾波后的每一幀點(diǎn)云提供了在空間直角坐標(biāo)系中以激光雷達(dá)中心為原點(diǎn)的x、y、z值和激光的反射強(qiáng)度信息。

在特征提取階段，將濾波后的點(diǎn)云根據(jù)曲率提取邊緣特征點(diǎn)和平面特征點(diǎn)。對(duì)于給定的第k幀點(diǎn)云，記表示第m個(gè)和第n個(gè)點(diǎn)云的坐標(biāo)（上標(biāo)L表示激光雷達(dá)坐標(biāo)系），S表示第m個(gè)點(diǎn)的臨近點(diǎn)數(shù)量，為了保證能正確反映點(diǎn)云的局部信息和實(shí)時(shí)運(yùn)算速度，一般取10。利用激光雷達(dá)局部曲面點(diǎn)云的曲率對(duì)點(diǎn)云進(jìn)行特征提取，記第m個(gè)點(diǎn)云的曲率為c，計(jì)算公式為：

1.2 激光里程計(jì)位姿估計(jì)

對(duì)于第一幀點(diǎn)云，直接存儲(chǔ)在特征子地圖中；對(duì)于第k(k≥2)幀點(diǎn)云，通過(guò)體素濾波對(duì)點(diǎn)云降采樣，與上一幀生成的全局特征子地圖匹配，利用匹配得到的位姿將第k幀點(diǎn)云加入到上一幀生成的全局特征子地圖中，然后對(duì)全局子地圖進(jìn)行閾值劃分，提取距激光雷達(dá)合適區(qū)域的點(diǎn)云。

其中，ρ為魯棒核函數(shù)，N(e)、N(s)表示角和面特征點(diǎn)的個(gè)數(shù)，通過(guò)高斯牛頓法求解此非線性方程得到激光里程計(jì)位姿估計(jì)。

2 基于快速回環(huán)檢測(cè)的激光SLAM 算法

2.1 算法整體框架

本文算法在機(jī)器人軟件平臺(tái)（Robot Operating System，ROS）上實(shí)現(xiàn)基于快速回環(huán)檢測(cè)的室外環(huán)境下激光SLAM 方法，其整體流程圖如圖2 所示。

圖2 算法流程框圖Fig.2 Algorithm frame work

在室外環(huán)境下，傳統(tǒng)的激光雷達(dá) SLAM 如A-LOAM 等由于缺乏回環(huán)檢測(cè)，位姿誤差會(huì)隨著時(shí)間積累導(dǎo)致定位精度低甚至定位失敗，從而建圖準(zhǔn)確度不高。基于迭代最近點(diǎn)（Iterative Closest Point,ICP）匹配的回環(huán)檢測(cè)算法存在匹配耗時(shí)、實(shí)時(shí)性不高，并且在室外環(huán)境下，經(jīng)過(guò)遞推獲得的回環(huán)幀間初值已經(jīng)存在較大累計(jì)誤差，依然用此初值會(huì)陷入局部最優(yōu)，導(dǎo)致迭代不能收斂到正確結(jié)果。針對(duì)傳統(tǒng)A-LOAM 存在的問(wèn)題，提出利用增加混合Scan Context和Lidar Iris的回環(huán)檢測(cè)模塊和位姿圖優(yōu)化模塊。在激光里程計(jì)線程中，輸入為激光雷達(dá)得到的點(diǎn)云，對(duì)點(diǎn)云進(jìn)行角點(diǎn)和面點(diǎn)的提取，然后進(jìn)行當(dāng)前幀與局部地圖匹配，通過(guò)點(diǎn)到直線和點(diǎn)到平面的距離構(gòu)建殘差方程，迭代求解殘差方程獲得激光里程計(jì)位姿。在建圖線程中，點(diǎn)云經(jīng)過(guò)由激光里程計(jì)線程獲取的位姿變換到世界坐標(biāo)系下，建立點(diǎn)云特征地圖。在回環(huán)檢測(cè)線程前，進(jìn)行關(guān)鍵幀的選取，然后利用關(guān)鍵幀點(diǎn)云獲取Scan Context描述子，通過(guò)Kd-tree 查找與當(dāng)前關(guān)鍵幀相似的候選關(guān)鍵幀，然后利用Lidar Iris描述子進(jìn)行特征圖像選取，計(jì)算關(guān)鍵幀與候選關(guān)鍵幀之間的相似度得分，獲取與當(dāng)前關(guān)鍵幀最相似的最優(yōu)關(guān)鍵幀。最后通過(guò)基于殘差加權(quán)的特征匹配約束求解當(dāng)前關(guān)鍵幀與最相似的候選關(guān)鍵幀之間的位姿變換，并將此變換加入到位姿圖中，利用全局位姿圖優(yōu)化進(jìn)行求解，獲取經(jīng)回環(huán)校正后的激光SLAM 位姿和全局一致的地圖。

2.2 關(guān)鍵幀選擇

在獲取激光里程計(jì)位姿之后，需進(jìn)行關(guān)鍵幀的選取?？紤]到回環(huán)幀間點(diǎn)云配準(zhǔn)是由特征匹配而不是ICP 匹配完成的，在關(guān)鍵幀中需要保證有足夠多的特征點(diǎn)云。如果滿足以下所有條件，則將激光雷達(dá)幀選取為關(guān)鍵幀：1）提取的角點(diǎn)和面點(diǎn)的特征點(diǎn)數(shù)量都需大于閾值εnum，以保證在回環(huán)檢測(cè)中有足夠的特征進(jìn)行點(diǎn)云配準(zhǔn)；2）里程計(jì)的平移變化顯著，需大于閾值εdis；3）里程計(jì)旋轉(zhuǎn)變化顯著，大于閾值εdeg。在室外環(huán)境下，設(shè)置更高的閾值可以降低成本，選取的關(guān)鍵幀存儲(chǔ)在位姿圖中。

2.3 提取描述子

假設(shè)激光雷達(dá)最大探測(cè)距離為L(zhǎng)max，以激光雷達(dá)為原點(diǎn)建立極坐標(biāo)系，并將以Lmax為半徑的圓沿徑向劃分為Nr個(gè)環(huán)區(qū)，沿角度方向劃分為Ns個(gè)扇區(qū)，形成彎曲網(wǎng)格，然后計(jì)算每個(gè)特征點(diǎn)云的極徑和極角，獲取該點(diǎn)云彎曲網(wǎng)格坐標(biāo)，并將每個(gè)網(wǎng)格中點(diǎn)云的最大高度作為網(wǎng)格值，沿徑向方向擴(kuò)展為二維圖像。

經(jīng)過(guò)上述過(guò)程，最終Scan Context 表示為N r×Ns的二維矩陣r，如圖3 所示，圖中不同的顏色表示每個(gè)彎曲網(wǎng)格內(nèi)的點(diǎn)云最大高度。

圖3 Scan Context 描述子矩陣Fig.3 Scan Context descriptor matrix

2.4 快速的最優(yōu)回環(huán)幀選取算法

由于Scan Context 描述符在選取最優(yōu)回環(huán)幀時(shí)采用暴力匹配，存在耗時(shí)的缺點(diǎn)，而Lidar Iris 特征則能快速高效的選取最優(yōu)回環(huán)幀。因此，本文在識(shí)別回環(huán)幀同時(shí)利用Scan Context 和Lidar Iris 特征進(jìn)行最優(yōu)回環(huán)幀的選取。在這一模塊，本文提出了一種快速的回環(huán)幀選取算法。算法的具體流程和步驟如下：

第一步：利用Scan Context 從關(guān)鍵幀中提取環(huán)向量作為描述符，每個(gè)環(huán)向量都被用來(lái)構(gòu)造Kd-tree，在Kd-tree 中通過(guò)最近鄰搜索選取與當(dāng)前關(guān)鍵幀的環(huán)鍵相似的環(huán)鍵對(duì)應(yīng)的關(guān)鍵幀作為回環(huán)候選關(guān)鍵幀。

使用L0范數(shù)計(jì)算 Scan Context 環(huán)向量平均值φ(rm)，計(jì)算公式為：

式中，rm表示第m行環(huán)向量，rj,m表示第m行向量的第j個(gè)元素。

利用Nr維φ(rm)組成對(duì)旋轉(zhuǎn)不敏感的描述符環(huán)向量，記為向量k，可表示為：

相比于二維矩陣Scan Context，環(huán)向量表示的信息更少，但是用做對(duì)于回環(huán)候選幀的選取具有更快的速度。

第二步：利用Lidar Iris 對(duì)多個(gè)回環(huán)候選關(guān)鍵幀進(jìn)行編碼，生成Lidar Iris 圖像，使用快速傅里葉變換對(duì)Lidar Iris 圖像進(jìn)行處理并對(duì)齊圖像，然后通過(guò)Log-Gabor 濾波器提取特征，利用獲取的二值化圖像進(jìn)行匹配求解兩片點(diǎn)云之間的變換，采用Hamming距離表示兩幅Lidar Iris 圖像之間的相似性。對(duì)于多個(gè)回環(huán)候選幀，選取Hamming 距離最小的候選幀作為最優(yōu)回環(huán)幀。

如圖4 所示，通過(guò)八位二進(jìn)制碼對(duì)每個(gè)彎曲網(wǎng)格內(nèi)點(diǎn)云的高度信息進(jìn)行編碼，從激光雷達(dá)特征點(diǎn)云中提取Lidar Iris 圖像，圖像的像素強(qiáng)度用八位二進(jìn)制碼轉(zhuǎn)化的十進(jìn)制數(shù)來(lái)表示，通過(guò)Log-Gabor 濾波器從Lidar Iris 圖像中提取特征：

圖4 提取Lidar Iris 圖像Fig.4 Extract Lidar Iris image

其中，G(f)表示濾波器的頻率響應(yīng)；f表示濾波器的中心頻率，依據(jù)激光雷達(dá)Iris 圖像生成，f0、σ為濾波器的參數(shù)，在Log-Gabor 濾波器中提取圖像Iris特征時(shí)，為定值，取0.75。濾波的結(jié)果為一個(gè)相位二值化圖像和一個(gè)幅值二值化圖像。

第三步：利用位姿閾值判斷唯一的最優(yōu)關(guān)鍵幀。為了剔除與當(dāng)前幀Lidar Iris圖像高度相似但是距離較遠(yuǎn)的最優(yōu)回環(huán)幀，我們采用當(dāng)前幀與最優(yōu)回環(huán)幀之間的位姿平移量是否小于某一閾值進(jìn)行判斷。此方案既保證了效率，又能有效地剔除實(shí)際不可靠的最優(yōu)候選幀。記表示當(dāng)前點(diǎn)云幀C到世界坐標(biāo)系W的位姿變換，表示回環(huán)幀到世界坐標(biāo)系的位姿變換，當(dāng)前幀到最優(yōu)回環(huán)幀的位姿變換表示為：

計(jì)算上述兩幀之間的位姿平移量t，如果小于閾值dthre，則粗略檢測(cè)回環(huán)成功：

其中，t x、t y和tz分別為t的x軸、y軸和z軸的分量，dthre取值為25。利用此方案能有效地減少錯(cuò)誤的回環(huán)檢測(cè)，從而提高SLAM 效率。

2.5 加權(quán)的回環(huán)幀間點(diǎn)云配準(zhǔn)

本文采用的回環(huán)幀間點(diǎn)云配準(zhǔn)方式為基于特征點(diǎn)的匹配方式，與1.2 節(jié)相似?；贗CP 的方式需要一個(gè)較好的初值作為迭代初值，但是在室外環(huán)境下，經(jīng)過(guò)遞推獲得的回環(huán)幀間的初值已經(jīng)存在較大累計(jì)誤差，依然用此初值會(huì)陷入局部最優(yōu)，導(dǎo)致迭代不能收斂到正確的結(jié)果。

考慮特征點(diǎn)云的位置和局部特性，根據(jù)特征點(diǎn)云到激光雷達(dá)的距離、特征點(diǎn)云的強(qiáng)度信息以及特征點(diǎn)云到平面或線的距離設(shè)計(jì)權(quán)重模型。記wi為參與點(diǎn)云匹配的第i個(gè)特征點(diǎn)云的總權(quán)重，其由三個(gè)權(quán)重因子組成：

其中，每個(gè)特征點(diǎn)云的權(quán)重wi、wj通過(guò)式(10)計(jì)算。

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

本文采用KITTI 數(shù)據(jù)集[13]來(lái)驗(yàn)證本文算法在室外環(huán)境下的定位精度、回環(huán)檢測(cè)效率和建圖效果。該數(shù)據(jù)集包含22 個(gè)不同室外環(huán)境的數(shù)據(jù)序列，11 個(gè)序列（00-10）提供了由GPS 獲取的相機(jī)軌跡真值，每個(gè)數(shù)據(jù)序列包含灰度圖像、彩色圖像和激光點(diǎn)云數(shù)據(jù)，由兩個(gè)高分辨率的彩色和灰度相機(jī)以及velodyne 激光雷達(dá)獲取。該數(shù)據(jù)集已成為室外驗(yàn)證視覺和激光SLAM 算法性能的基準(zhǔn)數(shù)據(jù)集。

驗(yàn)證數(shù)據(jù)集的實(shí)驗(yàn)平臺(tái)為具有16GB 運(yùn)行內(nèi)存、AMD Ryzen5-5600H 計(jì)算處理單元的個(gè)人電腦，操作系統(tǒng)為Ubuntu18.04LTS，ROS 版本為melodic。

本文采用帶有環(huán)路閉合的KITTI 里程計(jì)數(shù)據(jù)，即序列00、05、06 和08 來(lái)驗(yàn)證算法，四個(gè)序列的環(huán)境、位姿數(shù)量和路程如表1 所示。此外，我們還采用無(wú)人車（Unmanned Ground Vehicle,UGV）實(shí)驗(yàn)平臺(tái)在東南大學(xué)四牌樓校區(qū)記錄的序列對(duì)算法進(jìn)行了評(píng)估，UGV 實(shí)驗(yàn)平臺(tái)如圖5 所示。

表1 KITTI 數(shù)據(jù)集四個(gè)室外環(huán)境序列信息Tab.1 Four outdoor scene sequence information in the KITTI dataset

圖5 實(shí)驗(yàn)平臺(tái)Fig.5 Experiment platform

本文使用的評(píng)價(jià)指標(biāo)為KITTI 數(shù)據(jù)集定義的相對(duì)平移誤差（Relative Translation Error，RTE）和相對(duì)旋轉(zhuǎn)誤差(Relative Rotational Error，RRE)：

其中，Erot表示相對(duì)旋轉(zhuǎn)誤差（RRE），Etrans表示相對(duì)平移誤差（RTE），i和j表示幀號(hào)，l(i,j)表示第i幀和第j幀之間的距離，F(xiàn)表示為一個(gè)KITTI 序列幀的集合，Rot(·) 和Trans(·) 分別表示取位姿的旋轉(zhuǎn)量和平移量，?符號(hào)表示計(jì)算兩幀之間的相對(duì)位姿變換，和p分別表示位姿的估計(jì)值與真實(shí)值。

3.1 位姿精度實(shí)驗(yàn)

為了驗(yàn)證本文算法在室外環(huán)境下的性能，我們使用KITTI 數(shù)據(jù)集的00、05、06 和08 序列分別對(duì)本文的算法與真值進(jìn)行比較。這四個(gè)序列行駛路程均超過(guò)1000 m，并且包含回環(huán)檢測(cè)，能較合適地驗(yàn)證我們提出的改進(jìn)回環(huán)檢測(cè)算法的有效性，圖6-9 為在四個(gè)序列下本文算法（在圖中用our表示）、A-LOAM與KITTI數(shù)據(jù)集真值的軌跡對(duì)比。

圖6 KITTI00 俯視圖軌跡Fig.6 KITTI00 top view Trajectory

圖7 KITTI05 俯視圖軌跡Fig.7 KITTI05 top view trajectory

圖8 KITTI06 俯視圖軌跡Fig.8 KITTI06 top view trajectory

圖9 KITTI08 俯視圖軌跡Fig.9 KITTI08 top view trajectory

表2 為A-LOAM 算法與本文算法在四個(gè)室外環(huán)境下的相對(duì)平移誤差和相對(duì)旋轉(zhuǎn)誤差?？梢钥闯觯cA-LOAM 算法相比，本文算法的RTE 和RRE 都明顯減少，其中平均RTE 減少了0.9404%，平均RRE 減少了0.0032 °/m，平均性能提升50%左右。

表2 A-LOAM 和本文算法的RTE 和RRE 指標(biāo)對(duì)比Tab.2 Comparison of the RTE and RRE between A-LOAM and our algorithm

3.2 回環(huán)檢測(cè)實(shí)驗(yàn)

為了驗(yàn)證本文算法在回環(huán)檢測(cè)部分的性能，選取室外環(huán)境下05 序列來(lái)測(cè)試本文算法。如圖10 所示，A-LOAM 由于沒有回環(huán)檢測(cè)，產(chǎn)生的累計(jì)漂移使得軌跡產(chǎn)生了漂移，而本文算法應(yīng)用全局描述子進(jìn)行回環(huán)檢測(cè)，能夠準(zhǔn)確地搜索到正確的回環(huán)關(guān)鍵幀，并且能夠通過(guò)位姿圖優(yōu)化減小累計(jì)誤差的影響。

圖10 KITTI05 軌跡比較Fig.10 KITTI05 Trajectory comparison

為了體現(xiàn)本文算法在回環(huán)檢測(cè)算法的快速性，在獲取候選回環(huán)幀后，計(jì)算Lidar Iris 描述子和Scan Context 描述子提取最優(yōu)回環(huán)幀的時(shí)間。表3 為本文回環(huán)檢測(cè)算法與Scan Context 回環(huán)檢測(cè)算法的耗時(shí)性比較，可以看出，在提取最佳回環(huán)幀的階段，本文采用的 Lidar Iris 方案提取最佳回環(huán)幀過(guò)程耗時(shí)僅為150 ms 左右；在回環(huán)幀間點(diǎn)云配準(zhǔn)階段，可以從圖11看出，本文算法較傳統(tǒng)的ICP 方案，極大地縮短了回環(huán)幀間配準(zhǔn)時(shí)間。因此，本文算法能夠更快的執(zhí)行回環(huán)檢測(cè)。

表3 耗時(shí)性比較Tab.3 Time-consuming comparison

圖11 回環(huán)幀間配準(zhǔn)時(shí)間對(duì)比Fig.11 Comparison of registration time between loop frames

3.3 實(shí)測(cè)建圖實(shí)驗(yàn)

為了驗(yàn)證本文算法在室外環(huán)境下的準(zhǔn)確性，用UGV 實(shí)驗(yàn)平臺(tái)在東南大學(xué)四牌樓校區(qū)記錄的序列對(duì)算法進(jìn)行評(píng)估，衛(wèi)星地圖如圖12 所示，藍(lán)色區(qū)域?yàn)榛丨h(huán)部分。圖13 為本文算法建圖，圖14 為ALOAM 算法建圖，從圖中可以看出，A-LOAM 算法建立的地圖存在累計(jì)漂移，而本文算法建立的地圖消除了大部分由累計(jì)漂移產(chǎn)生的“鬼影”。因此，本文算法具有較高的實(shí)際場(chǎng)景魯棒性和定位精度。

圖12 衛(wèi)星地圖Fig.12 Satellite trajectory map

圖13 本文算法建圖Fig.13 Mapping of our algorithm

圖14 A-LOAM 建圖Fig.14 Mapping of A-LOAM

4 結(jié)論

本文提出了一種基于快速回環(huán)檢測(cè)的室外環(huán)境下激光SLAM 算法。在回環(huán)檢測(cè)上采用掃描上下文描述子粗選取多個(gè)回環(huán)幀，再利用激光雷達(dá)虹膜描述子精選取最優(yōu)的回環(huán)幀，有效的提高了回環(huán)幀的選取效率。在回環(huán)幀的匹配上，采用殘差項(xiàng)加權(quán)的特征匹配方案，快速準(zhǔn)確地實(shí)現(xiàn)了回環(huán)幀間的配準(zhǔn)。通過(guò)KITTI 數(shù)據(jù)集對(duì)本文算法進(jìn)行實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，與A-LOAM 相比，本文算法的平均性能提升50%左右，回環(huán)耗時(shí)有效減少，提升了室外環(huán)境下位姿估計(jì)精度和建圖準(zhǔn)確度。在實(shí)測(cè)的室外實(shí)驗(yàn)中，與A-LOAM 算法相比，本文算法在場(chǎng)景建圖的魯棒性上有較大提升。