基于改進(jìn)GICP 配準(zhǔn)的激光-慣性SLAM 算法

徐曉蘇，高佳譽(yù)，周 帥，姚逸卿

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

同步定位與建圖（Simultaneous Localization and Mapping,SLAM）是機(jī)器人導(dǎo)航和自主控制的核心技術(shù)之一[1]，也是自動(dòng)駕駛技術(shù)的關(guān)鍵。激光-慣性SLAM 系統(tǒng)結(jié)合了激光雷達(dá)和慣性傳感器的優(yōu)勢(shì)，能夠?qū)崿F(xiàn)高精度的自主定位和地圖構(gòu)建[2,3]。激光點(diǎn)云配準(zhǔn)通常被建模為一個(gè)非線性優(yōu)化問(wèn)題，配準(zhǔn)的性能對(duì)定位精度有決定性的影響[4]。多線激光雷達(dá)每幀掃描中包含的數(shù)據(jù)點(diǎn)數(shù)量龐大，而點(diǎn)云配準(zhǔn)的過(guò)程通常涉及最近鄰搜索，計(jì)算量隨掃描點(diǎn)數(shù)量的增長(zhǎng)呈指數(shù)級(jí)增長(zhǎng)，對(duì)算法的實(shí)時(shí)性帶來(lái)了很大的挑戰(zhàn)。

Shan 等[5]提出的LeGO-LOAM（Lightweight and Ground-Optimized Lidar Odometry and Mapping）通過(guò)提取線、面、地面特征，采用兩步優(yōu)化法實(shí)現(xiàn)位姿計(jì)算，達(dá)到了較好的精度和效率。周治國(guó)等[6]引入了權(quán)重函數(shù)來(lái)平衡匹配過(guò)程中線面特征的提取。Shan 等[7]改進(jìn)了LeGO-LOAM，提出了一種激光雷達(dá)與慣性測(cè)量單元（Inertial Measurement Unit,IMU）緊耦合的框架，提高了精度和效率，命名為L(zhǎng)IO-SAM（Lidar Inertial Odometry via Smoothing and Mapping）。以上基于特征的SLAM 算法能夠減少參與配準(zhǔn)的點(diǎn)云規(guī)模，并能夠利用同類點(diǎn)之間的配準(zhǔn)提高定位精度，但對(duì)特征的強(qiáng)依賴性使得系統(tǒng)容易在開(kāi)放的非結(jié)構(gòu)化環(huán)境中失效。因此，近年來(lái)研究人員提出了一系列基于直接配準(zhǔn)的SLAM 系統(tǒng)。HDL_GRAPH_SLAM[8]是一種基于正態(tài)分布變換（Normal Distributions Transform,NDT）配準(zhǔn)的算法[9]，該算法利用高斯分布重構(gòu)點(diǎn)云，進(jìn)行點(diǎn)到網(wǎng)格的配準(zhǔn)，但其對(duì)網(wǎng)格分辨率十分敏感，難以適用多種場(chǎng)景。FAST-LIO2[10]和FASTER-LIO[11]直接對(duì)所有點(diǎn)執(zhí)行點(diǎn)到面的配準(zhǔn)，但統(tǒng)一的點(diǎn)面配準(zhǔn)模型比較粗糙，且沒(méi)有回環(huán)檢測(cè)環(huán)節(jié)。廣義迭代最近點(diǎn)[12]（Generalized Iterative Closest Point,GICP）配準(zhǔn)算法不需要對(duì)點(diǎn)云進(jìn)行特征分類，利用馬氏距離整合了其他變種的ICP，在配準(zhǔn)過(guò)程中充分考慮了點(diǎn)云的局部特征，構(gòu)造了面-面配準(zhǔn)模型，具有更高的配準(zhǔn)精度，但這種算法高度依賴最近鄰搜索，計(jì)算量十分龐大。因此，Koide 等[13]提出了利用CPU 多線程進(jìn)行加速的Fast-GICP 和利用體素關(guān)聯(lián)的方式搜索最近鄰的VGICP（Voxelized GICP），VGICP 的計(jì)算效率更高，但體素化的操作同樣對(duì)分辨率敏感。Chen 等[14]提出了在GICP 過(guò)程中用NanoFLANN 搜索最近鄰的方法，提高了效率同時(shí)保證了精度?；谥苯优錅?zhǔn)法的SLAM 算法更加魯棒，不依賴結(jié)構(gòu)化特征，環(huán)境適應(yīng)性更強(qiáng)，但往往需要耗費(fèi)更高的算力，難以達(dá)成精度和效率的平衡，在更高的效率下追求精度是近年來(lái)激光SLAM 的主要發(fā)展方向。

本文受到文獻(xiàn)[14]的啟發(fā)，對(duì)GICP 算法提出了進(jìn)一步的改進(jìn)，作為SLAM 的配準(zhǔn)模型，平衡定位精度和效率。通過(guò)在背景點(diǎn)云中提取平面信息降低配準(zhǔn)規(guī)模，設(shè)計(jì)了一系列剪枝模型，減少錯(cuò)誤匹配的概率，并在迭代解算過(guò)程中加入了法向量作為目標(biāo)，提高對(duì)旋轉(zhuǎn)變換的約束。此外，本文針對(duì)改進(jìn)后的算法設(shè)計(jì)了一個(gè)高效的子地圖模型。最后，用因子圖融合了各項(xiàng)因子進(jìn)行全局優(yōu)化，提高了系統(tǒng)定位精度。

1 算法整體框架

基于改進(jìn)GICP 配準(zhǔn)的激光-慣性SLAM 系統(tǒng)的整體框架如圖1 所示。訂閱傳感器節(jié)點(diǎn)獲取激光點(diǎn)云和IMU 原始數(shù)據(jù)進(jìn)行點(diǎn)云預(yù)處理。利用IMU 數(shù)據(jù)對(duì)激光點(diǎn)云進(jìn)行畸變校正，后文統(tǒng)一將畸變校正后的點(diǎn)云稱為背景點(diǎn)云。接著對(duì)背景點(diǎn)云進(jìn)行體素降采樣，以獲取少量待配準(zhǔn)點(diǎn)云。將幀間IMU 預(yù)積分增量作為點(diǎn)云配準(zhǔn)的先驗(yàn)初值，以背景點(diǎn)云為基礎(chǔ)，提取降采樣后待配準(zhǔn)點(diǎn)云的局部平面信息。根據(jù)位姿鄰近關(guān)系提取關(guān)鍵幀點(diǎn)云集合，構(gòu)成子地圖，作為目標(biāo)點(diǎn)云進(jìn)行配準(zhǔn)。通過(guò)最小化平面間的馬氏距離及法向量約束迭代求解方程獲取激光里程計(jì)數(shù)據(jù)。根據(jù)激光里程計(jì)幀間變化進(jìn)行關(guān)鍵幀選取。最后，根據(jù)關(guān)鍵幀組成的激光里程計(jì)因子、回環(huán)因子和IMU 預(yù)積分因子進(jìn)行低頻的全局優(yōu)化，獲取全局一致的點(diǎn)云地圖和軌跡。

圖1 算法框架Fig.1 Overall frame of the system

2 改進(jìn)的GICP 配準(zhǔn)算法

2.1 標(biāo)準(zhǔn)GICP 算法介紹

GICP 是一種廣義的ICP 點(diǎn)云配準(zhǔn)算法，是ICP 算法的擴(kuò)展和改進(jìn)?？紤]每個(gè)點(diǎn)的局部特征，構(gòu)造局部平面，用馬氏距離替代歐式距離描述局部平面之間的差距，整合了點(diǎn)對(duì)點(diǎn)的ICP 和點(diǎn)對(duì)面的ICP 算法。

其中，Pi表示激光點(diǎn)的坐標(biāo)。在配對(duì)完美的情況下，正確的變換矩陣T*與配對(duì)點(diǎn)云之間存在以下關(guān)系：

對(duì)于剛性變換T，定義變換殘差di=bi-T⊙a(bǔ)i，則對(duì)應(yīng)的服從高斯分布：

2.2 基于背景點(diǎn)的平面特征提取

在多線激光雷達(dá)被廣泛應(yīng)用的背景下，單幀激光點(diǎn)云的數(shù)量通常在30000 到60000 之間，密集的點(diǎn)云會(huì)帶來(lái)爆炸性的計(jì)算量，因此，大多數(shù)SLAM 算法出于實(shí)時(shí)性考慮，會(huì)對(duì)原始點(diǎn)云做降采樣處理。但GICP算法的配準(zhǔn)精度隨點(diǎn)云數(shù)量下降而下降，主要原因是，算法通過(guò)最近鄰搜索收集待配準(zhǔn)點(diǎn)周圍的k個(gè)點(diǎn)（k通常取10～20）來(lái)計(jì)算協(xié)方差，而經(jīng)過(guò)大量降采樣后的點(diǎn)云難以準(zhǔn)確描述每個(gè)點(diǎn)真實(shí)的局部特征。GICP 算法頻繁使用昂貴的最近鄰搜索，精度與效率的矛盾尤為凸顯。

SLAM 中采用的GICP 算法通常在降采樣后的點(diǎn)云中搜索最近鄰，但為了保證精度，不會(huì)進(jìn)行分辨率過(guò)大的降采樣。針對(duì)這個(gè)問(wèn)題，本文提出了基于背景點(diǎn)的平面特征提取方法，僅對(duì)降采樣后的點(diǎn)做協(xié)方差計(jì)算，但在降采樣前的背景點(diǎn)云中搜索最近鄰，故計(jì)算量仍由降采樣后的點(diǎn)云數(shù)量決定，并且保證了不破壞局部平面特征。此外，為了減少孤立點(diǎn)對(duì)配準(zhǔn)結(jié)果的影響，在計(jì)算最近鄰時(shí)，根據(jù)近鄰點(diǎn)對(duì)應(yīng)的近鄰距離，剔除距離過(guò)于分散的采樣點(diǎn)，不讓其參與特征提取。本文用更高效的NanoFLANN 算法代替普通KDTree 進(jìn)行最近鄰搜索。

標(biāo)準(zhǔn)GICP 算法僅提取配準(zhǔn)點(diǎn)Pi所在局部平面的協(xié)方差Ci作為特征，本文在此基礎(chǔ)上增加法向量ni和曲率σi特征，用于提高后續(xù)匹配的精度。

設(shè)點(diǎn)Pi的k個(gè)鄰近點(diǎn)Pj,j=1,2…k，點(diǎn)Pi協(xié)方差矩陣通過(guò)式(6)(7)計(jì)算：

其中，μi表示均值。對(duì)協(xié)方差矩陣進(jìn)行奇異值分解（Singular Value Decomposition,SVD）后具有以下形式：

其中，λ1i、λ2i、λ3i表示按升序排列的特征值；Qi表示特征向量組成的矩陣 。本文用描述該點(diǎn)的曲率，σi越小，局部表面越平坦。對(duì)一幀示例點(diǎn)云進(jìn)行計(jì)算后，根據(jù)經(jīng)驗(yàn)，取0.05 作為閾值，曲率分布情況如圖2 所示。

圖2 曲率分布情況Fig.2 Distribution of curvature

圖2 中紅點(diǎn)表示曲率較大的點(diǎn)，藍(lán)點(diǎn)表示曲率較小的點(diǎn)，從圖中可以看出，利用該參數(shù)能達(dá)到與LeGOLOAM 系列特征分割類似的效果。

GICP 認(rèn)為真實(shí)世界中的表面至少是分段可微的，假設(shè)點(diǎn)云表征的環(huán)境是局部平面，每個(gè)點(diǎn)可以提供一個(gè)沿法線方向的約束，因此，可以將點(diǎn)云的原始協(xié)方差用局部平面的形式替代。已知協(xié)方差矩陣對(duì)稱，將SVD 分解后的特征值矩陣替換為代表平面的特征矩陣，則替換后的協(xié)方差矩陣具有以下形式：

其中，κ是一個(gè)小量，通常取0.001。

同時(shí)，提取最小特征值對(duì)應(yīng)的特征向量作為局部平面的法向量ni構(gòu)成特征用于后續(xù)配準(zhǔn)。

2.3 錯(cuò)誤配對(duì)剪枝

匹配的前提是將源點(diǎn)云通過(guò)變換矩陣投影到目標(biāo)點(diǎn)云坐標(biāo)系下，通過(guò)最近鄰搜索獲取與投影后的離源點(diǎn)最近的目標(biāo)點(diǎn)形成配對(duì)關(guān)系{i,j}，配對(duì)點(diǎn)分別表示為?？上攵?，錯(cuò)誤的配對(duì)數(shù)量越多，對(duì)收斂結(jié)果的影響越大，因而本文設(shè)計(jì)了一系列剪枝方案以減少配對(duì)錯(cuò)誤的概率，以下條件之一生效則剔除該配對(duì)關(guān)系：

（a）配對(duì)點(diǎn)之間的距離超過(guò)閾值εd：

這是最基本的剪枝思路，兩點(diǎn)間歐式距離差距過(guò)大，形成配對(duì)點(diǎn)的概率較低，本文εd取3.0 m。

（b）配對(duì)點(diǎn)對(duì)應(yīng)的曲率差距超過(guò)閾值εσ：

曲率差距過(guò)大意味著配對(duì)點(diǎn)周圍的局部特征可能有較大的差異，剪枝后可以降低角點(diǎn)與平面點(diǎn)配對(duì)的概率，本文εσ取1.3。

（c）配對(duì)點(diǎn)法向量?jī)?nèi)積差距超過(guò)閾值εn：

法向量?jī)?nèi)積差距過(guò)大常常意味兩個(gè)配對(duì)點(diǎn)云不在同一平面上，沒(méi)有配對(duì)價(jià)值，本文εn取0.9，也就是在兩點(diǎn)法向量夾角小于25°時(shí)才形成有效匹配。

2.4 基于馬氏距離和法向量的迭代優(yōu)化

與LeGO-LOAM 系列的迭代求解過(guò)程類似，每一次迭代，都將重新計(jì)算一系列配對(duì)點(diǎn){Pi,Pj}∈c，c表示配對(duì)點(diǎn)集合。標(biāo)準(zhǔn)GICP 算法將兩點(diǎn)之間的馬氏距離作為目標(biāo)函數(shù)，本文在此基礎(chǔ)上，增加法向量作為約束量，以提高對(duì)旋轉(zhuǎn)變換的約束。

定義向量η=(PT,nT)T，包含點(diǎn)的三維坐標(biāo)和局部平面對(duì)應(yīng)的法向量，定義符號(hào)⊕進(jìn)行如式(13)的運(yùn)算：

其中，η′ 表示變換后的向量。則六維殘差向量eij可表示為：

目標(biāo)函數(shù)被表示為：

最后利用Gauss-Newton 算法計(jì)算迭代增量，求解變換關(guān)系。定義六維狀態(tài)向量x為：

其中，θroll、θpitch、θyaw分別表示橫滾角、俯仰角、航向角；tx、ty、tz分別表示三軸的平移。

每次迭代求解線性方程：

其中，[ξ]×表示根據(jù)向量ξ得到的反對(duì)稱陣。

本節(jié)針對(duì)激光點(diǎn)云配準(zhǔn)性能與配準(zhǔn)效率平衡的問(wèn)題，研究并改進(jìn)了GICP 配準(zhǔn)算法。提出了用背景點(diǎn)云提取降采樣點(diǎn)云的局部特征的機(jī)制，設(shè)計(jì)了剪枝模型以減少點(diǎn)云配對(duì)出錯(cuò)的概率，最后采用馬氏距離和法向量同時(shí)作為約束迭代求解位姿，提高定位精度。

3 基于改進(jìn)的GICP 配準(zhǔn)的激光SLAM

本文遵循以下符號(hào)及坐標(biāo)系定義：L表示激光坐標(biāo)系，W表示世界坐標(biāo)系，B表示為IMU 坐標(biāo)系。在激光-慣性系統(tǒng)中，第一幀激光坐標(biāo)系被定義為世界坐標(biāo)系，B系與L系通過(guò)旋轉(zhuǎn)平移變換重合。移動(dòng)機(jī)器人的狀態(tài)χ為：

3.1 IMU 預(yù)積分

在激光-慣性系統(tǒng)中，IMU 頻率遠(yuǎn)高于激光雷達(dá)的頻率，對(duì)兩幀激光數(shù)據(jù)之間的IMU 進(jìn)行預(yù)積分可以很好地描述移動(dòng)機(jī)器人從時(shí)刻i到j(luò)的運(yùn)動(dòng)增量，假設(shè)兩幀激光分別處于i、j時(shí)刻，期間存在多幀時(shí)間間隔為Δt的IMU 數(shù)據(jù)。本文用文獻(xiàn)[15]給出的預(yù)積分公式計(jì)算i、j時(shí)刻間的旋轉(zhuǎn)與速度和位置的增量

其中，RbW表示從B系到W系的旋轉(zhuǎn)矩陣，表示重力加速度矢量。

IMU的幀間預(yù)積分增量能夠?yàn)榧す恻c(diǎn)云的配準(zhǔn)提供良好的初值，使配準(zhǔn)算法能夠更快收斂。

3.2 基于子地圖匹配的激光里程計(jì)

在大型場(chǎng)景中，當(dāng)前幀到前一幀的配準(zhǔn)容易積累誤差，因此，本文應(yīng)用更加全局一致的當(dāng)前幀到子地圖的配準(zhǔn)來(lái)抑制誤差的累積。子地圖由關(guān)鍵幀點(diǎn)云組成，根據(jù)IMU 預(yù)積分得到初值，用KD-Tree 搜索距當(dāng)前幀一定范圍內(nèi)的關(guān)鍵幀集合，并抽取關(guān)鍵幀位姿對(duì)應(yīng)的點(diǎn)云構(gòu)成子地圖集合。

子地圖內(nèi)所有的點(diǎn)以W系為參考坐標(biāo)系，所以要將L系下的關(guān)鍵幀點(diǎn)云PL投影至W系：

其中，PW表示世界坐標(biāo)系下的關(guān)鍵幀點(diǎn)云，表示對(duì)應(yīng)的旋轉(zhuǎn)矩陣。

此外，根據(jù)2.2 節(jié)的討論，每個(gè)點(diǎn)都需要計(jì)算三種特征參數(shù)，分別是法向量n、曲率σ、協(xié)方差矩陣C，這些參數(shù)同樣參與子地圖構(gòu)建過(guò)程。但對(duì)數(shù)量龐大的子地圖點(diǎn)云，逐個(gè)進(jìn)行最近鄰搜索計(jì)算特征顯然會(huì)耗費(fèi)大量算力，難以保證系統(tǒng)實(shí)時(shí)性，所以，本文通過(guò)旋轉(zhuǎn)變換將關(guān)鍵幀點(diǎn)云的特征直接由L系投影到W系。變換公式為：

直接投影的方法使得每個(gè)輸入系統(tǒng)的激光點(diǎn)只需進(jìn)行一次特征計(jì)算，極大地提高了系統(tǒng)的運(yùn)行效率。

本文采用第3 節(jié)提出的改進(jìn)后GICP 算法進(jìn)行點(diǎn)云配準(zhǔn)，將IMU 預(yù)積分后的結(jié)果作為初值，將降采樣后的去畸變點(diǎn)云作為待配準(zhǔn)點(diǎn)云，子地圖點(diǎn)云作為目標(biāo)點(diǎn)云，提取平面特征，進(jìn)行配準(zhǔn)，迭代求解位姿從而獲得激光里程計(jì)信息。

3.3 全局優(yōu)化

全局優(yōu)化問(wèn)題可以被描述為一個(gè)估計(jì)最大后驗(yàn)概率的問(wèn)題，本文用因子圖建模，將關(guān)鍵幀所在時(shí)刻的機(jī)器人狀態(tài)建模為因子圖中的節(jié)點(diǎn)，當(dāng)前幀與前一關(guān)鍵幀間的位姿變化超過(guò)某一閾值則被判定為關(guān)鍵幀，本文設(shè)定旋轉(zhuǎn)閾值為0.1 rad、平移閾值為1 m。

本系統(tǒng)應(yīng)用三種因子：激光幀間因子、預(yù)積分因子、回環(huán)因子。構(gòu)成的因子圖如圖3 所示。

圖3 因子圖Fig.3 Factor graph

1）激光幀間因子

激光幀間因子L（χ）由激光里程計(jì)相鄰關(guān)鍵幀間的變化定義：

其中，i、i+1分別表示相鄰關(guān)鍵幀的編號(hào)。

2）IMU 預(yù)積分因子

預(yù)積分因子I（χ）由3.1 節(jié)介紹的預(yù)積分增量定義：

3）回環(huán)因子

回環(huán)因子用于消除定位的累積誤差，不是本文的研究重點(diǎn)，采用LeGO-LOAM 系列基于歐氏距離的回環(huán)檢測(cè)方案，對(duì)檢測(cè)到的回環(huán)幀{i,j}進(jìn)行ICP 配準(zhǔn)，以前一幀為目標(biāo)點(diǎn)云，后一幀為待配準(zhǔn)點(diǎn)云，糾正后一幀的軌跡。本文對(duì)回環(huán)因子F（χ）定義如下：

本節(jié)闡述了改進(jìn)的GICP 配準(zhǔn)模型在激光-慣性SLAM 系統(tǒng)中的應(yīng)用，設(shè)計(jì)了一個(gè)高效的子地圖模型，能夠節(jié)省系統(tǒng)運(yùn)行時(shí)間，并介紹了IMU 預(yù)積分與全局優(yōu)化模型。

4 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證算法的精度和效率，本文進(jìn)行了公共數(shù)據(jù)集實(shí)驗(yàn)和實(shí)測(cè)實(shí)驗(yàn)。實(shí)驗(yàn)平臺(tái)為具有16 G 運(yùn)行內(nèi)存、AMD Ryzen 7 4800H 處理單元的個(gè)人電腦，操作系統(tǒng)為Ubuntu18.04，ROS 版本為melodic。通過(guò)與LIOSAM、LeGO-LOAM 算法對(duì)比，驗(yàn)證本文算法的優(yōu)越性能。評(píng)估精度采用的指標(biāo)為絕對(duì)軌跡誤差（Absolute Trajectory Error,ATE），第i幀ATE 定義為：

其中，S表示估計(jì)位姿Ni到真值Mi的轉(zhuǎn)移矩陣。本文用均方根誤差（Root Mean Squared Error,RMSE）來(lái)統(tǒng)計(jì)ATE，RMSE 的定義為：

其中，trans(EATE,i)表示三軸的平移誤差。

4.1 改進(jìn)效果對(duì)比實(shí)驗(yàn)

為了驗(yàn)證改進(jìn)GICP 配準(zhǔn)模型的定位精度優(yōu)于原始GICP 模型，采用KITTI 數(shù)據(jù)集00 序列進(jìn)行實(shí)驗(yàn)。KITTI 數(shù)據(jù)集包含城市、鄉(xiāng)村、高速公路等場(chǎng)景采集的真實(shí)數(shù)據(jù)，配置了64 線的Velodyne 激光雷達(dá)和采樣頻率為100 Hz 的IMU。00 是一個(gè)典型城市場(chǎng)景序列，包含大量直線和彎道，全長(zhǎng)3723.737 m，改進(jìn)前后的算法定位軌跡對(duì)比如圖4 所示。

圖4 改進(jìn)前后算法定位軌跡對(duì)比Fig.4 Comparison of trajectories between the original and proposed algorithms

圖4 中的original 表示改進(jìn)前的算法軌跡，proposed 表示改進(jìn)后的算法軌跡，與下文統(tǒng)一。從圖中可以直觀地看出，改進(jìn)后的軌跡更加貼近真值。用原始GICP 模型進(jìn)行配準(zhǔn)的SLAM 軌跡均方根誤差為10.971 m，改進(jìn)后的算法均方根誤差為3.294 m，改進(jìn)后精度提升了69.98%，說(shuō)明本文提出的改進(jìn)模型與原始的GICP 模型相比精度更高。

4.2 大場(chǎng)景數(shù)據(jù)集實(shí)驗(yàn)

本文選取KITTI 里程計(jì)分類的00、01、02 序列驗(yàn)證算法在大場(chǎng)景環(huán)境下的定位精度。00 序列已在4.1節(jié)中介紹。01 是一個(gè)高速公路場(chǎng)景，平均時(shí)速超過(guò)70 km/h，以直線行駛為主，全長(zhǎng)為2451.551 m。02 序列是里程計(jì)分類中軌跡最長(zhǎng)的序列，全長(zhǎng)5067.233 m。圖5-7 為本文算法、LIO-SAM、LeGO-LOAM 算法與真值的軌跡對(duì)比，表1 為三種算法在三個(gè)序列中的RMSE 對(duì)比。

表1 各算法在KITTI 數(shù)據(jù)集中RMSE 對(duì)比（單位：m）Tab.1 RMSE for algorithms on the KITTI dataset (Unit: m)

圖5 KITTI00 序列軌跡Fig.5 KITTI00 sequence trajectory

從表1 可以看出，本文算法在三個(gè)大型場(chǎng)景中的表現(xiàn)比LeGO-LOAM、LIO-SAM 算法更優(yōu)。圖5 顯示在00 序列中，本文算法與LeGO-LOAM 效果相似，在一些直線行駛的路段比LeGO-LOAM更貼近軌跡真值。LIO-SAM 表現(xiàn)相對(duì)不佳，主要由于該序列下的IMU 數(shù)據(jù)有一定缺失與跳變，而本文算法在IMU 表現(xiàn)不穩(wěn)定的情況下仍然能夠完成全局定位，魯棒性更好。圖6 顯示在01 序列中，LeGO-LOAM 算法的的軌跡誤差很大，這是由于在高速場(chǎng)景下缺乏IMU 輔助造成的，僅靠激光雷達(dá)自身無(wú)法區(qū)分高速公路上高度同質(zhì)化的特征，造成了退化。LIO-SAM 與本文算法在IMU 的輔助下表現(xiàn)更佳，且本文算法能更加充分地表征環(huán)境，因此全程的軌跡都比LIO-SAM 更貼合真值。在圖7 的長(zhǎng)距離序列02 中，本文算法與其他兩種算法相比也有更高的精度。在三個(gè)序列中，本文算法的平均軌跡均方根誤差相比LIO-SAM 算法降低62.42%，相比LeGO-LOAM 算法降低了88.44%。

圖6 KITTI01 序列軌跡Fig.6 KITTI01 sequence trajectory

圖7 KITTI02 序列軌跡Fig.7 KITTI02 sequence trajectory

4.3 實(shí)測(cè)實(shí)驗(yàn)

本文在校園中進(jìn)行了實(shí)測(cè)實(shí)驗(yàn)，測(cè)試了算法在64線Ouster 激光雷達(dá)和ZED2 相機(jī)中400 Hz 的IMU 配置下的性能表現(xiàn)，以中海達(dá)iRTK20 定位結(jié)果為真值，以松靈公司生產(chǎn)的bunker 底盤為載體，搭建了如圖8所示的實(shí)驗(yàn)平臺(tái)，底盤的最大速度為1.5 m/s。

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

實(shí)驗(yàn)場(chǎng)景包括操場(chǎng)、停車場(chǎng)、校園直道，與KITTI數(shù)據(jù)集相比，校園環(huán)境相對(duì)低速，操場(chǎng)場(chǎng)景比較空曠、停車場(chǎng)路線迂回曲折，校園直道場(chǎng)景幾何特征豐富。多種算法的軌跡對(duì)比結(jié)果如圖9-11 所示，精度對(duì)比如表2 所示。

表2 各算法在實(shí)測(cè)數(shù)據(jù)中RMSE 對(duì)比（單位：m）Tab.2 RMSE for algorithms in real test (Unit: m)

圖9 操場(chǎng)軌跡Fig.9 The trajectory of the playground

圖10 停車場(chǎng)軌跡Fig.10 The trajectory of the parking lot

圖11 校園直道軌跡Fig.11 The trajectory of the straight road

從圖9-11 可以看出，三種算法的軌跡在x、y軸上與真值比較接近，在z軸上有一定的區(qū)分，這與z軸尺度與x、y軸相比較小有關(guān)。在操場(chǎng)序列中，LIO-SAM算法的高程偏差很大，在平坦的操場(chǎng)地面上行駛時(shí)，最大與最小高程差距達(dá)到1 m 以上。LeGO-LOAM 由于分割地面點(diǎn)云并單獨(dú)配準(zhǔn)的機(jī)制，在平地上表現(xiàn)良好。本文的算法雖然沒(méi)有單獨(dú)對(duì)地面進(jìn)行分割，但利用背景點(diǎn)云提取局部平面特征的機(jī)制能更好地表征全局地面，且基于法向量夾角的誤匹配剪枝能夠剔除當(dāng)前幀與局部子地圖對(duì)應(yīng)點(diǎn)法向量差距過(guò)大的配對(duì)，比普通的點(diǎn)面配準(zhǔn)更加魯棒，所以在z軸上表現(xiàn)良好。停車場(chǎng)序列場(chǎng)景緊湊，彎道較多且轉(zhuǎn)彎半徑較小，LeGO-LOAM 由于缺乏IMU 輔助，定位效果不佳，LIO-SAM 算法與本文算法精度相似。校園直道序列全程無(wú)回環(huán)，主要考驗(yàn)算法的里程計(jì)性能，相比之下，本文算法的精度優(yōu)于其他兩種算法。從表2 可以看出，在三個(gè)場(chǎng)景中，本文算法的平均軌跡均方根誤差比LIO-SAM 算法降低24.77%，比LeGO-LOAM 算法降低了21.02%。

4.4 效率對(duì)比實(shí)驗(yàn)

為了驗(yàn)證本文算法對(duì)降采樣分辨率的包容度，對(duì)LIO-SAM 和本文算法在不同分辨率下的單幀處理時(shí)間和對(duì)應(yīng)的精度進(jìn)行了對(duì)比實(shí)驗(yàn)。以校園直道數(shù)據(jù)為例，對(duì)比了兩種算法配準(zhǔn)加優(yōu)化的單幀處理時(shí)間，如圖12 所示。事實(shí)上，本文對(duì)LIO-SAM 的時(shí)間統(tǒng)計(jì)并沒(méi)有包含特征提取過(guò)程，即使在這樣的情況下，本文算法在不同分辨率下仍然比LIO-SAM 節(jié)約了大約一半的時(shí)間，平均效率提高48.12%。

圖12 單幀處理時(shí)間對(duì)比Fig.12 Comparison of single-frame processing time

在不同降采樣分辨率下，兩種算法RMSE 的對(duì)比如表3 所示。

表3 各算法在不同降采樣分辨率下RMSE對(duì)比（單位：m）Tab.3 RMSE for algorithms at different downsample resolutions (Unit: m)

從表3 可以看出，本文算法相對(duì)于LIO-SAM，對(duì)降采樣分辨率的包容度更大，可以在更高的效率下達(dá)到更高的精度。

5 結(jié)論

本文提出了一種基于改進(jìn)GICP 配準(zhǔn)的激光-慣性SLAM 算法。該算法對(duì)標(biāo)準(zhǔn)GICP 進(jìn)行改進(jìn)，利用背景點(diǎn)云提取降采樣點(diǎn)云的局部特征，經(jīng)過(guò)一系列剪枝后，使用馬氏距離和法向量作為約束迭代求解位姿。同時(shí)，專門針對(duì)改進(jìn)的GICP 模型提出了一種高效的子地圖構(gòu)建方法。最后在KITTI 數(shù)據(jù)集上的多個(gè)序列和實(shí)際校園場(chǎng)景中進(jìn)行實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果表明：相比于LIO-SAM 和LeGO-LOAM，本文所提算法具有更好的精度、魯棒性和效率。