錨桿支護(hù)自動(dòng)作業(yè)二次定位修正方法

李航,李瑞,李響

(1.北京信息科技大學(xué) 儀器科學(xué)與光電工程學(xué)院,北京 100192;2.北京唯實(shí)深藍(lán)科技有限公司 北京 110105)

0 引言

在煤礦掘進(jìn)領(lǐng)域中,巷道的安全和暢通是保證快速掘進(jìn)、高效開采的必然前提。目前,針對(duì)巷道安全的眾多保護(hù)措施中,錨桿支護(hù)技術(shù)是應(yīng)用最廣泛,安全效果最佳的防護(hù)方式之一[1]。但目前的錨桿支護(hù)安裝和定位任務(wù)基本都是靠人工操作來完成的[1]。這種方式不僅對(duì)操作員的要求高,而且效率低,對(duì)操作員存在許多危險(xiǎn)[2]。因此,錨桿支護(hù)的自動(dòng)作業(yè)技術(shù)成為了煤礦領(lǐng)域研究的熱點(diǎn)。

錨桿支護(hù)的自動(dòng)作業(yè),核心是錨孔的自動(dòng)識(shí)別和機(jī)械臂的定位。錨桿支護(hù)的自動(dòng)作業(yè)是否成功完成主要在于機(jī)械臂定位鉆孔的位置是否存在偏差。在實(shí)際的工況要求中,機(jī)械臂對(duì)錨鉆孔的定位偏離誤差通常要求小于5 mm。但是由于煤礦井下環(huán)境惡劣,錨桿機(jī)長期進(jìn)行大重量操作,會(huì)使機(jī)械系統(tǒng)零部件的間隙逐漸增大,導(dǎo)致定位精度下降,無法保證機(jī)械臂達(dá)到鉆孔位置的精度和重復(fù)性要求[3-4]。在錨鉆孔定位中,基于視覺測(cè)量的輔助定位技術(shù)能夠?qū)崟r(shí)檢測(cè)鉆臂與錨鉆孔的空間位置關(guān)系,進(jìn)而可減小機(jī)械系統(tǒng)自身誤差累積造成的定位誤差[5]。目前,機(jī)器臂的視覺技術(shù)可分為兩類:二維視覺技術(shù)和三維視覺技術(shù)[6]。相較于二維視覺技術(shù),三維視覺技術(shù)在獲得平面信息的同時(shí),可以獲得目標(biāo)點(diǎn)的深度信息,進(jìn)而更能準(zhǔn)確地描述目標(biāo)點(diǎn)的空間位置,所以被廣泛應(yīng)用到機(jī)械臂定位中[7]。

目前市場(chǎng)上用于三維視覺定位的相機(jī)包括:結(jié)構(gòu)光相機(jī)、雙目相機(jī)和TOF相機(jī)。而結(jié)構(gòu)光相機(jī)既不需要進(jìn)行大量的雙目匹配計(jì)算,又因其高精度、小體積、低成本、低功耗等優(yōu)勢(shì)成為了主流的三維視覺相機(jī),并在巷道、隧道等領(lǐng)域逐漸興起[4]。但市場(chǎng)上的工業(yè)級(jí)結(jié)構(gòu)光相機(jī)由于加工工藝和裝配精度等原因,會(huì)使相機(jī)鏡頭存在畸變,嚴(yán)重影響定位測(cè)量精度[8]。為了消除畸變對(duì)測(cè)量誤差的影響,陳文藝等[9]提出基于深度學(xué)習(xí)的單張圖像畸變校正方法。丁先之等[10]對(duì)面向礦石破碎無人化系統(tǒng)的手眼標(biāo)定及誤差補(bǔ)償研究來提升定位精度。但這兩種方法計(jì)算量大,操作復(fù)雜,在實(shí)際的煤礦環(huán)境中實(shí)施困難。因此,本文提出了一種在不進(jìn)行畸變矯正的情況下,通過二次定位修正補(bǔ)償?shù)姆椒?可以滿足在0.5～1.5 m工作范圍內(nèi),錨鉆孔定位誤差小于5 mm的工作要求。

1 錨孔定位誤差理論建模

1.1 錨孔定位系統(tǒng)建模

整個(gè)錨鉆孔定位系統(tǒng)模型含有相機(jī)坐標(biāo)系、機(jī)械臂末端坐標(biāo)系和機(jī)械臂基坐標(biāo)系。通過“手眼”標(biāo)定的方法,可以將相機(jī)中的錨鉆孔的三維坐標(biāo)轉(zhuǎn)到機(jī)械臂基坐標(biāo)系下坐標(biāo),進(jìn)而讓機(jī)械臂進(jìn)行坐標(biāo)定位。而“手眼”系統(tǒng)又可以根據(jù)相機(jī)與機(jī)械臂的位置關(guān)系分為“眼在手上”和“眼在手外”[11]。

本文使用的是“眼在手上”的“手眼”系統(tǒng)。Tsai的兩步標(biāo)定法可以通過一個(gè)三變量的優(yōu)化搜索求解,來消除“手眼”標(biāo)定中相機(jī)畸變對(duì)標(biāo)定精度的影響[12]。因此,本文選擇Tsai兩步法進(jìn)行“手眼”標(biāo)定工作。

圖1 錨鉆機(jī)械臂

將圖1簡(jiǎn)化成幾何結(jié)構(gòu),如圖2所示,其中L1為機(jī)械臂旋轉(zhuǎn)軸與俯仰軸的水平距離;L2為機(jī)械臂旋轉(zhuǎn)軸與俯仰軸的垂直距離;L3為機(jī)械臂末端與機(jī)械臂俯仰軸的距離;L4為機(jī)械臂末端與機(jī)械臂俯仰軸的垂直距離。機(jī)械臂結(jié)構(gòu)由鐵架組成,L1、L2、L3、L4數(shù)值由卷尺測(cè)量得到。

圖2 機(jī)械臂幾何抽象圖

通過機(jī)械臂的運(yùn)動(dòng)學(xué)模型的測(cè)量結(jié)果,求解得到:

(1)

(2)

然后利用式(1)和(2),就可以將相機(jī)坐標(biāo)系下錨鉆孔的三維坐標(biāo)轉(zhuǎn)換到機(jī)械臂基座標(biāo)系下的三維坐標(biāo):

(3)

式中:xbase、ybase、zbase為機(jī)械臂基座坐標(biāo)系下的坐標(biāo);Xc、Yc、Zc為相機(jī)坐標(biāo)系下的坐標(biāo)。

旋轉(zhuǎn)機(jī)械臂將錨桿插入到錨鉆孔中,得到插入該位置的錨鉆孔所需的旋轉(zhuǎn)角度θ1和θ2,并且此時(shí)錨鉆孔在機(jī)械臂末端坐標(biāo)系下的坐標(biāo)為O(x′,0,0),通過機(jī)械臂的逆運(yùn)動(dòng)學(xué)模型[13]求坐標(biāo)x′的值,計(jì)算式如式(4)所示:

(4)

式中:T為機(jī)械臂基坐標(biāo)系到機(jī)械臂末端坐標(biāo)系的轉(zhuǎn)換矩陣。當(dāng)機(jī)械臂兩個(gè)旋轉(zhuǎn)軸的運(yùn)動(dòng)角度都小于5°時(shí),可將式(4)化簡(jiǎn)為

(5)

1.2 誤差模型理論推導(dǎo)

結(jié)構(gòu)光視覺測(cè)量系統(tǒng)含有像素坐標(biāo)系、圖像坐標(biāo)系、相機(jī)坐標(biāo)系以及世界坐標(biāo)系,通過相機(jī)的標(biāo)定可以得到各個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系[13]。根據(jù)結(jié)構(gòu)光相機(jī)的透視投影模型和相似三角形計(jì)算關(guān)系,可以得到圖像坐標(biāo)系與相機(jī)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系[14]:

(6)

式中:x、y為圖像坐標(biāo)系下的坐標(biāo);f為相機(jī)的焦距。化簡(jiǎn)后得到:

(7)

聯(lián)立式(3)和(7)得到錨鉆孔中心點(diǎn)的圖像坐標(biāo)與機(jī)械臂基座標(biāo)的轉(zhuǎn)換關(guān)系:

(8)

將式(5)中的xbase、ybase、zbase用式(8)算出的結(jié)果代替,最終得到了在結(jié)構(gòu)光相機(jī)鏡頭理想情況下機(jī)械臂定位到錨鉆孔中心點(diǎn)所需的角度與其圖像坐標(biāo)之間關(guān)系:

(9)

但由于鏡頭畸變的存在,導(dǎo)致圖像邊緣出現(xiàn)拉伸或收縮,如圖3所示。在結(jié)構(gòu)光視覺測(cè)量中,如果使用了帶有畸變的鏡頭進(jìn)行視覺測(cè)量,會(huì)大大影響視覺測(cè)量的精度,并且,越是靠近圖像邊緣位置的點(diǎn),其測(cè)量誤差越大,圖像四個(gè)角的點(diǎn)誤差達(dá)到最大值[15]。

圖3 畸變圖像示意圖

在所有畸變參數(shù)中造成影響較大的是徑向畸變,其數(shù)學(xué)模型為:

xu=x(1+k1r2+k2r4+k3r6+…)

(10)

yu=y(1+k1r2+k2r4+k3r6+…)

(11)

因此,當(dāng)結(jié)構(gòu)光相機(jī)鏡頭存在徑向畸變時(shí),取徑向畸變系數(shù)為k1,將式(9)、(10)、(11)聯(lián)立,得到錨鉆孔中心點(diǎn)的圖像坐標(biāo)受相機(jī)鏡頭徑向畸變影響的真實(shí)定位模型為

(12)

將式(9)與式(12)做差,即得到了由于相機(jī)鏡頭徑向畸變對(duì)錨鉆孔定位造成的誤差模型:

(13)

當(dāng)錨鉆孔與相機(jī)所在平面平行時(shí),令Zc=1 000 mm,f=1 000,k1=0.01,觀察機(jī)械臂定位圖像上不同點(diǎn)位置所需的旋轉(zhuǎn)角度θ1與θ2的關(guān)系,如圖4所示。

圖6(a)為當(dāng)圖像坐標(biāo)x為0時(shí),y從-40 mm到40 mm取值,分別用不帶相機(jī)鏡頭畸變的模型與相機(jī)帶徑向畸變的模型計(jì)算出的定位角的差值。圖6(b)為當(dāng)圖像坐標(biāo)y為0時(shí),x分別從-40 mm到40 mm取值,分別用不帶相機(jī)鏡頭畸變的模型與相機(jī)帶徑向畸變的模型計(jì)算出的定位角的差值。從圖6可以看出,當(dāng)圖像坐標(biāo)x、y越趨于0時(shí),機(jī)械臂定位的誤差角度Δθ1和Δθ2越小,定位的精度越高;反之,當(dāng)圖像坐標(biāo)x、y的絕對(duì)值越大時(shí),機(jī)械臂定位的誤差角度Δθ1和Δθ2越大,定位的精度越低。

圖4 無畸變時(shí)圖像x、y坐標(biāo)與定位角θ1、θ2的關(guān)系

圖5 有畸變時(shí)圖像x、y坐標(biāo)與定位角的關(guān)系

圖6 圖像坐標(biāo)與誤差角度的關(guān)系

根據(jù)弧長公式可以計(jì)算出機(jī)械臂在運(yùn)動(dòng)到不同角度時(shí),其在錨鉆孔平面所行走的距離:

(14)

式中:n為機(jī)械臂旋轉(zhuǎn)的角度;r為錨鉆孔所在平面到機(jī)械臂旋轉(zhuǎn)軸之間的距離;L為機(jī)械臂轉(zhuǎn)動(dòng)的距離。當(dāng)工作距離為1 m時(shí),若要定位誤差小于5 mm的精度要求,用式(14)計(jì)算出最大誤差角度為0.133°。從圖6可以看出,要想使定位的誤差角度小于0.133°,圖像坐標(biāo)x的取值范圍為:-22 mm

2 二次定位修正方法

針對(duì)上述分析結(jié)論,本文提出了使用二次定位修正的方法來減小第一次定位時(shí)的誤差,其流程如圖7所示。首先,從結(jié)構(gòu)光深度相機(jī)中獲取錨鉆孔的三維坐標(biāo);然后通過“手眼”標(biāo)定結(jié)果和機(jī)械臂的運(yùn)動(dòng)學(xué)模型,將相機(jī)坐標(biāo)系下錨鉆孔的三維坐標(biāo)轉(zhuǎn)到機(jī)械臂基坐標(biāo)系下坐標(biāo);最后通過機(jī)械臂運(yùn)動(dòng)學(xué)模型的反解,計(jì)算得到機(jī)械臂的定位角度θ1和θ2。

圖7 二次修正補(bǔ)償流程

3 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證本文提出的二次定位修正方法,搭建了圖8所示的實(shí)驗(yàn)系統(tǒng)。該系統(tǒng)中在錨桿鉆臂的一側(cè)安裝深度相機(jī)。錨桿鉆臂距離錨孔所在平面距離為1 m,與1.2節(jié)中的仿真條件相同。以墻上隨機(jī)分布的十字交叉點(diǎn)作為目標(biāo)錨孔位置,分別標(biāo)記第一次定位后錨桿達(dá)到的位置和二次修正后的位置。用刻度尺分別測(cè)量第一次定位的位置與目標(biāo)點(diǎn)的距離和二次修正后的位置與目標(biāo)點(diǎn)的距離,即為第一次定位誤差和二次修正定位后的誤差。實(shí)驗(yàn)結(jié)果記錄數(shù)據(jù)見表1。

圖8 錨桿機(jī)實(shí)物圖

將表1中的定位誤差數(shù)據(jù)進(jìn)行整理后如圖9所示,第一次定位誤差以“+”表示,二次修正定位后的誤差以“*”表示。從表1可以看出,第一次定位的誤差均大于5 mm。在二次修正后,12組定位角度均小于0.133°的閾值,且二次修正后的定位誤差均小于5 mm,平均為2.1 mm,與第一次定位誤差結(jié)果相比,定位誤差最大減小了83.7%,平均減小了74.7%。

表1 實(shí)驗(yàn)數(shù)據(jù)

4 結(jié)束語

本文通過對(duì)3D結(jié)構(gòu)光深度相機(jī)的鏡頭存在畸變導(dǎo)致測(cè)量誤差的問題進(jìn)行分析,得出了錨鉆孔定位誤差與錨鉆孔所在圖像的位置的關(guān)系。當(dāng)錨鉆孔距離圖像中心點(diǎn)越遠(yuǎn)時(shí),錨鉆孔的定位誤差越大,反之越小。根據(jù)此關(guān)系,用“手眼”標(biāo)定模型以及徑向畸變矯正模型,建立了錨桿鉆孔的二次定位修正模型。經(jīng)過多次實(shí)驗(yàn)驗(yàn)證,該二次定位修正方法只需要進(jìn)行一次修正就能滿足5 mm誤差的精度要求,實(shí)際錨鉆孔的定位誤差平均為2.1 mm。