基于局部塊特征優化的室內高精度激光SLAM方法

蔡 睿,章國寶*,朱宏偉

(1.東南大學 自動化學院,江蘇 南京 211189; 2.南京深地智能建筑技術研究院,江蘇 南京 210019)

近年來,隨著傳感器硬件技術的發展完善,多線三維激光雷達、慣性測量單元(Inertial Measurement Unit,IMU)和全球導航衛星系統等傳感器在精度方面已經達到了較高的水準[1]。然而,實際測繪工程所需的并非簡單對單個傳感器采集信息的直接輸出,往往是對多個不同傳感器在實時運動狀態下采集的多種信息的融合處理[2-3]。在這一背景下,激光SLAM(Simultaneous Localization and Mapping,即時定位與地圖構建)算法即是以解決上述問題為目的,利用一種或多種傳感器[4]的數據,同時進行測繪平臺的定位和全局點云地圖生成的技術。當前多數解決方案僅采用三維激光雷達數據作為傳感器,或同時融合IMU等數據作為輔助[5-6],解決GNSS(Global Navigation Satellite System,全球導航衛星系統)在部分極端環境下無信號或信號弱、無法精準采集軌跡信息的難題。

目前,激光SLAM仍然是以三維激光雷達數據為主要信息來源,部分以IMU數據作為輔助提升平臺位姿計算精度。與視覺相機相比,三維激光雷達可以避免受到環境亮度的影響,而IMU輔助位姿計算可以減弱激光點云數據在運動中產生的累積誤差帶來的影響。二十世紀以來,SLAM技術逐漸實際應用于室內掃地機器人、自主駕駛系統、測繪建圖機器人等多個領域的產品中[7],且國內外涌現出一批優秀的解決方案,其中包含了Gmapping[8-9]、Cartographer[10]、LOAM(LiDAR Odometry and Mapping)[11]以及LeGO-LOAM(Lightweight and Ground-Optimized LOAM)[12]等。然而對于部分特殊環境,現有解決方案建圖效果失真的情況時有發生。

針對幾何特征匱乏的室內環境提出融合IMU數據的激光SLAM方法,分割地面點與非地面點后在點云位置信息和強度信息的深度圖中劃分局部塊,在局部塊內進行離群點篩除、幾何光滑度梯度和強度梯度計算,作為此局部塊的特征向量。IMU積分值作為求解位姿的初值,以兩幀特征之間的差值作為目標函數迭代求解。方法融合了三維激光點云的幾何信息、強度信息和IMU數據,提升了對室內幾何特征匱乏環境的定位與建圖系統的精度。

1 系統概述

本文設計系統接收三維激光點云原始數據和IMU數據,通過設計算法實現對室內環境的高精度定位與建圖功能。系統主要組成部分包含如下模塊。

① 數據預處理和分割。根據原始點云的坐標數據將其分割為地面點與非地面點兩類,并對點云的坐標數據和強度數據分別投影至柱面形成深度圖以便特征計算。

② 去除離群值和特征提取。對非地面范圍內的坐標深度圖和強度深度圖分別劃分局部塊,對局部塊內離群值進行剔除,并針對每個局部塊分別計算子區域的特征值梯度值作為該局部塊的特征向量。而對于地面點區域的坐標信息采用直接計算法向量的方式處理。

③ IMU數據積分。IMU的數據的采集頻率是高于點云數據一個數量級的,對每兩幀點云之間的IMU數據進行積分處理。這一模塊和以上兩個模塊是并行的。

④ 特征匹配和幀間位姿求解。采用IMU數據積分模塊的結果作為初值,以幀間特征差構建目標函數,利用L-M(Levenberg-Marquardt)方法迭代求解。

⑤ 地圖構建。利用當前幀提取的地面法向量和全局地圖地面法向量配準修正,最終更新地圖。

系統框架如圖1所示。

2 算法設計

2.1 數據預處理

2.1.1 地面點集分割

由于實際環境中地面環境較為單一,一般呈現類固定平面的狀態,包含信息較少且相對變化較少。因此對初始點云先采用文獻[13]中擬合地面平面并進行標記的方法分割為地面點集與非地面點集,非地面點用于提取特征,地面點用于提取平面法向量,地面點集分割效果如圖2所示。

圖2 地面點集分割效果

2.1.2 點云坐標與強度深度圖生成

對于非地面點Vk=Pk-Gk,由于原始點云數據量較大,ICP(Iterative Closest Point,迭代最近點)等現有點云匹配方法對每個點都進行匹配處理導致運算量大,對運行設備造成較重的負擔,因此本方法采取了投影到柱面得到深度圖后再將其劃分為若干局部塊,對每個局部塊單獨計算特征用于匹配的方法。

(1)

(2)

圖3 深度圖生成示意圖

2.1.3 特征提取

受參考文獻[14]啟發,在非地面點深度圖上劃分固定尺寸局部塊并提取局部塊的主要特征,而在地面點集合上采用提取法向量的方法,能夠對特征信息豐富的非地面點和特征信息較少的非地面點分別進行處理。特征提取的具體步驟如下。

① 對非地面點深度圖進行局部塊的劃分,每個局部塊塊寬為m,塊高為n,通過對每個局部塊計算特征來描述該局部塊的主要特征,深度圖劃分局部塊示意圖如圖4所示(其中被劃分為地面點的像素點被置為無效值)。

圖4 局部塊劃分示意圖

② 對每個局部塊的所有深度像素判斷離群像素點(即該局部塊中與其余像素點的深度值相差過大)并剔除,不進入后續的計算過程。

③ 對每個局部塊選取中心點,以高為m/4,寬為n/4為界在該中心點周圍建立4個子區域,每個子區域為一個同心塊,如圖5所示。

圖5 局部塊子區域示意圖

④ 對中心點分別將4個子區域作為領域從內至外依次計算該點光滑度c1、c2、c3、c4。

(3)

⑤ 對每個子區域內的中心點以外的點,以其周邊高為m/4、寬為n/4的鄰域內的點計算c值,并對每個子區域q計算以其作為鄰域的中心點的二維梯度(其中x和y并非原始點云數據的三維坐標的x值和y值,而是經過處理到深度圖后點在圖上的像素坐標):

(4)

經過式(4)計算后得到4個二維梯度值,作為該局部塊B(i,j)的特征。

2.1.4 IMU數據積分

(5)

(6)

(7)

(8)

(9)

采用參考文獻[15]中提出的幀間積分方法,得到相鄰兩點云幀的時刻k和k+1之間的相對運動增量為

(10)

(11)

(12)

2.2 雷達里程計

2.2.1 相鄰幀點云配準

(13)

(14)

得到兩個深度圖每個局部塊B(i,j)的特征向量之間的差值函數,由兩個差值函數構建目標函數為

(15)

2.2.2 當前幀位姿估算

(16)

2.3 地圖構建

2.3.1 地圖配準與修正

在點云配準時,地面點可以提供俯仰角、橫滾角和z軸方向的約束,而非地面點可以提供偏航角、x軸和y軸方向的約束[13]。非地面點的約束實現已經在雷達里程計部分完成了,而在每幀點云數據中均會進行地面點的篩選并提取地面點的法向量,地圖配準與修正模塊對當前幀的法向量與已構建地圖的地面法向量進行匹配并優化當前幀位姿。

2.3.2 更新地圖

(17)

3 實驗結果與分析

3.1 實驗設備與數據

為測試本文方法的實際運行效果,采用公開數據集和設備實測數據集共同驗證的方式,并與主流算法LOAM、LeGO-LOAM的效果進行比對。

公開數據集選用了具有完備傳感器數據和真值的KITTI數據集,便于比對3種方法的結果與真值之間的絕對位姿誤差(Absolute Pose Error,APE)。使用到的傳感器數據包括Velodyne HDL-64E LiDAR和OXTS RT 3003 IMU,采集場景包括住宅區、公路等,特征較為豐富。

實測數據集采用設備為RS-LiDAR-16混合固態激光16線雷達和HWT901B-485高精度10軸姿態角度傳感器,如圖6所示。由于實驗設備限制無法提供ground-truth基準值進行數值分析,因此僅用作特殊場景的全局地圖生成效果比對。采集場景包括幾何特征信息單一且稀少的走廊環境和適用于測試提取點云強度特征影響的地下停車場,如圖7所示。

圖6 實測數據所用設備

圖7 實測數據場景

3.2 幾何特征匹配效果

由于KITTI數據集中不具備點云強度域信息,僅包含點云位置信息,但具備ground-truth基準值參考,可以有效進行不同算法之間的具體數值比對,因此選擇其中的00、05、07、09序列數據,對本文提出的點云坐標強度圖局部塊特征向量提取與匹配方法進行可行性驗證,并與主流算法的運行效果進行比對。

在運行數據集并分析效果的過程中,發現LOAM由于幾何特征約束的單一性和缺少閉環環節容易逐漸累積誤差,而LeGO-LOAM由于其閉環環節的存在,檢測到閉環時對位姿的調整會導致位姿數據的突變,這一點在高度項上體現得尤其明顯,如圖8(a)中軌跡所示。

而本文方法在加強地面約束的基礎上采用局部塊的方法劃定子區域并計算其光滑度梯度值作為表述該子區域的特征向量的方法,增強了對小范圍區域的描述性,與原始LOAM中劃分角特征點與面特征點并進行幀間匹配的方法相比,考慮了對特征點周邊環境的描述,在KITTI數據集上的實踐效果較好,減小了累積誤差的影響,且不存在位姿數據突變的問題。圖8(a)～圖8(d)分別為LOAM、LeGO-LOAM和本文方法在KITTI00、05、07、09數據集上的運行軌跡與真值的比對,其中x軸、z軸分別為水平方向的橫縱軸。

圖9(a)～圖9(d)分別為LOAM、LeGO-LOAM和本文方法在KITTI00、05、07、09數據集上的APE值比對圖,表1為公共數據集軌跡APE結果。由此可以看出,本文方法在精度上獲得了提升,且避免了位姿的突變。

表1 公共數據集軌跡APE結果 單位:m

圖9 公共數據集軌跡誤差比對

3.3 強度特征匹配結果

實地采集的數據中包含了點云位置信息、強度信息和IMU數據,因此用于測試附加強度特征匹配模塊的效果。在使用LeGO-LOAM測試實地數據集的過程中發現,對于特征單一導致匹配較為困難的走廊環境,LeGO-LOAM會出現“跑飛”現象,即生成地圖結果與實際相差較大。通過對比走廊環境與所用KITTI數據集的環境,得出出現這一情況的原因是走廊環境的地面較狹窄且壁面環境相似度高,導致了劃分地面點和提取特征的困難。而附加了強度特征匹配模塊的本文方法則會在這一環境下結合強度特征和IMU數據進行修正。對兩種方法的效果進行了比對,如圖10所示。

由圖10可以看出,本文方法通過強度特征匹配和融合IMU數據有效改善了“跑飛”現象,增強了算法的穩定性。

而對于地下停車場場景,本文方法增加的強度特征約束項提高了幀間匹配的準確性,建圖中墻壁、拐角等的“重影”現象有所減少,如圖11所示。

圖11 實測地下停車場環境效果比對

圖11(a)和圖11(b)分別為兩種算法對同一場景的整體效果對比(圖中紅框標注區分別為圖11(c)和圖11(d)所在位置),圖11(c)和圖11(d)中紅色橢圓標注區域分別為LOAM運行出現“重影”現象和本文方法“重影”改善效果。

3.4 結果分析

所設計的實驗采用了公共數據集和實測數據集對本文方法與已有的LOAM和LeGO-LOAM算法的運行結果進行了比對。

通過對KITTI公共數據集00、05、07、09序列的建圖并比對真值和計算誤差,可見幾何特征提取和匹配模塊的改進提升了特征對環境的表述能力,進一步提升了匹配的精度,此外也避免了位姿突變的發生。而附加了強度特征匹配模塊的本文方法在實測數據集上的結果也較已有算法獲得了較大的提升,改善了兩種已有算法運行結果中出現的“跑飛”和“重影”現象。

4 結束語

針對幾何特征稀少、幀間匹配難度較大的環境,提出了一種基于深度圖劃分局部塊并篩除離群點、提取塊內點云光滑度特征信息和強度特征信息,同時融合IMU數據求解幀間位姿的定位建圖方法。經過KITTI公共數據集的驗證,幾何特征提取與匹配模塊的改進有效地提升了位姿估算的精度,降低了匹配誤差的累積,提升了定位與建圖的準確性。經實測數據集實驗,強度特征與幾何特征的提取和結合可以有效地改善已有算法中會出現的“跑飛”和“重影”現象,即對于幾何信息較為匱乏的環境,融合點云幾何信息與強度信息的方法比使用單一幾何信息的匹配方法效果更好。