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

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

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

1 問題的提出

中國煤炭使用歷史已超千年,規(guī)?；?、機械化煤炭挖掘有百多年歷史,前期不科學(xué)的開采工作嚴(yán)重破壞部分礦區(qū)環(huán)境,包括地下水?dāng)?shù)量和質(zhì)量下降、地面沉降、采礦廢物儲存不當(dāng)、土地占用和其他影響,給當(dāng)?shù)鼐用裆顜聿槐?且該行業(yè)一直被廣泛認(rèn)知為高危、艱苦行業(yè),相關(guān)技術(shù)受關(guān)注度較低,發(fā)展緩慢,礦業(yè)工作人員安全存在風(fēng)險[1]。針對采煤產(chǎn)生的環(huán)境問題以及安全問題,“十三五”期間,全國礦山產(chǎn)業(yè)結(jié)構(gòu)不斷優(yōu)化,淘汰退出煤礦5 464處,涉及產(chǎn)能9.4億t,整頓關(guān)閉金屬非金屬礦山和尾礦庫1.9萬余座[2]。

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

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

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

2 LOAM算法

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

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

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

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

3.1 算法描述

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

3.2 特征點提取

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

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

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

計算點云的Harris響應(yīng)值:

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

3.3 點云配準(zhǔn)

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

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

圖2 點云配準(zhǔn)過程圖Fig.2 Point cloud registration process diagram

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

圖3 點云配準(zhǔn)圖Fig.3 The point cloud registration map

3.4 EKF擴展卡爾曼濾波

3.4.1 消除運動畸變

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

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

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

圖4 擴展卡爾曼濾波器的算法結(jié)構(gòu)圖Fig.4 Algorithm structure diagram of Extended Kalman Filter

3.4.2 運動估計

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

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

擴展卡爾曼濾波EKF的狀態(tài)轉(zhuǎn)移方程和觀測方程為

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

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

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

整體結(jié)構(gòu)如圖5所示。

圖5 EKF運動估計結(jié)構(gòu)圖Fig.5 The motion estimation structure of EKF

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

3.5 LOAM-SLAM算法

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

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

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

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

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

Step3:EKF濾波器對于點云數(shù)據(jù)進行運動畸變的消除,消除運動畸變之后,通過檢測出的點云關(guān)鍵點,進行配準(zhǔn),以及通過狀態(tài)方程和預(yù)測方程對于巡檢機器人進行運動估計,預(yù)測和更新巡檢機器人的位姿。

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

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

4 算法改進前后實驗結(jié)果對比

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

4.1 里程計信息對于旋轉(zhuǎn)角度變化實驗

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

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

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

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

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

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

4.2 井下巷道建圖實驗

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

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

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

5 結(jié) 論

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