基于3D 視覺的機(jī)械臂抓取控制方法①

潘 聰 方灶軍 張群莉 孫晨陽 戴伊寧

(*浙江工業(yè)大學(xué)機(jī)械工程學(xué)院 杭州 310014)

(**中國科學(xué)院寧波材料技術(shù)與工程研究所 寧波 315201)

(***浙江省機(jī)器人與智能制造裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室 寧波 315201)

0 引言

近年來,機(jī)器人抓取技術(shù)在工業(yè)生產(chǎn)中扮演著越來越重要的角色。但傳統(tǒng)的二維(2D)平面物體抓取滿足不了日趨復(fù)雜化的生產(chǎn)環(huán)境[1]。隨著三維(3D)視覺技術(shù)的發(fā)展,包含深度信息的立體抓取技術(shù)成為國內(nèi)外學(xué)者的研究重點(diǎn)。3D 視覺技術(shù)通過位姿估計(jì)實(shí)現(xiàn)定位,但物體的位姿識(shí)別容易受到環(huán)境干擾,因此,可靠、精確地識(shí)別物體位姿仍是該領(lǐng)域的一個(gè)難點(diǎn)。

國內(nèi)外各個(gè)研究者在這一領(lǐng)域提出了各種不同的位姿估計(jì)算法。其中,基于模板匹配的方法被廣泛用于物體的識(shí)別定位中。文獻(xiàn)[2]提出了點(diǎn)對(duì)特征算法(point pair features,PPF)來進(jìn)行三維目標(biāo)識(shí)別,它通過點(diǎn)對(duì)間的距離和法線夾角,利用投票方案得出最優(yōu)的位姿。文獻(xiàn)[3]使用計(jì)算機(jī)輔助設(shè)計(jì)(computer aided design,CAD)模型,并結(jié)合Hough 投票的方法來識(shí)別不同的零件。投票算法需要建立物體的特征庫,并進(jìn)行在線搜索,算法整體復(fù)雜度和處理時(shí)間大大增加。配準(zhǔn)算法不但復(fù)雜度低于投票算法,而且能保證有效的精度,算法主要分為粗配準(zhǔn)和精配準(zhǔn)兩步,精配準(zhǔn)通常選用迭代最近點(diǎn)算法(iterative closest point,ICP)[4]或者它的一些變種[5-6],但I(xiàn)CP 算法容易陷入局部最優(yōu)解[7]。因此,為了避免這種情況生成,需要粗配準(zhǔn)算法提供初始位置。其中,文獻(xiàn)[8]結(jié)合采樣一致性初始配準(zhǔn)算法(sample consensus initial aligment,SAC-IA)和ICP 算法實(shí)現(xiàn)了點(diǎn)云精確配準(zhǔn)定位,但SAC-IA 算法由于是一種貪婪算法,配準(zhǔn)效率低。文獻(xiàn)[9]采用了共面四點(diǎn)集算法(4-points congruent sets,4PCS),4PCS 算法會(huì)在點(diǎn)云中強(qiáng)制選取共面的4 個(gè)點(diǎn)進(jìn)行配準(zhǔn),但由于點(diǎn)云對(duì)稱會(huì)導(dǎo)致配準(zhǔn)錯(cuò)誤。因此,改進(jìn)現(xiàn)有算法和提出新算法對(duì)位姿估計(jì)問題很有必要。

可靠的位姿估計(jì)算法是視覺定位的關(guān)鍵,但點(diǎn)云的前期處理也能影響定位精度,包括點(diǎn)云濾波[10-11]、點(diǎn)云簡化[12-13]和點(diǎn)云分割[14-16]等。其中,點(diǎn)云濾波和點(diǎn)云簡化能夠減少無用點(diǎn)對(duì)位姿估計(jì)的影響,點(diǎn)云分割可以將整個(gè)點(diǎn)云按特征分為多個(gè)聚類,對(duì)其中特定聚類進(jìn)行處理能夠提高運(yùn)算效率。因此,如何選擇合適的前期處理算法來提高視覺定位的精度和效率也是本文研究的重點(diǎn)。

針對(duì)上述問題,本文提出了一種基于3D 視覺的機(jī)械臂抓取控制方法,主要對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行處理,獲取目標(biāo)物體的3D 位姿。本文主要貢獻(xiàn)如下:在點(diǎn)云預(yù)處理階段,通過差分法和加權(quán)下采樣算法簡化點(diǎn)云;在點(diǎn)云分割階段,使用歐式距離結(jié)合法線夾角變化率的二次分割方法分離點(diǎn)云;在位姿估計(jì)階段,使用主成分分析算法(principal component analysis,PCA)和ICP 算法進(jìn)行兩步配準(zhǔn)定位。通過在機(jī)械臂上的實(shí)驗(yàn),證明了該方法的有效性和可行性。

1 目標(biāo)物體位姿估計(jì)

為了識(shí)別目標(biāo)物體的三維位姿,本文使用結(jié)構(gòu)光相機(jī)采集目標(biāo)物體的三維點(diǎn)云,點(diǎn)云處理主要由點(diǎn)云預(yù)處理、點(diǎn)云分割和點(diǎn)云匹配3 部分組成。

通過三維建模軟件獲得目標(biāo)物體完整的計(jì)算機(jī)輔助設(shè)計(jì)(computer aided design,CAD)模型,經(jīng)離散化獲得模板點(diǎn)云,并在模板中定義物體的世界坐標(biāo)系,通過匹配算法可以獲取世界坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換矩陣,即位姿。此方法具有高精度、操作方便的特點(diǎn)。

1.1 點(diǎn)云預(yù)處理

點(diǎn)云預(yù)處理主要對(duì)原始點(diǎn)云數(shù)據(jù)進(jìn)行環(huán)境點(diǎn)云去除、去噪和下采樣等處理,能夠大幅度減少點(diǎn)云的數(shù)據(jù)量,提高計(jì)算機(jī)的處理速度。

1.1.1 環(huán)境點(diǎn)云去除

在機(jī)械臂抓取的工作場景中,環(huán)境點(diǎn)云主要為載物面,屬于平面點(diǎn)云的一種,本文提出用差分法去除載物面點(diǎn)云,如下所示:

其中,q1和q2是源點(diǎn)云和環(huán)境點(diǎn)云的對(duì)應(yīng)點(diǎn)對(duì),D(x,y,z)是差分點(diǎn),D(x,y,z)=1 代表前景點(diǎn),D(x,y,z)=0 代表背景點(diǎn),‖q1-q2‖代表q1與q2之間的距離,H為距離閾值。

利用式(1)提取出源點(diǎn)云中D=1 的點(diǎn)集便是差分結(jié)果。差分法不需要迭代,在相同作業(yè)環(huán)境中也不要更改參數(shù),效率高于擬合算法。

結(jié)構(gòu)光相機(jī)采集的點(diǎn)云數(shù)據(jù)會(huì)引入大量的噪點(diǎn),會(huì)對(duì)后續(xù)的操作產(chǎn)生極大的影響。本文采用半徑濾波算法來清除孤立點(diǎn)[17],通過雙邊濾波算法平滑點(diǎn)云,在去噪的同時(shí)能保持邊緣信息不丟失[18]。半徑濾波算法的原理是通過設(shè)定濾波半徑,計(jì)算每個(gè)點(diǎn)在其半徑范圍內(nèi)的其他點(diǎn)的個(gè)數(shù)。半徑范圍內(nèi)其他點(diǎn)個(gè)數(shù)少于某一設(shè)定的閾值的點(diǎn)將被濾除。雙邊濾波算法的原理是為每個(gè)點(diǎn)建立k鄰域并估算法向量,對(duì)該點(diǎn)及其鄰域點(diǎn)擬合的平面作為視平面,并建立一個(gè)雙邊濾波器來進(jìn)行噪點(diǎn)的處理。

1.1.2 點(diǎn)云下采樣

為了降低點(diǎn)云密度的同時(shí)保留局部特征,本文提出一種基于k鄰域和距離加權(quán)的點(diǎn)云下采樣方法。點(diǎn)云中的邊緣和圖像中的邊緣一樣,是蘊(yùn)含特征最豐富的地方[19]。因此,邊緣點(diǎn)不能和內(nèi)點(diǎn)同時(shí)下采樣,這會(huì)破壞邊緣的一部分特征。

首先,本文將邊緣點(diǎn)從源點(diǎn)云中剝離,然后著重處理內(nèi)點(diǎn)。將源點(diǎn)云中的全部內(nèi)點(diǎn)構(gòu)造軸對(duì)齊包圍盒(axis-aligned bounding box,AABB),通過AABB 包圍盒的三坐標(biāo)極值,將包圍盒劃分為邊長L的小立方體柵格,柵格的數(shù)量為

其中,mx、my和mz分別為x、y、z軸上包圍盒被分割的數(shù)量,max 和min 分別為x軸、y軸和z軸的最大值及最小值,ceil函數(shù)為四舍五入到大于或等于該元素的最接近整數(shù)。根據(jù)式(2)、(3)和式(4),小立方體柵格的總數(shù)m為

記錄點(diǎn)云中每個(gè)點(diǎn)所在的位置,再對(duì)每個(gè)立方體柵格中的點(diǎn)求取它們的重心p。KD 樹(K-dimensional tree,k-d tree)是一種高維索引樹形數(shù)據(jù)結(jié)構(gòu),用于高維空間中的快速最近鄰和近似最近鄰查找[20]。本文利用k-d tree 查找重心點(diǎn)p的k鄰域,k鄰域中的點(diǎn)通過與點(diǎn)p的距離遠(yuǎn)近來進(jìn)行加權(quán),離重心越遠(yuǎn)的點(diǎn)權(quán)重越小,通過k鄰域所有點(diǎn)的加權(quán)求取加權(quán)重心。因此,本文引入一個(gè)懲罰因子作為權(quán)重系數(shù)ωi,i代表第i次的k鄰域采樣:

其中d(p,pi) 為重心k鄰域中的點(diǎn)pi到重心p的距離。根據(jù)式(6)可得加權(quán)重心p′:

在所有的體素柵格中,通過式(7)計(jì)算出加權(quán)重心來代替柵格里的所有點(diǎn),以此來達(dá)到簡化目的。

最后再將之前剝離的邊緣點(diǎn)放入簡化后的點(diǎn)云中,以此就可以在保留邊緣特征的基礎(chǔ)上對(duì)點(diǎn)云進(jìn)行簡化。

1.2 點(diǎn)云分割

點(diǎn)云分割是從點(diǎn)云數(shù)據(jù)中分離出目標(biāo)物體。點(diǎn)云分割的結(jié)果對(duì)后面點(diǎn)云匹配起到了決定性作用,只有完整的分割出待匹配的物體點(diǎn)云,才能獲得精確的位姿。

歐式聚類分割算法是一種基于歐式距離的聚類算法,原理是將歐式距離小于閾值的點(diǎn)聚合成單個(gè)聚類,從而達(dá)到分割目的[21]。從中可以看出,歐式聚類分割的優(yōu)點(diǎn)在于只需比較點(diǎn)云間的歐式距離,所以處理速度較快;但缺點(diǎn)在于處理重疊在一起的物體點(diǎn)云時(shí),往往會(huì)把這些點(diǎn)云當(dāng)成一個(gè)聚類,無法進(jìn)行分割。因此,本文提出一種在歐式聚類分割的基礎(chǔ)上二次分割的方法。利用歐式聚類分割的優(yōu)勢,可以將一些易于分割的點(diǎn)云直接分離,對(duì)于那些重疊的點(diǎn)云,本文采用基于法線夾角變化率的方法在歐式聚類分割的基礎(chǔ)上進(jìn)行二次分割。

在歐式聚類分割后,需分辨出分割成功的點(diǎn)云和分割失敗的點(diǎn)云,這需要一個(gè)指標(biāo)來度量。本文提出用單個(gè)目標(biāo)物體的實(shí)際體積作為指標(biāo),體積通過求取點(diǎn)云聚類的有向包圍盒(oriented bounding box,OBB)得到。設(shè)V0為物體實(shí)際體積,V為包圍盒體積。若V＜V0,則分割成功;若V＞V0,則分割失敗,說明點(diǎn)云中含有2 個(gè)及以上物體點(diǎn)云,將進(jìn)行二次分割。

二次分割采用基于法線夾角變化率的方法。自然界大部分事物都是由凸面構(gòu)成,而不同物體的連接部分一般為凹面結(jié)構(gòu)[22],所以可以用凹凸面的特性將物體分割開。因此,本文使用近鄰點(diǎn)之間法線夾角變化率來代表凹凸面。首先,取一個(gè)物體的模型點(diǎn)云,估計(jì)表面法線,再計(jì)算法線間的平均夾角,記為θ。最后在處理的點(diǎn)云中,構(gòu)造物體點(diǎn)云的k-d tree,對(duì)每個(gè)點(diǎn)取k鄰域,計(jì)算點(diǎn)與其k領(lǐng)域中所有點(diǎn)的法線夾角,并計(jì)算夾角與θ的平均變化率σj:

其中,n為點(diǎn)的法向量,原理是根據(jù)鄰域內(nèi)的點(diǎn)擬合一個(gè)平面,則平面的法線方向即為點(diǎn)的法向量;ni為點(diǎn)的k鄰域中第i個(gè)點(diǎn)的法向量。計(jì)算模型點(diǎn)云中所有點(diǎn)的法線夾角平均變化率σ0:

其中,e為模型點(diǎn)云的點(diǎn)數(shù)。

將σ0作為閾值,在分割失敗的點(diǎn)云中用式(8)和(10)計(jì)算每個(gè)點(diǎn)與其k鄰域中每個(gè)點(diǎn)的法線夾角平均變化率σ,若σ＞σ0,說明這個(gè)點(diǎn)為連接點(diǎn)。在點(diǎn)云中,將所有連接點(diǎn)去除,隨后再次進(jìn)行歐式聚類分割,將分割出來的聚類放入第一次歐式聚類分割產(chǎn)生的聚類集合中。

點(diǎn)云分割完畢后,需要選取適合機(jī)械臂抓取的點(diǎn)云聚類,但不同的標(biāo)準(zhǔn)會(huì)造成不同的選取結(jié)果。本文設(shè)計(jì)了一種抓取分?jǐn)?shù)算法來選取合適的聚類,公式如下所示:

式(11)中G是得到的最終抓取分?jǐn)?shù),分?jǐn)?shù)越高代表物體越好抓取,其中μ和ξ是比例因子,可以隨具體場景改變。式(12)中Fz為z軸分?jǐn)?shù),zi為分割后的點(diǎn)云中屬于一個(gè)聚類的所有點(diǎn)的z軸坐標(biāo)。式(13)中Farea為面積分?jǐn)?shù),Sj為一個(gè)聚類的所有點(diǎn)在xoy平面上的投影面積,Smax是實(shí)際物體在xoy平面上的最大面積。由于相機(jī)垂直物體拍攝,所以抓取分?jǐn)?shù)越高代表越上方的物體和越平坦的物體是越好抓取的。

1.3 點(diǎn)云配準(zhǔn)

通過點(diǎn)云配準(zhǔn)方法計(jì)算物體的位姿,機(jī)械臂便可以根據(jù)這個(gè)位姿實(shí)現(xiàn)物體的抓取。為此,本文提出一種基于PCA 算法和ICP 算法的點(diǎn)云配準(zhǔn)算法,即使用PCA 算法實(shí)現(xiàn)點(diǎn)云的粗配準(zhǔn),使用ICP 算法實(shí)現(xiàn)點(diǎn)云的精配準(zhǔn)。

1.3.1 點(diǎn)云粗配準(zhǔn)

基于PCA 的粗配準(zhǔn)算法如下所示。

(1)取由式(11)得到分?jǐn)?shù)最高的點(diǎn)云聚類記為A,模板點(diǎn)云記為B,計(jì)算點(diǎn)云A和點(diǎn)云B的質(zhì)心點(diǎn)。

(2)計(jì)算兩組點(diǎn)云的協(xié)方差矩陣。

其中,CA為點(diǎn)云A協(xié)方差矩陣,CB為點(diǎn)云B協(xié)方差矩陣,T 為矩陣的轉(zhuǎn)置。

(3)計(jì)算特征向量和特征值。根據(jù)式CX=λX,得到協(xié)方差矩陣CA和CB的特征值λ=(λ0,λ1,λ2),且λ0≥λ1≥λ2,對(duì)應(yīng)特征向量X=(X0,X1,X2)。

(4)計(jì)算4 ×4 變換矩陣。其中,特征向量X=(X0,X1,X2)組成3 ×3 旋轉(zhuǎn)矩陣R:

其中,T為3 ×1 平移矩陣,t為點(diǎn)云的質(zhì)心坐標(biāo),t=。因此,H=(X0,X1,X2,T)為4 ×4 齊次變換矩陣?？傻命c(diǎn)云A和點(diǎn)云B之間的變換矩陣H:

其中,HA為點(diǎn)云A經(jīng)PCA 變換的矩陣,HB為點(diǎn)云B經(jīng)PCA 變換的矩陣,H代表從點(diǎn)云B變換到點(diǎn)云A。

由于PCA 算法存在主軸反向問題,因此傳統(tǒng)的PCA 配準(zhǔn)算法得到的初始矩陣H并不能直接用于精配準(zhǔn)中,錯(cuò)誤的初始矩陣會(huì)導(dǎo)致精配準(zhǔn)往錯(cuò)誤的方向迭代。為此,本文提出了一種誤差矯正方法,解決主軸反向問題。

首先將點(diǎn)云B通過矩陣HB變換到降維后的坐標(biāo)系得到點(diǎn)云Bnew:

其中,RB和TB分別為矩陣HB分解的旋轉(zhuǎn)矩陣和平移矩陣。

再將點(diǎn)云Bnew通過HA的逆矩陣(HA)-1轉(zhuǎn)換得到點(diǎn)云

其中,(RA)-1和(TA)-1分別為(HA)-1分解的旋轉(zhuǎn)矩陣和平移矩陣。

最后使用式(14) 求取點(diǎn)云Bnew_A的質(zhì)心點(diǎn)(xnew_A,ynew_A,znew_A),并且與點(diǎn)云A的質(zhì)心點(diǎn)(xA,yA,zA) 相比較,若滿足以下式子,則說明主軸反向。

其中,err為誤差。若滿足式(22)則說明x軸反向,滿足式(23)則說明y軸反向,滿足式(24)則說明z軸反向。

通過左乘旋轉(zhuǎn)矩陣的方式來矯正主軸反向,創(chuàng)建旋轉(zhuǎn)矩陣R180:

當(dāng)R1=1、R2=-1、R3=-1 時(shí),左乘R180代表繞x軸旋轉(zhuǎn)180 °;當(dāng)R1=-1、R2=1、R3=-1 時(shí),左乘R180代表繞y軸旋轉(zhuǎn)180 °;當(dāng)R1=-1、R2=-1、R3=1 時(shí),左乘R180代表繞z軸旋轉(zhuǎn)180 °。

1.3.2 點(diǎn)云精配準(zhǔn)

使用矩陣H為ICP 算法提供一個(gè)較好的初始位置。結(jié)合KD 樹的近鄰搜索[23],需要點(diǎn)云A中的點(diǎn)pi在點(diǎn)云B中通過迭代優(yōu)化找到一個(gè)點(diǎn)qi,使得誤差函數(shù)E最小:

其中,R為旋轉(zhuǎn)矩陣,t為平移矩陣,n為近鄰點(diǎn)對(duì)的數(shù)目。pi經(jīng)過R和t的變換得到計(jì)算的距離d:

若式(27)中的d小于設(shè)定的平均閾值則ICP算法停止迭代。矩陣M=(R,t)可以轉(zhuǎn)化為機(jī)械臂抓取需要的3D 位姿。

2 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本文提出方法的有效性,設(shè)置了積木抓取的任務(wù)場景,如圖1 所示。實(shí)驗(yàn)中使用的機(jī)械臂型號(hào)為TM 協(xié)作機(jī)器人;視覺檢測設(shè)備型號(hào)為RVC X mini 3D 相機(jī)。使用筆記本電腦作為上位機(jī)接收相機(jī)拍攝的點(diǎn)云圖,并控制機(jī)械臂運(yùn)動(dòng),筆記本電腦操作系統(tǒng)為位,處理器為I7-6500U,雙核,內(nèi)存為8 GB。編程環(huán)境為Visual Studio2019 和PCL 庫1.8.1,使用的編程語言為C/C++。

圖1 任務(wù)場景及設(shè)備

2.1 點(diǎn)云差分和濾波實(shí)驗(yàn)

從圖1 中的積木抓取任務(wù)場景中可知,環(huán)境點(diǎn)云主要是平面載物臺(tái)。在本次實(shí)驗(yàn)中,積木抓取任務(wù)場景點(diǎn)云如圖2(a)所示,環(huán)境點(diǎn)云如圖2(b)所示。根據(jù)式(1),設(shè)置參數(shù)H為10-4m,即設(shè)定對(duì)應(yīng)點(diǎn)之間的距離閾值為10-4m,若對(duì)應(yīng)點(diǎn)的距離小于這個(gè)閾值,則被認(rèn)為是背景點(diǎn);大于這個(gè)距離,則被認(rèn)為是前景點(diǎn)。最后差分的結(jié)果如圖2(c)所示。

根據(jù)實(shí)驗(yàn)結(jié)果可知差分法能夠有效去除環(huán)境點(diǎn)云。濾波算法能去除外部噪點(diǎn),平滑內(nèi)部點(diǎn)云。

2.2 點(diǎn)云下采樣實(shí)驗(yàn)

下采樣處理能夠減少無用點(diǎn)的數(shù)量,加快處理速度。為驗(yàn)證本文提出的下采樣算法的可靠性,對(duì)比傳統(tǒng)下采樣方式。首先給出一個(gè)點(diǎn)數(shù)為153 860的點(diǎn)云模型如圖3(a)所示,圖3(b)是采用傳統(tǒng)下采樣算法處理的結(jié)果,圖3(c)是采用本文提出的下采樣算法處理的結(jié)果,在本次實(shí)驗(yàn)中,將式(7)中的k定為10。通過對(duì)比可以看出,本文算法在邊緣以及角點(diǎn)特征較多的部位能夠比傳統(tǒng)算法保留更多特征。

圖3 模型下采樣結(jié)果

2.3 點(diǎn)云分割實(shí)驗(yàn)

第1 次分割采用傳統(tǒng)歐式聚類分割方式。如圖4所示,點(diǎn)云被分割成4 個(gè)聚類,其中,有3 塊積木被完整地分割出來,有2 塊因?yàn)檎尺B而導(dǎo)致欠分割。

圖4 傳統(tǒng)歐式聚類分割結(jié)果

通過點(diǎn)云聚類的OBB 包圍盒體積和積木實(shí)際體積的比較,可以選出欠分割的點(diǎn)云聚類。實(shí)際的積木體積V0為2.1924 ×10-5m3,圖4 中1～4 號(hào)聚類的OBB 包圍盒體積如表1 所示。

表1 點(diǎn)云聚類的OBB 包圍盒體積與V0 比較

對(duì)表格中的數(shù)據(jù)進(jìn)行分析,序號(hào)2～4 點(diǎn)云聚類的體積小于V0,代表分割成功。序號(hào)1 的點(diǎn)云聚類體積大于V0,處于欠分割狀態(tài),需要進(jìn)行二次分割。

在二次分割之前,需要計(jì)算模型點(diǎn)云法線間的平均夾角θ和法線夾角平均變化率σ0。本次實(shí)驗(yàn)的模型點(diǎn)云法線間的平均夾角θ=69.24°,通過式(8)和(10),設(shè)置k=10,σ0=32.0184,若點(diǎn)云中點(diǎn)的k鄰域的法線夾角平均變化率σ＞σ0,則刪去這個(gè)點(diǎn),一直循環(huán)到點(diǎn)云中點(diǎn)的σ值都小于σ0,最后再次進(jìn)行歐式聚類分割。圖5 表示點(diǎn)云經(jīng)第1 次分割和第2 次分割后的最終結(jié)果。

圖5 本文算法分割結(jié)果

通過與傳統(tǒng)的歐式聚類分割算法對(duì)比中可以看出,利用本文提出的基于改進(jìn)歐式聚類的二次分割算法可以有效地分割出場景中堆疊的物體點(diǎn)云。

2.4 點(diǎn)云配準(zhǔn)實(shí)驗(yàn)

為了比較SAC-IA +ICP 配準(zhǔn)算法、4PCS+ICP配準(zhǔn)算法和本文提出的配準(zhǔn)算法的定位性能,選擇圖1 任務(wù)場景中的其中一塊積木與其CAD 模型點(diǎn)云進(jìn)行配準(zhǔn)實(shí)驗(yàn)。圖6(a)是SAC-IA +ICP 算法配準(zhǔn)的結(jié)果,圖6(b)是4PCS +ICP 算法配準(zhǔn)的結(jié)果,圖6(c)是本文算法配準(zhǔn)的結(jié)果。點(diǎn)密集度小的形狀為源點(diǎn)云,點(diǎn)密集度大的形狀為模板點(diǎn)云。其中,經(jīng)過配準(zhǔn)變換,3 種算法得到的源點(diǎn)云與模板點(diǎn)云的重合率分別為81%、90%、96%。從中可以看出,本文提出的配準(zhǔn)算法使得源點(diǎn)云與模板點(diǎn)云的重合率更高,因此精度更高。

圖6 配準(zhǔn)結(jié)果比較

為了進(jìn)一步分析3 種算法的可靠性,選擇結(jié)果點(diǎn)云和源點(diǎn)云中對(duì)應(yīng)點(diǎn)的平均最小歐式距離來表示點(diǎn)云配準(zhǔn)的誤差,以此作為衡量算法定位性能的依據(jù)。圖7(a)為x軸誤差,圖7(b)為y軸誤差,圖7(c)為z軸誤差?？梢钥闯?本文算法在x、y、z軸上的誤差都要小于其他2 種算法,定位性能更加穩(wěn)定。

圖7 配準(zhǔn)誤差對(duì)比

本次實(shí)驗(yàn)統(tǒng)計(jì)了3 種算法的配準(zhǔn)時(shí)間,從表2中可以看出,3 種算法在實(shí)驗(yàn)場景中的平均配準(zhǔn)時(shí)間分別為83.5 s、5.9 s、1.0 s。因此,本文算法在計(jì)算效率上優(yōu)于其他2 種算法。

表2 配準(zhǔn)時(shí)間對(duì)比/s

本文提出的定位算法最終實(shí)驗(yàn)結(jié)果如圖8 所示,本文算法成功定位到了每塊積木。

圖8 精配準(zhǔn)結(jié)果

精配準(zhǔn)得到的矩陣便是物體在相機(jī)坐標(biāo)系下的位姿矩陣,將位姿矩陣與眼標(biāo)定矩陣結(jié)合,并轉(zhuǎn)換為3D 位姿發(fā)送給機(jī)械臂便能實(shí)現(xiàn)抓取,位姿結(jié)果如表3所示,抓取的結(jié)果如圖9 所示。

表3 3D 位姿結(jié)果

圖9 抓取結(jié)果

3 結(jié)論

機(jī)械臂抓取主要的難點(diǎn)是目標(biāo)物體的定位,本文提出了一種基于3D 視覺的機(jī)械臂抓取控制方法,通過差分法和加權(quán)下采樣的方法簡化了點(diǎn)云,大幅度提高了數(shù)據(jù)處理的時(shí)間,通過二次分割的方式成功分離出堆疊的點(diǎn)云聚類,使用PCA 算法和ICP算法成功計(jì)算目標(biāo)點(diǎn)云位姿。在達(dá)明協(xié)作機(jī)器人上的積木抓取實(shí)驗(yàn)證明了此方法的有效性,并且在相同的任務(wù)場景中,只需更換CAD 模板便能抓取到不同的物體。在本次任務(wù)場景中,對(duì)比需要提取特征點(diǎn)和計(jì)算特征描述子的配準(zhǔn)方法,速度更快。在點(diǎn)云分割上,本文的算法對(duì)比于傳統(tǒng)的分割算法能較好地處理堆疊場景。機(jī)械臂與3D 視覺結(jié)合是機(jī)器人抓取領(lǐng)域的研究熱門,不同的點(diǎn)云前期處理算法和位姿估計(jì)算法也層出不窮,本文后續(xù)將對(duì)各種算法進(jìn)行研究,使其能夠適應(yīng)更加復(fù)雜的環(huán)境。