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

祁 敏 宋勇磊

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

1 引言

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

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

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

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

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

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

2 整體算法流程

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

圖1 算法總體架構

第三步將重點描述激光雷達、IMU 數據融合方法以及特征提取算法。第四步重點講述自適應閾值的回環檢測方法。

3 基于Lidar/IMU的數據融合

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

3.1 IMU數據處理

3.1.1 IMU模型

對于IMU測量的角速度和加速度數據，構建其測量模型。其角速度（陀螺儀）測量模型和加速度（加速度計）測量模型為［10］

3.1.2 IMU預積分

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

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

如圖2 所示，利用式（7）和式（8）能夠將IMU 的離散信息轉換為連續變化的函數信息。

圖2 IMU預積分模型

3.2 激光里程計

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

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

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

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

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

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

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

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

圖3 平面特征檢測

至此，存儲每個聚類的平面特征點以及對應的法向量，用于后續幀間關聯。

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

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

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

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

4 后端優化

4.1 關鍵幀選取策略

將雷達的每一幀用于因子圖優化［11～14］是不現實的。因此，可以采用關鍵幀思想［7，15］選取因子圖的待優化節點，以減少待優化的幀數。

本篇文章對關鍵幀選取策略制定了標準：

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

2）當前幀跟蹤到的參考關鍵幀特征點數量小于75%。

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

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

4.2 自適應閾值的回環檢測

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

圖4 回環示意圖

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

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

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

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

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

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

5 實驗結果與分析

5.1 實驗設備

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

圖6 實驗設備

5.2 校園環境實驗評估

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

圖7 室內建圖效果

圖8 室外建圖效果

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

5.3 KITTI數據集評估

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

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

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

表1 KITTI 05回環效果評估

表2 KITTI數據集評估結果

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

6 結語

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

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