基于近紅外雙目立體視覺的懸臂式掘進機定位研究

王學， 周紅旭， 張雷， 王華英

（1. 河北工程大學 數理科學與工程學院，河北 邯鄲 056038；2. 河北省計算光學成像與光電檢測技術創新中心，河北 邯鄲 056038）

0 引言

自“十三五”以來，我國煤炭行業自主科研創新能力得到了大幅改善，但是創新推動能力依舊不足[1]。GB50213-2010《煤礦井巷工程質量驗收規范》規定我國煤礦掘進斷面的允許偏差為-50～+150 mm。但煤礦井下巷道掘進中懸臂式掘進機截割部位姿通常用固定在巷道頂板上的點光源來指示掘進方向，通過判斷激光投影點在巷道掘進斷面上的位置，確定下一步的掘進方向[2]。該方法存在掘進精度有限、測量工作量大、生產效率低、人身不安全等問題，很難保證巷道成形質量達到要求。為了實現掘進機的智能切割控制，亟需提高懸臂式掘進機位姿參數的自動實時檢測[3]。

為實現巷道無人化掘進，學者提出了多種定位方法。文獻[4]通過讀取全站儀激光光斑距目標板中心的偏移量來推斷當前隧道的偏差，實現隧道內掘進機的跟蹤和引導，但全站儀無法實時自主測量且不能適應井下掘進狀況。文獻[5]采用慣性導航技術實現掘進機實時自主測量，然而系統存在時間偏差、成本高、初始對準時間較長等問題，加上掘進機抖動幅度大、巷道測量環境較差等缺陷，增加了累計誤差。文獻[6-7]開發了一種基于iGPS的掘進機測量系統，以基于隨機誤差傳遞方程來評估掘進機的空間角度和位移，該系統的測量介質為激光，掘進機作業時存在遮擋問題，同時激光發射裝置結構復雜，易損壞，在復雜工況下，巷道掘進精度會受到很大影響。文獻[8-9]采用超寬帶技術實現掘進機位姿實時檢測，但由于巷道環境復雜，金屬材料設備較多，易導致超寬帶信號發生漫反射，使得掘進機位姿測量誤差偏大、失敗等情況。文獻[10]提出了一種基于激光束的懸臂式掘進機位姿測量系統。結合坐標系轉換關系得到掘進機機身在巷道坐標系中的位姿信息，但測量距離的增加或相機視野被遮擋等問題易導致測量失效。

針對現有掘進機無法實時定位、定位不準確、視覺定位中相機視野被遮擋導致定位失敗等問題，提出了一種基于近紅外雙目立體視覺的懸臂式掘進機定位方案。該方案以LED作為近紅外標靶構建掘進機特征信息，通過圖像處理、位姿解算實現掘進機機身與截割部的三維空間定位；為解決雙目立體視覺位姿解算失效問題[11]，引入基于一維卷積神經網絡（One Dimensional-Convolutional Neural Network，1D-CNN）[12-14]的井下磁場視覺輔助定位算法，通過構建神經網絡模型訓練磁場信號數據，預測掘進機截割部的位姿變化，為視覺定位掘進機位姿提供了一種在視覺測量失效情況下的輔助定位方案。

1 懸臂式掘進機定位系統

懸臂式掘進機機身及截割部位姿測量系統由懸臂式掘進機、機身近紅外LED標靶、機臂近紅外LED標靶、2臺近紅外相機組成的雙目立體視覺相機、三軸數字磁場計、永磁體組成，如圖1所示。機身近紅外LED標靶根據作業的實際情況固定在機身四周邊緣處，機臂近紅外LED標靶安裝在掘進機截割部護罩四周邊緣處。將雙目立體視覺相機安裝在巷道頂端，獲得足夠的相機視野對掘進工作面進行圖像采集。為防止在惡劣環境下圖像采集失敗、位姿測量失效等問題，在掘進機機身兩側各安裝高精度三軸數字磁場計，并在機臂處安裝永磁體，以磁場的強度分量和近紅外雙目立體視覺相機獲取的位姿數據作為訓練數據，構建1D-CNN模型，輸出在視覺測量失效情況下掘進機位姿。

圖1 懸臂式掘進機機身及截割部位姿測量系統結構Fig. 1 Structure of cantilever roadheader fuselage and cutting part pose measurement system

懸臂式掘進機機身及截割部位姿測量方案如圖2所示。首先，根據雙目立體視覺測量原理設計近紅外雙目立體視覺相機，并采用張正友標定原理對雙目立體視覺相機進行標定，計算相機相關參數。在掘進過程中，相機與掘進機間的外部參數也需要標定，并將其作為下次掘進機工作的初始數據。隨著掘進工作的不斷進行，掘進機與相機間距離誤差不斷累計，在掘進機等待支護階段需要對二者間的外部參數進行重新標定。其次，在掘進工作開始前利用迭代最近點（Iterative Closest Point，ICP）[15]算法對掘進機機身坐標系與相機坐標系進行外部參數解算。根據相機標定得到的參數，利用相機校正技術實現圖像立體校正。對近紅外圖像進行圖像增強、去噪，利用Canny邊緣檢測方法分割目標，并利用分散度篩選出真正的近紅外LED標靶，得到標靶質心坐標，根據歐氏距離等方法對標靶質心進行定位、跟蹤及匹配。再次，根據雙目立體視覺測量原理計算巷道坐標系下的三維空間坐標，在掘進機機身與截割部建立坐標系，根據ICP算法求解巷道坐標系與機身坐標系、巷道坐標系與截割部坐標系間的位姿關系。最后，根據位姿解算模型求解掘進機截割部與機身間的位姿關系。

圖2 懸臂式掘進機機身及截割部位姿測量方案Fig. 2 Pose measurement scheme of cantilever roadheader fuselage and cutting part

由于煤礦井下掘進機工作環境惡劣，巷道復雜多變，且雙目立體視覺測量距離有限，導致掘進機在掘進過程中視覺位姿估計失效。針對該問題，引入基于1D-CNN的井下磁場視覺輔助定位算法。在1D-CNN的基礎上搭建網絡模型，該模型包括2個輸入層，訓練數據經過3層1D-CNN卷積，每層卷積都經過批標準化（Batch Normalization，BN）使其加快收斂，防止過擬合。數據經Selu激活函數處理后輸入下一卷積層，不斷改善訓練參數。由于特征較少，所以未采用池化層，添加平展層將張量重新整形為矢量。數據經卷積后引入Selu激活函數連接輸出層，得到懸臂式掘進機截割部位姿預測值。

懸臂式掘進機機身及截割部位姿空間坐標系如圖3所示。以機身與機臂近紅外LED標靶的中心為原點，建立機身坐標系OfXfYfZf和機臂坐標系OeXeYeZe；以截割部中心點為原點，建立截割部坐標系OhXhYhZh；以近紅外相機1，2的光心為相機坐標系原點，設相機的光軸為Zc軸，垂直相機光軸向下為Yc軸，水平垂直相機光軸為Xc軸，建立相機坐標系Oc1Xc1Yc1Zc1和Oc2Xc2Yc2Zc2；將近紅外相機1坐標系設置為世界坐標系（即巷道坐標系）OwXwYwZw。

圖3 懸臂式掘進機機身及截割部位姿空間坐標系Fig. 3 Pose space coordinate systems of cantilever roadheader fuselage and cutting part

2 近紅外標靶圖像特征提取、定位與匹配

2.1 近紅外LED標靶特征提取

受煤礦井下作業面高粉塵、高濕度、強振動和電磁等干擾[16]的影響，雙目立體視覺相機采集到的圖像質量較差且伴有噪聲，因此采用濾波、平滑等預處理技術對立體校正后的圖像進行處理，以提高圖像對比度和亮度、改善圖像質量。

將近紅外LED標靶作為掘進機位姿估計的重要識別特征，通過最大類間方差法 OTSU 分割原理計算Canny邊緣檢測最佳閾值，對近紅外圖像進行處理。設近紅外圖像為二值函數I(x,y)，假設存在閾值f將圖像分為前景和后景2類（圖像的像素灰度值小于f的為前景，圖像的像素灰度值大于f的為后景），前景和后景的均值分別為 ωmv1和 ωmv2，整個圖像的均值為 ωmv， 圖像被分為前景和后景的概率分別為p1和p2，則有

根據方差知識，可得類間方差為

將式（3）簡化，可得

閾值的確定是控制邊緣檢測效果的關鍵，將通過OTSU得到的最佳閾值應用在Canny邊緣檢測雙閾值檢測步驟中，獲取不同目標的圖像邊緣特征。之后對獲取的邊緣輪廓信息進行擬合，根據分散度進行特征選擇，剔除非標靶的干擾目標。近紅外LED標靶特征提取結果如圖4所示。可看出近紅外LED標靶的特征提取與識別效果較好。

圖4 近紅外LED標靶特征提取結果Fig. 4 Characteristic extraction results of infrared LED target

2.2 近紅外LED標靶定位與匹配

懸臂式掘進機雙目視覺位姿測量方法中利用重心進行特征點匹配，利用圖像的矩計算近紅外LED標靶輪廓重心坐標，以一階矩和零階矩計算重心，實現近紅外雙目圖像中標靶質心的精確定位。匹配過程如下：

（1） 采集目標輪廓信息。計算左相機（近紅外相機1）圖像目標第j個邊緣輪廓點像素坐標Y方向坐標均值為計算右相機（近紅外相機2）圖像目標第j個邊緣輪廓點像素坐標，X，Y方向坐標均值為

（2） 計算X方向和Y方向的差值。將右相機圖像邊緣輪廓點像素坐標X方向平均值與左相機圖像邊緣輪廓點像素坐標X方向的平均值相減，得X方向上的差值，同理計算Y方向上的差值。

（3） 計算右相機圖像質心平移后的坐標點。將右相機圖像質心坐標與差值相減，即右相機圖像標靶質心平移至左相機圖像處，得到右相機圖像各質心平移到左相機圖像后的坐標點

（4） 計算歐氏距離。以左相機圖像第1個特征點 (xl1,yl1)為 匹配點，分別計算右相機圖像各質心平移后的特征點之間的歐氏距離D。

提取左相機圖像第1個特征點 (xl1,yl1)與距右相機圖像標靶質心平移后的特征點歐氏距離最小的最鄰點與次鄰近點。提取這2個關鍵點，用最近鄰點歐氏距離Dm除以次鄰近點的歐氏距離Dm+1，得到比率λ。

當λ＜0.8時，左相機圖像第1個特征點與最近鄰點為1對匹配點，否則重新匹配。依此類推，對左相機圖像中所有特征點進行匹配。

3 懸臂式掘進機位姿估計

3.1 掘進機位姿估計數學模型

將巷道坐標系與機身坐標系下的3D坐標作為輸入，通過剛性變換（旋轉變換和平移變換）使其轉換到同一坐標系下。設巷道坐標系下目標點集為S(X,Y,Z)， 機身坐標系下參考點集為J(X′,Y′,Z′)，將機身坐標系下參考點集通過剛性變換轉換到巷道坐標系下目標點集，其轉換關系為

式中M為映射矩陣。

三維點云圍繞X,Y,Z軸旋轉，可得如下轉換關系。

式中：α為偏航角； β為俯仰角；γ為翻滾角；tX,tY,tZ分別為圍繞X,Y,Z軸旋轉的偏移量。

3.2 掘進機位姿估計算法

ICP算法用于尋找目標點集S和參考點集J之間的剛性變換，使2個匹配數據在某種度量標準下滿足最優匹配。假設目標點集S的坐標為{Si|Si∈Z3,i=1,2,···,N}，N為目標點集中目標點，參考點集J的坐標為，第i個參考點Ji經過k次迭代后的坐標為。計算目標點集S與參考點集J經過k次迭代后對應點之間的變換矩陣并更新原始變換，直到目標點集與參考點集間的距離小于給定閾值τ。ICP算法步驟如下：

（1） 計算參考點集J經過k次迭代后第i個對應點與目標點集S經過k次迭代后第i個對應點的對應關系，使得最小。

（2） 計算經過k次迭代后旋轉矩陣Rk和平移矩陣Tk，滿足關系最小。

（3） 運用經過k次迭代后旋轉矩陣Rk與平移矩陣Tk變換目標點集S的位置，即

（4） 將經過k+1次迭代后的均方誤差dk+1作為目標函數。

（5） 當dk+1≥τ 時 ，則返回步驟（1），直到dk+1＜τ或迭代次數k大于預設的最大迭代次數，算法結束。

當算法結束后有效收斂到局部最小值，則最后1次迭代得到的目標點集即為最終的目標點集。

3.3 掘進機位姿估計實現

搭建模擬測量掘進機位姿實驗平臺，分別在機身與機臂安裝近紅外LED標靶。掘進機機身近紅外LED標靶在巷道坐標系及機身坐標系下的點集分別為A和A′。掘進機機臂近紅外LED標靶在巷道坐標系及機身坐標系下的點集分別為B和B′。

通過ICP算法使得不同坐標系下的空間點數據合并到同一坐標系中，即得出巷道坐標系與掘進機機身坐標系的變換矩陣分別為旋轉矩陣，平移矩陣。根據位姿解算模型可得巷道坐標系與掘進機機臂坐標系的變換矩陣H2，分別為旋轉矩陣，平移矩陣。

由懸臂式掘進機的構造可知掘進機機臂通過回轉結構和液壓系統與機身進行連接，實現掘進機機臂的上下擺動與左右回轉。通過位姿解算模型可得掘進機機身與機臂在巷道坐標系下的位姿信息，從而計算出掘進機機身與機臂間的位姿關系。

以巷道坐標系為中間量，根據巷道坐標系與機身坐標系、巷道坐標系與機臂坐標系的關系，計算掘進機機身坐標系與機臂坐標系的變換矩陣H3，

由此可解算出機身坐標系與機臂坐標系間的變換關系，即旋轉矩陣R3與 平移矩陣T3。 根據 α，β，γ解算出繞機臂坐標系Xe，Ye，Ze軸的旋轉角度α′′，β′′，γ′′。

綜上，可計算出掘進機在巷道坐標系下掘進機機身與機臂的空間位姿及機臂相對于機身的空間位姿。

因截割部與掘進機機臂的運動相同，則掘進機機身坐標系與截割部坐標系的變換矩陣H4=H3，截割部平移矩陣為T5=T3+T4，T4為機臂坐標系與截割部坐標系的平移矩陣，， 其中，a為y方向上軸心距，b為掘進機截割部中心點距離機臂中心的初始位置，設置初始位置為0，d為掘進機截割部液壓關節處的伸縮量，d=0。

4 雙目視覺位姿輔助估計

4.1 數據采集與處理

對處于不同強度范圍的數據進行歸一化處理，用于網絡訓練。

式中：為第n個輸入變量歸一化值；Cmin,Cmax分別為輸入變量的最小，最大實測值；Cn為第n個輸入變量的實測值。

4.2 1D-CNN截割部位姿估計網絡模型

3層卷積層的一維卷積核大小均為3，卷積核的數量依次為16，32，64，模型使用Adam優化方法訓練參數，學習率為0.003。采用Selu激活函數對輸入信息進行非線性變換，之后輸入下一卷積層，不斷改善訓練參數，批大小為64，迭代次數為1 000。數據經卷積后引入Selu激活函數連接輸出層，經過不斷改進優化，最后得到1D-CNN截割部位姿估計網絡模型。

4.3 視覺測量輔助方法

雙目視覺測量失效下磁場輔助定位時相機前移如圖5所示。隨著掘進機不斷前進，雙目立體視覺相機與掘進機距離不斷增加，當掘進距離較遠時，近紅外LED標靶采集出現缺失，此時位姿解算模型位姿解算失敗。為保證掘進機位姿解算的實時連續性，磁場輔助定位算法開始工作，掘進機兩側磁場計采集的磁場信號經過數據處理，調用已經訓練好的1D-CNN位姿估計模型（視覺位姿解算數據實時更新，磁場數據訓練模型需不斷進行更新，以符合掘進機當時的巷道磁場環境），輸出此時掘進機截割部位姿信息。雙目立體視覺相機從地點A移動到適宜工作的地點B，雙目立體視覺相機開始正常工作。以此類推，保證掘進機截割部位姿解算保持連續性，其中網絡模型需實時訓練，即邊掘進邊訓練，以防止井下磁場異常變化導致模型失效。

圖5 雙目視覺測量失效下磁場輔助定位時相機前移Fig. 5 Camera moves forward in magnetic field assisted positioning under the failure of binocular vision measurement

5 實驗結果與分析

5.1 雙目立體視覺位姿解算

為驗證近紅外雙目立體視覺的懸臂式掘進機截割部及機身位姿測量方案的可行性，從深度信息和掘進機位姿2個方面進行驗證分析。

本實驗采用2部Mako G-223B NIR近紅外視覺相機搭建雙目立體視覺相機測量系統，由8組LED燈組成近紅外LED標靶，采用690 nm工業激光測距模塊對近紅外LED標靶進行深度信息測量，采用角度傳感器對掘進機截割部相對機身坐標系的位姿變換信息進行測量。

在實驗過程中模擬掘進機截割部做切割煤層運動，將高精度傳感器安裝在機臂中間位置，對截割部的空間位置進行檢測，雙目立體視覺相機測量系統對截割部模擬切割煤層過程進行實時數據采集，獲取數據后傳輸至計算機，計算機對圖像進行分析處理，計算出截割部與機身的三維空間信息及截割部與機身的位姿信息，最后根據截割部與機身的轉換關系解算截割部空間位姿。

在巷道坐標系下掘進機機身與截割部空間坐標變化如圖6所示。可看出在掘進機機身靜止、截割部做截割煤層運動時，機身空間坐標未出現明顯波動，截割部空間坐標發生變化，符合實際截割部運動狀態，說明基于近紅外雙目立體視覺空間位置定位方法可行。

圖6 掘進機機身與截割部空間坐標變化Fig. 6 Space coordinate change of roadheader fuselage and cutting part

在巷道坐標系下掘進機機身與截割部空間深度及誤差見表1。可看出掘進機機身在Zw軸方向的測量誤差在±11 mm以內，相對誤差在0.4%以內。截割部在Zw軸方向的測量誤差在±50 mm以內，相對誤差在1%以內。該誤差在井下煤礦施工標準誤差允許范圍內，說明該系統滿足掘進過程中懸臂式掘進機機身及截割部空間位置的精確測量需求。

表1 掘進機機身與截割部空間深度及誤差Table 1 Space depth and error of roadheader fuselage and cutting part

巷道坐標系下的機身與截割部位姿變化如圖7所示。可看出在掘進機機身靜止、截割部做截割煤層運動時，機身空間位姿角未出現明顯波動，截割部空間位姿角發生變化，其位姿角變化符合實際截割部運動狀態，說明基于近紅外雙目立體視覺空間姿態定位方法可行。

圖7 巷道坐標系下的機身與截割部位姿變化Fig. 7 The relation between the cutting part and pose of the fuselage in the tunnel coordinate system

通過位姿解算模型解算出機身與巷道坐標系、截割部與巷道坐標系的位姿變化及截割部與機身的位姿變化關系。實驗過程中機身固定，掘進機模型通過不斷改變截割部的動作模擬掘進機切割煤層，其位姿測量誤差如圖8所示。可看出掘進機機身與截割部間的相對位姿誤差在 ± 2.5°以內。通過計算可得俯仰角的均方根誤差為0.930 1°，偏航角的均方根誤差為0.922 0°（由于截割部是旋轉的，不考慮翻滾角的變化）。此誤差在巷道作業允許范圍之內，說明采用基于近紅外雙目立體視覺的懸臂式掘進機位姿測量方案進行掘進機姿態解算具有較高的有效性和可靠性。

圖8 掘進機機身與截割部相對位姿誤差Fig. 8 The relative pose error of roadheader fuselage and cutting part

5.2 視覺位姿解算失效下的磁場位姿解算

為驗證基于1D-CNN的掘進機截割部磁場輔助定位方法的有效性，在實驗室內搭建了模擬掘進機實驗平臺進行實驗。為模擬井下復雜磁場環境，在掘進機附近隨機添加干擾磁源，將采集到的2 228組磁場強度信號作為數據集，選取80%數據作為訓練集，剩下20%數據作為測試集。基于1D-CNN的掘進機截割部磁場輔助定位方法對截割部俯仰角、偏航角、翻滾角預測值與實測值對比如圖9-圖11所示。可看出掘進機截割部俯仰角、偏航角、翻滾角預測值與實測值基本吻合。

圖9 掘進機截割部俯仰角預測結果Fig. 9 Prediction results of pitch angle of roadheader cutting part

圖11 掘進機截割部翻滾角預測結果Fig. 11 Prediction results of rolling angle of roadheader cutting part

圖10 掘進機截割部偏航角預測結果Fig. 10 Prediction results of yaw angle of roadheader cutting part

為了更加精準評價基于1D-CNN的掘進機截割部磁場輔助定位方法的有效性，采用決定系數G2作為評價指標（G2越接近1，說明預測結果的準確率越高）。基于1D-CNN的掘進機截割部磁場輔助定位方法預測俯仰角G2為0.992 4，偏航角G2為0.995 9，翻滾角G2為0.917 4，說明該方法能夠很好地滿足在視覺定位失效下的掘進機定位需求。

將基于近紅外雙目立體視覺的懸臂式掘進機位姿測量方案與基于1D-CNN的磁場輔助定位方法相結合，解決了井下復雜環境導致掘進機截割部位姿實時定位問題，最終實現基于近紅外雙目立體視覺的懸臂式掘進機定位測量研究。

6 結論

（1） 從深度信息和掘進機位姿2個方面對掘進機測量進行驗證分析。結果表明：機身測量誤差在±11 mm以內，相對誤差在0.4%以內。截割部測量誤差在±50 mm以內，相對誤差在1%以內；掘進機機身與截割部間的相對位姿誤差在±2.5°以內，俯仰角的均方根誤差為0.930 1°，偏航角的均方誤差為0.922 0°。

（2） 掘進機截割部俯仰角、偏航角、翻滾角預測值與實測值基本吻合。基于1D-CNN的掘進機截割部磁場輔助定位方法預測俯仰角G2為0.992 4，偏航角G2為0.995 9，翻滾角G2為0.917 4，說明該方法能夠很好地滿足在視覺定位失效下的掘進機定位需求。

（3） 為進一步提高掘進機定位的可行性，未來應改進視覺測量的解算結果，加入濾波平滑等技術使其在實際應用中穩定輸出，不受干擾；在磁場輔助定位方向，應將磁源與機身融合，使其模塊化，從而保證在實際生產中穩定高效運行。