基于三激光束標靶的煤礦井下長距離視覺定位方法

楊文娟，張旭輝，張 超，萬繼成，杜昱陽，楊紅強，石 碩

(1.西安科技大學 機械工程學院，陜西 西安 710054；2.陜西省礦山機電裝備智能監測重點實驗室，陜西 西安 710054)

煤礦巷道掘進裝備的定向導航和定形截割、掘支運設備群位姿感知與協同控制，以及掘進設備群遠程精準測控，均受制于煤礦井下巷道長距離定位的準確性和穩定性。國家發改委等部門聯合印發《關于加快煤礦智能化發展的指導意見》，提出到2030年各類煤礦基本實現智能化。相比于綜采工作面智能化技術迅速發展和應用，掘進工作面施工工藝復雜，智能化水平低，“掘進失衡”矛盾制約了生產效率。煤礦井下巷道掘進裝備位姿自主測量和動態定位成為掘進裝備智能化發展面臨的首要難題。

針對掘進裝備位姿測量問題，國內外眾多研究機構在慣性導航、激光標靶、UWB、iGPS和全站儀等方面開展了卓有成效的研究。陶云飛等提出了基于iGPS的掘進機單站多點分時機身位姿測量方法、符世琛等提出基于超寬帶定位的掘進機自主定位導向方法、劉超等提出了超寬帶測距和TSOA原理的混合算法解算掘進機三維空間位姿。上述位姿測量方法存在諸如時間累積誤差、測量精度不穩定等不足。近幾年，智能視覺感知技術在煤礦井下生產視頻監控、工作面異常狀態識別方面得到初步應用。基于智能視覺技術實現井下動態目標的位姿非接觸測量已經成為該領域的研究熱點。

視覺定位技術采用光學成像原理和位姿解算模型求解被測目標的相對位置和姿態參數，具有結構簡單、無累積誤差、性價比高等優勢。煤礦井下巷道的高粉塵水霧、低照度、雜光干擾大等特點，使得視覺測量應用面臨巨大挑戰。德國、澳大利亞等礦業發達國家，國內中國礦業大學吳淼、遼寧工程技術大學陳洪月、西安科技大學張旭輝等研究團隊在掘進設備視覺定位技術進行了深入研究。奧地利萊奧本礦業大學GUGG C等研發全集成有源激光靶定位系統，利用射影變換進行平面到平面映射的方法。中國礦業大學童敏明團隊采用十字激光器與激光標靶的圖像識別測量方式構建位姿實時檢測系統，吳淼團隊提出基于空間交匯測量技術的懸臂式掘進機位姿自主測量方法，到懸臂式掘進機在固定坐標系下的位姿狀態，實現了掘進機機身的位姿檢測。中國煤炭科工集團田原研究了基于慣性導航與四點式光靶組合的掘進機自動導航定位方法。上述方法為井下定位提供了新思路，但受接收器安裝尺寸等因素限制，工作面底板起伏或者距離增大時存在脫靶可能性，系統穩定性尚待進一步研究。張旭輝教授團隊開展了一系列井下定位方面的聯合攻關，利用激光線標靶在煤礦井下低照度、高粉塵、復雜背景下的抗遮擋、遠距離特征明顯的獨特優勢，巷道平行布置2個激光指向儀，在掘進機機身安裝防爆工業相機采集激光束圖像，通過構建基于激光點線特征的兩點三線(2P3L)煤礦井下動態目標單目視覺位姿測量模型，解算得到懸臂式掘進機機身的位置和姿態參數，結合截割臂位姿數據，實現了掘進機的精確定位、定向導航和定形截割控制。但是該方法在井下標定相對復雜，惡劣環境下的測量穩定性尚需進一步提高。

相比于點特征定位模型，基于直線特征的單目視覺定位方案具有良好抗遮擋能力，適合井下低照度、高粉塵環境下獲取幾何特征。由于激光指向儀發射出有一定寬度的線結構光，線狀光束圖像有一定的像素寬度，準確提取激光線圖像的中心是實現高精度測量的關鍵。傳統提取技術有Hough變換、Steger法、灰度重心法等。華北理工大學曾凱等提出融合自適應卷積模板濾波和大津法閾值分割的條紋中心線亞像素位置提取方法，李偉明等提出一種新的快速激光條紋中心提取算法，該方法利用閾值輪廓跟蹤算法實現對激光條紋中心的快速提取，且具有較好的抗噪聲或冗余點能力。武漢理工大學曾超等提出基于圖像差分法的結構光光條圖像分割算法，進而結合方向模板與灰度重心法實現光條中心的提取。上述研究主要針對背景簡單、單束且光條均勻的激光條紋中心進行提取，需要對具有背景復雜、激光束不連續、非均勻等特點的煤礦巷道環境多激光束中心亞像素級特征提取方法進行研究。

獲得的點線特征依賴視覺定位模型才能求解獲得被測目標的位姿參數。PNP/PNL是出現最早、也是近年來研究最多的基于模型的定位方法。國防科技大學ZHANG Lilian等研究了曼哈頓世界下的基于統一相機模型的消失點估計和直線分類，所提出方法可用于未標定相機的焦距估計以及圖像中3個正交線的方向估計。南京航空航天大學WANG Ping等提出從已知的參考點確定標定攝像機的位置和方向的PNP問題。中國科學院自動化研究所王波等從幾何觀點出發證明當攝像機光心與3個控制點之間的距離相對比較大時，至少存在2個解可以同時滿足P3P問題的所有3個約束條件。沈陽理工大學秦麗娟等研究了應用線特征進行攝像機位姿測量的三線透視(P3L)問題，證明當攝像機光心不在過3條直線的2個交點且垂直于3條直線的平面時存在惟一解。新加坡A*STAR研究所XU Chi等研究了基于二維/三維直線對應關系的相機位姿估計的PnL問題，給出了PnL問題的完備性分析并提出基于子集的PnL問題的求解方法。

筆者團隊前期以2個激光指向儀構建平行激光束作為目標標靶，用2根激光線和2個出射激光點構建了2P3L測量模型，利用2條平行直線與2個激光點構建的虛擬直線，建立了掘進機機身位姿解算模型，通過閉式解解算出掘進機相對巷道的全局位姿，井下工業試驗驗證了該定位模型在煤礦井下的適用性。但是，2P3L測量模型需要保證2條激光束平行，在煤礦井下安裝時保證激光指向儀平行度難度較大，且系統外參標定復雜、難度大，需要進一步通過優化提高定位精度和穩定性。另外，在方案論證和系統測試時也發現：3條及以上激光束都可以構建視覺空間位姿測量模型，但是4線以上會提高現場安裝難度，也容易造成特征線的空間交叉，多激光線特征難以提取，導致視覺定位算法失效。

因此，筆者在前期研究基礎上，提出一種新型三激光束掘進機機身位姿單目視覺測量方法，以位姿檢測要求和難度最大的懸臂式掘進機為測試對象，在開拓巷道上方安裝3個礦用激光指向儀形成視覺測量合作標靶，采用機載防爆相機采集3條激光束，通過激光束點-線特征分割與提取，建立三點三線(3P3L)單目視覺定位模型，解算獲得掘進機機身位姿，有效提高系統位姿測量精度和穩定性。

1 掘進設備位姿視覺測量系統

考慮直線特征在高粉塵水霧、低照度的煤礦井下環境中具有更強的抗遮擋能力，筆者提出基于三激光束的單目視覺測量方法，創新設計了由3個礦用激光指向儀構建的三激光束標靶，建立了基于點-線特征的三點三線(3P3L)單目視覺測量及定位數學模型，獲得了掘進機機身的相對巷道安裝點的位置和姿態參數。煤礦井下使用時借助全站儀進行外參標定，可獲得大地坐標下的掘進機機身的位置和姿態絕對坐標數據。圖1為懸臂式掘進機機身視覺測量系統測量原理示意。

圖1 懸臂式掘進機機身視覺測量原理示意

該系統由三激光束標靶、礦用防爆相機和防爆計算機組成，其中計算機完成圖像特征提取、定位模型解算、位姿顯示等功能。三激光束標靶固定在安裝架，掛載于巷道上方。選用的激光指向儀發射660 nm波長的紅色激光，利用激光顏色分量約束可簡化井下復雜光照條件下圖像的處理，有效避免雜散光對圖像分割與提取的干擾。

2 掘進機機身三點三線位姿解算模型

借助3條非平行激光束的直線特征及3個出射激光點特征，構建了基于三點三線(3P3L)的掘進機機身位姿視覺測量模型。圖2為三點三線(3P3L)透視投影位姿測量模型。

圖2 三點三線(3P3L)透視投影位姿測量模型

(1)

(2)

(3)

其中，

(4)

(5)

(6)

(7)

其中，

(8)

(9)

其中，

由式(8)，(9)可得到如下的掘進機機身三點三線位姿視覺測量模型的矩陣約束方程：

=

(10)

其中，

=[…,]

假設= ()為矩陣的廣義逆矩陣。將式(10)中的替換為=，得到

(11)

考慮到視覺測量系統不可避免的存在誤差，根據式(11)，可得到掘進機機身三點三線位姿視覺測量模型的最小二乘法損失函數如下：

(12)

(13)

根據獲得的旋轉矩陣的參數，，，，結合式(14)可以獲得攝像機坐標系到激光標靶坐標系的相對旋轉矩陣：

(14)

根據式(15)可獲得攝像機坐標系到三激光標靶坐標系的平移矩陣:

=

(15)

建立的最小化重投影誤差目標函數為

(16)

(,,,)=(+)

(17)

式中，為第個激光點的像素坐標；為第割激光點的空間三維坐標；為相機內參數矩陣；，分別為利用GB求解器獲得的第個旋轉矩陣和平移矩陣；為將激光束上任意一點的空間三維坐標轉化為圖像投影像素點坐標的函數；為將投影像素點坐標轉化為齊次坐標的系數。

最后結合全站儀標定獲得的三激光束標靶坐標系與巷道坐標系的相對位姿轉換關系，獲得掘進機機身在巷道坐標系下的位置和姿態角信息。

筆者利用3條激光線以及3個激光點構建的虛擬直線，通過攝像機光心與圖像投影直線所構成投影平面的法向量和空間直線的垂直約束，構建了掘進機機身3P3L定位模型，建立測量模型的最小二乘法損失函數，最后結合最小化重投影誤差實現掘進機機身位姿的非迭代全局最優解估計。相比于2P3L模型，該模型無需激光束平行等位置約束，標定時只需借助全站儀獲得激光束上的6個點坐標，解決了井下現場安裝和標定難題，減少了誤差產生環節，可有效提高掘進機機身的定位精度及穩定性。

3 激光束標靶圖像分割與特征提取

3.1 三激光束標靶圖像特性

三激光束標靶是由3個礦用激光指向儀形成的3條激光束所構成，激光指向儀采用波長約660 nm的紅色激光。圖3(a)為三激光束標靶圖像，理想情況下每個激光束分布均勻且截面灰度呈高斯分布，如圖3(b)所示，灰度最大值處為激光束中心。但是由于煤礦井下掘進工作面的粉塵濃度分布并不均勻且受氣流擴散、顆粒散射等影響，大多情況激光束分布非均勻且截面灰度呈近似高斯分布，如圖3(c)所示。

圖3 三激光束圖像及其截面灰度分布特性

通過上述分析可知，三激光束標靶的二維圖像的特征分割與提取可以轉化為圖像行截面的一維灰度信號來進行處理，通過提取具有一定寬度閾值的3個波峰特征，可以穩定、有效的獲取激光束標靶圖像的直線特征。但是，對于因粉塵分布不均影響導致的非均勻分布的激光束，需要考慮實際灰度的分布差異(圖4)，因此，利用上述激光束峰值特性獲取初始粗略中心直線提取的基礎上，還需要進一步對激光束中心直線進行優化。

圖4 實際采集的激光束圖像灰度差異

3.2 三激光束標靶圖像分割與特征提取

適用于煤礦井下三激光束標靶圖像處理，包括激光束區域分割與特征提取、激光光斑的區域分割與特征提取、三激光束點-線特征定位等3個部分。該三激光束標靶圖像的區域分割、特征提取與定位流程如圖5所示。

圖5 三激光束標靶圖像的區域分割、特征提取與定位流程

圖6為實驗中的圖像特征分割、提取與定位結果。圖6(a)為采集的原始圖像。

圖6 三激光束標靶圖像特征提取與定位結果

..三激光束標靶圖像ROI分割

根據紅色激光束的HSV顏色空間分量，，的對應范圍，可以區分出復雜背景下的激光束與雜散光，其中的范圍設為0～10，的范圍設為40～250，的范圍設為40～250，如圖6(b)所示，可以濾除礦燈及其他雜光，并允許紅色較亮的激光束像素通過。

通過顏色空間約束獲取的紅色激光束像素點聚類，可以有效地獲得雜光背景下的激光束區域。假設三激光束標靶圖像中的第個像素點表示為(,)，則激光束的像素點集合可定義為

={,,…,},=[,,(,)]

(18)

0<(,)<10,40<(,)<250

(19)

40<(,)<250

(20)

..激光光斑區域分割與光斑中心定位

利用歐氏距離約束進一步對分割得到的光斑區域進行篩選，分別通過計算不同光斑區域中心沿軸和軸的像素距離來確定激光光斑。激光光斑區域分割結果如圖6(d)所示。光斑定位算法有加權質心法、曲面擬合法等，這里激光光斑中心定位采用高斯擬合算法。

..三激光束區域聚類分割

霍夫變換直線檢測方法沒有考慮激光束線寬，對于線寬變化的、不連續的激光束中心線檢測是不準確的，Steger激光條紋檢測精度高，但算法時間復雜度高。根據前面對三激光束標靶圖像的特性分析，激光標靶二維圖像的特征分割與提取可以通過激光束圖像行截面的一維灰度信號來進行處理，通過峰值法提取具有一定寬度閾值的3個波峰特征，進而通過霍夫變換建立的累加器對初始分割聚類結果進行雜點濾波處理。

首先利用峰值法求取每行圖像的峰值，形成初始左側激光束像素點簇、初始中間激光束像素點簇和初始右側激光束像素點簇(圖7)，獲取三激光束初始粗略中心線上像素點聚類信息，進而利用和構建離散參數空間，建立累加器={,}，其中,為橫坐標軸與垂直于激光線矢量的夾角;為坐標系原點到激光線的距離，通過Hough變換將獲得的激光束初始中心線上的像素點聚類由圖像空間域轉換到-參數空間。如圖8所示，激光束所在直線對應于參數空間的投票數量最多的點，因此，根據投票數量確定的局部極大值以及所對應的直線-參數，可以對所提取的激光束初始中心線像素點聚類中的雜點進行濾除，獲得激光束初始中心線像素點聚類集合。

圖7 三激光束初始中心線像素點聚類的參數空間表示

圖8 三激光束標靶圖像聚類及雜點濾除過程

采用峰值法提取的激光束初始中心點信息有效克服了Hough變換無法有效提取具有一定寬度的激光束直線的問題，同時降低了Hough變換的計算量；而Hough變換采用可以有效濾除峰值法提取的初始中心線像素點聚類的雜點干擾。

..三激光束亞像素中心直線提取

煤礦井下掘進工作面所采集的激光束圖像因粉塵分布不均影響導致激光束呈非均勻分布，在上述獲取初始粗略中心直線提取的基礎上，還需要進一步對激光束中心直線進行優化處理。

(21)

式中，(，)為二維高斯函數；(，)為粗提取的激光束中心線處的像素點灰度;(，)為對應圖像方向的高斯函數的二階偏導模板;(，)為對應圖像方向的高斯函數的二階偏導模板；(，)為對應圖像方向的高斯函數的二階偏導模板。

因此，每一處初始激光束中心線的像素點可以獲得一個Hessian矩陣，Hessian矩陣的最大特征值所對應的特征向量為該像素點處的法線方向=(,)，提取的像素點處法線方向如圖9所示。

圖9 三激光束初始粗略中心線處的法線方向提取結果

假設

(22)

其中，

在激光束粗略中心線像素點的法線方向利用泰勒展開可以得到激光束亞像素中心線位置，激光束中心的亞像素坐標可以表示為

=(,)，=,=

(23)

(24)

其中，

式中，為圖像沿方向的一階偏導數；為圖像沿方向的一階偏導數；為圖像沿方向的二階偏導數；為圖像沿方向的二階偏導數；為圖像沿方向的二階偏導數。

采用該方法可以快速獲取亞像素級的三激光束中心聚類，不受掘進機機身在巷道移動過程中距離變化導致的激光束標靶成像大小改變的影響，具有較高的提取精度和良好的穩定性；對于因遮擋或粉塵分布不均影響導致的激光束區域邊界不連續而具有容錯性和魯棒性。

..激光束中心線的參數求解

結合得到的激光束聚類結果，分別用帶約束條件的最小二乘擬合方法得到激光束的線性方程。假設左側、右側、中間激光束的像素聚類簇內每個像素點到擬合直線的距離方程為

(25)

(26)

(27)

式中，，，為左激光束的中心線參數；，和為右激光束的中心線參數；,,分別為3個激光束像素點聚類簇內的第個、第個和第個像素點。

三激光束的中心線所在直線的擬合線性方程可分別用以下帶約束條件的最小二乘法擬合：

(28)

+=1

(29)

定義如式(30)所示的直線擬合的誤差目標函數，當直線擬合誤差小于最大允許誤差時得到最優解。

(30)

4 掘進機機身位姿視覺測量精度分析

視覺測量主要誤差影響因素包括通過外參標定獲得激光束點-線特征的三維空間先驗信息、通過圖像處理獲取的激光光斑中心的像素坐標，以及激光束中心線直線方程等。為分析多因素影響下掘進機機身位姿測量精度，本節采用Mento Carlo方法對多因素影響下的掘進機機身視覺測量精度和穩定性進行數值仿真測試。

利用上述給定的平移矩陣與旋轉矩陣，借助透視投影模型將標靶坐標系下的激光束特征點-線轉換到相機坐標系，得到激光束標靶點-線特征在二維圖像平面內的理想像素坐標，進而利用Mento Carlo仿真對位姿測量精度進行評估。

激光光斑中心定位誤差、激光束中心線斜率提取誤差以及外參數標定誤差等多因素對掘進機機身位置和姿態角測量精度的影響如圖10，11所示。可以看出，位置和姿態誤差都隨著測量距離的增大而增大，其中，軸、軸2個方向具有較高的位置測量精度，軸方向的位置測量誤差較大，繞軸、軸的俯仰角和航向角具有較高的角度測量精度，繞軸的翻滾角誤差略大于繞軸、軸的角度誤差。相機與激光標靶間距離在100 m內時，軸、軸、軸方向的位置測量誤差分別在4.994，5.608，127.113 mm以內，俯仰角、偏航角、翻滾角的最大角度誤差分別為0.048°，0.093°，0.282°，能夠滿足煤礦掘進工作面巷道施工允許的最大測量誤差。

圖10 不同距離時掘進機機身位置和姿態角的視覺測量誤差

5 掘進機視覺測量系統性能測試

搭建懸臂式掘進機機身位姿視覺測量平臺進行系統性能測試，如圖12所示。測試系統由懸臂式掘進機(5∶1縮比)、礦用激光指向儀、三激光標靶固定裝置、工業相機(MV_EM510C)、數字全站儀(SOKKIA IR1)、煙霧發生器、工業計算機等組成，長距離測試中利用履帶式移動機器人模擬掘進機在巷道中的運動，用煙霧發生器模擬煤礦粉塵環境。采用礦用相機采集三激光標靶圖像，利用全站儀對懸臂式掘進機機身視覺測量系統進行評估。

圖11 不同距離下的不同截面的位置和姿態角的誤差分布

5.1 測試系統的環境適應性實驗

實驗室模擬掘進工作面的低照度、高粉塵、雜光等煤礦井下復雜背景環境，搭建的測試系統環境適應性驗證平臺如圖12所示。

圖12 懸臂式掘進機機身位姿視覺測量系統實驗平臺

實驗中在不同位置和姿態的3處固定位置各采集100幅三激光束標靶圖像，并利用本文提出的激光束分割、提取與定位算法進行重復性測試，采集的激光束圖像及其處理結果如圖13所示，其中圖13分別表示3處相對固定位置的特征提取結果，從左到右表示不同粉塵濃度的激光標靶圖像，每個位置上面一行為原始圖像，對應的下面一行為激光束特征提取結果。激光束中心線定位的均方根誤差評估對比結果見表1。

表1 不同位置處激光束中心線定位的均方根誤差評估

圖13 三激光束標靶點-線特征提取與定位結果

結果表明，該方法能夠在低照度、雜散光干擾的復雜背景下對激光標靶進行有效的分割、特征提取與定位，相比于Hough，Steger等算法，本文方法獲得的激光束中心線均方差較小，對激光束中心直線檢測的準確性更高，如圖14所示。

圖14 掘進機機身位置和姿態角測量誤差對比

5.2 機身位姿視覺測量系統性能實驗

為了對不同距離視覺測量系統性能進行測試與驗證，搭建的測量系統實驗平臺如圖15所示。實驗中控制移動機器人在樓道移動，模擬掘進機在巷道的移動，使用礦用相機對三激光束標靶圖像進行采集，對每一固定距離處采集50幅激光束圖像，解算獲得掘進機機身位置和姿態角信息，從而對視覺測量系統性能進行評估。實驗過程中，以全站儀測量值為真實值，進而利用前面提出的外參標定方法，在構建的全站儀統一坐標系下，對掘進機機身位姿視覺測量系統性能進行對比評估。

圖15 基于激光束標靶的掘進機機身位姿視覺測量系統實驗平臺

對于6處不同位置采集到的激光束圖像及其處理結果如圖16所示。結果表明，本文算法在60 m測試范圍內穩定性良好，具有較好的魯棒性。

圖16 不同距離下的三激光束標靶點-線特征提取與定位結果

履帶式機器人在樓道移動時機身位姿測量數據與真實值(全站儀數據)對比結果見表2，可見位置和姿態角誤差隨著測量距離的增大而增大，在測試距離范圍內，利用本文提出的位姿估計方法的所測位置和姿態角結果接近于真實值，具有較好的定位精度。表3給出了移動機器人在模擬巷道6處不同距離時的位置和姿態角誤差評估結果。

表2 相機與激光標靶之間不同距離時的位置和姿態角對比結果

表3結果顯示，相比于軸方向，軸、軸2個方向具有較高的位置測量精度，繞軸、軸、Z軸的俯仰角和航向角都具有較高的角度測量精度。相機與激光標靶間距離在50 m范圍內時，沿軸、軸、軸的平均測量誤差分別為8.02，27.62，95.32 mm。俯仰角、偏航角、翻滾角的平均測量誤差分別為0.26°，0.27°，0.32°。根據煤礦井下巷道施工規程允許的最大位姿估計誤差，在測試距離范圍內所提出的基于三激光標靶的視覺定位方法能夠滿足懸臂式掘進機機身位姿測量的要求。

表3 相機與激光標靶之間不同距離時的位置和姿態角誤差評估

6 結 論

(1)井下巷道定位用三激光束視覺測量合作標靶可應對煤礦高粉塵、水霧嚴重、易遮擋等因素影響。通過采用顏色空間約束與激光束截面灰度峰值聚類方式實現了激光束標靶圖像的分割；采用基于霍夫變換建立的累加器與歐氏距離約束對初始圖像粗略中心線的像素點聚類，可以有效濾除激光束圖像雜點；利用Hessian矩陣對初始粗略中心線的像素點處的法線方向進行求解，結合泰勒展開實現了三激光束中心線的亞像素級特征提取和定位。

(2)基于三點三線(3P3L)的掘進機機身位姿視覺測量模型解決了非平行激光束遠距離定位難題。針對測量模型構建的最小二乘法損失函數，分別對旋轉矩陣參數進行微分處理，獲得四階方程系統，利用GB求解器對所構建的方程系統進行求解，得到了旋轉矩陣的四元數參數，最后結合最小化重投影誤差實現非迭代全局最優解估計，獲得了掘進機機身位姿的最優解，提高了視覺測量精度。

(3)對多因素影響下的掘進機機身位姿視覺測量系統精度與穩定性進行了測試。數值仿真結果表明，在100 m測量距離范圍內，軸、軸、軸方向的位置測量誤差分別在4.994，5.608，127.113 mm以內，俯仰角、偏航角、翻滾角的最大角度誤差分別為0.048°，0.093°，0.282°。同時搭建實驗臺進行定位性能測試，實驗測試結果與理論仿真分析符合，結果表明：在50 m測量距離范圍內，姿態角測量精度在0.4° 以內，軸位置測量誤差小于10.28 mm，軸位置測量平均誤差小于31.21 mm，軸的位置測量誤差小于100.49 mm。