基于雙目視覺的掘錨機器人行駛位移檢測方法

馬宏偉，晁勇，薛旭升，毛清華，王川偉

（1.西安科技大學 機械工程學院，陜西 西安 710054；2.陜西省礦山機電裝備智能檢測與控制重點實驗室，陜西 西安 710054）

0 引言

掘錨機器人是實現綜掘工作面掘進與支護作業的集成裝備，能夠有效提高工人的工作效率。目前在巷道掘進過程中，主要依靠工人的施工經驗調整掘錨機器人以完成掘進與鉆錨作業，難以保證其定位精度和效率，容易造成工作面嚴重的超挖、欠挖與錨桿安裝偏移等問題[1]。因此，為了實現巷道斷面的高質量成型及精準支護，必須保證對掘錨機器人的行駛位移實現精準把控，以提升掘錨機器人系統的安全性、可靠性、自動化和智能化。

目前常用于井下掘錨機器人的定位方法主要有慣性導航系統、超聲定位、全站儀定位、激光雷達定位及超寬帶定位等[2]。慣性導航系統能夠在短時間內精確檢測位姿信息，但在長時間的運行過程中累計誤差較大[3]；超聲定位性價比高，但受距離、溫度、光線等因素影響較大[4]；全站儀定位速度快，精度高，但需要人工搬運[5]；激光雷達體積小，分辨率高，但抗干擾能力差[6]；超寬帶定位功耗低、抗干擾能力強，但通信距離短[7]。由于煤礦巷道環境復雜，各種機械設備繁多，使用傳統定位方法具有局限性。雙目視覺定位技術是一種基于雙目視差原理計算像素點深度信息的技術，其采用非接觸的視覺測量方式，能夠有效利用巷道環境中已有特征，從中提取出必要的特征信息，以此解算機器人的位姿信息，能夠有效降低定位系統的復雜度，極大地提升掘錨機器人定位系統的自主性[8]。雙目視覺定位技術的核心是立體匹配算法，Census變換算法是目前使用最廣泛的立體匹配算法[9]，但是采用傳統Census變換算法得到的視差圖具有局限性，為了增強傳統Census變換算法的立體匹配性能，本文對Census變換算法進行了改進。文獻[10]針對紋理弱和深度不連續區域誤匹配的問題，在傳統Census變換算法中引入像素差平均值的計算來自動調整匹配窗口的大小，該方法能夠有效減少算法的運行時間并降低誤匹配率，但對于遮擋區域的大面積視差失真現象仍不能有效地解決。文獻[11]針對中心像素點突變問題，在傳統Census變換算法中引入自適應中心像素值權重的視差估計，通過計算鄰域像素點的平均值，以此來代替中心像素值，該方法減少了因中心像素點突變而發生的誤匹配，但在色差變化過大的場景下，鄰域像素平均值與中心像素值相差過大，仍然會造成較多誤匹配。文獻[12]在傳統Census變換算法中引入自適應支持權值方法，在計算中心像素的同時，考慮歐氏距離與輻射差異的權重，能夠有效保持圖像邊緣信息，但會將輪廓邊緣的黑洞區域填滿，呈現錯誤的物體輪廓。文獻[13]在傳統Census變換算法中引入多中心點融合監督方法，通過擴大匹配窗口，將1個中心像素點增加到4個像素點，通過相互融合監督的方式排除誤匹配點，該方法能夠大幅提升匹配率，但是增加了計算量，導致運行時間過長。

針對掘錨機器人行駛過程中存在位移檢測精度低的問題，本文提出了一種基于雙目視覺的掘錨機器人行駛位移檢測方法。首先，針對煤礦井下復雜環境，對傳統Census變換算法中的相似性檢測函數進行改進，提高了雙目立體匹配的精度；其次，利用圖像處理算法識別并提取錨桿特征點，對特征點進行最小二乘法空間直線擬合，在此直線上建立平行于截面的平面；最后建立“掘錨機器人?已支護錨桿”定位模型，進行模擬定位實驗并分析定位誤差。

1 掘錨機器人視覺測距方法原理

通過在掘錨機器人機身頂部安裝雙目相機，監測機器人后方巷道頂部的已支護錨桿，分析掘錨機器人、雙目相機與已支護錨桿的空間位置關系，建立基于分布式錨桿的雙目視覺測距模型，并通過模型計算得出雙目相機位置信息，求解掘錨機器人在巷道中相對于錨桿的行駛距離。

視覺定位原理如圖1所示。以雙目相機光心為坐標原點，以沿掘錨機器人垂直于側幫方向、豎直方向和前進方向分別為x軸、y軸和z軸，建立三維坐標系，設D為雙目相機與錨桿特征點之間的空間距離，E為雙目相機與錨桿所在平行于截面的平面（簡稱平行截面）之間的垂直距離，通過幾何關系解算E，即可得到掘錨機器人相對于已支護錨桿的行駛距離。

圖1 視覺定位原理Fig.1 Visual positioning principle

視覺測距系統主要由視覺信息獲取模塊、誤差分析模塊和機身運動控制模塊組成，如圖2所示。視覺信息獲取模塊對圖像進行預處理，提取圖像中所需的特征信息，并將其轉換為視覺信息。誤差分析模塊對視覺信息進行分析，將計算得到的位移距離與理論位移距離進行對比，得到機身行駛位移誤差。將運動距離誤差傳入機身運動控制模塊，由機身運動控制卡發送誤差補償數據，同時向行駛機構下發控制指令以驅動機身行駛到預計的位置。

圖2 視覺測距系統結構Fig.2 Structure of vision ranging system

視覺信息獲取模塊是視覺測距系統的核心部分，主要步驟如下：

（1）相機標定與立體校正。通過雙目相機采集錨桿的原始圖像，再通過相機標定后得到相機的畸變系數、內外參矩陣等參數，對采集到的圖像進行立體校正。

（2）雙目立體匹配。對校正后的左右視圖采用Census變換算法進行匹配代價計算，得到視差圖，由此可得到圖像中像素點的深度信息。

（3）錨桿輪廓特征提取。采用Sobel邊緣檢測算法對視差圖中的錨桿進行輪廓特征提取，突出錨桿輪廓特征。

（4）錨桿特征點提取。根據錨桿輪廓特征，采用最小外接矩形與最大外接矩形組合算法（簡稱組合外接矩形算法）提取錨桿輪廓的邊界點，選取錨桿與頂板相接處的邊界點為特征點，輸出其像素坐標。

（5）特征點坐標轉換。通過分析錨桿特征點像素坐標與掘錨機器人世界坐標的轉換關系，將錨桿特征點像素坐標轉換為世界坐標。

（6）特征點空間直線擬合。采用最小二乘法將同一排錨桿的特征點世界坐標擬合成一條直線，在該直線上建立平行截面，計算相機與平行截面的距離。

2 雙目立體匹配視差圖獲取

2.1 分布式錨桿多目標雙目視差原理

雙目視覺測距原理如圖3所示。其中，P為錨桿特征點，Ol，Or分別為左右相機光心，Pl，Pr分別為P點在左右相機成像平面上的成像點，OCl-XYlZl，OCr-XYrZr分別為左右相機的圖像坐標系。左右相機光心發射2條射線，在同名點處相交，以此確定唯一目標點，通過三角法計算出目標點與相機之間的距離信息[14]。

圖3 雙目視覺測距原理Fig.3 Binocular vision ranging principle

根據掘錨機器人作業需求及錨桿分布特點，建立基于分布式錨桿的雙目視覺定位模型，如圖4所示。其中Pi（i=0，1，···，n，n為特征點總數）為第i個錨桿特征點，f為相機焦距，B為光心之間的距離（基線），Z為被測特征點與相機光心之間的距離，xl，xr分別為特征點在左右相機像素坐標系上的x軸坐標值，兩值相減即為該特征點的視差值d。

圖4 雙目視覺定位模型Fig.4 Binocular vision positioning model

根據相似三角原理有

2.2 傳統Census變換算法原理

傳統Census變換算法是在一幅圖像中以某個像素為中心創建一個3×3矩形窗口，將窗口內鄰域與中心像素的灰度值進行比較，若鄰域像素灰度值比中心像素灰度值小，則比特串相應位置的值記為1，反之記為0[15]。

式中：為結果判定函數；p，q分別為窗口中心像素灰度值和其他鄰域像素灰度值。

將判定結果串聯，得到Census變換算法的比特串CT。

式中：W為中心像素灰度值p的鄰域；為按位連接符。

窗口中所有像素點經過Census變換算法后，得到相應的比特串，通過漢明距離對2個比特串值進行異或運算，以此作為匹配代價的值。

式中：A（s,d）為2個比特串對應位數值不同的數量，即匹配代價值；s為左圖像素點；H為漢明距離計算函數；CTl（s）為左圖像素點的比特串；CTr（s，d）為右圖中對應像素點的比特串。

傳統Census變換算法的核心思想是由匹配代價值A（s,d）來判定2個像素點的相似性，A（s,d）的值越小，則2個像素點的相似度越高。

2.3 改進Census變換算法

煤礦井下環境中存在大量重復紋理與深度不連續區域，采用傳統Census變換算法的誤匹配率高。因此，本文在傳統Census變換算法的基礎上加入絕對值比較參數，對傳統Census變換算法進行改進，使原算法獲得更好的匹配窗口判斷性，提高匹配精度與視差圖效果，以適應煤礦井下的復雜環境。

將2個待匹配窗口的中心像素點p與其他鄰域像素點q的像素值相減并將結果進行串聯，再將2組串聯結果按位做差并取絕對值。

式中：LT（p）為左圖窗口經過變換得到的比特串；RT（p）為右圖窗口經過變換得到的比特串；WT為2組比特串按位做差并取絕對值得到的新比特串。

再取2組WT中的元素按位進行比較，其值大的元素在相應位上記為1，反之記為0。

式中φ（WT）為WT的相似度比較函數。

將相似度比較函數φ（WT）得到的比較結果相加，其值越小則2個窗口的相似度越大，其值越大則2個窗口的相似度越小，取值最小的窗口作為正確的匹配窗口。

傳統Census變換算法和改進Census變換算法對3個高相似度窗口的匹配結果如圖5所示。可看出傳統算法中，左圖窗口與2個右圖窗口內的像素信息都存在不同程度的差異，經過傳統Census變換算法得到的比特串一致，由漢明距離計算得到相同的匹配代價值，表明左圖窗口與2個右圖窗口有著相同的相似度，而實際上，左圖窗口與右圖窗口2的相似度更高，故在匹配過程中容易發生誤匹配現象；經過改進Census變換算法對2組窗口進行相似度計算并選擇最小值，成功匹配到了相似度更高的窗口。由此可知，改進Census變換算法具有更強的相似度判斷性，可以更精確地反映2組窗口的相似程度。

圖5 Census變換算法改進前后對比Fig.5 Comparison of Censustransform algorithm before and after improvement

3 錨桿特征點識別與定位

3.1 錨桿特征點識別

為了保證錨桿特征點處在一條水平線上，選取錨桿與巷道頂板的相接點作為特征點，對其進行識別與提取。采用圖像處理技術在改進Census變換算法得到的視差圖中分割并提取出感興趣區域（ROI）[16]，將ROI作為輸入圖像進行處理，如圖6所示。首先將獲取的錨桿視差圖轉為灰度圖像，對其進行高斯去噪處理，并采用Sobel算子計算圖像梯度，得到圖像邊緣幅值與角度。然后沿梯度方向進行非極大值抑制，對錨桿輪廓進行邊緣細化，經過雙閾值邊緣連接處理，消除細小的尖刺，修復斷開的連接并去除局部小面積的干擾特征，輸出較為完整的錨桿輪廓的二值化圖像。最后對二值化圖像采用組合外接矩形算法提取錨桿特征點，選取最頂部的點作為特征點，并輸出其像素坐標，實現錨桿特征點提取的目標。

圖6 圖像處理流程Fig.6 Image processing flow

3.2 像素坐標到世界坐標系的轉換

經過圖像處理后得到錨桿特征點的像素坐標值，采用坐標轉換原理將像素坐標值轉換為世界坐標值，轉換關系如圖7所示。其中，OW-XWYWZW為世界坐標系，OC-XCYCZC為相機坐標系，oxy為圖像坐標系，uv為像素坐標系，像素坐標系是以像素為單位，其余坐標系均以毫米為單位。Q點為世界坐標系中的任意一點，其在世界坐標系下的坐標為Q（xW，yW，zW），在相機坐標系下的坐標為Q（xC，yC，zC），G點為Q點在圖像中的成像點，在圖像坐標系中的坐標為（x，y），在像素坐標系中的坐標為（u0，v0）。

圖7 坐標系轉換關系Fig.7 Coordinate system conversion relationship

從世界坐標系到相機坐標系的轉換涉及到旋轉和平移，繞著不同的坐標軸旋轉不同的角度，得到相應的矩陣。Q點在相機坐標系中的坐標為

式中：S為機身坐標系與世界坐標系間的旋轉矩陣；T為相機坐標系與世界坐標系間的平移向量。

結合雙目視差原理和坐標系轉換關系，即可求得圖像中像素點對應的世界坐標值。從相機坐標系到圖像坐標系的轉換屬于透視投影關系，由相似三角形原理可得

圖像坐標系到像素坐標系的轉換關系為

像素坐標系到世界坐標系的轉換關系為

3.3 錨桿特征點直線擬合

根據掘錨機器人定位原理，需將同一排錨桿特征點擬合成平行于截面的直線。由于受到錨桿安裝誤差及計算誤差影響，在同一排錨桿中，各個特征點與相機間的距離各不相同，故采用最小二乘法對同一排錨桿特征點進行空間直線擬合[17]。因為特征點空間坐標的y值只影響擬合直線的上下位置，與前后位置無關，故在直線擬合時只需考慮x值與z值。

式中：Kε為真實值與預測值誤差；（xi，zi）為已知坐標點；z為zi的擬合值；a為直線斜率；b為y軸截距。

因為要求擬合出的空間直線在平行截面中，故該直線必須平行于x軸，即令a=0，可得

由此可根據坐標點得到擬合直線所在平面xoz上的垂直投影。

式中l為相機與平行截面之間的垂直距離。

經過直線方程構建平行截面，建立“掘錨機器人?已支護錨桿”定位模型，如圖8所示。其中α，β，χ分別為錨桿特征點擬合直線所建立的平行截面，l1，l2，l3分別為相機光心與平行截面之間的垂直距離。將li作為相機與平行截面之間的距離信息，來計算掘錨機器人的行駛位移。

圖8 “掘錨機器人?已支護錨桿”定位模型Fig.8 Positioning model of “anchor digging robot-supported bolt”

4 機器人視覺定位實驗驗證

為了驗證掘錨機器人定位方法的可行性，搭建移動機器人平臺進行行駛位移檢測實驗。位移檢測系統由雙目相機、錨桿、計算機及履帶式移動機器人組成，其實驗平臺如圖9所示。將錨桿并排放置在地面上，代替安裝在巷道頂部的錨桿；將雙目相機置于距離錨桿1 m處進行拍攝，在實驗過程中，保持相機水平向前移動，以模擬掘錨機器人在巷道中的行駛狀態。

圖9 位移檢測系統實驗平臺Fig.9 Experiment platform of displacement detection system

以掘錨機器人為研究對象，相機光心為坐標系原點，建立世界坐標系。其中以垂直于側幫方向為x軸，垂直于頂板方向為y軸，掘錨機器人前進方向為z軸。實驗場地如圖10所示，設錨桿排距I=800 mm，每排錨桿的分布距離M=2 000 mm，相機距第3排錨桿的距離N=1 400 mm。由于雙目相機的有效測距范圍為4 000 mm，故僅取距相機最近的前3排錨桿進行測量。通過對測量值與理論模型計算值進行誤差分析，從而實現實驗平臺的視覺測距、移動控制和軌跡跟蹤。

圖10 實驗場地Fig.10 Sketch map of experimental filed

4.1 Census變換算法改進效果

為了獲取準確的錨桿圖像，在定位系統實驗平臺采用Matlab雙目相機工具箱進行相機標定與立體校正，校正前后結果如圖11所示。

圖11 校正前后結果Fig.11 Resultsbefore and after correction

使用雙目相機采集一排錨桿圖像，提取其ROI圖形，分別采用改進前后Census變換算法對ROI圖形進行視差圖處理，如圖12所示。可看出改進后Census變換算法改善了錨桿邊緣的失真區域，得到了較明顯的錨桿輪廓特征及更準確的深度信息。

圖12 改進前后視差圖效果對比Fig.12 Effect comparison of disparity before and after improvement

算法性能對比結果見表1。可看出改進Census變換算法的匹配精度顯著提升，比傳統Census變換算法誤匹配率降低了41.96%，但是由于計算過程較為復雜，導致運行時間也有一定的增加。

表1 算法性能對比Table 1 Comparison of algorithm performance

為驗證不同視差初始值對算法性能的影響，取初始視差值為[?13，7]對算法性能進行驗證，對比結果如圖13所示。可看出當視差范圍縮小時，誤匹配率及運行時間均有所降低。因此，在保留完整錨桿圖像的情況下，可設定較大的視差初始值，以降低誤匹配率并減少運行時間。

圖13 不同視差初始值下的性能對比Fig.13 Performancecomparison under different parallax initial values

4.2 錨桿特征點識別與定位

（1）錨桿特征點提取與坐標轉換。對校正后的錨桿圖像進行特征點坐標提取，由于錨桿放置在地面上，故以錨桿底部的端點為特征點，對其進行提取。首先采用改進后的Census變換算法對相機原圖進行處理，得到錨桿圖像的視差圖，然后對視差圖進行Sobel算子輪廓檢測提取錨桿輪廓，最后采用組合外接矩形算法對錨桿輪廓進行處理并將錨桿特征點用藍色圓點標記出來，錨桿特征點提取結果如圖14所示。

圖14 特征點提取結果Fig.14 Extraction results of anchor feature points

將提取出的特征點像素坐標轉換為世界坐標，坐標轉換結果見表2。

表2 特征點坐標轉換結果Table 2 Coordinate conversion resultsof featurepoints

（2）特征點直線擬合。由于同一排錨桿特征點位置在z軸上存在偏差，所以無法確定相機與這排錨桿之間的距離，因此，根據特征點直線擬合原理，將特征點擬合成平行于x軸的直線，擬合結果如圖15所示。可看出經過直線擬合后，消除了不同特征點間的位置誤差，得到了與特征點位置緊密關聯的直線。

圖15 特征點擬合直線結果Fig.15 Straight line fitting results of feature points

（3）特征點平行截面建立。經過特征點擬合直線建立平行截面，如圖16所示。可看出相機與3個平行截面之間的距離分別為3 010.428，2 215.910，1 415.127 mm。

圖16 特征點平行截面Fig.16 Parallel section of feature points

在機器人移動過程中，通過計算得到相機與平行截面之間的距離，根據錨桿在模擬巷道中的安裝位置，即可得到移動機器人在世界坐標系下的行駛位移。

4.3 移動機器人定位實驗

在實驗場地上放置10排錨桿，排距為800 mm，移動機器人起始位置在距第1排錨桿800 mm處，控制移動機器人沿著z軸方向前進8 000 mm，前進速度為0.25 m/s，雙目相機采集錨桿圖像頻率為5 Hz。以第1排錨桿為定位基準，控制機器人沿z軸前進，依次拍攝錨桿圖像，計算相機與錨桿平行截面的距離，以此求解機器人行駛的位移。

在進行機器人定位實驗時，將計算位移與理論位移進行對比，如圖17所示。可看出移動機器人在整個定位實驗過程中，理論位移曲線與計算位移曲線基本重合。

圖17 運動軌跡對比曲線Fig.17 Comparison curve of motion track

通過對比相機的理論位移與計算位移得到兩者的誤差分布，如圖18所示。可看出理論位移與計算位移誤差不超過20 mm，能夠保證掘錨機器人的定位精度，可實現掘錨機器人的自主、準確、實時位移檢測。

圖18 運動軌跡誤差分布Fig.18 Error distribution of motion track

5 結論

針對掘錨機器人行駛過程中存在位移檢測精度低的問題，提出了一種基于雙目視覺的掘錨機器人行駛位移檢測方法。該方法以已支護錨桿為定位基準，利用雙目視覺檢測掘錨機器人的行駛位移，解決了掘錨機器人在井下行駛無參考目標的問題，提高了雙目視覺在煤礦井下環境中的適應性與定位精度，實現了掘錨機器人行駛位移的精確檢測，為掘錨機器人的定位提供了有效解決方案。

（1）改進后的Census變換算法修復了錨桿圖像的失真區域，能夠得到效果較好的視差圖，并使誤匹配率從19.85%降低到11.52%，較傳統Census變換算法的誤匹配率降低了41.96%。

（2）當視差范圍縮小時，誤匹配率及運行時間均有所降低。因此，在保留完整錨桿圖像的情況下，可設定較大的視差初始值，以降低誤匹配率并減少運行時間。

（3）經過直線擬合后，消除了不同特征點間的位置誤差，得到了與特征點位置緊密關聯的直線。在擬合直線上建立平行截面，得到相機與3個平行截面的距離分別為3 010.428，2 215.910，1 415.127 mm。

（4）由模擬實驗可知，理論位移曲線與計算位移曲線基本重合，理論位移與計算位移誤差不超過20 mm。說明基于雙目視覺的掘錨機器人行駛位移檢測方法能夠保證掘錨機器人的定位精度，可實現掘錨機器人的自主、準確、實時位移檢測。