徐曉蘇，高佳譽，周 帥，姚逸卿

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

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

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

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

1 算法整體框架

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

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

2 改進的GICP 配準算法

2.1 標準GICP 算法介紹

GICP 是一種廣義的ICP 點云配準算法，是ICP 算法的擴展和改進。考慮每個點的局部特征，構(gòu)造局部平面，用馬氏距離替代歐式距離描述局部平面之間的差距，整合了點對點的ICP 和點對面的ICP 算法。

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

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

2.2 基于背景點的平面特征提取

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

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

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

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

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

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

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

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

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

其中，κ是一個小量，通常取0.001。

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

2.3 錯誤配對剪枝

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

（a）配對點之間的距離超過閾值εd：

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

（b）配對點對應(yīng)的曲率差距超過閾值εσ：

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

（c）配對點法向量內(nèi)積差距超過閾值εn：

法向量內(nèi)積差距過大常常意味兩個配對點云不在同一平面上，沒有配對價值，本文εn取0.9，也就是在兩點法向量夾角小于25°時才形成有效匹配。

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

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

定義向量η=(PT,nT)T，包含點的三維坐標和局部平面對應(yīng)的法向量，定義符號⊕進行如式(13)的運算：

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

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

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

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

每次迭代求解線性方程：

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

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

3 基于改進的GICP 配準的激光SLAM

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

3.1 IMU 預(yù)積分

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

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

IMU的幀間預(yù)積分增量能夠為激光點云的配準提供良好的初值，使配準算法能夠更快收斂。

3.2 基于子地圖匹配的激光里程計

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

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

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

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

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

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

3.3 全局優(yōu)化

全局優(yōu)化問題可以被描述為一個估計最大后驗概率的問題，本文用因子圖建模，將關(guān)鍵幀所在時刻的機器人狀態(tài)建模為因子圖中的節(jié)點，當前幀與前一關(guān)鍵幀間的位姿變化超過某一閾值則被判定為關(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（χ）由激光里程計相鄰關(guān)鍵幀間的變化定義：

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

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

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

3）回環(huán)因子

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

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

4 實驗結(jié)果與分析

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

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

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

4.1 改進效果對比實驗

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

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

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

4.2 大場景數(shù)據(jù)集實驗

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

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

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

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

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

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

4.3 實測實驗

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

圖8 實驗平臺Fig.8 Experiment platform

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

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

圖9 操場軌跡Fig.9 The trajectory of the playground

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

4.4 效率對比實驗

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

圖12 單幀處理時間對比Fig.12 Comparison of single-frame processing time

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

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

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

5 結(jié)論

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