六自由度姿態(tài)變換視覺(jué)實(shí)驗(yàn)平臺(tái)及應(yīng)用

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

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

引 言

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

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

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

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

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

六自由度姿態(tài)模擬平臺(tái)如圖1所示。實(shí)驗(yàn)平臺(tái)由x方向平移臺(tái)、y方向平移臺(tái)、z方向升降臺(tái)組成位置調(diào)節(jié)系統(tǒng)；由γ方向360°旋轉(zhuǎn)臺(tái)、β方向轉(zhuǎn)角臺(tái)、α方向轉(zhuǎn)角臺(tái)構(gòu)成姿勢(shì)調(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個(gè)直線運(yùn)動(dòng)、3個(gè)旋轉(zhuǎn)運(yùn)動(dòng)的組合，具備位置及姿態(tài)調(diào)整功能，末端工作平面可模擬空間限定范圍內(nèi)的任意位置、任意姿態(tài)。姿態(tài)變換平臺(tái)與視覺(jué)系統(tǒng)相結(jié)合，可實(shí)現(xiàn)不同姿態(tài)下的工件視覺(jué)識(shí)別及視覺(jué)測(cè)量等功能。

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

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

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

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

六自由度模擬平臺(tái)可以抽象為連桿-關(guān)節(jié)機(jī)器人結(jié)構(gòu)，從而應(yīng)用D-H方法進(jìn)行模型構(gòu)建。正常情況下，機(jī)器人的每個(gè)關(guān)節(jié)有一個(gè)自由度，可為滑動(dòng)的或轉(zhuǎn)動(dòng)的。圖3為典型的用D-H法表示的機(jī)器人連桿-關(guān)節(jié)模型結(jié)構(gòu)。圖中連續(xù)的3個(gè)關(guān)節(jié)由兩個(gè)連桿連接,關(guān)節(jié)分別為Jn,Jn+1及Jn+2，連桿分別為n及n+1。3個(gè)旋轉(zhuǎn)關(guān)節(jié)的z軸與關(guān)節(jié)旋轉(zhuǎn)方向之間遵循右手螺旋法則，旋轉(zhuǎn)角θ為關(guān)節(jié)變量；若為滑動(dòng)關(guān)節(jié)，則沿z軸的連桿滑動(dòng)長(zhǎng)度定義為關(guān)節(jié)變量[11-12]。關(guān)節(jié)的x軸一般定義在兩個(gè)相鄰關(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é)之間的變換，在每一個(gè)關(guān)節(jié)上建立一個(gè)參考坐標(biāo)系。從關(guān)節(jié)Jn+1到關(guān)節(jié)Jn+2的變換過(guò)程為[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軸平行,此時(shí)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)通過(guò)旋轉(zhuǎn)αn+1角度，將zn軸繞xn+1軸轉(zhuǎn)到與zn+1軸重疊，記為rot(x,an+1),此時(shí)，關(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，對(duì)應(yīng)的矩陣An+1由上述變換過(guò)程各自對(duì)應(yīng)的矩陣右乘得到，有：

(1)

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

RT6=RT11T22T33T44T55T6=A1A2A3A4A5A6

(2)

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

2.2 六自由度模擬平臺(tái)D-H運(yùn)動(dòng)學(xué)模型

六自由度姿態(tài)變換平臺(tái)坐標(biāo)系如圖4所示。坐標(biāo)系由3個(gè)滑動(dòng)副、3個(gè)轉(zhuǎn)動(dòng)副組成。每?jī)蓚€(gè)運(yùn)動(dòng)副公垂線之間的距離分別為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)變換平臺(tái)從平臺(tái)的基座到末端之間變換的D-H參量表，如表1所示。

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

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

(3)

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

(4)

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

3.1 小孔成像模型

姿態(tài)變換平臺(tái)的視覺(jué)系統(tǒng)采樣單目相機(jī)結(jié)合激光結(jié)構(gòu)光結(jié)構(gòu)，可實(shí)現(xiàn)對(duì)平臺(tái)上的工件進(jìn)行姿態(tài)識(shí)別及尺寸測(cè)量。相機(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)

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

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

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

r=Htanθ0

(9)

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

(10)

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

空間工件的姿態(tài)由橫滾角、俯仰角和偏轉(zhuǎn)角3個(gè)自由度確定。設(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)測(cè)量

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

Fig.7 Image acquisition and processing for circular laser stripe

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

Table 2 Measurement results for workpiece attitude

圖8為工件在實(shí)驗(yàn)臺(tái)上給定的姿態(tài)角及實(shí)際測(cè)量的姿態(tài)角及對(duì)應(yīng)點(diǎn)的測(cè)量誤差。從實(shí)驗(yàn)結(jié)果看，被測(cè)工件的姿態(tài)測(cè)量值與真實(shí)值基本相同，但存在一定誤差，影響姿態(tài)測(cè)量結(jié)果的誤差來(lái)源于攝像機(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為工件位移測(cè)量原理。圖中O為激光結(jié)構(gòu)光幾何中心，l1,l2分別為點(diǎn)O到參考點(diǎn)1所在邊的相對(duì)位移。若參考點(diǎn)1不在y軸右側(cè)，則測(cè)量點(diǎn)O到參考點(diǎn)2所在邊的相對(duì)位移。

Fig.9 Relative displacement measurement for workpiece

5 結(jié) 論

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

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