基于激光SLAM 的綜采工作面實(shí)時(shí)三維建圖方法

亓玉浩，關(guān)士遠(yuǎn)

（1. 山東能源集團(tuán)有限公司，山東 濟(jì)南 250013；2. 北京天瑪智控科技股份有限公司，北京 101399）

0 引言

“十三五”以來，國家發(fā)展改革委、國家能源局等部門相繼印發(fā)《能源技術(shù)革命創(chuàng)新行動計(jì)劃（2016-2030 年）》《關(guān)于加快煤礦智能化發(fā)展的指導(dǎo)意見》，大力推進(jìn)煤礦智能化建設(shè)。對綜采工作面進(jìn)行智能感知，實(shí)時(shí)構(gòu)建工作面三維地圖，進(jìn)而實(shí)現(xiàn)采煤機(jī)自主作業(yè)，是煤礦智能化發(fā)展的關(guān)鍵一環(huán)[1-2]。

煤礦三維地圖的構(gòu)建方法主要包括站點(diǎn)式建圖[3]和移動式建圖[4]。站點(diǎn)式建圖方法先將整個(gè)區(qū)域劃分成多個(gè)子區(qū)域，然后逐站掃描獲取激光點(diǎn)云，同時(shí)用全站儀測量各個(gè)站點(diǎn)的坐標(biāo)，最后結(jié)合激光點(diǎn)云和站點(diǎn)坐標(biāo)進(jìn)行整個(gè)區(qū)域的點(diǎn)云拼接。該方法所需時(shí)間較長，實(shí)效性較差，不能滿足綜采工作面實(shí)時(shí)建圖需求。移動式建圖方法主要采用激光雷達(dá)進(jìn)行實(shí)時(shí)掃描，利用慣導(dǎo)和里程計(jì)計(jì)算位姿，實(shí)現(xiàn)激光點(diǎn)云實(shí)時(shí)拼接[5-6]，進(jìn)而實(shí)現(xiàn)實(shí)時(shí)建圖。該方法依賴高精度的光纖慣導(dǎo)和里程計(jì)進(jìn)行位姿計(jì)算，而在實(shí)際工程實(shí)踐中里程計(jì)精度難以滿足應(yīng)用需求，導(dǎo)致獲取的工作面三維激光點(diǎn)云不完整，從而難以獲取完整、準(zhǔn)確的工作面三維地圖。

同時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）[7-10]是實(shí)時(shí)移動建圖技術(shù)研究熱點(diǎn)，通過搭載特定的傳感器主體，在沒有環(huán)境先驗(yàn)信息的情況下，在運(yùn)動過程中建立環(huán)境模型，同時(shí)估計(jì)傳感器位姿，如果傳感器采用激光雷達(dá)，即為激光SLAM。本文提出一種基于激光SLAM 的綜采工作面實(shí)時(shí)三維建圖方法。實(shí)時(shí)獲取綜采工作面激光點(diǎn)云后，采用慣導(dǎo)的姿態(tài)角對激光點(diǎn)云進(jìn)行去畸變處理，采用主成分分析法[11-13]提取激光點(diǎn)云的幾何張量特征，通過計(jì)算特征點(diǎn)距離進(jìn)行點(diǎn)云匹配和位姿估計(jì)，最后對姿態(tài)進(jìn)行優(yōu)化，并增量式構(gòu)建全工作面三維激光點(diǎn)云。

1 綜采工作面實(shí)時(shí)三維建圖方法原理

綜采工作面三維激光掃描硬件主要包括2 個(gè)16 線激光雷達(dá)、慣導(dǎo)、車載工控機(jī)，部署在工作面巡檢機(jī)器人上，如圖1 所示。2 個(gè)16 線激光雷達(dá)旋轉(zhuǎn)軸呈45°安裝，負(fù)責(zé)激光掃描；慣導(dǎo)安裝于水平激光雷達(dá)底部，測量值用于去畸變處理及為后期優(yōu)化提供初始值；車載工控機(jī)運(yùn)行激光SLAM 程序，實(shí)現(xiàn)實(shí)時(shí)建圖。

圖 1 綜采工作面三維激光掃描硬件Fig. 1 Hardware for three dimensional laser scanning in fully mechanized working face

1.1 激光掃描原理

16 線激光雷達(dá)采用飛行時(shí)間測量法，激光發(fā)射器發(fā)出一束超短激光脈沖，激光投射到物體上并反射，激光接收器接收反射光，通過測量激光束在空中的飛行時(shí)間，可以準(zhǔn)確計(jì)算目標(biāo)物體與激光雷達(dá)的距離。16 線激光雷達(dá)有16 個(gè)激光發(fā)射器和16 個(gè)激光接收器，各發(fā)射器垂直方向均勻分布，垂直分辨率為2°，水平分辨率為0.2°，如圖2 所示，其中紅色點(diǎn)為測量原點(diǎn)，黃色點(diǎn)為激光發(fā)射器，右側(cè)和下方數(shù)據(jù)為雷達(dá)結(jié)構(gòu)尺寸。

圖 2 激光雷達(dá)線束分布Fig. 2 Lidar harness distribution

激光雷達(dá)工作時(shí)，機(jī)械旋轉(zhuǎn)部分以10 r/s 的速度高速旋轉(zhuǎn)，各激光發(fā)射器和接收器每隔0.2°完成1 次發(fā)射和接收，16 線激光雷達(dá)每秒完成約30 萬次測量。每次測量結(jié)果為目標(biāo)物體上某點(diǎn)的坐標(biāo)、反射強(qiáng)度、時(shí)間戳、激光器ID，指定時(shí)間內(nèi)所有的測量結(jié)果稱為點(diǎn)云。

激光雷達(dá)數(shù)據(jù)通過以太網(wǎng)UDP（User Datagram Protocol）傳輸，通常1 次傳輸1 s 或0.1 s 內(nèi)的測量數(shù)據(jù)，用幀表示，則對應(yīng)的幀率分別為1 Hz 和10 Hz。幀率的選擇與移動建圖的速度有關(guān)，若移動速度慢，可以降低幀率；若移動速度快，則需要提高幀率。由于激光雷達(dá)是移動的，幀內(nèi)第1 個(gè)測量點(diǎn)與最后1 個(gè)測量點(diǎn)所在的坐標(biāo)系不一致，會產(chǎn)生畸變。通過提高幀率能有效抑制畸變，但不能消除畸變，本文采用慣導(dǎo)測量值消除畸變。

1.2 激光SLAM 原理

激光雷達(dá)獲取的環(huán)境坐標(biāo)基于移動的激光雷達(dá)坐標(biāo)系，而要構(gòu)建的實(shí)時(shí)地圖基于固定的世界坐標(biāo)系，因此，需要估計(jì)出激光雷達(dá)坐標(biāo)系相對于世界坐標(biāo)系的實(shí)時(shí)姿態(tài)，將測量值變換到世界坐標(biāo)系，實(shí)現(xiàn)實(shí)時(shí)移動建圖。建圖和位姿估計(jì)可能是互相耦合的，建圖依賴位姿估計(jì)結(jié)果，位姿估計(jì)一般根據(jù)已建地圖進(jìn)行。激光SLAM 主要解決以下問題：① 通過實(shí)時(shí)讀取的點(diǎn)云數(shù)據(jù)和已經(jīng)建立的三維地圖估計(jì)某時(shí)刻的位姿。② 通過該時(shí)刻的位姿建立基于世界坐標(biāo)系的三維地圖，即實(shí)現(xiàn)定位及建圖。

2 綜采工作面實(shí)時(shí)三維建圖方法實(shí)現(xiàn)

基于激光SLAM 的綜采工作面實(shí)時(shí)三維建圖方法主要包括激光點(diǎn)云去畸變、點(diǎn)云特征提取、位姿估計(jì)、優(yōu)化建圖等步驟。

2.1 激光點(diǎn)云去畸變

在去畸變處理前，假設(shè)已將點(diǎn)云和慣導(dǎo)數(shù)據(jù)變換到同一坐標(biāo)系，故慣導(dǎo)坐標(biāo)系和激光雷達(dá)坐標(biāo)系重合。實(shí)際測試采用16 線激光雷達(dá)，在運(yùn)動中執(zhí)行掃描任務(wù)會產(chǎn)生運(yùn)動畸變。雷達(dá)本身的姿態(tài)因載體運(yùn)動而變化，而激光雷達(dá)計(jì)算周圍障礙物距離時(shí)是基于激光雷達(dá)坐標(biāo)系的，如果不考慮激光雷達(dá)本身的姿態(tài)改變，就會產(chǎn)生誤差。該類誤差可以通過慣導(dǎo)數(shù)據(jù)進(jìn)行補(bǔ)償。根據(jù)點(diǎn)云中每個(gè)點(diǎn)的時(shí)間戳檢索對應(yīng)的慣導(dǎo)數(shù)據(jù)，獲得對應(yīng)每個(gè)點(diǎn)的姿態(tài)角。如果沒有檢索到對應(yīng)姿態(tài)角，則采用四元數(shù)法插補(bǔ)，用對應(yīng)時(shí)間戳前后2 個(gè)慣導(dǎo)數(shù)據(jù)進(jìn)行插補(bǔ)，從而獲得對應(yīng)的姿態(tài)角。將所有點(diǎn)云都變換到該次掃描初始時(shí)刻的坐標(biāo)系，一次掃描得到雷達(dá)從0°旋轉(zhuǎn)到360°所獲得的全部點(diǎn)云，即1 幀點(diǎn)云。變換矩陣可通過慣導(dǎo)初始時(shí)刻的姿態(tài)角和當(dāng)前時(shí)刻的姿態(tài)角求得。

2.2 點(diǎn)云特征提取

綜采工作面具有空間狹窄、環(huán)境特征重復(fù)、振動大等特點(diǎn)，從激光點(diǎn)云中獲取的特征點(diǎn)數(shù)少，誤匹配概率較高，若采用簡單幾何特征實(shí)現(xiàn)三維建圖，效果不佳[14-15]。本文采用主成分分析法提取幾何張量特征，先求點(diǎn)集的協(xié)方差矩陣，再進(jìn)行特征值分解，得到幾何張量特征[16-17]。求得特征后，再融合慣導(dǎo)數(shù)據(jù)，通過非線性優(yōu)化求得雷達(dá)姿態(tài)，最后進(jìn)行后端優(yōu)化建圖。

在點(diǎn)云中，以每個(gè)點(diǎn)為球心、r為半徑作球，r與所建地圖的空間大小有關(guān)，對于綜采工作面，一般設(shè)為0.1～0.3 m。設(shè)點(diǎn)云中第i（1 ≤i≤N，N為點(diǎn)云內(nèi)所有點(diǎn)的個(gè)數(shù)）個(gè)點(diǎn)的坐標(biāo)為（xi，yi，zi），計(jì)算點(diǎn)云P中所有樣本的協(xié)方差矩陣：

式中：E(·)為期望函數(shù)；為點(diǎn)云中所有點(diǎn)對應(yīng)坐標(biāo)的算術(shù)平均值。

計(jì)算協(xié)方差矩陣的所有特征值及特征向量，進(jìn)行特征值分解，得

式中：V為特征向量組成的變換矩陣；λ1，λ2，λ3為特征值。

3 個(gè)特征值間接表征了區(qū)域的曲率特征，當(dāng)λ1> > λ2時(shí)，該區(qū)域?yàn)橹本€結(jié)構(gòu)；當(dāng) λ1≈λ2> > λ3時(shí)，該區(qū)域?yàn)槠矫娼Y(jié)構(gòu)；當(dāng) λ1≈λ2≈λ3時(shí)，該區(qū)域是橢球結(jié)構(gòu)。設(shè)L，F(xiàn)，S分別為直線特征、平面特征、橢球特征參數(shù)，其計(jì)算公式為

針對3 個(gè)特征參數(shù)分別設(shè)置閾值，當(dāng)所求特征參數(shù)大于相應(yīng)閾值時(shí)，將其歸類為對應(yīng)特征。閾值需要根據(jù)具體環(huán)境選擇，對于綜采工作面，直線、平面、橢球特征參數(shù)可分別設(shè)置為0.65，0.33，0.71。整個(gè)點(diǎn)云按照3 個(gè)特征參數(shù)進(jìn)行分類，結(jié)果以KDTree結(jié)構(gòu)存儲。

2.3 位姿估計(jì)

設(shè)當(dāng)前時(shí)刻k+1 獲得的點(diǎn)云為Pk+1，點(diǎn)云內(nèi)的點(diǎn)為pk+1,i，前一時(shí)刻k對應(yīng)的點(diǎn)云為Pk，點(diǎn)云內(nèi)的點(diǎn)為pk,i，使用最近鄰算法搜索Pk+1與Pk中對應(yīng)的特征點(diǎn)，進(jìn)行數(shù)據(jù)關(guān)聯(lián)。前一時(shí)刻的姿態(tài)是已知的，通過前后2 幀之間的變換矩陣T，可得到當(dāng)前時(shí)刻的姿態(tài)，從而將姿態(tài)估計(jì)問題轉(zhuǎn)換為變換矩陣求解問題。變換矩陣可表示為

式中：R為旋轉(zhuǎn)矩陣；t為平移矩陣。

計(jì)算2 幀中3 類特征點(diǎn)之間的距離。對于直線特征點(diǎn)，采用點(diǎn)到線的距離，在Pk中使用最近鄰算法搜 索 與 點(diǎn)pk+1,i最 近 的2 個(gè) 點(diǎn)，設(shè) 為pk,i1，pk,i2，將 這2 個(gè)點(diǎn)的連線作為目標(biāo)直線；對于平面特征點(diǎn)，采用點(diǎn)到面的距離，在Pk中使用最近鄰算法搜索與點(diǎn)pk+1,i最近的3 個(gè)點(diǎn)，設(shè)為pk,i3，pk,i4，pk,i5，將這3 個(gè)點(diǎn)所在平面作為目標(biāo)平面；對于橢球特征點(diǎn)，采用點(diǎn)pk+1,i到最近點(diǎn)pk,i6的距離。距離計(jì)算公式為

式中：dL為點(diǎn)到直線的距離；dF為點(diǎn)到平面的距離；dS為點(diǎn)到點(diǎn)的距離。

構(gòu)建目標(biāo)函數(shù)[18-19]：

式中：R*，t*分別為旋轉(zhuǎn)矩陣、平移矩陣目標(biāo)值；ωa，ωb，ωc為3 種距離的對應(yīng)權(quán)重；A，B，C分別為直線特征點(diǎn)、平面特征點(diǎn)、橢球特征點(diǎn)個(gè)數(shù)。

為了避免偏差過大，即出現(xiàn)所謂的離群點(diǎn)，采用Huber 魯棒核函數(shù)處理殘差[20]，得

式中：dδ為對距離進(jìn)行殘差處理后的值；d為距離，d∈{dL,dF,dS}；δ為閾值，本文取0.3 m。

因?yàn)镽是正交矩陣，無法直接對R求導(dǎo)，求得最優(yōu)解。將T變換到其對應(yīng)的切空間，在局部的李代數(shù)[21]結(jié)構(gòu)下對R求導(dǎo)，再采用Levenberg-Marquardt算法求解目標(biāo)函數(shù)。

求得當(dāng)前時(shí)刻的姿態(tài)后，要判斷該幀是否作為關(guān)鍵幀保留，并進(jìn)行后續(xù)優(yōu)化建圖。若當(dāng)前幀姿態(tài)相對于前一幀姿態(tài)的平移超過0.5 m，或3 個(gè)軸中任意一軸相對于前一幀對應(yīng)軸的旋轉(zhuǎn)角大于1°，則可判斷為關(guān)鍵幀。

2.4 優(yōu)化建圖

得到關(guān)鍵幀后，要與歷史關(guān)鍵幀進(jìn)行聯(lián)合優(yōu)化，以進(jìn)一步減小姿態(tài)估計(jì)誤差。如果將所有的關(guān)鍵幀一起進(jìn)行優(yōu)化，隨著巡檢機(jī)器人運(yùn)行距離增加，優(yōu)化變量將變得非常多。因此，本文采用滑動窗口法[22]選取鄰近的10 個(gè)歷史關(guān)鍵幀，與當(dāng)前關(guān)鍵幀一起進(jìn)行聯(lián)合優(yōu)化。構(gòu)建姿態(tài)圖[23]，其節(jié)點(diǎn)為關(guān)鍵幀的姿態(tài)及該幀點(diǎn)云姿態(tài)，邊為前后幀之間的姿態(tài)約束。所有姿態(tài)圖的節(jié)點(diǎn)存儲在KDTree 內(nèi)，便于后續(xù)遍歷操作。

為保證實(shí)時(shí)性，采用增量式優(yōu)化算法，使用GTSAM[11]優(yōu)化庫實(shí)現(xiàn)聯(lián)合優(yōu)化。聯(lián)合優(yōu)化部分為獨(dú)立進(jìn)程，優(yōu)化頻率為10 Hz。同時(shí)，在歷史關(guān)鍵幀中進(jìn)行匹配，如果歷史關(guān)鍵幀與當(dāng)前關(guān)鍵幀的對應(yīng)特征點(diǎn)較多，即為回環(huán)，將該歷史關(guān)鍵幀及其前后各5 幀加入目標(biāo)函數(shù)進(jìn)行聯(lián)合優(yōu)化，即共計(jì)21 幀同時(shí)進(jìn)行優(yōu)化?；丨h(huán)部分為獨(dú)立進(jìn)程，優(yōu)化頻率為5 Hz。將獲得的所有關(guān)鍵幀點(diǎn)云疊加到一起，即為全局實(shí)時(shí)三維地圖。

3 井下工業(yè)性試驗(yàn)

在兗礦能源集團(tuán)股份有限公司某工作面進(jìn)行試驗(yàn)，該工作面采高為4.5 m，長度為260 m。將激光雷達(dá)安裝在軌道式巡檢機(jī)器人上，現(xiàn)場建圖數(shù)據(jù)及指令通過WiFi 傳輸，如圖3 所示。

圖 3 軌道式巡檢機(jī)器人Fig. 3 Orbital inspection robot

3.1 SLAM 建圖效果

遠(yuǎn)程遙控巡檢機(jī)器人從刮板輸送機(jī)的機(jī)頭運(yùn)行到機(jī)尾，進(jìn)行實(shí)時(shí)建圖，結(jié)果如圖4 所示。圖4（a）中，紅色為綜采工作面三維激光點(diǎn)云，青色線為巡檢機(jī)器人實(shí)時(shí)軌跡，從圖4（b）可清晰看出采煤機(jī)輪廓。

圖 4 綜采工作面實(shí)時(shí)三維重建效果Fig. 4 Real time 3D reconstruction effect of fully mechanized working face

3.2 誤差分析

在支架上選8 個(gè)標(biāo)記點(diǎn)，用全站儀測出這些點(diǎn)相對于地圖原點(diǎn)的坐標(biāo)，作為實(shí)測值。由于實(shí)時(shí)SLAM 所生成的點(diǎn)云稠密程度不夠，采用在線錄制的3 個(gè)數(shù)據(jù)包生成稠密點(diǎn)云，在點(diǎn)云中識別這些標(biāo)記點(diǎn)，并計(jì)算絕對誤差均值：

式中：n為標(biāo)記點(diǎn)個(gè)數(shù)；為 標(biāo)記點(diǎn)坐標(biāo)測量值；Xj為標(biāo)記點(diǎn)坐標(biāo)實(shí)測值。

激光點(diǎn)云標(biāo)記點(diǎn)坐標(biāo)測量誤差分析結(jié)果見表1?？煽闯? 組數(shù)據(jù)中最大絕對誤差為0.19 m，滿足綜采工作面監(jiān)控及刮板輸送機(jī)找直精度需求。

表 1 激光點(diǎn)云標(biāo)記點(diǎn)坐標(biāo)測量誤差分析結(jié)果Table 1 The error analysis result of marked points coordinate of laser point cloud m

4 結(jié)論

（1） 提出了基于激光SLAM 的綜采工作面實(shí)時(shí)三維建圖方法，給出了激光點(diǎn)云去畸變、點(diǎn)云特征提取、位姿估計(jì)、優(yōu)化建圖等關(guān)鍵步驟。通過井下工業(yè)性試驗(yàn)進(jìn)行定性及定量分析，結(jié)果表明，該方法能實(shí)時(shí)、完整、高精度地構(gòu)建全工作面范圍的三維地圖，最大絕對誤差均值為0.19 m，滿足綜采工作面監(jiān)控及刮板輸送機(jī)找直精度需求。

（2） 該方法也存在一些不足，如沒有考慮振動對三維建圖設(shè)備的影響，因?yàn)榫氯粘Ｉa(chǎn)造成的振動較大，而激光雷達(dá)帶有旋轉(zhuǎn)部件，隨著使用時(shí)間增加，激光雷達(dá)精度會降低。此外，對于實(shí)時(shí)產(chǎn)生的激光點(diǎn)云，沒有生成三角網(wǎng)等便于渲染的數(shù)據(jù)格式，未來需要對實(shí)時(shí)三維點(diǎn)云數(shù)據(jù)進(jìn)行網(wǎng)格化處理，進(jìn)一步改善其顯示效果。