基于Lidar/IMU 緊耦合的SLAM 算法?

祁 敏 宋勇磊

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

機器人同時定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM），指的是機器人在未知環(huán)境中，通過傳感器連續(xù)感知周圍環(huán)境，完成自身定位與環(huán)境地圖創(chuàng)建的過程，是機器人領(lǐng)域研究的熱點和難點。目前，根據(jù)傳感器，SLAM 系統(tǒng)又可分為基于視覺、基于雷達以及基于多傳感融合等。由于視覺具有豐富的紋理和色彩信息，基于視覺的SLAM 系統(tǒng)在定位方面很有優(yōu)勢，但由于相機對光照變化敏感，將它作為唯一傳感器用于SLAM是不可靠的。

激光雷達能夠穩(wěn)定捕捉環(huán)境的精確信息的優(yōu)勢，但如果機器人劇烈運動，激光數(shù)據(jù)會發(fā)生畸變，并且由于輸出數(shù)據(jù)頻率較低，隨著運動劇烈的增加，會產(chǎn)生較大的累積誤差，不能滿足機器人定位和導(dǎo)航的需求。慣性測量單元（Inertial Measurement Unit，IMU）是一種不受外界干擾的位姿估計設(shè)備，可以用于估計機器人姿態(tài)的六個自由度：位置信息（x，y，z）和三個方向（橫滾、偏航、俯仰），具有較高的測量精度，并且數(shù)據(jù)的輸出頻率很高，通常在100Hz～1kHz，能夠提供激光掃描之間的穩(wěn)健狀態(tài)變化。將雷達與慣導(dǎo)能夠彌補激光雷達在劇烈運動時產(chǎn)生的數(shù)據(jù)畸變，并且可以用于因子圖的優(yōu)化以累積誤差的減少，兩者結(jié)合是實現(xiàn)6 自由度位姿估計并進行增量式地圖構(gòu)建的有效手段［1～2］。

現(xiàn)有的激光雷達/IMU 融合的工作主要分為兩類：松耦合和緊耦合。松耦合通過分別處理兩個傳感器，以推斷它們的運動約束。與松散耦合方法相比，緊耦合方法［3］則通過聯(lián)合優(yōu)化直接融合激光雷達和慣性測量，表現(xiàn)出較高的魯棒性和準確性，因此近年來引起了越來越多的研究興趣。LOAM［4］和Lego-LOAM［5］都使用了IMU 數(shù)據(jù)對激光獲得的數(shù)據(jù)進行畸變矯正，在一定程度上使得激光數(shù)據(jù)更可靠；LIOM［6］介紹了一種緊密耦合的激光雷達和IMU融合的方法，使用原始IMU 測量和Lidar 測量數(shù)據(jù)優(yōu)化系統(tǒng)狀態(tài)，在激光退化和運動快速的情況能有效優(yōu)化系統(tǒng)狀態(tài)。LIO-SAM［7］對IMU 數(shù)據(jù)進行預(yù)積分用于消除點云的畸變，并在圖優(yōu)化階段將預(yù)積分信息作為圖優(yōu)化的約束條件，進一步提高了IMU數(shù)據(jù)的利用率并且提高了地圖構(gòu)建的精度。R-LINS［8］使用誤差狀態(tài)卡爾曼濾波模型（ESKF）將雷達數(shù)據(jù)和IMU數(shù)據(jù)進行緊密耦合。

對于3D SLAM 技術(shù)，目前主要研究集中在估計機器人位姿［9］。SLAM 的整體框架大致可以分為激光里程計和后端優(yōu)化。在激光里程計部分，通常采用提取整幀點云中較為穩(wěn)定的點進行匹配，并從點與點的匹配上升到點與線、點與面的匹配。例如：LOAM、Lego-LOAM、LIO-SAM 等主流算法通過提取點云中結(jié)構(gòu)性較強的點進行幀間匹配，從而實現(xiàn)實時性較高的SLAM 系統(tǒng)。在后端優(yōu)化時，通過回環(huán)檢測以及因子圖［13～14］構(gòu)建點云幀之間的因子約束。針對提高6 自由度的地圖構(gòu)建的精度，本文提出兩點創(chuàng)新：

1）利用IMU 預(yù)積分值對激光數(shù)據(jù)進行畸變矯正，并提出一種自適應(yīng)參數(shù)RANSAC平面擬合算法進行面特征點的精提取。

2）提出自適應(yīng)閾值的回環(huán)檢測方法，摒棄固定閾值的回環(huán)檢測算法，利用機器人位姿變換自適應(yīng)閾值。

2 整體算法流程

整體算法框架如圖1，分為兩部分：激光Lidar/IMU 數(shù)據(jù)融合以及后端優(yōu)化。激光雷達以10Hz 的速率輸入點云幀，慣性測量單元以2kHz 的速度輸入傳感器的角速度和線加速度。第一步進行數(shù)據(jù)的預(yù)處理，對IMU 獲得數(shù)據(jù)進行預(yù)積分，將激光的噪聲點去除，然后利用IMU數(shù)據(jù)的預(yù)積分結(jié)果對激光點云進行畸變矯正；第二步使用自適應(yīng)參數(shù)RANSAC 擬合算法對矯正后的點云進行幾何特征提取并根據(jù)幀間關(guān)聯(lián)進行位姿估計；后端優(yōu)化部分使用因子圖進行地圖的全局優(yōu)化，此外，通過一些關(guān)鍵幀選取策略進行關(guān)鍵幀的篩選，并在對每一幀關(guān)鍵幀利用提出的自適應(yīng)閾值的回環(huán)檢測方法進行回環(huán)檢測，最后將激光里程計因子、回環(huán)因子與IMU 預(yù)積分因子作為因子圖的約束進行位姿的優(yōu)化。

圖1 算法總體架構(gòu)

第三步將重點描述激光雷達、IMU 數(shù)據(jù)融合方法以及特征提取算法。第四步重點講述自適應(yīng)閾值的回環(huán)檢測方法。

3 基于Lidar/IMU的數(shù)據(jù)融合

激光點云數(shù)據(jù)通過機械雷達獲取的顯著缺點是在橫向的數(shù)據(jù)采集時，由于時間的差異，存在一定程度的誤差，具體表現(xiàn)在激光獲取數(shù)據(jù)的頻率為10Hz，因此同一幀點云數(shù)據(jù)在激光初始旋轉(zhuǎn)獲得的數(shù)據(jù)點與當前幀結(jié)束獲得的點存在0.1s 的誤差。因此在進行點云特征提取之前，需要利用IMU數(shù)據(jù)對激光點云進行去畸變操作，以提高激光點的準確性，然后對激光點云進行特征提取，以在保留較多信息的基礎(chǔ)上降低數(shù)據(jù)的規(guī)模。

3.1 IMU數(shù)據(jù)處理

3.1.1 IMU模型

對于IMU測量的角速度和加速度數(shù)據(jù)，構(gòu)建其測量模型。其角速度（陀螺儀）測量模型和加速度（加速度計）測量模型為［10］

3.1.2 IMU預(yù)積分

根據(jù)IMU 動力學模型以及傳感器獲取的角速度和加速度，在t+?t內(nèi)機器人運動的線速度、空間中的位置以及狀態(tài)可以估計為［10］

相比于激光雷達，由于IMU 的輸出頻率很高，通常在100Hz～1kHz，為保持兩者數(shù)據(jù)的一致性，采用IMU預(yù)積分對相對運動約束進行再參數(shù)化，將高頻的加速度和角速度轉(zhuǎn)換為位姿變換，用于關(guān)鍵幀的位姿變換。我們通過［1］提出的IMU 預(yù)積分方法獲得時i 和時間j 獲得的采樣數(shù)據(jù)之間的預(yù)積分［10］測量值：

如圖2 所示，利用式（7）和式（8）能夠?qū)MU 的離散信息轉(zhuǎn)換為連續(xù)變化的函數(shù)信息。

圖2 IMU預(yù)積分模型

3.2 激光里程計

利用IMU 預(yù)積分可以獲取任意i、j時刻的平移和旋轉(zhuǎn)變換信息。通過激光的獲取頻率f以及水平方向的分辨率r，能夠計算每一個激光點pi獲取的時刻ti：

其中t0為當前幀第一列點云獲取時間，通常為點云的時間戳，col(i)為激光點在水平方向上的坐標偏移。由式（7）和式（9），pi去畸變后的坐標為

將式（10）應(yīng)用于點云的每個點，即可獲得去畸變點云信息，相比于原始點云，去畸變后的點云空間位置更精確，有利于后續(xù)的特征檢測。

進行特征檢測前，首先計算點云中點的曲率值，并通過曲率的大小對點進行線面特征點的分類，獲取粗糙的線面特征點信息，詳細步驟參照文獻［4］。

對于上述獲取的粗糙的面特征點，本文提出一種自適應(yīng)參數(shù)RANSAC 擬合算法進一步篩選面特征點并保存面特征點的法向量用于幀間估計，步驟如下：

1）對去畸變后的點云進行地面點的去除，提出一種高效地面去除算法，首先將三維空間的點云信息壓縮到二維平面，并將二維空間劃分為小顆粒的柵格圖，統(tǒng)計柵格內(nèi)點的數(shù)量信息，對于小于閾值n的柵格加入地面的候選點；

2）將步驟1）獲得的兩類點云集：地面候選點PG和PNG，分別進行歐式聚類，將點云轉(zhuǎn)換為空間內(nèi)離散的點云類；

3）此時步驟2）獲得的聚類由兩類可能：該類別內(nèi)只含一個平面和該類別含有多個平面。由于無法判斷聚類所屬類別，因此提出一種通用的平面檢測方法，如圖3，首先使用RANSAC 方法擬合其中的一個平面特征，包含的點云數(shù)量為nx，然后動態(tài)縮小擬合平面所需的點云閾值，m 為自定義參數(shù)，進行迭代的平面擬合。

圖3 平面特征檢測

至此，存儲每個聚類的平面特征點以及對應(yīng)的法向量，用于后續(xù)幀間關(guān)聯(lián)。

在點云匹配階段，利用IMU 預(yù)積分的先驗數(shù)據(jù)，將第k 幀點云特征變換到k+1 幀，假設(shè)，則有：

對于線特征點i?，特征點j、k?為i點最近的兩個點，有：

對于面特征點i?，特征點j?為距離點i最近的點，有：

在進行位姿估計之前，首先通過前一幀點云位姿結(jié)果與IMU預(yù)積分得到初始化當前幀位姿，將其作為初始位姿輸入Levenberg-Maquardt 算法最小化線、面特征點之間的距離以獲得機器人位姿ξ=(R,t)。

4 后端優(yōu)化

4.1 關(guān)鍵幀選取策略

將雷達的每一幀用于因子圖優(yōu)化［11～14］是不現(xiàn)實的。因此，可以采用關(guān)鍵幀思想［7，15］選取因子圖的待優(yōu)化節(jié)點，以減少待優(yōu)化的幀數(shù)。

本篇文章對關(guān)鍵幀選取策略制定了標準：

1）關(guān)鍵幀位姿之間的歐式距離需要滿足大于dε且兩幀獲取時間滿足不小于tε。

2）當前幀跟蹤到的參考關(guān)鍵幀特征點數(shù)量小于75%。

3）當前幀的線、面特征點數(shù)量不少于pline、pedge。

其中，1）、2）兩點主要解決關(guān)鍵幀之間需要有一定的差異性，3）主要用于保證當前幀的質(zhì)量，當前幀的特征點應(yīng)該是充足的且分布均勻的。

4.2 自適應(yīng)閾值的回環(huán)檢測

在SLAM 問題中，位姿估計往往是一個遞推的過程，即由上一幀的位姿解算當前幀位姿，因此存在將上一幀的位姿累計到下一幀的過程即累計誤差。回環(huán)的主要思想是通過減少中間過程的約束達到降低累積誤差的效果。如圖4，由于環(huán)境的復(fù)雜性，機器人建圖過程中往往會回到之前經(jīng)過的地方，通過將當前幀與累計誤差較少的幀進行位姿的推算從而達到減少累計誤差的效果。

圖4 回環(huán)示意圖

回環(huán)幀往往是機器人曾經(jīng)走過，并且再次經(jīng)過的幀，只有兩幀點云之間的位姿足夠近并且能夠相互關(guān)聯(lián)（重疊（關(guān)鍵點）），才能夠進行回環(huán)檢測。本文進行回環(huán)檢測的步驟為如圖5。

假設(shè)歷史關(guān)鍵幀為Ki(i?(1,2,…c-1))，當檢測到某一幀為關(guān)鍵幀Kc后，以空間位置Pi(xi,yi,zi)(i=1,2,…c-1)構(gòu)建KDTree 并篩選距離最近的k 幀作為候選回環(huán)幀，針對計算當前幀Kc與候選回環(huán)幀K?(? ?(1...k))之間的距離dc?：

當且僅當距離和時間滿足式（15）時，將K?作為候選回環(huán)幀。

此時使用的閾值dmax為固定值，一般為預(yù)設(shè)的，實際上存在隨著建圖規(guī)模的增加，回環(huán)幀檢測兩幀之間差距非常大的情況，此時需要人工對dmax的選取進行地圖規(guī)模的干預(yù)。針對這一問題，本文提出自適應(yīng)閾值的回環(huán)檢測方法，利用幀間關(guān)聯(lián)計算機器人的t時刻累積運動距離Dt：

其中，d0為回環(huán)距離的最小值，Dmin為預(yù)設(shè)的檢測回環(huán)幀的最短距離。最后，當檢測到回環(huán)時，將當前dmax折半以補充回環(huán)修正時修正的精度。

對于候選回環(huán)幀K?，將當前幀Kc與候選幀K?進行精配準。即找到兩幀的局部點云地圖Kc-k∪…∪Kc-1∪Kc∪Kc+1∪Kc+k與K?-k∪…∪K?-1∪K?∪K?+1∪K?+k，并使用ICP 算法進行精配準，并將最后得到的結(jié)果加入因子圖進行優(yōu)化［15］。

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

5.1 實驗設(shè)備

如圖6，搭建一種手持式建圖設(shè)備進行實驗驗證。該設(shè)備搭建了Velodyne 16 線激光雷達獲取周圍環(huán)境的點云信息，inertialLabs KERNEL-100 IMU獲取設(shè)備運動過程中的角速度和加速度。激光雷達以10Hz 的頻率采集點云信息，IMU 以1kHz 的頻率采樣傳感器的運動。

圖6 實驗設(shè)備

5.2 校園環(huán)境實驗評估

使用上述實驗設(shè)備在校園環(huán)境內(nèi)采集室內(nèi)走廊以及室外教學樓周圍的環(huán)境的激光和IMU數(shù)據(jù)，并使用上述算法進行地圖構(gòu)建，圖7 和圖8（a）展示了本文提出的算法在室內(nèi)和室外環(huán)境建地圖的效果圖。

圖7 室內(nèi)建圖效果

圖8 室外建圖效果

圖8（b）為室外環(huán)境建圖時回環(huán)檢測的效果，分析可得，提出的自適應(yīng)閾值回環(huán)策略通過能夠有效檢測回環(huán)幀。

5.3 KITTI數(shù)據(jù)集評估

通過KITTI 數(shù)據(jù)集［16］對提出的激光/IMU 緊耦合的SLAM 算法（以下簡稱為LI-SAM）進行了評估。KITTI數(shù)據(jù)集是目前國際上最大的自動駕駛場景下的激光雷達SLAM 算法評測數(shù)據(jù)集。KITTI包含市區(qū)、鄉(xiāng)村和高速公路等場景采集的真實城市與公路的激光數(shù)據(jù)，并提供了平均平移誤差以及平均旋轉(zhuǎn)誤差評工具。采用KITTI 數(shù)據(jù)集下的Raw Data［16］對應(yīng)的序列對LOAM、Lego-LOAM 和LI-SLAM進行評估比較。圖9（a）為使用LI-SLAM 算法運行KITII 05 序列的建圖效果圖，圖9（b）為三種SLAM算法運行KITTI 05序列數(shù)據(jù)集的軌跡，由軌跡預(yù)測圖可得，本文提出的LI-SALM 算法在大規(guī)模建圖且?guī)в谢丨h(huán)的軌跡預(yù)測精度優(yōu)于LOAM 和Lego-LOAM。

圖9 KITTI 05序列建圖與軌跡對比

表2 為本文提出的自適應(yīng)閾值的回環(huán)算法在KITTI 05 序列上的準確率評估效果，通過評估三個固定的dmax與自適應(yīng)dmax回環(huán)準確率，驗證本文提出的算法的有效性。由實驗可得，相比于固定閾值，自適應(yīng)dmax算法在通用的基礎(chǔ)上，回環(huán)檢測的準確率提升至95%左右。

表1 KITTI 05回環(huán)效果評估

表2 KITTI數(shù)據(jù)集評估結(jié)果

表2 為序列04～07 序列下的KITTI 數(shù)據(jù)集評估結(jié)果，由實驗數(shù)據(jù)可得，本文提出的LI-SLAM 算法在多數(shù)情況下對軌跡預(yù)測的精度提升起到了積極作用，由于KITTI 數(shù)據(jù)集IMU 數(shù)據(jù)參數(shù)未知，且06數(shù)據(jù)集不存在回環(huán)場景，因此位姿預(yù)測存在較大誤差。

6 結(jié)語

本文提出了一種Lidar/IMU 緊耦合的建圖方法，利用IMU 高頻率的特性對低頻Lidar 獲取到的點云進行去畸變，以提高激光原始點云的精度，在激光特征選取階段，對曲率檢測點云的線面特征點，提出自適應(yīng)閾值的RANSAC平面擬合算法獲取面特征點的法向量以提高激光里程計的精度。在后端優(yōu)化部分，提出自適應(yīng)閾值的回環(huán)檢測方法，消除累積誤差的影響。通過在校園環(huán)境內(nèi)建圖以及KITTI 數(shù)據(jù)集的評估，驗證了所提出的SLAM 算法的精確度。

本文提出的建圖方法采用融合了Lidar 和IMU的信息以提高建圖精度。但在結(jié)構(gòu)化不明顯的場景下難以進行地圖的精確構(gòu)建，后續(xù)將加入視覺傳感器，融合Lidar、IMU、視覺信息進行地圖構(gòu)建。