礦山環境三維激光雷達SLAM算法建圖與定位

張清宇,崔麗珍,杜秀鐸,馬寶良

(內蒙古科技大學信息工程學院,內蒙古 包頭 014010)

近年來,我國煤礦智能化發展迅速,礦山環境下的精確定位導航技術研發更是重中之重。由于煤炭工業的工作環境相對頑劣,安全問題一直制約著煤礦的生產建設。同步定位與地圖構建是無人駕駛領域解決惡劣環境建圖定位問題的關鍵技術,三維激光雷達SLAM算法正被廣泛應用于煤礦環境的高精度制圖工作。

針對煤礦復雜環境,文獻[1]設計了一種煤礦巡檢機器人,以滿足實際礦山探測工作的需求。文獻[2]利用地面激光雷達計算復雜地形土方量。文獻[3]對煤礦井下巷道的難定位問題提出了一種改進的巷道環境建模算法。文獻[4]針對煤礦環境地圖構建過程中出現的點云運動畸變提出了點云雙邊濾波算法。文獻[5]研究了煤礦井下無GPS的機器人自主定位問題,實現了三維激光雷達SLAM算法的仿真試驗。

對于三維激光雷達SLAM算法的應用與發展,文獻[6]提出的LOAM改變了特征提取的方式,通過點云匹配對雷達位姿進行優化。文獻[7]提出了迭代最近點ICP算法。文獻[8]提出了通過慣性測量單元(inertial measurement unit,IMU)估計運動狀態消除激光雷達運動畸變的方法。文獻[9]提出的BALM算法,在雷達建圖過程中引入光速法平差以降低建圖過程中的累積誤差。文獻[10]提出了激光-慣性里程計相結合的方法LIO。文獻[11]提出的LOL方法,將 LOAM和分割場景識別算法相結合。文獻[12]提出的LIRO方法,將UWB、雷達和慣導緊耦合。文獻[13—14]提出了LeGO-LOAM和LIO-SAM算法,前者在算法的后端添加地面優化且特征提取方式更輕量級,提高了特征點的提取效率;后者將LiDAR里程計因子、IMU預積分因子、GPS因子和回環因子通過因子圖優化的方式融合,實現多傳感器融合定位。針對激光雷達耗時長、受動態障礙物影響大的問題,文獻[15]在閉環檢測部分添加了SC-Context。文獻[16]提出了FAST-LIO算法,緊耦合迭代卡爾曼濾波和雷達慣性里程計,提高了穩健性。

本文為了解決礦山復雜環境下建圖定位的難點,首先對各傳感器進行內外參數標定,然后通過巡檢機器人采集到的數據,對比多種三維激光雷達SLAM算法,以期驗證在礦山環境中多傳感器融合算法LIO-SAM具有更好的建圖效果與定位精度,并構建在LIO-SAM特征提取部分利用點的強度和環境值消除不必要的特征點的IALIO算法,以期在定位精度上有進一步的提高。

1 傳感器參數標定

單一的激光雷達、里程計、慣性測量單元很難單獨滿足高效、實時、準確、穩健等性能,因此多傳感器融合的方式應用于復雜環境下的建圖定位是目前SLAM算法主要的發展趨勢。

對于LiDAR融合IMU的系統,為解決機器人運動產生的激光雷達運動畸變,需要標定LiDAR與IMU的外參。本文采用LiDAR＿align的方法進行激光雷達與IMU的外參標定,利用NLopt庫處理定義沖突問題并改寫里程計接口為IMU接口,錄制包含大量旋轉量和平移量的LiDAR數據及IMU數據,多次標定迭代后取收斂結果最小的參數。

IMU內參未標定時會造成建圖的漂移現象甚至無法建圖,嚴重影響定位精度。本文采用的IMU的觀測模型中存在兩種誤差:一種是變化緩慢的零偏b;另一類是變化比較劇烈的測量白噪聲n。其中,零偏是由傳感器的溫度等物理因素產生的傳感器內部誤差的綜合參數;測量白噪聲是由于模/數轉換器引起的外部噪聲,角速度測量值ωo和真實值ω之間的關系為

ωo(t)=ω(t)+b(t)+n(t)

(1)

本文通過內參標定工具imu＿utils進行IMU內參的標定,即標定隨機游走噪聲和白噪聲。通過ceres優化庫分析IMU的Allan方差,將IMU保持靜止錄制2 h的數據包,確定最終的高斯白噪聲和隨機游走噪聲。

2 特征匹配及相對位姿估計

不同SLAM算法的特征提取方式是有差別的,特征點的獲取決定了機器人相對位姿數量的多少,直接影響建圖效果與定位精度。

LOAM通過計算當前幀點云pk中每個點i的平滑度c,將c最小的點作為平面點,c最大的點作為邊緣點,將一幀掃描到的點云分為4段,每段只提取最多4個平面點和2個邊緣點,以保證特征點的分布均勻,其中平滑度c的計算公式為

(2)

LeGO-LOAM是在LOAM基礎上改進的激光雷達建圖方法,與LOAM相比,在輕量級和地面優化部分有了顯著提升,可以在平坦路面有效處理點線特征和點面特征,但是由于提取的線面特征較多,會使得累計誤差較大。LeGO-LOAM的主要特點包括:①對地面進行分割,減小了特征搜索范圍;②提取特征之前進行聚類分析,提高了特征質量;③以幀為單位進行優化,使得全局地圖可以多次調整;④增加了回環修正。LeGO-LOAM的地面優化主要是在LiDAR Odometry的特征提取部分,通過使用采集到的地面面特征優化高度和水平角、采集到的線特征優化水平位移和航向角,構建點與線之間、面與面之間的對應關系,具體框架如圖1所示。

圖1 LiDAR Odometry框架

本身促進兩者聯動的活動較少。社工機構是讓社會工作和志愿服務的聯動的主要組織一，在這個載體上可以開展很多活動。但目前，我國社會工作和志愿服務的聯動模式和機制都處于探索過程中，還沒有完全探索出本土化的聯動模式和機制，因此，除了政府購買的社會服務合作項目之外，社工機構很少有能力有主動性去開展合作項目和活動，還是自上而下的項目比較多，有社工機構主導的自下而上的合作項目相對較少，還處于初始階段。

本文提出LIO-SAM在特征提取部分僅計算每個點的曲率,通過曲率的大小提取角點和平面點,規定當前激光點為pi,利用pi前后的5個點計算當前的曲率K。前后5個點與pi的距離差值為M,曲率K為距離差值的平方,具體計算公式為

M=M[i-5]+M[i-4]+M[i-3]+M[i-2]+

M[i-1]+M[i]·10+M[i+1]+M[i+2]+

M[i+3]+M[i+4]+M[i+5]

(3)

K=M·M=M2

(4)

這種方法沒有區分“平面”與“近似平面”,將沒有計算分類的點全部加入“近似平面”中,會丟失一部分符合曲率要求的特征點。本文采用的IALIO算法增加了曲率的閾值,利用點的強度和環境刪除不必要的點,一定程度上彌補了丟失點的問題,在定位精度上也有一定提高。

3 多傳感器融合算法LIO-SAM

LIO-SAM是通過激光雷達、IMU和GPS多種傳感器融合,利用因子圖優化的方式在因子圖的頂端設置了激光雷達慣性里程計,收集不同來源的多種相對測量和絕對測量值作為因子,全部合并到系統中。將激光雷達里程計因子、IMU預積分因子、GPS因子和回環因子全部優化,在礦山復雜環境下能夠針對性地應用。

3.1 IMU預積分因子

IMU預積分因子部分的具體流程如圖2所示。

圖2 IMU預積分因子

其中,Xi表示每個關鍵幀,線條表示IMU預積分因子,它由兩個相鄰關鍵幀之間的IMU測量積分得到。IMU的角速度和加速度的測量值公式為

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

式中,ΔVij、ΔPij和ΔRij分別為時間i至j之間的相對速度、位置和旋轉矩陣;Vi和Vj分別為i、j時刻的速度;Pi和Pj分別為i、j時刻的位置;Ri和Rj分別為i、j時刻的旋轉矩陣;g為重力加速度;Δtij為i、j時刻的時間間隔。

3.2 激光里程計因子

圖3 激光里程計因子

構建得到的局部地圖Mi公式為

(13)

3.3 回環檢測因子

在后端優化部分,LIO-SAM引入了回環檢測,添加了回環因子,回環檢測因子如圖4所示。

圖4 回環檢測因子

當因子圖中加入一個新的狀態Xi+1時,首先搜索整個因子圖,找到與之相近的先驗狀態,X3是返回的候選之一,然后嘗試匹配Fi+1到子關鍵幀{F3-m,…F3,…,F3+m}。本文對LeGO-LOAM算法的回環檢測部分添加了Scan-Context描述子,定位精度更優。

4 試驗與分析

4.1 試驗設備及試驗方案

為驗證礦山環境三維激光點云SLAM算法建圖定位的有效性,采用了集成Velodyne VLP-16 LiDAR、CH110-usb IMU、北斗星通C201 GNSS系統和NVIDAI AGX工控機的巡檢機器人,如圖5所示。

圖5 試驗設備

利用巡檢機器人,本文選擇采集3組場景(街區道路、礦山斜坡、采礦區)進行了數據獲取,礦山數據采集地點為哈爾烏素露天煤礦。數據采集前,為實現激光雷達與IMU的時間同步,將雷達數據點云格式由XYZI更改為XYZIRT,并用本文方法對各傳感器參數進行標定。在試驗區中,街區道路旁有大量樹木與房屋,并且有移動車輛和行人的干擾。礦山斜坡具有輕微的坡度,地形較為平坦。采礦區地形復雜多變,有嚴重的拐角和地面顛簸。3組場景的數據集長度均大于1 km,采集時間均在500 s以上。本文采用軌跡評估工具EVO繪制誤差曲線,各組試驗軌跡誤差對比的真值均為GNSS信息。

4.2 街區道路

街區道路的建圖效果與軌跡對比如圖6所示,圖6 (c)為LIO-SAM算法與GNSS信息的軌跡對比曲線。

圖6 街區道路試驗

由圖6可以看出街區道路試驗在拐角處軌跡漂移的現象比較嚴重,利用軌跡評估工具EVO,得到街區道路下多種SLAM算法的定位誤差,見表1。

表1 街區道路SLAM軌跡APE統計結果 m

由表1可知,ALOAM的RMSE為6.267 415 m,采用Eigen庫和Ceres優化庫直接計算旋轉矩陣與雅可比矩陣的方式,能夠在街區道路取得更優的定位結果。由于受樹木、行人車輛的影響,LOAM單激光與LeGO-LOAM地面優化的方式均沒有多傳感器融合算法LIO-SAM定位精度高,而在LIO-SAM特征提取部分增加強度特征的IALIO算法,可以提取更多的特征點,定位誤差較LIO-SAM降低了20.17%。

4.3 礦山斜坡

礦山斜坡的建圖效果與軌跡對比如圖7所示,圖7 (c)為LIO-SAM算法與GNSS信息的軌跡對比。

圖7 礦山斜坡試驗

可以看出,在礦山斜坡環境拐彎時會因運動畸變產生輕微的失真漂移現象,受建筑物的影響較小,地形較為平坦,建圖效果平滑。利用EVO工具對礦山斜坡環境下多種SLAM算法的定位誤差結果見表2。

表2 礦山斜坡SLAM軌跡APE統計結果 m

由表2可知,在礦山斜坡地形環境,LIO-SAM的RMSE為0.811 041 m,相較于ALOAM和LeGO-LOAM分別提高了2.70%和45.28%。因此在礦山斜坡地形環境下,多傳感器融合算法LIO-SAM可以有效地解決表面傾斜所造成的運動畸變問題,使建圖效果更平滑清晰。

4.4 采礦區

采礦區的建圖效果與軌跡對比如圖8所示,圖8(c)為LIO-SAM算法與GNSS信息的軌跡對比曲線。

圖8 采礦區試驗

由圖8可以看出,采礦區環境巡檢機器人在朝一個方向行進時誤差累積較大,且在拐角處軌跡偏移量較大,軌跡誤差見表3。

表3 采礦區SLAM軌跡APE統計結果 m

由表3可知,在采礦區地形顛簸及周圍環境特征不明顯的問題下,多傳感器融合算法LIO-SAM更能適應復雜環境,具有更好的定位結果,它的RMSE為22.055 274 m,相較于ALOAM和SC-LeGO誤差分別降低了21.53%和55.18%。LIO-SAM以局部地圖優化的方式可以有效減少誤差的累積,而對LIO-SAM特征提取部分添加強度特征的算法IALIO在采礦區有更好的定位精度,它的RMSE為21.855 380 m,相較于LIO-SAM誤差降低了0.91%。

5 結 語

本文針對礦山地形未知環境中建圖定位需求, 首先配置了激光雷達、IMU、GPS等多傳感融合的巡檢機器人,并對激光雷達和IMU進行內外參數標定,解決了建圖漂移失真問題。考慮礦山環境的復雜性,巡檢機器人分別采集 3組場景(街區道路、礦山斜坡、采礦區)的數據集,以GNSS信息作為軌跡真值,對比了多種三維激光雷達SLAM算法的建圖效果并分析了軌跡誤差與定位精度。試驗表明,在采礦區復雜環境,LIO-SAM通過多傳感器融合,不僅以局部地圖優化的方式減少了累積誤差,更在后端優化部分添加了回環因子,具有更優的定位精度。針對LIO-SAM特征提取部分計算曲率提取特征點的不足之處,引入了強度特征,驗證了IALIO算法定位精度在采礦區更優于LIO-SAM。

本文驗證了礦山環境下三維激光雷達SLAM算法的可行性,在解決礦山環境地形多變造成的累積誤差問題上,LOAM和LeGO-LOAM算法的穩健性和定位精度都較差,使用多傳感器融合算法LIO-SAM才能得到精確的定位精度。LIO-SAM的特征提取部分還有所欠缺,改變特征點的提取方式對提高定位精度具有可行之處。