葉文達(dá)，侯宇瀚，陳洪佳，李藝帆

(廣東工業(yè)大學(xué) 機(jī)電工程學(xué)院，廣東 廣州 510006)

0 引言

大部分攀爬機(jī)器人是在本體兩端安裝夾持器[1-3]，一端吸附或者抓取環(huán)境中的桿狀物，另一端尋找下一個(gè)抓夾點(diǎn)[4],兩個(gè)夾持器交替輪流抓夾來(lái)實(shí)現(xiàn)機(jī)器人攀爬運(yùn)動(dòng)。機(jī)器人基座夾持器需要緊緊地抓住目標(biāo)桿件以防止本體的掉落，這就要求機(jī)器人末端能正確估計(jì)目標(biāo)桿件的位姿?；诜叫螤钗矬w的截面為矩形的思想,我們提出一種方形狀物體位姿估計(jì)方法，該方法的原理是將兩束線結(jié)構(gòu)光投射到方形狀物體表面上，產(chǎn)生兩個(gè)矩形條紋用以擬合矩形，再將矩形的信息映射到三維空間中用以估計(jì)目標(biāo)方形狀物體的軸線向量、參考點(diǎn)及邊長(zhǎng)，從而實(shí)現(xiàn)非接觸自動(dòng)化操作。

1 硬件系統(tǒng)搭建

我們提出了一種新穎、簡(jiǎn)單、低成本的傳感裝置來(lái)估計(jì)方形狀物體的位姿，其結(jié)構(gòu)如圖1所示，主要組成部分有工業(yè)相機(jī)、線結(jié)構(gòu)光發(fā)生器、濾光片以及安裝夾具。當(dāng)系統(tǒng)運(yùn)行時(shí)，線結(jié)構(gòu)光發(fā)生器投射一字型線結(jié)構(gòu)光到方形狀物體表面并反射，在相機(jī)的CMOS芯片中形成了反映方形狀物體特征的條紋。此外濾光片的存在，可以濾除部分環(huán)境干擾的自然光以及工件表面反射出來(lái)的光，從而只有特定波長(zhǎng)的光能夠在相機(jī)中成像。由于在檢測(cè)過(guò)程中需要同時(shí)利用到兩個(gè)線結(jié)構(gòu)光發(fā)生器，所以需要根據(jù)實(shí)際情況來(lái)確定相機(jī)與線結(jié)構(gòu)光發(fā)生器的安裝位置。兩個(gè)線結(jié)構(gòu)光發(fā)生器均與相機(jī)呈一定夾角放置,如此設(shè)置可以在方形狀物體上形成明顯的光條紋，且可以較好地吸收來(lái)自被測(cè)對(duì)象的反射光，適用于表面光滑、反光能力強(qiáng)的工件。

圖1 硬件系統(tǒng)搭建

2 系統(tǒng)標(biāo)定

系統(tǒng)標(biāo)定是根據(jù)系統(tǒng)的模型標(biāo)定出相關(guān)的系統(tǒng)參數(shù)，這是將相機(jī)中的像素信息與三維坐標(biāo)系下的方形狀物體位姿信息進(jìn)行轉(zhuǎn)化的重要步驟之一，標(biāo)定結(jié)果的準(zhǔn)確度直接影響著估算結(jié)果的準(zhǔn)確性。

2.1 相機(jī)標(biāo)定

采用張正友相機(jī)標(biāo)定法[5]獲取相機(jī)的內(nèi)參和外參。相機(jī)的成像模型是由相機(jī)坐標(biāo)系、圖像物理坐標(biāo)系、像素坐標(biāo)系、世界坐標(biāo)系之間相互轉(zhuǎn)換建立的，相機(jī)的成像模型[6]如圖2所示。

圖2 相機(jī)成像模型

假設(shè)空間點(diǎn)P為在世界坐標(biāo)系OW-XWYWZW中的一點(diǎn),點(diǎn)P對(duì)應(yīng)在相機(jī)坐標(biāo)系OC-XCYCZC中的坐標(biāo)為(xc,yc,zc),投影在圖像物理坐標(biāo)系O1-XY中的點(diǎn)P1的坐標(biāo)為(x,y),轉(zhuǎn)換在像素坐標(biāo)系o-uv中的坐標(biāo)為(u,v)。相機(jī)的成像模型如下：

(1)

其中：fx、fy分別為x軸和y軸上的歸一化焦距；u0、v0為相機(jī)圖像的主點(diǎn)坐標(biāo)；R、T分別為相機(jī)外參中的旋轉(zhuǎn)矩陣和平移矩陣；MC為相機(jī)內(nèi)參；MW為相機(jī)外參。

2.2 線結(jié)構(gòu)光標(biāo)定

線結(jié)構(gòu)光發(fā)生器投射出來(lái)的線結(jié)構(gòu)光可以理解為在三維坐標(biāo)系下的一個(gè)平面[7]，因此線結(jié)構(gòu)光標(biāo)定可以等效為擬合在相機(jī)坐標(biāo)下的一個(gè)平面，當(dāng)測(cè)量的參考坐標(biāo)系為相機(jī)坐標(biāo)系時(shí)，線結(jié)構(gòu)光平面的方程為:

ZC=AlXC+BlYC+Cl.

(2)

其中：Al、Bl、Cl均為光平面參數(shù)。假設(shè)Mi為第i張標(biāo)定圖像的外參，如下式所示:

(3)

其中：n=[nx,ny,nz],o=[ox,oy,oz],a=[ax,ay,az]分別為世界坐標(biāo)系OW-XWYWZW的X軸、Y軸、Z軸在圖像物理坐標(biāo)系O1-XY中的方向向量；p=[px,py,pz]為世界坐標(biāo)系OW-XWYWZW的原點(diǎn)在圖像物理坐標(biāo)系O1-XY中的坐標(biāo)。由此可知標(biāo)定板平面在相機(jī)坐標(biāo)系下的平面方程為：

ax(x-px)+ay(y-py)+az(z-pz)=0.

(4)

通過(guò)聯(lián)立式(1)和式(2),可以得到線結(jié)構(gòu)光平面上的特征點(diǎn)在相機(jī)坐標(biāo)系下的三維坐標(biāo)，通過(guò)對(duì)這些點(diǎn)進(jìn)行平面擬合，即可得到相機(jī)坐標(biāo)系下的光平面方程，從而求得Al、Bl、Cl。線結(jié)構(gòu)光平面擬合結(jié)果如圖3所示。

圖3 線結(jié)構(gòu)光平面擬合結(jié)果

2.3 手眼標(biāo)定

我們采用Eye-in-Hand的安裝方式，手眼標(biāo)定目的是求得相機(jī)坐標(biāo)系與機(jī)械手坐標(biāo)系的轉(zhuǎn)換關(guān)系。標(biāo)定過(guò)程中的變量有兩個(gè)，一個(gè)是機(jī)器人末端相對(duì)于機(jī)器人基坐標(biāo)系的位姿關(guān)系，要求能夠記錄不同拍攝角度下機(jī)器人當(dāng)前的位姿，這個(gè)變量會(huì)顯示在機(jī)器人的示教面板上，是已知參數(shù)；另一個(gè)是標(biāo)定板坐標(biāo)系相對(duì)于相機(jī)坐標(biāo)系的位姿矩陣，這個(gè)變量可以通過(guò)相機(jī)的內(nèi)外參數(shù)計(jì)算得到。對(duì)于標(biāo)定過(guò)程中任意兩個(gè)拍攝角度，可以列出如下關(guān)系式：

(5)

式(5)可以改寫(xiě)成下面的形式：

(6)

上式中，X即為需要求解的手眼關(guān)系矩陣。

3 位姿估計(jì)算法實(shí)現(xiàn)

3.1 圖像預(yù)處理

原始圖像如圖4所示，傳感系統(tǒng)所采集的圖像并不能直接應(yīng)用于方形狀物體位姿的信息提取, 必須先通過(guò)特定的圖像預(yù)處理方法提取方形狀物體表面的特征點(diǎn)，再進(jìn)行計(jì)算以求解位姿。

圖4 原始圖像

為了盡可能地排除環(huán)境光的干擾，需要對(duì)原始圖像進(jìn)行處理。首先將相機(jī)設(shè)置為完全曝光，再經(jīng)過(guò)中值濾波去掉圖像中的部分噪聲，最后通過(guò)閾值分割后得到只保留激光線條紋的二值化圖像，處理結(jié)果如圖5所示。

圖5 圖像預(yù)處理

3.2 中心線提取及聚類(lèi)

如圖6所示, 我們利用Steger算法[8]對(duì)激光線條紋進(jìn)行中心線提取，并過(guò)濾掉部分噪聲。為了區(qū)分兩個(gè)線結(jié)構(gòu)光發(fā)生器對(duì)應(yīng)的激光線條紋，在進(jìn)行后續(xù)算法前，需要對(duì)所提取的條紋進(jìn)行聚類(lèi)，對(duì)應(yīng)兩個(gè)線結(jié)構(gòu)光發(fā)生器。本文采用DBSCAN算法[9]對(duì)圖像中的激光線條紋進(jìn)行聚類(lèi)，將圖像中的激光線條紋的像素點(diǎn)分成兩類(lèi)。

3.3 特征點(diǎn)提取

因?yàn)閮杉す饩€條紋的特征點(diǎn)提取過(guò)程相同，故只需討論其中一激光線條紋的特征點(diǎn)提取情況。為了獲取條紋上的特征點(diǎn)從而進(jìn)行矩形的擬合，需要提取激光線條紋端點(diǎn)的像素坐標(biāo)以及兩直線交叉點(diǎn)的像素坐標(biāo)。

在圖像像素坐標(biāo)系下，其端點(diǎn)的像素坐標(biāo)的u值分別對(duì)應(yīng)于圖像中像素坐標(biāo)的最大值umax和最小值umin，因此對(duì)像素點(diǎn)進(jìn)行遍歷找到最大值和最小值，即可獲取端點(diǎn)的坐標(biāo)值。根據(jù)下式可得出端點(diǎn)對(duì)應(yīng)的像素坐標(biāo)ue1和ue2的值：

(7)

同時(shí)根據(jù)u值找到對(duì)應(yīng)的v值,即得到兩端點(diǎn)的像素坐標(biāo)(ue1,ve1)和(ue2,ve2)。

對(duì)于交叉點(diǎn)的像素坐標(biāo)，本文采用霍夫變換算法[10]尋找直線，先是檢測(cè)到兩直線并求得直線的方程l1和l2，再聯(lián)立兩方程求解交叉點(diǎn)坐標(biāo)(ui,vi)。

根據(jù)以上步驟，可獲取端點(diǎn)和直線交叉點(diǎn)的像素坐標(biāo)，即可得到矩形擬合所需特征點(diǎn)的像素坐標(biāo)，如圖7所示。

3.4 矩形擬合及點(diǎn)映射

將已經(jīng)計(jì)算出來(lái)的端點(diǎn)和交叉點(diǎn)的像素坐標(biāo)通過(guò)OpenCV的矩形擬合算法對(duì)特征點(diǎn)計(jì)算最小外包旋轉(zhuǎn)矩形,如圖8所示。

圖6 提取中心線 圖7 特征點(diǎn)提取 圖8 矩形擬合

設(shè)p1(x1,y1)、p2(x2,y2)、p3(x3,y3)、p4(x4,y4)為求得擬合矩形的四個(gè)頂點(diǎn)坐標(biāo)，矩形中心點(diǎn)的像素坐標(biāo)為[11]：

(8)

根據(jù)下式將像素點(diǎn)(uc,vc)映射到空間三維點(diǎn)中，得到空間矩形的三維中心點(diǎn)(xw,yw,zw)[12]：

(9)

3.5 位姿估計(jì)

由于可以同時(shí)獲取兩擬合矩形的像素中心點(diǎn)坐標(biāo)，我們可以分別得到兩組空間矩形的三維中心點(diǎn)坐標(biāo)：

(10)

因此根據(jù)兩中點(diǎn)的坐標(biāo)，我們可以獲得方形狀物體的軸線，軸線的方程可以表示為：

(11)

(12)

同理，通過(guò)把交叉點(diǎn)和端點(diǎn)映射到空間三維點(diǎn)，我們可以求得方形狀物體的邊長(zhǎng)，矩形相鄰的兩邊長(zhǎng)分別為：

(13)

(14)

若方形狀物體截面為一般矩形，則d1≠d2,若為正方形，則d1≈d2。對(duì)于方形狀物體的軸線方向的測(cè)量，如果捕獲了不止一幀圖像，則可以在花費(fèi)更多計(jì)算時(shí)間的情況下獲取更精準(zhǔn)的結(jié)果，對(duì)多幀圖像分別提取擬合的矩形中心點(diǎn)坐標(biāo)和特征點(diǎn)坐標(biāo)，并對(duì)這些坐標(biāo)進(jìn)行最小二乘法擬合[13]，得到更加準(zhǔn)確的軸線向量和邊長(zhǎng)。

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

為了驗(yàn)證所提出的方案和分析結(jié)果，本文進(jìn)行實(shí)驗(yàn)來(lái)驗(yàn)證傳感方法的有效性，實(shí)驗(yàn)平臺(tái)如圖9所示。

圖9 實(shí)驗(yàn)平臺(tái)

兩端帶有尖錐的方形桿相對(duì)于機(jī)器人底座固定，規(guī)格為5 cm×5 cm×50 cm。傳感系統(tǒng)通過(guò)固定夾具安裝在UR5機(jī)器人末端，設(shè)計(jì)一個(gè)機(jī)器人尖點(diǎn)探頭將機(jī)器人的工具中心點(diǎn)轉(zhuǎn)移到尖點(diǎn)處。實(shí)驗(yàn)中被檢測(cè)方形桿件兩端安裝有尖點(diǎn)，通過(guò)尖點(diǎn)探頭與桿件尖點(diǎn)觸碰的方式獲取桿件位姿的真實(shí)值，然后采用下式計(jì)算方形桿件軸線方向向量：

(15)

其中:Qt和Qb分別為從UR5示教器中讀取的方形桿件兩端尖錐的中心點(diǎn)相對(duì)于基坐標(biāo)系的坐標(biāo)，即Qt=(xt,yt,zt),Qb=(xb,yb,zb)。

為實(shí)現(xiàn)機(jī)器人對(duì)桿件的抓夾，通過(guò)手眼關(guān)系轉(zhuǎn)換得到被測(cè)方形桿件相對(duì)于機(jī)器人基坐標(biāo)系的位姿。在實(shí)驗(yàn)中，機(jī)械手?jǐn)y帶傳感系統(tǒng)進(jìn)行移動(dòng)，使得觀測(cè)點(diǎn)每次都發(fā)生變化。為獲得對(duì)單幀圖片檢測(cè)結(jié)果，我們對(duì)每個(gè)觀測(cè)點(diǎn)拍攝一張圖像，共進(jìn)行10次；為檢驗(yàn)算法的迭代優(yōu)化能力，我們逐輪組織檢測(cè)實(shí)驗(yàn)，每輪10個(gè)觀察點(diǎn)，總共進(jìn)行10輪實(shí)驗(yàn)。利用真值與被測(cè)軸線向量的角度誤差以及真值與被測(cè)的邊長(zhǎng)誤差來(lái)驗(yàn)證檢測(cè)結(jié)果的準(zhǔn)確性。單幀檢測(cè)結(jié)果如圖10所示，迭代檢測(cè)結(jié)果如圖11所示。

圖10 單幀檢測(cè)結(jié)果

圖11 迭代檢測(cè)結(jié)果

從圖10中可以看到，在僅僅提供一幀圖片時(shí)，本文提出的檢測(cè)系統(tǒng)檢測(cè)軸線的向量角度誤差控制在2°以內(nèi)，方形桿長(zhǎng)的誤差小于2 mm。如圖11所示，對(duì)多幀圖片的檢測(cè)，軸線向量角度的最大誤差小于1.63°，可以返回一個(gè)平均角度誤差為1.58°的軸線向量檢測(cè)值；邊長(zhǎng)的最大誤差小于1.67 mm，可以返回一個(gè)平均誤差為1.62 mm的邊長(zhǎng)檢測(cè)值，因此我們所提出的傳感系統(tǒng)不僅能夠通過(guò)一次檢測(cè)返回高精度的結(jié)果，而且能夠?qū)Χ啻螜z測(cè)的結(jié)果進(jìn)行優(yōu)化。

5 總結(jié)

桿件的位姿估計(jì)對(duì)于攀爬機(jī)器人尤為重要，可以賦予機(jī)器人感知外界的能力，指導(dǎo)機(jī)器人進(jìn)行抓夾或者攀爬。本文提出一種基于線結(jié)構(gòu)光視覺(jué)的方形狀物體位姿估計(jì)方法，首先通過(guò)對(duì)激光線條紋圖像進(jìn)行處理提取特征點(diǎn)，再通過(guò)矩形擬合的方法得出中心點(diǎn)坐標(biāo)，繼而映射到空間三維中，獲取目標(biāo)桿件的軸線向量、軸上的參考點(diǎn)以及邊長(zhǎng)，從而估計(jì)出被測(cè)方形狀物體的位姿。本文提出的位姿估計(jì)方法簡(jiǎn)單、有效、易行，通過(guò)實(shí)驗(yàn)驗(yàn)證了方法的可行性，利用該方法能較好地實(shí)現(xiàn)爬桿機(jī)器人對(duì)方形狀物體的位姿估計(jì)。