懸臂式掘進機視覺位姿檢測系統(tǒng)外參標定方法

張旭輝，張楷鑫，張超，楊文娟，杜昱陽，謝楠，周創(chuàng)

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

國際煤炭領域的熱門研究，懸臂式掘進機作為廣泛使用的采掘裝備占有重要地位。掘進機機身位姿檢測技術是實現(xiàn)智能化與無人化的關鍵，位姿測量精度是檢測技術的前提[1-5]。

近年來，國內(nèi)外各研究機構(gòu)針對懸臂式掘進機機身位姿檢測精度評價方法都進行了相應的研究。符世琛等[6-7]提出一種基于超寬帶測距技術的機身位姿檢測方法，利用3個機身定位點在基站群中的三維坐標來得到機身定位點的位姿，針對超寬帶測距技術進行測量誤差實驗評價，但未對機身定位點相對于機身的外參標定進行研究。朱信平等[8]介紹了一種基于全站儀的機身位姿測量系統(tǒng)，得到棱鏡坐標系與全站儀坐標系之間的位姿關系，未研究棱鏡相對于機身和全站儀相對于巷道之間的外參標定，其精度誤差較大且沒有實驗數(shù)據(jù)驗證。陶云飛等[9-10]提出基于iGPS的掘進機機身位姿測量方法，利用巷道頂板上的1套單站式測量平臺和機身上3個機載接收器進行位姿檢測，利用基于激光跟蹤儀的位姿檢測對其精度進行驗證，但未考慮測量平臺在巷道坐標系中的外參標定誤差，評價方法精度難以保證。陳慎金等[11]在掘進機機身位姿激光測量系統(tǒng)的基礎上提出全站儀自主標定策略，對不同距離下的位姿測量誤差進行仿真分析，理論上實現(xiàn)了對懸臂式掘進機位姿測量精度的要求，但未解決系統(tǒng)外參標定的問題，導致其誤差隨著標定距離的增大而增大；Hargrave等[12]搭建了一種基于雷達傳感器的井下設備定位系統(tǒng)獲得井下設備位姿信息，但并未考慮雷達傳感器在巷道中的位姿從而加大了系統(tǒng)誤差。到目前為止，國內(nèi)外針對煤巷懸臂式掘進機位姿檢測、自主定位方法展開了許多研究，但由于掘進巷道工況環(huán)境復雜、惡劣，易對位姿測量結(jié)果產(chǎn)生影響，很少有系統(tǒng)外參標定方法的研究與實驗驗證。張旭輝團隊[13-16]提出一種懸臂式掘進機截割頭與機身視覺測量系統(tǒng)，利用Vicon 對截割頭進行系統(tǒng)系統(tǒng)標定，精度達到了預期目標。但由于Vicon 系統(tǒng)只適合小范圍的系統(tǒng)外參標定，并不能適用于機身外參標定，故機身系統(tǒng)外參標定結(jié)果主要靠人工測量與傾角傳感器進行獲取，降低了視覺位姿檢測系統(tǒng)精度。

因此，本文提出一種懸臂式掘進機機身視覺位姿檢測系統(tǒng)的外參標定方法。針對視覺測量系統(tǒng)存在的外參標定誤差，提出基于全站儀測量系統(tǒng)的外參標定方法，并對全站儀的位姿檢測方法進行研究，從而保證該標定方法的可靠性。與現(xiàn)有外參標定方法相比，該方法適用場合廣，安裝要求低，適宜于掘進機機身位姿檢測系統(tǒng)的外參標定。

1 掘進機機身視覺位姿檢測系統(tǒng)及誤差分析

1.1 機身視覺位姿檢測系統(tǒng)

懸臂式掘進機的位姿檢測就是獲得機身在巷道坐標系中的位置。視覺位姿檢測系統(tǒng)由圖1所示，通過機身上的防爆相機采集激光指向儀的兩個激光光斑，將兩個激光光斑、光斑之間的直線與激光線簡化為兩點三線特征，通過視覺測量方法計算出相機相對于激光指向儀下的位姿關系，然后通過建立的掘進機機身姿態(tài)解算模型，解算出懸臂式掘進機在巷道坐標系中的位姿。

圖1 視覺位姿檢測系統(tǒng)

1.2 誤差來源分析

懸臂式掘進機視覺位姿檢測方法是根據(jù)視覺測量模型與矩陣轉(zhuǎn)換關系得到掘進機機身在巷道中的位姿關系，矩陣轉(zhuǎn)換關系如圖2所示。

圖2 矩陣轉(zhuǎn)換關系

設巷道坐標系與機身坐標系之間的齊次變換矩陣為haT；巷道坐標系與激光測量坐標系之間的齊次變換矩陣為由兩點三線特征來確定激光測量坐標系與相機坐標系之間的齊次變換矩陣dcT。根據(jù)空間矩陣變換可得

防爆相機外參不能直接測量得到，通過調(diào)試保證機身、激光指向儀和巷道平行，人工多次測量得到機身與激光指向儀在巷道中的位置平均值，并結(jié)合視覺測量結(jié)果dcT 得到相機外參矩陣acT。根據(jù)已經(jīng)得到的相機外參與矩陣轉(zhuǎn)換關系實時得到掘進機機身位姿。

從測量系統(tǒng)結(jié)構(gòu)與位姿解算方法分析可知，視覺位姿檢測系統(tǒng)誤差主要來源于視覺測量誤差與系統(tǒng)外參標定誤差，其中視覺測量誤差由特征提取方法決定，系統(tǒng)外參標定誤差由相機外參標定、機身與激光指向儀相對于巷道的測量誤差導致。

1.3 誤差處理方法

針對系統(tǒng)外參標定誤差對視覺測量系統(tǒng)精度的影響，利用全站儀測量結(jié)果與坐標系轉(zhuǎn)換矩陣對視覺位姿檢測系統(tǒng)進行外參標定。坐標系轉(zhuǎn)換原理圖如圖3所示。

圖3 坐標系轉(zhuǎn)換原理

設巷道坐標系與全站儀坐標系之間的其次變換矩陣為hoT，全站儀坐標系與機身坐標系之間的齊次變換矩陣為oaT，全站儀坐標系與激光指向儀坐標系之間的齊次變換矩陣為odT；根據(jù)空間矩陣變換得：

全站儀坐標系與巷道坐標系重合，故其之間的齊次變換矩陣hoT為1，全站儀坐標系與機身坐標系、激光指向儀坐標系之間變換矩陣由全站儀位姿檢測方法得到。故全站儀坐標系與機身坐標系、激光指向儀坐標系之間的變換矩陣即為巷道坐標系與機身坐標系、激光指向儀坐標系之間的變換矩陣。并根據(jù)式（1）視覺位姿檢測系統(tǒng)中的空間矩陣變換關系得到相機外參矩陣。

該誤差處理方法主要是利用全站儀對視覺位姿檢測系統(tǒng)的系統(tǒng)外參進行高精度標定，避免了人工測量誤差與相機外參標定誤差對視覺檢測系統(tǒng)精度的影響。因此建立基于全站儀的掘進機機身位姿檢測系統(tǒng)成為提高視覺精度的關鍵。

2 掘進機機身視覺位姿檢測系統(tǒng)外參標定方案

懸臂式掘進機機身視覺位姿檢測系統(tǒng)外參標定方案如圖4所示，包括懸臂式掘進機模型、防爆工業(yè)相機、全站儀、3個全方位棱鏡和兩個激光指向儀。將防爆相機固定于掘進機機身平面且方向與巷道掘進方向相反，使用錨桿將兩個激光指向儀固定于煤層頂部，并通過特定的加持裝置保證其平行，在掘進機工況過程中保證相機實時可以拍到激光光斑與激光光束。全站儀位于機身后側(cè)，確保其安全性。將棱鏡A 安裝在機身中心（回轉(zhuǎn)臺中心）,棱鏡B與棱鏡C分別安裝在掘進機機身中軸線兩側(cè)，保證和棱鏡A 在機身同一個水平面，以棱鏡A為圓點Oa建 立機身坐標系OaXaYaZa。設巷道坐標系OhXhYhZh與 全站儀坐標系OoXoYoZo重合且為大地絕對坐標系；

圖4 視覺位姿檢測系統(tǒng)外參標定方案

該外參標定方案主要由視覺測量系統(tǒng)與全站儀測量系統(tǒng)組成，首先通過全站儀測量原理得到掘進機機身3個360°棱鏡的三維坐標；然后利用全站儀機身位姿解算方法得到全站儀與機身、激光指向儀之間的空間變換矩陣，實現(xiàn)視覺位姿檢測系統(tǒng)的外參標定；最后分別將全站儀與人工測量得到的系統(tǒng)外參標定結(jié)果代入視覺位姿檢測系統(tǒng)，將得到的掘進機機身在巷道中的位姿進行對比，驗證基于全站儀系統(tǒng)外參標定方法的準確性與可靠性。其中保證基于全站儀外參標定方法的精度是視覺位姿檢測系統(tǒng)的前提。

3 全站儀外參標定方法與精度分析

研究全站儀位姿檢測方法，并對其進行精度分析，保證系統(tǒng)外參標定方法的精度。本文采用全站儀外參標定方法對機身與巷道、激光指向儀與巷道之間的外參進行標定。通過全站儀測量激光指向儀兩個光斑與其不共線外一點的三維信息，通過位姿解算模型得到全站儀坐標系與激光指向儀坐標系之間的位姿關系。以機身為例，分析位姿解算模型精度，最后驗證全站儀外參標定方法的可靠性。

3.1 機身位姿測量

在剛體上建立剛體坐標系OaXaYaZa， 另外在剛體之外建立一個全站儀坐標系OoXoYoZo，如圖5所示。將機身上的3個棱鏡A、B、C確定為機身平面，用于反射全站儀發(fā)射的電磁波，構(gòu)成全站儀測量系統(tǒng)的3個特征點[17-18]。通過全站儀采集棱鏡A的三維坐標作為掘進機機身位置信息；機身的姿態(tài)測量采用平面與向量之間的角度關系進行解算，通過計算機身平面下的機體坐標系與全站儀坐標系的旋轉(zhuǎn)角度，來確定掘進機在實際工況中的姿態(tài)，包括掘進機機身在全站儀坐標系下的偏航角α、俯仰角 β和橫滾角γ。

圖5 機身姿態(tài)檢測模型

根據(jù)棱鏡B與C在全站儀坐標系下的三維坐標B(xB,yB,zB)、C(xC,yC,zC)，計算出棱鏡B與C在空間坐標系下的中點 Oa為：

按照機體坐標系的方向，中點坐標(xa,ya,za)指向棱鏡A(xA,yA,zA) 形成向量 OaYa(xaA,yaA,zaA)，中點坐標(xa,ya,za)指 向棱鏡B(xB,yB,zB)形成向量OaXa(xaB,yaB,zaB)。設垂直于XOY 面的向量OaZa為( x1,y2,z3)，由于坐標系特性，向量OaZa與 OaYa、OaXa兩兩正交，故可以列出：

其中向量OaYa、OaXa的坐標已知，令x1=0，則YOZ 平面上的向量即為法向量OaZa。用向量OaXa、OaYa、OaZa代表全站儀坐標系下機體XYZ 坐標軸，用向量OoXo、OoYo、OoZo代表全站儀坐標系的3個坐標軸，機身姿態(tài)測量方法如下：

機體坐標系縱軸在全站儀坐標系縱軸和橫軸平面內(nèi)的投影與全站儀坐標系縱軸夾角為偏航角α（如圖6a）所示），機體坐標系豎軸在全站儀坐標系縱軸和豎軸平面內(nèi)的投影與全站儀坐標系豎軸夾角為俯仰角β（如圖6b）所示），機體坐標系豎軸在全站儀坐標系橫軸和豎軸平面內(nèi)的投影與全站儀坐標系豎軸的夾角為橫滾角 γ（如圖6c）所示）。機身姿態(tài)解算方法為：

圖6 機身姿態(tài)測量方法

3.2 精度分析

利用全站儀對棱鏡的三維坐標進行測量時，由于全站儀本身存在測量誤差I，導致該位姿檢測方法的精度也有所下降，因此對外參標定方法中全站儀位姿檢測方法進行精度分析[19-20]。本文以掘進機機身偏航角為例建立如圖7所示全站儀位姿檢測方法的誤差模型。

圖7航向角誤差模型

圖7 分析了偏航角誤差模型，棱鏡理論值建立的平面坐標系為OaXaYa，全站儀實際測量得到的棱鏡坐標誤差為I ，實際偏航角的坐標軸為O′aYa′。因此所建立的機身偏航角存在角度誤差 Δα，且滿足：

式中： D 為全站儀與棱鏡群的距離，mm； L為棱鏡B和C中點到棱鏡A的距離，mm。

4 實驗結(jié)果與分析

4.1 全站儀位姿檢測方法評價

本系統(tǒng)對懸臂式掘進機進行位置測量中，直接采用機身中心棱鏡A的三維坐標。為了評價全站儀位姿解算方法下航向角、俯仰角與橫滾角的精確性。搭建了懸臂式掘進機全站儀姿態(tài)評價實驗平臺，如圖8所示。平臺由全站儀（索佳iX-1000）、3個全方位棱鏡、三軸轉(zhuǎn)臺和計算機組成。采取三軸轉(zhuǎn)臺對航向角進行測量，并與基于全站儀的定位方法進行姿態(tài)評價。實驗具體步驟如下：

圖8 全站儀位姿檢測評價平臺

1）將全站儀與三腳架調(diào)平，3個棱鏡按照搭建系統(tǒng)要求放置于三軸轉(zhuǎn)臺，保證移動過程中位置不會發(fā)生偏移。

2）通過調(diào)節(jié)按鈕將三軸轉(zhuǎn)臺分別在航向角、俯仰角與橫滾角3個方向進行旋轉(zhuǎn)，三軸轉(zhuǎn)臺在每個方向一共設置11個觀測點，且每次轉(zhuǎn)動2°。

3）記錄每個位置下3個棱鏡的三維坐標，最后通過全站儀姿態(tài)解算方法得出每個位置下機身的姿態(tài)。

4）對比相鄰位置間轉(zhuǎn)臺角度與全站儀解算的變化角度。

根據(jù)以上實驗步驟進行實驗，將全站儀位姿檢測評價平臺中測量所得到的L 、 D代入式（7），得到該全站儀檢測方法理論誤差 Δα為0.0381°。得到實驗結(jié)果和誤差如下所示。

將測量得到的由于不能保證三軸轉(zhuǎn)臺與全站儀在不同水平面完全平行，故擬采取11個觀測點之間的變換量評價全站儀方法的準確性。圖9a）、圖9b）、圖9c）分別表示偏航角、俯仰角和橫滾角基于全站儀和三軸轉(zhuǎn)臺下11 個數(shù)據(jù)點之間的10 個變化量。由圖9d）可知本文使用的全站儀姿態(tài)解算方法在偏航角、俯仰角、橫滾角方向與三軸轉(zhuǎn)臺的最大誤差分別在0.06°、0.06°、0.08°之內(nèi)，平均誤差分別為0.0426°、0.0335°、0.0516°。根據(jù)上述分析：本文使用的全站儀姿態(tài)解算方法在3 個角度下的平均誤差為0.0425°，與理論誤差相符，可以作為視覺評價系統(tǒng)的基準。

圖9 全站儀姿態(tài)測量結(jié)果誤差

4.2 系統(tǒng)外參標定方法精度評價

為評價懸臂式掘進機視覺測量系統(tǒng)外參標定方法精度，建立基于全站儀測量方法的評價平臺，如圖10所示。平臺由履帶機器人、全站儀（索佳 iX-1000）、3個全方位棱鏡、YHJ-800激光指向儀、防爆工業(yè)相機（MV_EM510C）、煙霧機和計算機組成。

圖10 系統(tǒng)外參標定下視覺位姿測量精度實驗

將全站儀坐標系確定為巷道坐標系，使用煙霧機制造煙霧來模擬掘進巷道環(huán)境。且該系統(tǒng)環(huán)境下，通過式（7）和式（8）可知全站儀姿態(tài)測量精度理論最大值為0.07142°，也適用于此實驗的系統(tǒng)外參標定。首先通過全站儀位姿檢測方法得到機身、激光指向儀在全站儀坐標系下的位姿，將全站儀系統(tǒng)外參標定結(jié)果與人工測量系統(tǒng)外參標定結(jié)果分別代入視覺位姿檢測系統(tǒng)，并將履帶機器人進行移動，兩側(cè)履帶差速運動將掘進機進行位置變化，側(cè)幫輔助履帶轉(zhuǎn)動進行姿態(tài)變化。為了模擬掘進機實際工況，控制機器人朝巷道掘進方向運動20 m，一共采集20組數(shù)據(jù)進行處理，為了保證其系統(tǒng)的可靠性，在運動過程中隨機控制機器人進行運動，使用防爆計算機采集3個棱鏡的三維坐標進行位姿計算。最后對比兩次外參標定下機身在巷道中的位姿，從而驗證基于全站儀系統(tǒng)外參標定方法的精確性。

表1中列舉了履帶機器人在運動過程中5個觀測點下的實驗數(shù)據(jù)，由全站儀系統(tǒng)外參標定和人工測量系統(tǒng)外參標定下全站儀測量得到的機身位姿信息組成。由實際工況了解到：機器人在偏航角、與俯仰角方向變化較大，故實驗中偏航角 α、俯仰角β擺動角度較大。所有實驗數(shù)據(jù)如圖11所示。

表1 系統(tǒng)外參標定方法的機身位姿部分實驗結(jié)果

實驗結(jié)果表明本文所述基于全站儀系統(tǒng)外參標定方法可以提高懸臂式掘進機視覺位姿測量精度。圖11a）為兩種系統(tǒng)外參標定結(jié)果下全站儀測量得到的掘進機位置誤差，其中掘進機X 軸方向平均誤差提高了13.073 mm；Y 軸方向平均誤差提高了21.511 mm；Z 軸方向平均誤差提高了18.159 mm。圖11b）為兩種系統(tǒng)外參標定結(jié)果下全站儀測量得到的掘進機姿態(tài)角角度誤差，其中機身偏航角角度平均誤差提高了0.225°；俯仰角角度平均誤差提高了0.246°；橫滾角角度平均誤差提高了0.246°。

圖11 機身位姿測量結(jié)果誤差

5 結(jié)論

論文分析了懸臂式掘進機位姿視覺測量系統(tǒng)誤差來源，并提出一種基于全站儀的系統(tǒng)外參標定方法，根據(jù)全站儀機身位姿解算模型與系統(tǒng)坐標系轉(zhuǎn)換關系，評價基于全站儀的系統(tǒng)外參標定方法對機身視覺位姿檢測系統(tǒng)的影響。研究獲得以下結(jié)論：

1）掘進機視覺測量方法的誤差主要來源于視覺測量誤差與系統(tǒng)外參標定誤差。

2）基于全站儀位姿測量的掘進機機身靜態(tài)定位方法在精度方面滿足驗證需求，通過三軸轉(zhuǎn)臺進行精度對比分析表明，其中位置誤差在±3 mm 內(nèi)，姿態(tài)角角度誤差在0.08°內(nèi)。

3）搭建了全站儀與視覺組成的機身測量系統(tǒng)，并建立該系統(tǒng)下各坐標系之間的轉(zhuǎn)換關系，利用全站儀位姿檢測方法評價視覺位姿測量系統(tǒng)中外參標定對其精度的影響息。結(jié)果表明：該外參標定方法可以提高視覺位姿檢測系統(tǒng)的精度，其中X、Y 和Z 方向的位置平均誤差分別提高了13.073 mm、21.511 mm 和18.159 mm，偏航角、俯仰角和姿態(tài)角的角度平均誤差分別提高了0.225°、0.246°和0.246°。