基于單目視覺的鉆桿位姿識別技術研究

馬斌，彭光宇

(中煤科工西安研究院(集團)有限公司，陜西 西安 710077)

2015 年原國家安監總局印發了關于開展《“機械化換人、自動化減人”科技強安專項行動》的通知；2016 年中華人民共和國國家發展和改革委員會與國家能源局印發了《能源技術革命創新行動計劃(2016-2030 年)》的通知；2020 年國家能源局等八部委聯合發布了《關于加快煤礦智能化發展的指導意見》。這些重要的通知與意見均將煤礦井下設備的自動化、智能化列為重點建設內容之一[1-3]。為響應國家號召，提高鉆機鉆進效率，降低工人勞動強度，實現鉆桿裝卸自動化，煤礦相關企業、科研院所均開展了鉆桿自動裝卸技術的相關研究[4-6]。

姚亞峰等[7]設計了一款煤礦坑道鉆機自動加卸鉆桿裝置，利用擺動油缸實現機械手定位，再和轉輪式桿倉相互配合完成鉆桿的自動裝卸；王清峰等[8]設計了基于路徑規劃的鉆桿自動輸送系統，通過主機械手、副機械手和轉運器的相互協作，實現鉆桿自動裝卸。這2 種鉆桿自動系統主要依靠機械結構和接近開關實現鉆桿的定點取放，在完成既定功能的同時，存在因機械磨損與累計誤差等因素導致系統定位精度低的問題。同時，依靠機械結構定位的鉆桿自動裝卸系統自動化程度有限，智能化程度較低。如果將視覺測量技術應用到煤礦井下鉆桿位姿識別中，勢必可以實現鉆桿定位智能識別，提高鉆桿自動裝卸精度和鉆機裝備整體的智能化水平。

視覺測量技術通常分為單目、雙目和多目等形式[9-11]。單目視覺測量系統具有結構簡單、成本低、實時性強等優點，在工業系統中得到了廣泛的認可[12-13]。為此，提出一種基于單目視覺的鉆桿位姿識別技術，利用攝像機拍攝含有合作目標的圖像，結合提取到的特征信息，解算攝像機與合作目標之間的相對距離和相對姿態，并根據鉆桿與合作目標之間的相對靜止關系以及攝像機與機械臂的相對靜止關系，推算出鉆桿相對機械臂的位姿數據，引導機械手實現鉆桿自動裝卸。

1 總體方案

單目視覺測量分為基于合作目標和基于非合作目標兩種。基于合作目標單目測量在簡化特征提取難度的同時提高了特征提取的精度[14]。在此，為了準確獲得鉆桿的位姿信息，結合井下工作環境和鉆機工作特點，選擇簡單方便的棋盤格作為合作目標，既簡化了相機標定過程，同時也容易提取圖像特征，提高測量速度。系統的整體方案如圖1 所示。首先，固定攝像機，將攝像機放置在合適的地方，再對攝像機進行標定，求取攝像機的內參和外參；然后拍攝帶有合作目標的圖片，將圖片進行灰度和濾波預處理，再對合作目標上距離世界坐標系最近的方塊進行特征點提取，結合標定所得攝像機內參和外參，對鉆桿進行距離測量與姿態估計，獲得鉆桿當前的位姿信息，將計算得到的鉆桿位姿參數矩陣傳輸給機械臂控制器，引導機械手實現對鉆桿的自動抓取。

圖1 系統整體方案Fig.1 Overall system scheme

2 攝像機成像模型及其標定

在視覺測量中，通常利用攝像機模型將空間中三維點與攝像機上的二維點聯系起來[15]。在實際應用中，攝像機模型可用數學模型來描述，有小孔成像模型(線性模型)和非線性模型兩種，其中小孔成像模型最為成熟，也應用最廣泛[16]。

2.1 攝像機成像模型

小孔成像模型包括了世界坐標系、攝像機坐標系、圖像坐標系和像素坐標系4 個坐標系，如圖2 所示。其中P為三維空間點，p為P在圖像平面投影的成像點。

圖2 小孔成像模型Fig.2 Pinhole imaging model

世界坐標系(Ow-XwYwZw) 是三維空間中的絕對坐標系，用于描述三維環境中任何一個物體的位置，一般用(Xw,Yw,Zw)表示其坐標值。攝像機安放在三維空間中，其位置信息通過世界坐標系這個基準坐標系來描述。攝像機坐標系(O-XYZ)以攝像機光心為原點，Z為主坐標軸，與攝像機光軸重合。圖像坐標系(O1-xy)中O1為攝像機光軸與圖像平面的交點，O-O1為攝像機的焦距，用(x,y)表示其坐標值。

像素坐標系以成像平面的左上角頂點為原點，u軸和v軸分別平行于圖像坐標系的x軸和y軸，其與圖像坐標系的關系如圖3 所示，用(u,v)表示其坐標值。攝像機采集的圖像首先形成標準電信號，再通過模數轉換變換為數字圖像。每幅圖像可定義為M×N的數組，M行N列的圖像中的每一個元素的數值代表的是圖像點的灰度。這樣的每個元素叫像素，像素坐標系就是以像素為單位的圖像坐標系。而圖像坐標系是用物理單位(例如：mm)表示像素在圖像中的位置。像素坐標和成像坐標體現在同一圖像坐標系中，成像坐標的原點O1(u0,v0)位于中心像素點上。

圖3 像素坐標系Fig.3 Pixels coordinate systems

理想情況下O1(u0,v0)位于圖像的中心處，設圖像中的一個像素對應于圖像坐標系在x軸的物理尺寸可表示為dx，對應于y軸方向上的物理尺寸為dy，其具體尺寸可從攝像機的參數表中獲得。像素坐標與其變換關系為：

那么，圖像坐標系下點p與在世界坐標系中對應點P的關系可由以下表達式描述：

式中：f為焦距(像平面與攝影機坐標系原點的距離)；t為三維平移向量；M1為內部參數矩陣；ax、ay、u0、v0為內部參數；M2為相機的外部參數矩陣；R和t1為相機外部參數；r1、r2、r3為矩陣R中的3 個矢量；M為投影矩陣；Pw為世界坐標系中點坐標組成的矩陣。

M1中只包含由相機內部結構決定的焦距、主點坐標等參數，M2中包含由相機坐標系相對于世界坐標系的旋轉矩陣和平移向量。

實際上由于相機光學系統存在加工和裝配誤差，相機圖像平面上實際所成的像與理想成像之間會存在畸變。畸變屬于成像的幾何失真，是由于焦平面上不同區域對圖像的放大率不同而形成的畫面扭曲變形現象，這種變形的程度從畫面中心至畫面邊緣依次遞增，主要在畫面邊緣反映比較明顯，包括徑向畸變、偏心畸變和薄棱鏡畸變等。實際計算過程中，引入過多的非線性參數(如離心畸變和薄棱畸變)，不僅不能提高精度，還會引起解的不穩定，一般情況下引入徑向畸變就足以描述非線性畸變。

2.2 張正友標定法

相機標定是指根據攝像機圖像像素位置與空間點位置之間的關系，建立攝像機模型，由已知特征點的圖像坐標求解攝像機的內部參數和外部參數的過程。“張正友標定”[17]方法介于傳統標定法和自標定法之間，克服了傳統標定法需要高精度標定物的缺點，僅需使用打印出來的棋盤格就可以完成標定。相對于自標定而言，張正友標定法精度高，操作簡單，被廣泛應用于計算機視覺方面。使用大華DH-IPC-HF5221E 型號相機，其標定的主要步驟如下：

(1)準備標定圖片。標定板以浮法玻璃為基質，棋盤格圖案由熱膨脹率較低的氧化鋁材料精密加工而成，棋盤圖案為12×9 棋盤格陣列，邊長30 mm，角點位置精度為0.01 mm。選取18 張在不同位置、不同角度、不同姿態下拍攝的標定板圖片，如圖4 所示。

圖4 標定照片Fig.4 Calibration photos

(2)提取每一張圖片的角點信息和亞像素角點信息。角點專指標定板上的內角點，與標定板的邊緣不接觸。為了進一步提高標定精度，降低相機標定偏差，在初步提取的角點信息基礎上進一步提取亞像素信息。如圖5 所示。

(3)相機標定。獲取到棋盤標定圖的內角點圖像坐標之后，就可對相機進行標定，計算相機內部參數和外參系數，標定中主要考慮影響較大的徑向畸變，標定完后可生成相機的內部參數和畸變系數。

對相機進行3 次重復性標定，標定結果見表1。

表1 相機3 次標定結果Table 1 Three calibration results of the camera

表1 中，第一行為未考慮徑向畸變時標定所得的內部參數，其余3 行為考慮徑向畸變時對相機進行3 次實時標定的結果。為了減少誤差，將3 次考慮畸變的標定結果求平均值得到：=2 645.79，=2 645.66，像平面中心點像素=(959.76,540.06)。

dx、dy的計算指CCD 面陣單元水平和垂直間距，已知相機的分辨率為1 920×1 080 像素，成像尺寸為4.8 mm×3.6 mm，則dx=dy=0.002 5 mm，可求出焦距f=6.61 mm。

3 單目測距及姿態評估

機器視覺中，手眼系統是指攝像機與機械手相互協調對目標進行識別和控制的系統。攝像機作為眼睛，引導機械手運動，攝像機和機械手的位置關系一般可分為2 種，一種是將攝像機(眼)固定在機械手(手)上面，眼隨手移動；第二種是攝像機(眼)和機械手(手)分離，眼的位置相對于手是固定的。而手眼標定則是得到像素坐標系和機械手坐標系的坐標轉化關系。

手眼標定需要將相機坐標系、世界坐標系和機械手坐標系進行關聯。在本系統中，攝像機通過支架固定在機械臂底座上，在機械臂世界坐標系的固定位置處，攝像機位置如圖6 所示，圖中鉆機主機可繞支撐點進行旋轉和平移運動。世界坐標系和機械手坐標系之間的轉換關系固定不變，相機坐標系和世界坐標系之間的關系可通過測量獲得，因此相機坐標系與機械手坐標系之間的關系也可通過計算獲得。

圖6 攝像機位置Fig.6 Schematic diagram of camera position

實際工作過程中，通過攝像機觀察鉆機上鉆桿的運行情況，選取固定在鉆機上的合作目標為標志物，判斷攝像機距離標志物之間的距離以及標志物的空間姿態，并將此位姿信息傳輸給機械臂控制器，經固定坐標變換后，控制機械手運動到指定位置，進行鉆桿裝卸。

3.1 單目測距模型建立

為了節省資源和使用方便，使用棋盤格標定板作為鉆機上的合作目標，自定義合作目標上特征點進行距離計算。待測物為空間任意平面，參考張正友平面標定方法，根據小孔成像模型和空間成像關系，建立空間任意平面的測距模型，如圖7 所示。

圖7 空間任意平面測距模型Fig.7 Spatial arbitrary plane ranging model

其中世界坐標系建立在標定板平面B上，滿足右手定則，標定板平面B為空間中任意平面，已知平面B上任意點的Zw=0，結合攝像機成像數學表達式和圖7 可知：

由式(4)可知，若已知平面B中4 個空間點的坐標值，代入上式建立方程組聯合求解，即可得到投影矩陣M。反之，若已知投影矩陣M，即可根據空間平面B上任意點P的圖像像素坐標值，求得該點的世界坐標值。針對這個模型，單目測距算法的關鍵是求解線性方程式(4)，步驟如下：

(1)首先，選取標定板的左上角為世界坐標系的原點，平面上的點Zw=0，標定板平面任意放置，已知標定板上各個黑白相間矩形的邊長為30 mm，可計算得到標定板上角點的世界坐標。

(2)取標定板平面上距離原點坐標最近的4 個角點(P1，P2，P3，P4)，根據標定板尺寸可求得4 個點的世界坐標值。

(3)在圖像中獲得這4 個點相應的圖像像素坐標值，根據式(4)，求解矩陣M。

(4)選取由4 個點所構成的矩形中心點為目標點，計算出目標點的像素坐標值，結合上一步已求解的矩陣M，計算該點的平移向量t。

(5)相機距離目標點的距離可轉化為相機光心距目標點P的距離L：

式中：C0P′為目標點在像素坐標系中的投影點P′到攝像機光心的距離。

3.2 姿態評估

通常以世界坐標系為基準，描述空間中一個物體的姿態，合作目標在空間中的姿態也是通過世界坐標系來表述，求解合作目標成像在世界坐標系中的相對關系，即為姿態的估計。設定世界坐標系固定不動，根據旋轉矩陣可求出相機坐標系的旋轉角，即為待測物體的旋轉角。設坐標系旋轉角度θ，等同于目標點繞坐標原點反方向旋轉同樣的角度θ。

計算時，設定物體進行定點旋轉，即先圍繞X軸旋轉φ，然后圍繞Y軸旋轉θ，最后圍繞Z軸旋轉φ，注意逆時針為正方向。則旋轉矩陣為：

本系統中旋轉角主要來自鉆機自身的振動和微小移動，X軸的旋轉角度范圍為-90°＜φ＜90°，且φ≠±90°，合作目標姿態可由以下公式獲得：

3.3 試驗結果與分析

3.3.1 試驗結果

2021 年11 月進行了試驗。采用基于OpenCV 的VC++進行程序的編寫，測距過程中，選取固定不變的棋盤格為合作目標，合作目標特征清晰，且不會因為外界環境的影響而難以識別。為了減少運算量，測距時直接選取棋盤格作為特征區域，首先存一張標定的棋盤格原始圖像，特征區域選取時，采用匹配的方法，在測距圖像中選定棋盤格區域定位，與標定棋盤格在世界坐標系中的選擇一樣，以棋盤格的左上角為世界坐標系的原點坐標，進行特征點的提取。

鉆機在工作過程中，鉆桿平行鉆進，鉆機的位置和姿態不會有太大的改變，噪聲主要來源于鉆機的振動和微小移動，因此引入的噪聲主要為無規律的椒鹽噪聲，預處理后圖像如圖8 所示。

圖8 預處理后的圖像Fig.8 Pre-processed images

根據實際測試鉆桿自動裝卸所積累的經驗數據，當識別的距離偏差大于0.2%，任意姿態角偏差大于1.2%時，鉆桿裝卸機械手與鉆機夾持器在交換鉆桿的瞬間會產生相互沖擊，影響機械手抓桿精度和鉆桿自動裝卸系統的使用壽命。因此，鉆桿裝卸距離偏差不大于0.2%，任意姿態角偏差不大于1.2%，才能滿足使用要求。

試驗選取了6 張現場圖片進行鉆桿的距離測量和姿態的估計，對同一張圖片進行4 次重復測距，求得其距離見表2。由于實際中準確距離不易測量，在試驗時以多次測量的平均值為基準，計算單次測量值相對于平均測量值的偏差來分析視覺測量精度。從表2 中不難看出，測距的標準偏差均不大于0.12%，精度滿足要求。隨后對姿態角度進行3 次測量，結果見表3，繞X軸的最大標準偏差為0.12%，繞Y軸的最大標準偏差1.08%，繞Z軸的最大標準偏差為0.14%，均滿足鉆桿裝卸的精度要求。

表2 距離測量結果Table 2 Distance measuring results

表3 姿態角度測量結果Table 3 Attitude angle measurement results

實際鉆孔時，根據鉆孔傾角不同，會適當調節鉆機主機相對履帶車體平臺的傾角和高度。由于鉆機主機可調范圍有限，無論如何調節，最終合作目標都在攝像機的視野中心范圍內。

試驗中對每張照片測量的結果求平均值，引導機械手自動裝卸鉆桿，機械手每次都能準確地抓取到鉆桿。

3.3.2 偏差分析

由于攝像機的位置是固定的，而鉆機的移動僅僅由鉆機工作過程中的振動和微小移動造成，所以在進行鉆機的距離估計時其變化量微小。距離的測量計算在鉆機工作時同步實時進行，實際距離不好測量，采用多次測量同一姿態下的相對標準偏差來驗證測距的準確性。由表2 可知，經過4 次的距離測算，測距的標準偏差均不大于于0.12%，精度滿足要求。

在室內進行姿態評估試驗，試驗過程中鉆機與合作目標的姿態變化較小。輕微的振動導致的姿態變化可從表3 中看出，繞X軸的最大標準偏差為0.12%，繞Y軸的最大標準偏差為1.08%，繞Z軸的最大標準偏差為0.14%，滿足鉆桿裝卸的精度要求。

在同一圖片中進行多次測量仍會出現偏差，一是因為在標定過程中直接采用了OpenCV 庫中自帶的標定函數，雖然對于亞像素點的提取和畸變參數進行了修正，但還不能完全消除，導致對角點的提取有一定的偏差，且越靠近攝像機中心的偏差越小，反之，距離越遠，偏差就會漸漸增大。二是因為在測距方法中，假定了棋盤格的世界坐標系是已知的，但是由于拍攝角度不同，棋盤格在攝像機上的成像會有一定的畸變，畸變后的成像點投影到世界坐標系中與原來的測量點無法完全重合，導致設定的世界坐標系存在偏差，最終導致了距離偏差。

4 結論

a.通過對國內坑道鉆機鉆桿自動裝卸系統的分析，發現現有鉆桿自動裝卸系統主要依靠機械結構和接近開關進行定位，存在定位精度差和自動化程度低的問題。利用基于合作目標的單目視覺技術，建立了攝像機成像數學模型，構建了空間任意平面的單目測距模型與合作目標姿態矩陣，實現了鉆桿的位姿識別。

b.試驗結果表明，基于單目視覺技術的鉆桿位姿識別算法真實有效，利用該方法推導的攝像機數學模型和鉆桿姿態估計矩陣準確，為單目視覺技術在鉆桿自動裝卸領域的應用提供了參考依據，提高了鉆桿自動裝卸精度。

c.在后續的研究工作中，可將單目視覺技術應用于鉆桿安裝與拆卸的中間過程，進一步提高鉆桿自動裝卸系統的智能化程度。