王福斌，劉 洋，程 月，劉海濤，徐 傲

(1. 華北理工大學(xué) 電氣工程學(xué)院，唐山 063210； 2. 唐山勞動技師學(xué)院 電氣工程系，唐山 063300； 3. 卡爾加里大學(xué) 機(jī)械及制造工程系，卡爾加里 T2N 1N4)

引 言

自動化生產(chǎn)線上的工件姿態(tài)測量及抓取是重要工藝環(huán)節(jié)。在姿態(tài)變換實(shí)驗(yàn)平臺的研究方面，HUANG[1]等人采用歐拉角物體姿態(tài)對機(jī)器人的空間運(yùn)動進(jìn)行描述。JIANG等人[2]研究了海豚尾鰭俯仰-沉浮運(yùn)動的數(shù)學(xué)模型，并建立了描述豚尾各運(yùn)動參量之間關(guān)系的運(yùn)動學(xué)方程。CHEN等人[3]提出了圓結(jié)構(gòu)光視覺姿態(tài)測量模型，基于單目視覺和激光圓結(jié)構(gòu)光實(shí)現(xiàn)了目標(biāo)姿態(tài)測量。YU等人[4]采用中軸線法對火箭空間軸對稱目標(biāo)的俯仰角和偏航角進(jìn)行測量，并進(jìn)行了誤差分析；利用目標(biāo)圖像信息提高測量精度，采用間接方法提取目標(biāo)中軸線，避免了多相機(jī)目標(biāo)匹配問題。LI等人[5]對相機(jī)系統(tǒng)進(jìn)行了標(biāo)定，并用單相機(jī)對六自由度(six-degree of freedom，6-DOF)機(jī)構(gòu)的6個自由度進(jìn)行了基于圖像的位移測量；將6個自由度位移的仿真結(jié)果作為理想數(shù)據(jù)與實(shí)測數(shù)據(jù)進(jìn)行比較獲得測量誤差。

國外學(xué)者在20世紀(jì)80年代前后，提出了姿態(tài)測量中的透視n點(diǎn)定位問題(perspective-n-point,PNP)。該方法為基于單幀圖像的姿態(tài)解算方法：已知被測目標(biāo)物體上n個特征點(diǎn)中任意兩個點(diǎn)間的對應(yīng)坐標(biāo)關(guān)系，根據(jù)攝像機(jī)的成像模型可以確定目標(biāo)物體上這n個特征點(diǎn)在攝像機(jī)坐標(biāo)系中的坐標(biāo)，再利用標(biāo)定后的攝像機(jī)內(nèi)外參量求出特征點(diǎn)在世界坐標(biāo)系中的坐標(biāo)值，最終解算出目標(biāo)物體的姿態(tài)參量。1981年,FISCHLER和BOLLES建立了數(shù)學(xué)模型，提出了一種封閉式解法，為視覺姿態(tài)測量提供了理論基礎(chǔ)[6]。2009年,LEPETIT等人提出了一種對N≥4的PNP問題均適用的解算方法，將所有特征點(diǎn)表示為4個虛擬控制點(diǎn)的PNP問題非迭代算法，將PNP問題簡單地轉(zhuǎn)換為估計解算4個虛擬控制點(diǎn)在攝像機(jī)坐標(biāo)系的坐標(biāo)值問題[7]。STEWéNIUS等人[8]提出了5點(diǎn)相對姿態(tài)的定位算法，且該方法多數(shù)情況下優(yōu)于直接法。

為實(shí)現(xiàn)工件的姿態(tài)變換，并測量不同變換條件下對應(yīng)的工件姿態(tài)參量，設(shè)計了6-DOF姿態(tài)變換實(shí)驗(yàn)平臺，實(shí)驗(yàn)平臺具有兩種基本的實(shí)驗(yàn)功能：工件的各種姿態(tài)變換，由變換平臺3個平移自由度及3個旋轉(zhuǎn)自由度組合實(shí)現(xiàn)；視覺測量系統(tǒng)，由單目相機(jī)配合激光結(jié)構(gòu)光實(shí)現(xiàn)。

構(gòu)建的實(shí)驗(yàn)平臺由x方向平移臺、y方向平移臺及z方向升降臺組成位置調(diào)節(jié)部件；由γ方向水平旋轉(zhuǎn)臺、β方向俯仰轉(zhuǎn)角臺及α方向側(cè)傾轉(zhuǎn)角臺構(gòu)成姿勢調(diào)節(jié)部件，通過滑動副及旋轉(zhuǎn)副的組合運(yùn)動，實(shí)現(xiàn)工件的不同姿態(tài)變換。在分析Denavit-Hartenberg(D-H)模型原理基礎(chǔ)上，構(gòu)建了姿態(tài)變換實(shí)驗(yàn)平臺的運(yùn)動學(xué)數(shù)學(xué)模型，求得了D-H模型參量及姿態(tài)傳遞變換矩陣；同時，依據(jù)小孔成像機(jī)理，構(gòu)建了視覺系統(tǒng)內(nèi)外參量模型，為基于實(shí)驗(yàn)平臺的工件姿態(tài)視覺測量提供了條件[9-10]。通過對環(huán)形激光結(jié)構(gòu)光圖像的分割，獲得環(huán)形光條，并用光條圖像的法向量結(jié)合坐標(biāo)系間的轉(zhuǎn)換，得到工件表面在世界坐標(biāo)系中的法向量，進(jìn)而反演實(shí)現(xiàn)工件姿態(tài)的計算。

1 六自由度姿態(tài)變換實(shí)驗(yàn)平臺原理

六自由度姿態(tài)模擬平臺如圖1所示。實(shí)驗(yàn)平臺由x方向平移臺、y方向平移臺、z方向升降臺組成位置調(diào)節(jié)系統(tǒng)；由γ方向360°旋轉(zhuǎn)臺、β方向轉(zhuǎn)角臺、α方向轉(zhuǎn)角臺構(gòu)成姿勢調(diào)節(jié)系統(tǒng)。

Fig.1 Functional component of 6-DOF attitude simulation experiment platform system

Fig.2 6-DOF attitude simulation experiment platform and its coordinate system

由3個直線運(yùn)動、3個旋轉(zhuǎn)運(yùn)動的組合，具備位置及姿態(tài)調(diào)整功能，末端工作平面可模擬空間限定范圍內(nèi)的任意位置、任意姿態(tài)。姿態(tài)變換平臺與視覺系統(tǒng)相結(jié)合，可實(shí)現(xiàn)不同姿態(tài)下的工件視覺識別及視覺測量等功能。

圖2為姿態(tài)變換平臺6個自由度的功能部件組合及其坐標(biāo)系示意圖。主要技術(shù)參量為：繞x軸的旋轉(zhuǎn)角度α=±15°，分辨率0.1°；繞y軸旋轉(zhuǎn)的角度β=±15°，分辨率0.1°；繞z軸旋轉(zhuǎn)的角度γ=360°，分辨率0.1°。沿x軸方向的直線運(yùn)動范圍為75mm，分辨率0.01mm；沿y軸方向的直線運(yùn)動范圍為75mm，分辨率0.02mm；沿z軸方向的直線運(yùn)動范圍為13mm，分辨率0.01mm。

2 六自由度姿態(tài)變換實(shí)驗(yàn)平臺模型

D-H方法適用于任何由關(guān)節(jié)和連桿組成的機(jī)器人模型構(gòu)建，而機(jī)器人本質(zhì)上大多由一組關(guān)節(jié)和連桿構(gòu)成。當(dāng)所有關(guān)節(jié)變量為已知時，可用正運(yùn)動學(xué)模型確定機(jī)器人末端位姿；相反，若對末端位置及姿態(tài)有特定要求，則可用逆運(yùn)動學(xué)實(shí)現(xiàn)對每一關(guān)節(jié)變量的逆向求解。

2.1 機(jī)器人關(guān)節(jié)變換的D-H建模原理

六自由度模擬平臺可以抽象為連桿-關(guān)節(jié)機(jī)器人結(jié)構(gòu)，從而應(yīng)用D-H方法進(jìn)行模型構(gòu)建。正常情況下，機(jī)器人的每個關(guān)節(jié)有一個自由度，可為滑動的或轉(zhuǎn)動的。圖3為典型的用D-H法表示的機(jī)器人連桿-關(guān)節(jié)模型結(jié)構(gòu)。圖中連續(xù)的3個關(guān)節(jié)由兩個連桿連接,關(guān)節(jié)分別為Jn,Jn+1及Jn+2，連桿分別為n及n+1。3個旋轉(zhuǎn)關(guān)節(jié)的z軸與關(guān)節(jié)旋轉(zhuǎn)方向之間遵循右手螺旋法則，旋轉(zhuǎn)角θ為關(guān)節(jié)變量；若為滑動關(guān)節(jié)，則沿z軸的連桿滑動長度定義為關(guān)節(jié)變量[11-12]。關(guān)節(jié)的x軸一般定義在兩個相鄰關(guān)節(jié)軸線的公垂線上。圖中，關(guān)節(jié)Jn與Jn+1之間的關(guān)節(jié)偏移量為an，關(guān)節(jié)Jn+1與Jn+2之間的關(guān)節(jié)偏移量為an+2；兩相鄰的公垂線之間的距離為d，兩相鄰的關(guān)節(jié)的z軸之間夾角定義為關(guān)節(jié)扭轉(zhuǎn)角，分別為αn和αn+1。

Fig.3 Typical D-H bar-joint model for robot

為實(shí)現(xiàn)關(guān)節(jié)之間的變換，在每一個關(guān)節(jié)上建立一個參考坐標(biāo)系。從關(guān)節(jié)Jn+1到關(guān)節(jié)Jn+2的變換過程為[13-14]：(1)關(guān)節(jié)Jn+2繞關(guān)節(jié)Jn+1旋轉(zhuǎn)θn+1角度，有旋轉(zhuǎn)算子rot(zn,θn+1)，使得xn軸與xn+1軸平行,此時xn軸與xn+1軸在同一平面上，將xn+1軸平移dn+1，有平移算子trans(0,0,dn+1)，即可使二者重疊;(2)將關(guān)節(jié)Jn+1沿xn軸平移an+1，記為trans(an+1,0,0)，使得xn軸與xn+1軸的坐標(biāo)原點(diǎn)重合;(3)通過旋轉(zhuǎn)αn+1角度，將zn軸繞xn+1軸轉(zhuǎn)到與zn+1軸重疊，記為rot(x,an+1),此時，關(guān)節(jié)Jn+1的坐標(biāo)系與關(guān)節(jié)Jn+2的坐標(biāo)系重合，實(shí)現(xiàn)了坐標(biāo)系之間的轉(zhuǎn)換。

從關(guān)節(jié)Jn+1到關(guān)節(jié)Jn+2的變換矩陣記作nTn + 1，對應(yīng)的矩陣An+1由上述變換過程各自對應(yīng)的矩陣右乘得到，有：

(1)

對于具有6個關(guān)節(jié)的機(jī)器人，從機(jī)器人的基座R開始，到機(jī)器人手爪末端的變換過程可表示為：

RT6=RT11T22T33T44T55T6=A1A2A3A4A5A6

(2)

可知，具有6個自由度的機(jī)器人，每個自由度對應(yīng)一個A矩陣。

2.2 六自由度模擬平臺D-H運(yùn)動學(xué)模型

六自由度姿態(tài)變換平臺坐標(biāo)系如圖4所示。坐標(biāo)系由3個滑動副、3個轉(zhuǎn)動副組成。每兩個運(yùn)動副公垂線之間的距離分別為d1,d2,d3,d4,d5,d6。按右手螺旋法則，確定關(guān)節(jié)變量旋轉(zhuǎn)角θ1～θ6及兩相鄰關(guān)節(jié)的z軸之間關(guān)節(jié)扭轉(zhuǎn)角α0～α5。

Fig.4 D-H coordinate system of attitude simulation experiment platform

由坐標(biāo)系，得到姿態(tài)變換平臺從平臺的基座到末端之間變換的D-H參量表，如表1所示。

Table 1 D-H parameter table of attitude simulation experiment platform

由D-H參量表，求得各個關(guān)節(jié)的變換矩陣分別為：

(3)

由(3)式可得基座R到末端H總的變換矩陣為：

(4)

3 六自由度姿態(tài)變換平臺視覺系統(tǒng)模型

3.1 小孔成像模型

姿態(tài)變換平臺的視覺系統(tǒng)采樣單目相機(jī)結(jié)合激光結(jié)構(gòu)光結(jié)構(gòu)，可實(shí)現(xiàn)對平臺上的工件進(jìn)行姿態(tài)識別及尺寸測量。相機(jī)的單目小孔成像模型如圖5所示。圖中，xwywzw為世界坐標(biāo)系，相機(jī)光軸中心點(diǎn)O與xc,yc,zc軸構(gòu)成相機(jī)直角坐標(biāo)系，相機(jī)等效成像平面Γ的坐標(biāo)原點(diǎn)O0是以像素為單位的圖像坐標(biāo)系的原點(diǎn)，(u,v)為圖像像素坐標(biāo)系中任一點(diǎn)的坐標(biāo)，O1是以mm為單位的圖像坐標(biāo)系原點(diǎn)，其像素坐標(biāo)記為(u0,v0)。OO1即為相機(jī)焦距f。設(shè)景物點(diǎn)p1在相機(jī)坐標(biāo)系下的坐標(biāo)為(xc，yc，zc)，p1點(diǎn)在成像平面上的成像點(diǎn)p2的坐標(biāo)為(X，Y，Z)[15-16]。

Fig.5 Pinhole imaging coordinate system of experimental platform

采用齊次坐標(biāo)與矩陣形式，成像平面上的任一點(diǎn)在u-O0-v及X-O1-Y坐標(biāo)系間的坐標(biāo)關(guān)系可表示為：

(5)

景物點(diǎn)p1與其成像點(diǎn)p2間的坐標(biāo)關(guān)系為：

(6)

由(5)式將成像平面上的成像點(diǎn)(X，Y)轉(zhuǎn)換為圖像點(diǎn)(u,v)，再代入(6)式，得到相機(jī)內(nèi)參量模型：

(7)

(8)

3.2 環(huán)形激光結(jié)構(gòu)光系統(tǒng)

為測量工件在變換平臺上的姿態(tài)，采用圓結(jié)構(gòu)光激光器、相機(jī)構(gòu)成結(jié)構(gòu)光系統(tǒng)，如圖6所示。圖6a為激光結(jié)構(gòu)光視覺測量原理及相機(jī)與激光器之間的幾何安裝結(jié)構(gòu)；圖6b為相機(jī)與激光投射器的整體幾何模型。為保證激光投射器軸線與攝像機(jī)的光軸平行，采用特制的安裝底板，將相機(jī)與激光器分別固定在底板的兩側(cè)，保證二者之間的幾何尺寸。

Fig.6 a—geometric installation structure b—geometric installation model

圖6中，相機(jī)光軸與激光圓環(huán)軸心間距為L，圓環(huán)激光出射半角為θ0，相機(jī)物距為H，激光投射器在工件表面投影為圓環(huán)形結(jié)構(gòu)光條，半徑為r，并有:

r=Htanθ0

(9)

在構(gòu)建相機(jī)與激光投射器相對位置時，要保證激光圓環(huán)在視場范圍內(nèi)，要求相機(jī)光軸與激光圓環(huán)軸心間距L滿足:L

(10)

式中，S為鏡頭靶面尺寸，f為鏡頭焦距。

空間工件的姿態(tài)由橫滾角、俯仰角和偏轉(zhuǎn)角3個自由度確定。設(shè)nw=(nw,x,nw,y,nw,z)為空間工件表面的法向量，根據(jù)幾何三角關(guān)系可推算出工件的姿態(tài)參量橫滾角θ、俯仰角φ和偏轉(zhuǎn)角ξ：

(11)

激光圓環(huán)光條中心坐標(biāo)(x0′,y0′,z0′)及光條法向量n′=(nx′,ny′,nz′)分別為：

(12)

式中，λ1,λ2,λ3為矩陣的特征值，r為圓結(jié)構(gòu)光投射在工件表面的半徑[19-20]。

4 工件端面姿態(tài)測量

實(shí)驗(yàn)時，將工件置于姿態(tài)變換實(shí)驗(yàn)平臺上，調(diào)節(jié)姿態(tài)變換平臺的3個姿態(tài)角參量進(jìn)行測量，如圖7a所示。圖7b為采集的3種不同姿態(tài)的工件圖像。圖7c為利用閾值分割及形態(tài)學(xué)膨脹處理方法將激光環(huán)形光條區(qū)域從工件圖像中分割出來。對結(jié)構(gòu)光光條區(qū)域應(yīng)用Hessian矩陣提取結(jié)構(gòu)光光條中心的亞像素坐標(biāo)，再利用最小二乘橢圓擬合法將轉(zhuǎn)換到成像平面坐標(biāo)系中的亞像素中心坐標(biāo)擬合成橢圓方程，見圖7d。最后由橢圓擬合結(jié)果測算工件姿態(tài)。

Fig.7 Image acquisition and processing for circular laser stripe

被測工件的姿態(tài)參量(θ,φ,ξ)實(shí)驗(yàn)測量值如表2所示。經(jīng)實(shí)驗(yàn)可得姿態(tài)角的測量值與給定值間的平均誤差為：姿態(tài)參量的橫滾角θ平均誤差0.373°，俯仰角φ平均誤差0.253°，偏轉(zhuǎn)角ξ平均誤差0.673°。

Table 2 Measurement results for workpiece attitude

圖8為工件在實(shí)驗(yàn)臺上給定的姿態(tài)角及實(shí)際測量的姿態(tài)角及對應(yīng)點(diǎn)的測量誤差。從實(shí)驗(yàn)結(jié)果看，被測工件的姿態(tài)測量值與真實(shí)值基本相同，但存在一定誤差，影響姿態(tài)測量結(jié)果的誤差來源于攝像機(jī)與圓結(jié)構(gòu)光投射器之間的支架誤差、結(jié)構(gòu)光光條中心亞像素坐標(biāo)的提取誤差等。

Fig.8 The given attitude angle and the actual measurement angle

a—(θ,φ,ξ)=(2.0,5.0,20.0) b—(θ,φ,ξ)=(3.0,4.0,35.0) c—(θ,φ,ξ)=(5.0,3.0,-60.0)

圖9為工件位移測量原理。圖中O為激光結(jié)構(gòu)光幾何中心，l1,l2分別為點(diǎn)O到參考點(diǎn)1所在邊的相對位移。若參考點(diǎn)1不在y軸右側(cè)，則測量點(diǎn)O到參考點(diǎn)2所在邊的相對位移。

Fig.9 Relative displacement measurement for workpiece

5 結(jié) 論

在實(shí)驗(yàn)室環(huán)境下搭建了六自由度姿態(tài)變換實(shí)驗(yàn)平臺，為了建立姿態(tài)變換實(shí)驗(yàn)平臺的機(jī)構(gòu)模型及視覺系統(tǒng)模型，實(shí)現(xiàn)工件變姿態(tài)的視覺識別與測量，完成了如下主要工作：分析了機(jī)器人關(guān)節(jié)變換D-H建模原理基礎(chǔ)上，構(gòu)建了六自由度模擬平臺的D-H運(yùn)動學(xué)模型，導(dǎo)出了六自由度姿態(tài)變換平臺的D-H參量表，并求出了實(shí)驗(yàn)平臺基座到平臺末端總的變換矩陣；應(yīng)用小孔成像原理，建立了六自由度姿態(tài)變換平臺視覺系統(tǒng)模型，為基于變換實(shí)驗(yàn)平臺的工件姿態(tài)測量奠定了基礎(chǔ)。

對激光環(huán)形光條圖像進(jìn)行在成像平面的數(shù)學(xué)表達(dá)式推導(dǎo)，得到了工件表面在攝像機(jī)坐標(biāo)系中的法向量，通過坐標(biāo)系之間的轉(zhuǎn)換得到工件表面在世界坐標(biāo)系中的法向量，進(jìn)而推算出工件的姿態(tài)參量橫滾角θ、俯仰角φ和及偏轉(zhuǎn)角ζ。實(shí)驗(yàn)結(jié)果表明，3個姿態(tài)角的測量值平均誤差均小于1°，被測工件的姿態(tài)測量值與真實(shí)值基本相同。