六自由度姿態變換視覺實驗平臺及應用

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

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

引 言

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

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

為實現工件的姿態變換，并測量不同變換條件下對應的工件姿態參量，設計了6-DOF姿態變換實驗平臺，實驗平臺具有兩種基本的實驗功能：工件的各種姿態變換，由變換平臺3個平移自由度及3個旋轉自由度組合實現；視覺測量系統，由單目相機配合激光結構光實現。

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

1 六自由度姿態變換實驗平臺原理

六自由度姿態模擬平臺如圖1所示。實驗平臺由x方向平移臺、y方向平移臺、z方向升降臺組成位置調節系統；由γ方向360°旋轉臺、β方向轉角臺、α方向轉角臺構成姿勢調節系統。

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個直線運動、3個旋轉運動的組合，具備位置及姿態調整功能，末端工作平面可模擬空間限定范圍內的任意位置、任意姿態。姿態變換平臺與視覺系統相結合，可實現不同姿態下的工件視覺識別及視覺測量等功能。

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

2 六自由度姿態變換實驗平臺模型

D-H方法適用于任何由關節和連桿組成的機器人模型構建，而機器人本質上大多由一組關節和連桿構成。當所有關節變量為已知時，可用正運動學模型確定機器人末端位姿；相反，若對末端位置及姿態有特定要求，則可用逆運動學實現對每一關節變量的逆向求解。

2.1 機器人關節變換的D-H建模原理

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

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

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

從關節Jn+1到關節Jn+2的變換矩陣記作nTn + 1，對應的矩陣An+1由上述變換過程各自對應的矩陣右乘得到，有：

(1)

對于具有6個關節的機器人，從機器人的基座R開始，到機器人手爪末端的變換過程可表示為：

RT6=RT11T22T33T44T55T6=A1A2A3A4A5A6

(2)

可知，具有6個自由度的機器人，每個自由度對應一個A矩陣。

2.2 六自由度模擬平臺D-H運動學模型

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

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

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

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

由D-H參量表，求得各個關節的變換矩陣分別為：

(3)

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

(4)

3 六自由度姿態變換平臺視覺系統模型

3.1 小孔成像模型

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

Fig.5 Pinhole imaging coordinate system of experimental platform

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

(5)

景物點p1與其成像點p2間的坐標關系為：

(6)

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

(7)

(8)

3.2 環形激光結構光系統

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

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

圖6中，相機光軸與激光圓環軸心間距為L，圓環激光出射半角為θ0，相機物距為H，激光投射器在工件表面投影為圓環形結構光條，半徑為r，并有:

r=Htanθ0

(9)

在構建相機與激光投射器相對位置時，要保證激光圓環在視場范圍內，要求相機光軸與激光圓環軸心間距L滿足:L

(10)

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

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

(11)

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

(12)

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

4 工件端面姿態測量

實驗時，將工件置于姿態變換實驗平臺上，調節姿態變換平臺的3個姿態角參量進行測量，如圖7a所示。圖7b為采集的3種不同姿態的工件圖像。圖7c為利用閾值分割及形態學膨脹處理方法將激光環形光條區域從工件圖像中分割出來。對結構光光條區域應用Hessian矩陣提取結構光光條中心的亞像素坐標，再利用最小二乘橢圓擬合法將轉換到成像平面坐標系中的亞像素中心坐標擬合成橢圓方程，見圖7d。最后由橢圓擬合結果測算工件姿態。

Fig.7 Image acquisition and processing for circular laser stripe

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

Table 2 Measurement results for workpiece attitude

圖8為工件在實驗臺上給定的姿態角及實際測量的姿態角及對應點的測量誤差。從實驗結果看，被測工件的姿態測量值與真實值基本相同，但存在一定誤差，影響姿態測量結果的誤差來源于攝像機與圓結構光投射器之間的支架誤差、結構光光條中心亞像素坐標的提取誤差等。

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為激光結構光幾何中心，l1,l2分別為點O到參考點1所在邊的相對位移。若參考點1不在y軸右側，則測量點O到參考點2所在邊的相對位移。

Fig.9 Relative displacement measurement for workpiece

5 結 論

在實驗室環境下搭建了六自由度姿態變換實驗平臺，為了建立姿態變換實驗平臺的機構模型及視覺系統模型，實現工件變姿態的視覺識別與測量，完成了如下主要工作：分析了機器人關節變換D-H建模原理基礎上，構建了六自由度模擬平臺的D-H運動學模型，導出了六自由度姿態變換平臺的D-H參量表，并求出了實驗平臺基座到平臺末端總的變換矩陣；應用小孔成像原理，建立了六自由度姿態變換平臺視覺系統模型，為基于變換實驗平臺的工件姿態測量奠定了基礎。

對激光環形光條圖像進行在成像平面的數學表達式推導，得到了工件表面在攝像機坐標系中的法向量，通過坐標系之間的轉換得到工件表面在世界坐標系中的法向量，進而推算出工件的姿態參量橫滾角θ、俯仰角φ和及偏轉角ζ。實驗結果表明，3個姿態角的測量值平均誤差均小于1°，被測工件的姿態測量值與真實值基本相同。