基于礦區巷道巡檢機器人的LOAM-SLAM地圖重建改進算法的研究

秦學斌 王 炳 景寧波 薛宇強 朱信龍 張俊樂

(1.西安科技大學電氣與控制工程學院,陜西 西安 710054;2.西安科技大學工程訓練中心,陜西 西安 710054;3.陜西陜煤陜北礦業有限公司,陜西 榆林 719000;4.陜西省計量科學研究院,陜西 西安 710100)

1 問題的提出

中國煤炭使用歷史已超千年,規模化、機械化煤炭挖掘有百多年歷史,前期不科學的開采工作嚴重破壞部分礦區環境,包括地下水數量和質量下降、地面沉降、采礦廢物儲存不當、土地占用和其他影響,給當地居民生活帶來不便,且該行業一直被廣泛認知為高危、艱苦行業,相關技術受關注度較低,發展緩慢,礦業工作人員安全存在風險[1]。針對采煤產生的環境問題以及安全問題,“十三五”期間,全國礦山產業結構不斷優化,淘汰退出煤礦5 464處,涉及產能9.4億t,整頓關閉金屬非金屬礦山和尾礦庫1.9萬余座[2]。

近年來,在關停不合格礦區礦井以增加行業安全性、投入使用“少人/無人化”智能礦用機械以提高開采科學性和效率性的多重影響下,我國礦業智能化建設取得積極進展,這是礦業工業發展的必然選擇,且符合“中國制造2025”的國家戰略[3]。2019年國家煤監局提出的《煤礦機器人重點研發目錄》[4]明確了礦區機器人的分類及相應標準,智能化采掘工作面從2015年僅有3個增加到275個,截至2021年1月增至494個、同比增加80%,掘進、巡檢、運輸等19種機器人在礦區現場已有示范應用[5]。2020年12月,國家電投集團內蒙古白音華煤電有限公司礦用卡車無人駕駛項目實現階段性目標,并穩定參與到剝離生產作業中,礦區已完成12臺無人駕駛礦用卡車編組運行,每臺核載約為50 t物料,行駛速度為12 km/h,實現了單車中間運輸路段無人運行;2021年3月,陜煤集團黃陵礦業公司部署煤礦巡檢機器人群,該機器人群可以實現遠程操控、智能巡檢,減輕了約180位巡檢工人的工作量[6]。

根據《煤礦機器人重點研發目錄》,需要環境感知、路徑規劃技術支撐的煤礦機器人可以分為5類:掘進類(掘進機器人)、采煤類(采煤機機器人)、運輸類(搬運機器人、巷道清理機器人、井下無人駕駛運輸車、露天礦卡車無人駕駛系統)、安控類(工作面巡檢機器人、皮帶機巡檢機器人、巷道巡檢機器人)、救援類(井下搶險作業機器人、礦井救援機器人、災后搜救水陸兩棲機器人)[7]。本研究根據現有行業指導方案及國內外研究進展,以巷道巡檢機器人SLAM(simultaneous localization and mapping)建圖算法為切入點,結合礦區井下巷道復雜環境,同時考慮礦區成本,實現巡檢機器人巡檢地圖路線精度的提升。傳統井下巷道巡檢機器人SLAM建圖算法存在以下弊端:①對傳感器數量及配置要求較高,需要多傳感器聯合調配,從而進行數據采集,增加礦區成本;②建圖與定位過程所需數據量較高,使得算法的時間復雜度較高,大大降低了運行效率;③難以適應礦區井下復雜路況,在井下采集數據時,算法自身無法處理采集的數據,從而降低地圖路線精度,導致巡檢機器人無法完成井下巡檢任務,從而存在井下安全隱患。

對上述弊端進行分析之后,在 LOAM(Lidar Odometry and Mapping in Real-time)建圖算法的基礎上,提出了一種基于礦區巷道巡檢機器人的LOAMSLAM地圖重建改進算法。該算法選用單軸單線激光雷達進行數據采集,降低礦區成本。算法層面利用Harris 3D角點檢測,選取角點作為點云幀數據中的關鍵點進行匹配,保證計算量的同時,去除部分冗余數據。通過EKF(Extended Kalman Filter,擴展卡爾曼濾波)消除由于激光雷達抖動所產生的點云運動畸變,提高建圖定位精度,同時對機器人進行運動估計,保證巡檢機器人巡檢路線的準確性和安全性。

2 LOAM算法

由于點云中的點隨著激光雷達運動會產生運動畸變,也就是點云中的點會相對實際環境中的物品表面上的點存在位置上的誤差。運動畸變會造成點云在匹配時發生錯誤,從而不能正確獲得兩幀點云的相對位置關系也就無法獲得正確的里程計信息[8]。LOAM算法將SLAM問題分成了2部分,使用2個算法并行運行來處理這2部分問題,以達到實時構建低漂移的地圖(即使用1個三維空間中運動的兩軸單線激光雷達來構建實時激光里程計并建圖)。其中一個算法以10 Hz的頻率運行,得到1個較為粗糙的里程計信息。該算法使用基于特征點的scan-to-scan match(點集與點集配準)方法,可以快速計算得到里程計信息。另一個算法以1 Hz的頻率運行,輸出1個更為精確的里程計信息。

如圖1為LOAM算法主要流程圖:①Point Cloud Registration,使用IMU數據進行點云運動畸變去除;②特征點提取,LOAM中提取的特征點有2種——Edge Point和Planar Point,2種特征使用曲率來進行區分,曲率最大的為Edge Point,曲率最小的為Planar Point;③Lidar Odometry,激光幀的時間同步、點云匹配與誤差函數;④Lidar Mapping,機器人位姿變換;⑤Transform Integration,融合了Lidar Mapping得到的位姿變換和Lidar Odometry得到的位姿變換。最終發布1個頻率與Lidar Odometry發布頻率一致的位姿變換。

圖1 LOAM算法主要流程圖Fig.1 The flow-process diagram of LOAM algorithm

3 基于礦區巷道巡檢機器人的 LOAMSLAM地圖重建改進算法

3.1 算法描述

該算法繼承LOAM算法對于點云數據的配準scan-to-map match(點集與子圖配準)方式,在此基礎上運用Harris-3D角點檢測算法,在選定區域內檢測點云數據幀的關鍵點,以此為中心點進行點云數據的配準,保證計算量的同時,去除了部分冗余數據。由于該算法選用單軸單線激光雷達,故不能通過原算法中利用三維空間中運動的兩軸單線激光雷達將定位和建圖分別進行的方式,從而消除運動畸變。故考慮運用EKF(擴展卡爾曼濾波)消除由于激光雷達抖動所產生的運動畸變,同時通過狀態方程和預測方程,對機器人進行運動估計,提高定位精度。

3.2 特征點提取

利用單軸單線激光雷達采集點云數據后,傳統SLAM建圖算法中相互關聯的特征點是通過特征值和特征向量來獲得,這種方法計算量大,且容易造成局部差異。本算法利用Harris 3D角點檢測,選取角點作為點云幀數據中的關鍵點進行點云數據幀的匹配,保證計算量的同時,可去除部分冗余數據,而且具備穩定性質的特征。

Harris 3D確定角點的方法是方塊體內點云的數量變化。在點云中存在1點p,在p上建立1個局部坐標系:z方向是法線方向,x、y方向和z垂直。在p上建立1個小正方體:①如果小正方體沿z方向移動,那小正方體里的點云數量不變;②如果小正方體位于邊緣上,則沿邊緣移動,點云數量幾乎不變,沿垂直邊緣方向移動,點云數量改變;③如果小正方體位于角點上,則有2個方向都會大幅改變點云量。

由二維圖像的矩陣M擴展至三維點云,矩陣M元素為法向量(包含法線和方向2個信息)x、y、z構成協方差矩陣,屬于對稱矩陣,該矩陣的特征向量有1個方向是法線方向,另外2個方向和法線垂直。其中,半徑r可以用來控制角點的規模,r小,則對應的角點越尖銳(對噪聲更敏感),r大,則可能在平緩的區域也檢測出角點。

計算點云的Harris響應值:

通過提取方塊內的點云數據,計算Harris 3D算法中的矩陣M的跡和行列式,最后由計算出來的結果,配合公式,輸出關鍵角點的坐標(x,y,z)。

3.3 點云配準

SLAM中點云匹配的問題分為scan-to-scan(點集與點集)、scan-to-map(點集與子圖)、map-to-map(子圖與子圖)。而三者各有特點,scan-to-scan匹配:優點是計算量小速度快,缺點是誤差累計大,長距離誤差累計后地圖精度下降。map-to-map的匹配:優點是精度高,誤差累計小;缺點就是計算量大,實時性壓力大;scan-to-map的匹配居中。由于該算法利用Harris 3D選取角點作為點云幀數據中的關鍵點進行點云數據幀的匹配,去除了部分冗余數據,故選用scan-tomap點云匹配方式。

如圖2(a)所示,通過提取角點作為關鍵點選取某幾幀點云數據組成子圖,之后點云數據幀如圖2(b)所示,與子圖相互匹配,配合上述完成的Harris 3D點云檢測,最終完成點云配準。

圖2 點云配準過程圖Fig.2 Point cloud registration process diagram

圖3為通過Harris 3D角點所配準的最終效果圖,圖中白色點表示所檢測出來的Harris 3D角點,所選場景為井下模擬礦道。

圖3 點云配準圖Fig.3 The point cloud registration map

3.4 EKF擴展卡爾曼濾波

3.4.1 消除運動畸變

礦區井下路況復雜,利用激光雷達采集點云數據時,點云中的點會隨著激光雷達的抖動產生運動畸變,從而無法獲得正確的里程計信息,導致既定路線精度較低,容易造成井下安全問題。故通過選用EKF擴展卡爾曼濾波消除運動畸變,同時對巡檢機器人進行運動估計。

擴展卡爾曼濾波(Extended Kalman Filter,EKF)是標準卡爾曼濾波在非線性情形下的一種擴展形式,屬于一種高效率的遞歸濾波器(自回歸濾波器)。其基本思想是利用泰勒級數展開將非線性系統線性化,然后采用卡爾曼濾波框架對信號進行濾波[9]。

圖4為擴展卡爾曼濾波器的算法結構和步驟:卡爾曼濾波利用點云目標的動態信息,去掉其噪聲,得到1個關于目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波),也可以是對于將來位置的估計(預測),也可以是對過去位置的估計(插值或者平滑)[10]。

圖4 擴展卡爾曼濾波器的算法結構圖Fig.4 Algorithm structure diagram of Extended Kalman Filter

3.4.2 運動估計

任何含有不確定信息的動態系統中使用卡爾曼濾波,對系統下一步的走向做出有根據的預測,即使伴隨著各種干擾,其總是能指出真實發生的情況。

在連續變化的系統中使用卡爾曼濾波是非常理想的,具有占用內存小的優點(除了前一個狀態量外,不需要保留其他歷史數據),并且速度很快,很適合應用于實時問題和嵌入式系統。

擴展卡爾曼濾波EKF的狀態轉移方程和觀測方程為

利用泰勒展開式對(4)式在上一次的估計值(θk-2)處展開得:

再利用泰勒展開式對式(6)在本輪的狀態預測值θ′k處展開得:

其中,F k-1和H k分別表示函數f(θ)和h(θ)在θk-2和θ′k處的雅可比矩陣。這里對泰勒展開式只保留到一階導數,二階導數以上的都舍去,噪聲假設均為加性高斯噪聲。

整體結構如圖5所示。

圖5 EKF運動估計結構圖Fig.5 The motion estimation structure of EKF

通過傳感器采集數據后,進行位姿估計階段,需限定點云數量的范圍,之后通過點云配準,以及傳感器采集數據,進行點云位姿的原始估計,最終利用EKF濾波器進行位姿的預測與更新。

3.5 LOAM-SLAM算法

根據以上描述,算法整體框架如圖6所示。

圖6 LOAM-SLAM算法主要流程圖Fig.6 The flow-process diagram of LOAM-SLAM algorithm

基于礦區巷道巡檢機器人的LOAM-SLAM地圖重建改進算法的整體流程圖,此過程描述數據采集、濾波處理、運動估計以及建圖與定位。算法步驟如下:

Step1:利用單軸單線激光雷達采集所需點云數據,本研究選用Velodyne型號激光雷達,場景為井下模擬礦道。

Step2:通過 Harris 3D角點檢測算法,對于獲取的點云數據進行關鍵點的檢測,用于之后的點云配準。Harris 3D確定角點的方法是方塊體內點云的數量變化。若小正方體位于角點上,則在不同方向都會大幅改變點云數量。

Step3:EKF濾波器對于點云數據進行運動畸變的消除,消除運動畸變之后,通過檢測出的點云關鍵點,進行配準,以及通過狀態方程和預測方程對于巡檢機器人進行運動估計,預測和更新巡檢機器人的位姿。

Step4:通過EKF對于點云數據的處理以及運動估計,得到機器人的位姿變換。

Step5:融合了Lidar Mapping得到的位姿變換和經EKF處理后的點云。最終發布1個頻率與里程計發布頻率一致的位姿變換。

4 算法改進前后實驗結果對比

實驗環境:操作系統為Ubuntu 18.04;ROS環境為ros-melodic;cuda-10.2,cudnn-7.6。計算機硬件:Intel(R)Core(TM)i5-9300H CPU,8GB RAM,512G固態。編譯語言:C++。傳感器為單軸單線激光雷達。數據采集環境為礦區井下模擬巷道。本次實驗主要驗證:①改進前后算法中里程計信息對于旋轉,2幀數據推算時,角度是否變化較大;②改進前后算法對于建圖效果的對比。

4.1 里程計信息對于旋轉角度變化實驗

對于點云旋轉,實際上里程計的前后2幀推算時,角度不會變化很大,大多數情況下都是歐拉角中偏航角(yaw)發生變化,同時由于角度變化過小時,點云幀數據不會有太大差異,故該實驗對點云施加5°旋轉,按照步長為5°增加,從而對比改進前后算法精度。

實驗1:2幀點云幀之間yaw角相差5°,改進前后算法點云幀變化如圖7所示。

圖7 yaw角相差5°時點云幀數據圖Fig.7 Point cloud frame data diagram when yaw angle difference is 5 degrees

實驗2:2幀點云幀之間yaw角相差10°,改進前后算法點云幀變化如圖8所示。

圖8 yaw角相差10°時點云幀數據圖Fig.8 Point cloud frame data diagram when yaw angle difference is 10 degrees

上述實驗均為單軸單線激光雷達測試結果,可以看出改進后算法在偏航角變化時,精度更好。

4.2 井下巷道建圖實驗

實驗場景為礦區井下模擬巷道,效果對比如圖9所示。

圖9 算法改進前后效果對比圖Fig.9 Comparison of effects before and after algorithm improvement

上述實驗均為單軸單線激光雷達測試結果,可以看出改進前算法會出現地圖疊影的現象,而且巷道中的岔路口也沒有體現出來。改進后,疊影現象消失,且可以完整呈現出井下巷道的情況。

5 結 論

提出了一種基于礦區巷道巡檢機器人的LOAMSLAM地圖重建改進算法,以LOAM算法為基礎,在此基礎上改用單軸單線激光雷達進行數據采集。同時進行Harris 3D算法、EKF擴展卡爾曼濾波的融合,解決了點云數據存在大量冗余數據、運動畸變和運動估計等問題,完成了單軸單線激光雷達對于井下巷道地圖的建立。后續將其進行多傳感器數據融合,提升建圖定位精度,適用于更多場景。