基于線結(jié)構(gòu)光視覺的方形狀物體位姿估計*

葉文達，侯宇瀚，陳洪佳，李藝帆

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

0 引言

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

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

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

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

2 系統(tǒng)標定

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

2.1 相機標定

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

圖2 相機成像模型

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

(1)

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

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

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

ZC=AlXC+BlYC+Cl.

(2)

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

(3)

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

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

(4)

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

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

2.3 手眼標定

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

(5)

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

(6)

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

3 位姿估計算法實現(xiàn)

3.1 圖像預處理

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

圖4 原始圖像

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

圖5 圖像預處理

3.2 中心線提取及聚類

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

3.3 特征點提取

因為兩激光線條紋的特征點提取過程相同，故只需討論其中一激光線條紋的特征點提取情況。為了獲取條紋上的特征點從而進行矩形的擬合，需要提取激光線條紋端點的像素坐標以及兩直線交叉點的像素坐標。

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

(7)

同時根據(jù)u值找到對應(yīng)的v值,即得到兩端點的像素坐標(ue1,ve1)和(ue2,ve2)。

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

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

3.4 矩形擬合及點映射

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

圖6 提取中心線 圖7 特征點提取 圖8 矩形擬合

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

(8)

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

(9)

3.5 位姿估計

由于可以同時獲取兩擬合矩形的像素中心點坐標，我們可以分別得到兩組空間矩形的三維中心點坐標：

(10)

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

(11)

(12)

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

(13)

(14)

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

4 實驗驗證

為了驗證所提出的方案和分析結(jié)果，本文進行實驗來驗證傳感方法的有效性，實驗平臺如圖9所示。

圖9 實驗平臺

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

(15)

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

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

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

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

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

5 總結(jié)

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