田青華,白瑞林,李 杜

1(江南大學(xué) 輕工過程先進(jìn)控制教育部重點(diǎn)實驗室,江蘇 無錫 214122) 2(無錫信捷電氣股份有限公司,江蘇 無錫 214072)

1 引 言

為使機(jī)器人從隨機(jī)放置且未排序的箱體中以正確的方向定位和抓取工件,需對目標(biāo)工件進(jìn)行位姿估計.近年來,使用三維掃描設(shè)備(如三維激光掃描儀、立體視覺系統(tǒng)等)獲取工件表面的3D點(diǎn)云數(shù)據(jù),并將三維點(diǎn)云和位姿已知模板點(diǎn)云進(jìn)行匹配得到兩者之間的剛性變換,從而實現(xiàn)工件的識別和定位[1,2],成為了機(jī)器視覺領(lǐng)域的研究熱點(diǎn).

常見的配準(zhǔn)算法是迭代最近點(diǎn)算法[3](Iterative Closest Point,ICP)及后續(xù)研究者的改進(jìn)算法,ICP算法原理簡單易實現(xiàn),配準(zhǔn)精度高,缺點(diǎn)為:1)當(dāng)兩幅點(diǎn)云之間不存在包含關(guān)系或初始位置偏差較小時,算法易陷入局部最優(yōu);2)迭代收斂速度慢.因此,在進(jìn)行精確配準(zhǔn)之前先進(jìn)行初始配準(zhǔn)很有必要.常見的初始配準(zhǔn)算法有基于圖的方法、基于特征的方法以及基于視圖的方法.其中,基于圖的方法利用圖來提取3D形狀的幾何屬性,包含各形狀分量的類型和空間關(guān)系,其突出優(yōu)點(diǎn)為姿態(tài)的獨(dú)立性和部分匹配的可行性;然而每個匹配過程需要不同的匹配策略,且計算量大.此外,基于圖的方法相似性度量的計算對圖的變化比較敏感.

基于視圖方法的原理:兩個3D對象僅在它們從所有視角看起來都相似時才是類似的.這些方法具有位姿獨(dú)立性,因為其相似性比較是旋轉(zhuǎn)獨(dú)立的.該方法可以應(yīng)用在幾乎所有類型的對象,且不需要任何特征提取.然而,在數(shù)據(jù)庫和圖像獲取方面,基于視圖計算復(fù)雜性較高,這使得該算法不適用于實時應(yīng)用之中.

基于特征的方法[4]可以分為全局特征和局部特征,通過測量和比較3D形狀的幾何形狀和拓?fù)鋵傩詠韰^(qū)分3D對象.基于全局特征描述的配準(zhǔn)算法對于雜亂、有遮擋場景的抗干擾能力不強(qiáng),而通過局部特征匹配建立的對應(yīng)關(guān)系對視點(diǎn)、雜波和遮擋的變化具有很好的魯棒性,因此,該類方法在工業(yè)上采用的最多.

常見的局部特征描述子有快速點(diǎn)特征直方圖描述子[5](FPFH,Fast Point Feature Histograms)、3D形狀內(nèi)容描述子(3D Shape Context)、旋轉(zhuǎn)圖像(Spin image)、方向直方圖簽名描述子(SHOT,Signature of Histograms of OrienTations)、旋轉(zhuǎn)投影統(tǒng)計(RoPS,Rotational Projection Statistics)等.其中,RoPS描述子具有很好的抗噪能力,鑒別力最好,但實時性較差;對實時性要求高的場合下,FPFH和 SHOT是最好的選擇.當(dāng)局部表面上的點(diǎn)較少時,FPFH描述子是最佳選擇,這是由于 FPFH 鑒別力強(qiáng)、運(yùn)算速度快且占用存儲資源少.反之,SHOT描述子是最佳選擇,其在鑒別力與計算效率之間能取得很好的折中.SHOT描述子是由Salti S等[6]于2010年提出,該描述子同時具有直方圖特征的抗噪性和基于標(biāo)簽特征的鑒別性,主要用于曲面模型匹配,且對旋轉(zhuǎn)和尺度具有不變性;文獻(xiàn)[7]將顏色特征加入描述子,增加了特征的描述性,但是需要獲取特征點(diǎn)的顏色信息,且增加了計算量;文獻(xiàn)[8]提出一種法矢改進(jìn)點(diǎn)云特征C-SHOT的3D 物體識別方法,利用特征點(diǎn)處法矢與鄰域法矢的夾角余弦構(gòu)造點(diǎn)云形狀直方圖,同時統(tǒng)計點(diǎn)云紋理直方圖并與形狀直方圖融合成C-SHOT 描述符,該特征描述符可以實現(xiàn)復(fù)雜場景下3D物體識別和位姿估計,但是該算法亦需要獲取點(diǎn)云RGB字段信息.文獻(xiàn)[9]提出了一種基于特征提取的點(diǎn)云自動配準(zhǔn)算法,利用點(diǎn)云局部點(diǎn)的三種幾何特征搜索匹配點(diǎn)對,得到較高的匹配精度但耗時多.

綜上所述,考慮隨機(jī)箱體抓取[10]過程中散亂工件識別和定位的精度和速度要求,本文選取特征描述性和實時性均優(yōu)的SHOT描述子進(jìn)行配準(zhǔn),由于該特征在相似區(qū)域易得到錯誤的對應(yīng)關(guān)系,提出一種SHOT特征融合的點(diǎn)云配準(zhǔn)算法.算法針對點(diǎn)云相似區(qū)域單一的特征匹配識別度低的問題,提出SHOT和空間位置結(jié)合的特征描述子,實現(xiàn)對特征點(diǎn)的唯一性描述,從而降低誤匹配的概率,并提高精配準(zhǔn)的速度;采用線下建立不同姿態(tài)下的模板點(diǎn)云特征描述庫的方法,對分割出的單個工件點(diǎn)云的關(guān)鍵點(diǎn)在特征庫中進(jìn)行特征匹配,尋找對應(yīng)點(diǎn),從而得到單個工件位姿信息.

2 整體算法方案

本文配準(zhǔn)算法流程圖如圖1所示,分為線下模板特征建立和線上點(diǎn)云配準(zhǔn)部分.通過線下處理建立工件模板庫,提取模板點(diǎn)云關(guān)鍵點(diǎn),并計算關(guān)鍵點(diǎn)的特征描述子;線上配準(zhǔn)根據(jù)單個工件點(diǎn)云關(guān)鍵點(diǎn)特征描述子查找模板點(diǎn)云中的對應(yīng)點(diǎn),選取最佳變換確定兩者之間的轉(zhuǎn)換關(guān)系.

算法實現(xiàn)的關(guān)鍵部分在于初始配準(zhǔn)實現(xiàn),初始配準(zhǔn)分為關(guān)鍵點(diǎn)提取、關(guān)鍵點(diǎn)特征描述以及特征匹配三個部分.分別提取單個工件點(diǎn)云和模板點(diǎn)云的關(guān)鍵點(diǎn),并進(jìn)行特征描述與匹配,通過特征對應(yīng)點(diǎn)獲取兩者的剛性變換矩陣.最后,利用ICP算法優(yōu)化兩者之間的變換矩陣,進(jìn)一步提高匹配精度,從而得到工件精確的位姿信息.

圖1 算法流程圖Fig.1 Flow chart of algorithm

3 點(diǎn)云初始配準(zhǔn)

3.1 關(guān)鍵點(diǎn)提取

3.1.1 方向包圍盒

包圍盒是用體積稍大且特性簡單的幾何體來近似地代替復(fù)雜的幾何對象.常見的包圍盒有包圍球(Sphere)、軸向包圍盒(Axis-aligned bounding box,AABB)以及方向包圍盒(Oriented bounding box,OBB)[11].其中OBB具有方向性、可以旋轉(zhuǎn)、緊密性好,總體性能優(yōu)于Sphere和AABB,因此,本文通過求取點(diǎn)云的OBB進(jìn)行關(guān)鍵點(diǎn)選取.OBB可以由基準(zhǔn)角C(xc,yc,zc)和三個矢量CC1(xmax,ymax,zmax),CC2(xmid,ymid,zmid),CC3(xmin,ymin,zmin) 表示,分別對應(yīng)于OBB的最大、中間和最小維度.利用PCA算法計算點(diǎn)云在3D空間下的3個特征向量,根據(jù)特征向量構(gòu)建點(diǎn)云的局部坐標(biāo)系,將原點(diǎn)云變換到新坐標(biāo)系下,計算變換后點(diǎn)云在三個軸方向的最大最小值以及對角線中心點(diǎn),根據(jù)最大最小值得到OBB的長寬高,結(jié)合對角線中心點(diǎn)得出方向包圍盒.

具體步驟如下:

1)計算點(diǎn)云重心Centriod和歸一化協(xié)方差矩陣covariance;

(1)

(2)

2)計算本征向量e0,e1,e2,得到新的參考坐標(biāo)系(e0,e1,e0×e1);

3)將點(diǎn)云轉(zhuǎn)換到坐標(biāo)系(e0,e1,e0×e1)下,計算沿坐標(biāo)軸方向點(diǎn)云的最大、最小和對角線中心點(diǎn)坐標(biāo);

4)根據(jù)給定的對角線中心點(diǎn)和包圍盒尺寸得到點(diǎn)云包圍盒.

3.1.2 獲取關(guān)鍵點(diǎn)

關(guān)鍵點(diǎn)為點(diǎn)云中一些特殊的點(diǎn),通常表示為角點(diǎn)、灰度梯度等急劇變換的點(diǎn)[12],通過對關(guān)鍵點(diǎn)進(jìn)行信息描述可以有效的表示整幅點(diǎn)云.關(guān)鍵點(diǎn)的選取可以減少噪聲點(diǎn),提高匹配的速度.由于目標(biāo)點(diǎn)云在深度方向上的分辨率是不同的,呈現(xiàn)在實際空間中就是位于深度邊緣處的點(diǎn)云相對于目標(biāo)內(nèi)部點(diǎn)更稀疏,這會導(dǎo)致稀疏點(diǎn)的特征描述子在尋找模板中的對應(yīng)匹配點(diǎn)時的差異增大,不利于配準(zhǔn),因此,提出如下關(guān)鍵點(diǎn)提取算法,實驗結(jié)果如圖2所示.

圖2 關(guān)鍵點(diǎn)提取Fig.2 Key point extraction

1)以變換后點(diǎn)云的深度為依據(jù)(即OBB的高),取OBB高的一半為閾值;

2)采用直通濾波器對點(diǎn)云進(jìn)行濾波處理,去除深度小于閾值的點(diǎn),保留余下的點(diǎn)記作候選關(guān)鍵點(diǎn);

3)將候選關(guān)鍵點(diǎn)點(diǎn)云進(jìn)行反變換到原坐標(biāo)系下,用于后續(xù)的特征提取與配準(zhǔn);

4)對關(guān)鍵點(diǎn)云進(jìn)行均勻采樣,提高匹配速度.

3.2 點(diǎn)云特征描述

3.2.1 SHOT 特征描述子

圖3 球形域Fig.3 Spherical domain

方向直方圖簽名描述子( SHOT) 是一個3D特征，它結(jié)合了幾何分布信息的魯棒性和直方圖統(tǒng)計信息的特異性． 文獻(xiàn)[13]把3D特征描述符分為兩類: 基于標(biāo)簽和基于直方圖特征描述符，分別強(qiáng)調(diào)特征的描述性和魯棒性，而SHOT圖3球形域特征結(jié)合了兩者的優(yōu)點(diǎn),具有很好的可重復(fù)檢測性,且特征描述具有旋轉(zhuǎn)和尺度不變性.

SHOT具體實現(xiàn)步驟如下:

1)建立特征點(diǎn)的r半徑球鄰域局部參考坐標(biāo)系,并對特征點(diǎn)的球鄰域分別沿徑向、方位角方向和高度方向進(jìn)行區(qū)域劃分.徑向劃分為2,方位角方向劃分為8,高度方向劃分為2,總共32個網(wǎng)格,如圖3所示.

2)分別統(tǒng)計每個網(wǎng)格內(nèi)的法向量夾角余弦值分布情況,即網(wǎng)格的直方圖.直方圖分為11個小單元,各單元的值由特征點(diǎn)處法線與鄰域點(diǎn)法線夾角余弦計算而得,如公式(3)所示,最終SHOT描述子的長度為32×11=352.

cosθq=zknq

(3)

其中,zk為r半徑球鄰域內(nèi)局部坐標(biāo)系的Z軸,也是特征點(diǎn)的法線;nq是當(dāng)前標(biāo)簽內(nèi)所包含點(diǎn)的法線.采用夾角余弦而非夾角值有2個優(yōu)勢:1)計算速度快,只需要計算兩向量之間的夾角余弦值;2)對cosθ的等距劃分等效于在空間上對θ進(jìn)行劃分,由此對接近參考法線的方向進(jìn)行粗略的劃分,在正交方向上進(jìn)行精細(xì)的劃分,以這種方式,正交方向上的小差異(可能是最具信息性的差異)導(dǎo)致點(diǎn)在不同單元格中累積,從而導(dǎo)致不同的直方圖.此外,在近平面區(qū)域,描述性不強(qiáng),這種選擇通過將計數(shù)集中在較少的單元中來限制由于噪音而導(dǎo)致的直方圖差異.

由于SHOT描述子是局部特征描述子,支持空間細(xì)分,因此需要考慮邊界效應(yīng).為消除邊界影響,采用四線性插值法將鄰域內(nèi)各點(diǎn)累加到局部直方圖特定的單元格,即在一個局部直方圖的相鄰單元插值,相鄰直方圖的同一單元格進(jìn)行插值[8].插值過程中,局部直方圖單元格增量是1-d,對于法線夾角余弦,d表示直方圖相鄰單元格插值,如圖4所示.其中d的定義如公式(4)所示,同理,在方位角方向、仰角方向和徑向方向插值與之類似.

(4)

圖4 法線夾角余弦插值Fig.4 Interpolation on normal cosines

3.2.2 空間位置描述子

f1(Pqi)=‖Centriod-qi‖

(5)

(6)

其中m表示點(diǎn)云中特征點(diǎn)的數(shù)目.

3.3 初始配準(zhǔn)實現(xiàn)

在匹配過程中,需要對隨機(jī)選取的兩個特征進(jìn)行比較,特征值不近似相等的特征組合對應(yīng)的轉(zhuǎn)換關(guān)系一定是錯誤的,反之,即使特征值近似,也可能獲得錯誤的轉(zhuǎn)換關(guān)系.對于目標(biāo)中的一個確定特征,在模板點(diǎn)云中,可能存在多個特征點(diǎn)具有相等的特征值,這些特征中只有一個和目標(biāo)特征相互對應(yīng),當(dāng)提取的兩個特征對應(yīng)關(guān)系正確時,才可以獲得最佳的目標(biāo)位姿參數(shù).具體初始配準(zhǔn)實現(xiàn)步驟如下:

離線階段:

1)采用2.1節(jié)方法提取模板點(diǎn)云M的關(guān)鍵點(diǎn)集Pk,記作Model_KeyPoints;

在線階段:

1)分割得到單個工件的點(diǎn)云子集O={Oj,j=0,1,…,n},采用2.1節(jié)方法提取目標(biāo)點(diǎn)云關(guān)鍵點(diǎn)Scene_KeyPoints;

2)計算各個點(diǎn)云子集的SHOT描述子F1O={F1Oj,j=1,2,…,m}和空間位置描述子F2O={F2Oj,j=1,2,…,m};

3)對于點(diǎn)集Oj中的任意一點(diǎn)qi,在模板點(diǎn)云中搜索其對應(yīng)點(diǎn),若pi為其對應(yīng)點(diǎn),需滿足公式(7)(8)所設(shè)條件,即采用最小平方誤差的方法獲取特征點(diǎn)在模板點(diǎn)云中的對應(yīng)點(diǎn),若該組對應(yīng)點(diǎn)的空間位置描述子的差異也在預(yù)設(shè)閾值范圍內(nèi),則保留當(dāng)前對應(yīng)點(diǎn),反之則舍棄;

|FM1pi-F1Oqi|/[|FM1pi+F1Oqi|]<ε1

(7)

|FM2pi-F2Oqi|/[|FM2pi+F2Oqi|]<ε2

(8)

4)對對應(yīng)點(diǎn)對采用奇異值分解算法求解模板點(diǎn)云到目標(biāo)點(diǎn)云之間的旋轉(zhuǎn)矩陣R和平移矩陣T,即初始配準(zhǔn)結(jié)果;

(9)

5)對初始配準(zhǔn)結(jié)果進(jìn)行性能評估.

(10)

4 精確配準(zhǔn)

初始配準(zhǔn)得到了場景點(diǎn)云相對于模板點(diǎn)云的變換矩陣,使得變換后的兩幅點(diǎn)云大致 “重合”,為了進(jìn)一步提高匹配精度,使用ICP算法進(jìn)行優(yōu)化,過程如下:

3)根據(jù)四元數(shù)法計算出旋轉(zhuǎn)矩陣Rk和平移矩陣Tk;

4)計算誤差函數(shù)為:

(11)

如果誤差函數(shù)保持不變或者達(dá)到設(shè)定要求,則停止迭代,當(dāng)前旋轉(zhuǎn)矩陣Rk和平移矩陣Tk就是最終的配準(zhǔn)參數(shù)矩陣.如果沒有達(dá)到設(shè)定要求,則Sk+1=RkSk+Tk,返回公式(11).式中k表示迭代次數(shù),n表示源點(diǎn)云隨機(jī)點(diǎn)集中元素個數(shù).

5 實驗與分析

為驗證配準(zhǔn)算法的有效性,采用文獻(xiàn)[14]的點(diǎn)云數(shù)據(jù)采集系統(tǒng)獲取箱體內(nèi)散亂堆放工件點(diǎn)云和單個工件點(diǎn)云進(jìn)行的實驗,工件為柱形塑料瓶,瓶身直徑為2.15cm,長4.7cm.分別采用基于SHOT特征配準(zhǔn)算法和基于FPFH特征配準(zhǔn)算法對散亂工件點(diǎn)云進(jìn)行識別和定位,作為對比實驗.實驗所用PC機(jī)為Windows 7 64位操作系統(tǒng),配置為Intel(R)Core(TM)i3-3220 CPU,3.3GHz,8.00 GB內(nèi)存,使用編程語言為C++,軟件平臺為64位VS2013,涉及庫文件為點(diǎn)云庫PCL1.8.0.

分別對模板點(diǎn)云進(jìn)行預(yù)處理,包括去除噪聲點(diǎn)、背景點(diǎn)和冗余點(diǎn),如圖5(a)所示.對場景點(diǎn)云的處理包括預(yù)處理和分割.預(yù)處理同樣包括去除背景點(diǎn),離群點(diǎn),分割的目的是將單個工件從雜亂的場景點(diǎn)云中分離出來,用于后續(xù)的位姿定位.

圖5 模板點(diǎn)云和目標(biāo)點(diǎn)云Fig.5 Model point cloud and scene point cloud

采用文獻(xiàn)[15]的方法對場景點(diǎn)云進(jìn)行分割,得到散亂工件點(diǎn)云的分割結(jié)果如圖5(b)所示.

將分割得到的單個工件點(diǎn)云和模板點(diǎn)云進(jìn)行配準(zhǔn)實驗,相關(guān)參數(shù)選取為:ε1=ε2=0.02,鄰域搜索半徑5mm,SHOT描述子差異閾值為0.15.為驗證本文算法的有效性,采用3組對比試驗,統(tǒng)計不同算法下,粗配準(zhǔn)、精確配準(zhǔn)的誤差值以及算法耗時,統(tǒng)計結(jié)果如表1所示.

為直觀判斷配準(zhǔn)結(jié)果優(yōu)劣,將目標(biāo)點(diǎn)云2,4和8的配準(zhǔn)結(jié)果顯示輸出,統(tǒng)計結(jié)果如圖6所示.結(jié)合表1對實驗結(jié)果進(jìn)行分析,可以得出如下結(jié)論:

圖6 點(diǎn)云配準(zhǔn)結(jié)果對比Fig.6 Point cloud registration results contrast

1)在精確配準(zhǔn)前加入初始配準(zhǔn)可以得到更好的配準(zhǔn)結(jié)果.由配準(zhǔn)誤差和配準(zhǔn)效果排名均值可以看出,其他三種方法均優(yōu)于ICP配準(zhǔn)算法;

2)本文算法的配準(zhǔn)效果優(yōu)于FPFH特征配準(zhǔn)、SHOT特征配準(zhǔn)算法.相對于FPFH特征配準(zhǔn),本文算法的配準(zhǔn)誤差減少了30.07%,相對于SHOT配準(zhǔn)算法37.10%.

3)本文算法的耗時少,約0.7s,但由于加入初始配準(zhǔn),耗時大于ICP配準(zhǔn)算法.本文算法相對于FPFH特征配準(zhǔn)減少了35.64%,相對于SHOT特征配準(zhǔn)減少了21.21%.

6 結(jié) 論

提出一種基于SHOT特征融合的點(diǎn)云配準(zhǔn)方法,用于解決隨機(jī)箱體抓取過程中的目標(biāo)工件的識別和定位問題.得出結(jié)論如下:

1)提出OBB裁剪結(jié)合US算法獲取點(diǎn)云的關(guān)鍵點(diǎn),消除了稀疏邊緣點(diǎn)特征差異過大帶來的影響,降低了算法的計算量;針對特征相似區(qū)域?qū)?yīng)點(diǎn)估計錯誤問題,提出將SHOT特征和空間位置特征結(jié)合的方法計算模板點(diǎn)云和目標(biāo)點(diǎn)云的對應(yīng)點(diǎn),從而減少了初始配準(zhǔn)的誤差,縮小了精確配準(zhǔn)的迭代次數(shù),降低了算法的整體耗時.

表1 點(diǎn)云配準(zhǔn)實驗結(jié)果Table 1 Point cloud registration experiment results

2)實驗結(jié)果表明,本文算法相較于SHOT特征配準(zhǔn)、FPFH特征配準(zhǔn)算法在配準(zhǔn)精度分別提高了30.07%和37.10%,配準(zhǔn)速度分別提高了35.64%和21.21%,滿足工業(yè)生產(chǎn)過程中的準(zhǔn)確性和實時性要求.