基于點(diǎn)云的部件識別和姿態(tài)估計(jì)方法研究

吳浩然，于 海，彭 林，李春龍

(1.國網(wǎng)無錫供電公司，江蘇 無錫 210096；2.全球能源互聯(lián)網(wǎng)研究院，南京 210003)

基于點(diǎn)云的部件識別和姿態(tài)估計(jì)方法研究

吳浩然1，于 海2，彭 林2，李春龍2

(1.國網(wǎng)無錫供電公司，江蘇 無錫 210096；2.全球能源互聯(lián)網(wǎng)研究院，南京 210003)

為了識別與理解裝配環(huán)境中的真實(shí)物體對象，提出一種基于配準(zhǔn)的3D感知方法，利用點(diǎn)云的相似度對場景中真實(shí)物體進(jìn)行感知,為降低噪聲對配準(zhǔn)結(jié)果的影響，提出一個(gè)基于密度自適應(yīng)的匹配算法，自適應(yīng)調(diào)整不同來源的點(diǎn)云密度，利用共面四點(diǎn)特征的仿射不變性實(shí)現(xiàn)對真實(shí)物體的識別。該方法可以減少點(diǎn)云的數(shù)據(jù)量，降低運(yùn)算復(fù)雜度，還能提高匹配精確度。實(shí)驗(yàn)結(jié)果表明，該方法能準(zhǔn)確在裝配環(huán)境點(diǎn)云中識別出零部件并能計(jì)算出它們的姿態(tài)，可用于噪聲條件下增強(qiáng)現(xiàn)實(shí)交互應(yīng)用。

增強(qiáng)現(xiàn)實(shí)；場景重構(gòu)；增強(qiáng)現(xiàn)實(shí)裝配

在增強(qiáng)現(xiàn)實(shí)裝配環(huán)境中，為了對零部件進(jìn)行交互操作或者裝配，要求對場景進(jìn)行深度感知。除了要識別不同的零部件，還要求了解它們在場景中的6自由度姿態(tài)[1-2]。而傳統(tǒng)的增強(qiáng)現(xiàn)實(shí)也可以針對某個(gè)目標(biāo)物體進(jìn)行交互，但所有的交互操作事實(shí)上是在2D空間上進(jìn)行的。傳統(tǒng)增強(qiáng)現(xiàn)實(shí)利用平面特征表示增強(qiáng)現(xiàn)實(shí)環(huán)境，通過二維空間中對虛擬3D模型處理和分析，這雖然可以實(shí)現(xiàn)對場景中的物體識別，但不能獲取物體的空間姿態(tài)。傳統(tǒng)增強(qiáng)現(xiàn)實(shí)無法進(jìn)行真正的虛實(shí)融合交互操作，無法對操作的場景進(jìn)行感知和反饋，一旦工作場景發(fā)生變化，增強(qiáng)現(xiàn)實(shí)系統(tǒng)就無法工作。為此本文研究場景感知方法，可以工作在裝配環(huán)境中，有很強(qiáng)的識別和感知能力，能實(shí)現(xiàn)3D交互和裝配任務(wù)。

1 識別框架與匹配問題

1.1識別框架

2D圖像上的物體識別和3D空間中的物體識別是當(dāng)今計(jì)算機(jī)視覺的難題之一，識別與模型表達(dá)緊密相關(guān)，想獲得一個(gè)通用的識別方法，首先場景和模型必須有一致的描述格式[3-4]。為了識別物體以及計(jì)算它們的姿態(tài)，本文用點(diǎn)云格式來描述CAD模型和由視頻圖像重構(gòu)的場景。通過匹配CAD模型點(diǎn)云和場景點(diǎn)云，可以分析場景中的目標(biāo)，并進(jìn)行識別。

本文場景識別方法思路是：(1)場景中出現(xiàn)模型都有一個(gè)CAD模型與之匹配；(2)CAD模型與它的點(diǎn)云可以相互轉(zhuǎn)換，點(diǎn)云的配準(zhǔn)過程為一種剛體變換；(3)在參與配準(zhǔn)兩個(gè)點(diǎn)云中，當(dāng)CAD模型點(diǎn)云中所有的點(diǎn)與它們最近鄰的場景點(diǎn)歐式距離足夠接近，就認(rèn)為識別成功，場景中和CAD模型點(diǎn)云配準(zhǔn)的那塊點(diǎn)云是目標(biāo)物體。

該框架有兩個(gè)輸入，一是場景點(diǎn)云，二是CAD模型點(diǎn)云庫，由很多常用的零件模型和它們的點(diǎn)云構(gòu)成，在離線階段建好。為了在場景點(diǎn)云中找到目標(biāo)模型，本文從模型點(diǎn)云庫中取出一個(gè)模型的點(diǎn)云，與場景點(diǎn)云匹配。如果模型的點(diǎn)和場景某一區(qū)域中的點(diǎn)的匹配誤差小于閾值，則認(rèn)為匹配成功由此識別出場景中出現(xiàn)的目標(biāo)物體。如果匹配的誤差較大，則認(rèn)為CAD模型點(diǎn)云不能在場景點(diǎn)云中找到對應(yīng)，則認(rèn)為該物體在場景不存在，從CAD模型點(diǎn)云庫中提取另一個(gè)點(diǎn)云進(jìn)行新一輪的配準(zhǔn)和驗(yàn)證。在識別成功之后，得到CAD模型點(diǎn)云在場景中的一個(gè)初始姿態(tài)。為了計(jì)算出物體在場景中的精確6自由度姿態(tài)，通過ICP迭代算法對模型點(diǎn)云和場景點(diǎn)云進(jìn)行進(jìn)一步匹配，如果匹配誤差小于閾值則認(rèn)為姿態(tài)估算完成，場景中的物體被成功識別和姿態(tài)求解，把該物體的CAD模型疊加在場景對應(yīng)位置上。由于虛擬模型在虛擬空間的姿態(tài)己知，通過一個(gè)虛實(shí)之間的關(guān)系轉(zhuǎn)換，可以求得該目標(biāo)模型在虛擬空間的姿態(tài)。

1.2匹配問題

已知同一物體經(jīng)過不同角度掃描或者由不同的生成方法得到的帶有任意不同初始姿勢的兩個(gè)三維點(diǎn)云數(shù)據(jù)P和Q，則配準(zhǔn)的基本問題是尋找一個(gè)最佳的變換，包括旋轉(zhuǎn)、平移和縮放，使得兩個(gè)點(diǎn)云重合在一起的，其差異應(yīng)該小于一定的閾值。

如果從P和Q中選取不同的基集，計(jì)算相應(yīng)的剛性變換，那么一對分別含有三個(gè)點(diǎn)的集合，一個(gè)在P上，另一個(gè)在Q上，就足夠唯一定義一個(gè)剛性變換了，描述如下：

pp，j∈P，pq，j∈Q

(1)

要計(jì)算從點(diǎn)云P中點(diǎn)轉(zhuǎn)換到點(diǎn)云Q中的點(diǎn)對應(yīng)的剛體變換關(guān)系，即

果農(nóng)們也來了，搬走了幾箱子除蟲劑；大戶來生騎著電驢子，愣是在帶貨架上塞滿了多個(gè)品種秋播下地的種子、農(nóng)藥、肥料……

?i={1，…n}pq，j= rotP→Q(pp，j)+tranP→Q+noise

由于點(diǎn)云的空間點(diǎn)很多，按照窮盡搜索的方法來找到這個(gè)三點(diǎn)對應(yīng)顯然不太可能。為此，很多研究者對參與配準(zhǔn)的點(diǎn)云進(jìn)行分析，希望能找一種通用的描述方法，能夠在相似的點(diǎn)云中建立對應(yīng)關(guān)系，降低計(jì)算復(fù)雜度，把兩者的轉(zhuǎn)換關(guān)系計(jì)算出來。為場景和模型點(diǎn)云構(gòu)建簡化的描述基集，在描述基集中找到對應(yīng)關(guān)系，再映射到全局點(diǎn)云，由此計(jì)算模型到場景的6自由度的空間轉(zhuǎn)換矩陣。

為了得到被測的場景物體的姿態(tài)，把虛擬的CAD模型轉(zhuǎn)換成點(diǎn)云，在場景坐標(biāo)系統(tǒng)中，把變換后的CAD模型點(diǎn)云與場景物體點(diǎn)云匹配，通過匹配，識別場景出現(xiàn)的物體以及它們的6自由度姿態(tài)。為了降低參與運(yùn)算的數(shù)據(jù)量，對輸入的兩個(gè)點(diǎn)云提取描述基集，獲得各自的描述基集，采用Ransac的方法匹配對基集進(jìn)行迭代匹配，去除一些錯(cuò)誤的矢量對應(yīng)，估計(jì)兩者的剛體轉(zhuǎn)換關(guān)系，接著再把這個(gè)轉(zhuǎn)換關(guān)系應(yīng)到全局點(diǎn)云，如果兩者的匹配誤差小于一定閾值，就認(rèn)為這個(gè)匹配成功。

2 密度自適應(yīng)的共面四點(diǎn)算法

2.1點(diǎn)云描述基集的提取

點(diǎn)云的描述基集有很多種，其中最常用的是與法向量相關(guān)的描述基集，例如一些對局部或者全局形狀進(jìn)行描述的算子SHOT、FPFH、VFH等[1]。這些描述基集用于噪聲干擾較小的場合時(shí)，可以大幅度提高點(diǎn)云之間的配準(zhǔn)成功率和效率。但以SFM重構(gòu)的點(diǎn)云有很多噪聲，使得點(diǎn)云表示的表面特征不全，即使經(jīng)過濾波處理和增強(qiáng)，和真實(shí)場景的表面相比，還是有很大差距。尤其經(jīng)過濾波之后，點(diǎn)云的密度又下降了許多，所以很難提取點(diǎn)云表面的描述矢量。或者提取到的描述向量無法反應(yīng)點(diǎn)云的特征，導(dǎo)致錯(cuò)誤匹配。

為此采用一種共面四點(diǎn)基集，這種方法有較強(qiáng)抗噪性能，最初用于處理不完整點(diǎn)云之間的縫合。假設(shè)如果可以找到特殊的四點(diǎn)基集，以代替三點(diǎn)基集，則配準(zhǔn)的精度會(huì)更高，效果會(huì)更好，尤其有噪聲的情況下。從點(diǎn)云P上提取一個(gè)不在同一直線的共面點(diǎn){a，b，c，d}，從Q上提取的四個(gè)共類似面點(diǎn){a′，b′，c′，d′}，如果這兩對共面點(diǎn)是對應(yīng)的，那么這兩對共面點(diǎn)滿足仿射不變，也就是說這兩個(gè)四點(diǎn)集合經(jīng)過仿射變換后，可以完全吻合。

對于不在同一直線的共面點(diǎn)集{a，b，c，d}，設(shè)線段ab和線段cd相交于e點(diǎn)，定義了兩個(gè)比率：

r1和r2在仿射變換下是不變的，并且可以唯一地確定仿射變換中的4點(diǎn)。現(xiàn)在，給定一個(gè)含有n個(gè)點(diǎn)的集合Q，和兩個(gè)仿射不變比率r1和r2，提取所有由這兩個(gè)不變量確定的4點(diǎn)集合，對于每一對點(diǎn)：{a′，b′，c′，d′}∈Q，計(jì)算下面兩個(gè)相交點(diǎn)：

e1=a′-r1(b′-a′)

e2=c′-r1(d′-c′)

2.2密度自適應(yīng)匹配算法

雖然共面四點(diǎn)基集不需要計(jì)算和提取法向量，但應(yīng)用過程中要求參與配準(zhǔn)的點(diǎn)云的重疊部分的密度基本相同。本文的場景點(diǎn)云來自SFM，而虛擬模型由虛擬掃描儀生成，兩者的來源不同，因此不能做點(diǎn)云密度相同的假設(shè)。針對這些問題，采取密度自適應(yīng)方法，把兩個(gè)點(diǎn)云的下采樣處理作為算法的一個(gè)步驟，采用由稀疏到稠密的配準(zhǔn)策略，多層次的采樣處理除了增加兩者的相似度，還能減少點(diǎn)云的數(shù)據(jù)量，提高算法的效率。

2.3基于體素點(diǎn)權(quán)重的點(diǎn)云下采樣

在實(shí)際配準(zhǔn)過程中，為了提高共面四點(diǎn)基集的查找效率，一般采用縮小搜索范圍的策略，直接在兩個(gè)點(diǎn)云的重疊部分進(jìn)行搜索是最合理的選擇。但由于重疊部分也是未知，還需要專門的分析和計(jì)算來確定這一塊區(qū)域。有的研究者為縮小搜索范圍而做了一些改變，比如在噪聲很小的點(diǎn)云上，利用局部描述向量來描述點(diǎn)云，再在描述向量集合的基礎(chǔ)上提取進(jìn)行共面四點(diǎn)基集，再進(jìn)行點(diǎn)云間的配準(zhǔn)。但是，搜索的范圍還是很廣，算法經(jīng)常得到局部優(yōu)化而不是全局最優(yōu)，另外效率也很低。為了提高點(diǎn)云之間的配準(zhǔn)效率，有很多解決方法被提出。但其中最重要的是對點(diǎn)云進(jìn)行下采樣，通過降低點(diǎn)云密度，從而降低了描述基集生成數(shù)量，從而縮小了查找對應(yīng)基集的搜索范圍。在提取共面四點(diǎn)基集之前，將點(diǎn)云密度進(jìn)行處理，目的是讓參與配準(zhǔn)的點(diǎn)云密度基本相同。

常規(guī)的方法主要有三維體素下采樣法，為輸入的點(diǎn)云數(shù)據(jù)創(chuàng)建一個(gè)包圍盒，再把包圍盒細(xì)分成很多微小的三維立方體包圍盒，最后對體素內(nèi)的點(diǎn)進(jìn)行簡化處理。對在三維立方體包圍盒內(nèi)的點(diǎn)，不論形狀和分布都以包圍盒中心近似表示，這樣該包圍盒內(nèi)所有點(diǎn)就縮減成一個(gè)點(diǎn)表示，大幅度降低了點(diǎn)云的數(shù)據(jù)量。該方法下采樣的效率很高，但一般用于點(diǎn)云無噪聲的情況下，有噪聲條件下很少使用。

為了既能減少點(diǎn)云數(shù)據(jù)，又能保持點(diǎn)云的表面特征，有研究人員使用體素內(nèi)所有點(diǎn)的重心來近似表示體素中其他點(diǎn)，雖然結(jié)果和體素中心法一樣，也就是體素內(nèi)所有點(diǎn)簡化成一個(gè)點(diǎn)。但這種方法利用了點(diǎn)云的數(shù)據(jù)聚集特點(diǎn)，在有噪聲的條件下，得到的點(diǎn)云結(jié)果比體素中心的下采樣法得到的點(diǎn)云結(jié)果更真實(shí)，表達(dá)的表面輪廓更為清晰。

3 模型點(diǎn)云庫構(gòu)建與試驗(yàn)

3.1模型點(diǎn)云庫構(gòu)建

本節(jié)構(gòu)造的CAD模型數(shù)據(jù)庫包括常用零部件的CAD模型和它們的點(diǎn)云。首先要對零部件進(jìn)行3D建模，很多產(chǎn)品在設(shè)計(jì)階段，3D模型已經(jīng)被建好，也可以用一些建模軟件構(gòu)建。有了CAD模型，就可以利用虛擬3D掃描儀，根據(jù)點(diǎn)云的密度以及噪聲要求，把CAD模型轉(zhuǎn)換成為基本上沒有噪聲的點(diǎn)云。虛擬3D掃描儀類似真實(shí)的掃描儀，可在CAD模型的虛擬坐標(biāo)系中按規(guī)劃路徑行走，對目標(biāo)模型進(jìn)行掃描，生成模型的表面點(diǎn)云。

3.2 3D識別與配準(zhǔn)試驗(yàn)

3D識別與配準(zhǔn)實(shí)驗(yàn)分為兩個(gè)部分：一是仿真實(shí)驗(yàn)，另一個(gè)是真實(shí)場景的實(shí)驗(yàn)。為了驗(yàn)證本文算法的可行性，首先在沒有噪聲的虛擬數(shù)據(jù)上測試算法。實(shí)驗(yàn)采用臺(tái)式機(jī)，Solidworks建模，Point cloud library進(jìn)行點(diǎn)云可視化處理。

從CAD模型點(diǎn)云庫中任選一個(gè)零件的CAD模型和點(diǎn)云。為了模擬真實(shí)場景中零件的擺放狀態(tài)，重新對CAD模型進(jìn)行處理，只對零件CAD模型的局部表面進(jìn)行掃描，被遮擋部分不進(jìn)行處理，生成的點(diǎn)云只反映的零件的部分表面結(jié)構(gòu)特征。對零件點(diǎn)云(以P表示)做一個(gè)姿態(tài)的調(diào)整，使之與Q有一個(gè)位置和姿態(tài)的差異。參與配準(zhǔn)的兩個(gè)點(diǎn)云沒有任何噪聲，能準(zhǔn)確反映虛擬模型表面特征。接下來，向零件局部點(diǎn)云Q添加的高斯噪聲以模擬真實(shí)的場景，點(diǎn)云P不做改變再進(jìn)行新的匹配。在接近真實(shí)噪聲的條件下，零件點(diǎn)云P也能在點(diǎn)云空間中找到對應(yīng)的局部點(diǎn)云Q。

用SFM方法重構(gòu)了一個(gè)裝配場景，為了提高配準(zhǔn)效率和精確度，場景點(diǎn)云經(jīng)過降噪和下采樣。場景點(diǎn)云經(jīng)過降噪處理后，仍然有許多噪聲點(diǎn)和場景中物體表面真實(shí)的點(diǎn)混在一起。這些噪聲有的是來自場景中的背景，這部分噪聲是由背景的紋理、明暗差異產(chǎn)生的：另一部分的噪聲來自前景的物體，有的是由于光照或者遮擋原因產(chǎn)生的，有的是因?yàn)閿z像機(jī)的鏡頭畸變導(dǎo)致計(jì)算錯(cuò)誤而產(chǎn)生的。CAD模型點(diǎn)云庫中的點(diǎn)云是干凈的沒有噪聲，能精確反映零部件的表面特征。為了識別裝配場景中的真實(shí)工具和它們的姿態(tài)，每次識別都從虛擬的CAD模型點(diǎn)云庫中取出一個(gè)零件點(diǎn)云。

4 結(jié)語

本文提出一種對裝配場景點(diǎn)云進(jìn)行感知的方法。以點(diǎn)云數(shù)據(jù)為對象，通過CAD模型點(diǎn)云與場景點(diǎn)云的匹配來對場景中出現(xiàn)的部件進(jìn)行識別，再利用ICP配準(zhǔn)算法獲取該部件在場景中的6自由度姿態(tài)。提出一個(gè)基于密度自適應(yīng)的匹配算法，自適應(yīng)調(diào)整不同來源的點(diǎn)云密度，降低點(diǎn)云共面四點(diǎn)基集的搜素范圍，利用基于體素點(diǎn)權(quán)重的點(diǎn)云下采樣方法降低所需匹配的點(diǎn)數(shù)，提高匹配速度。在對裝配場景中出現(xiàn)的物體進(jìn)行匹配的實(shí)驗(yàn)中，本方法成功識別裝配場景中多種常見的工具和零件。實(shí)驗(yàn)采用兩種下采樣方法對點(diǎn)云進(jìn)行下采樣，而采用基于體素點(diǎn)權(quán)重的下采樣方法的。點(diǎn)云配準(zhǔn)成功率比利用體素重心法下采樣的點(diǎn)云配準(zhǔn)成功率提升了15%。

[1] ALDOMA A, MARTON Z C, TOMBARI F, et al. Point cloud library[J]. IEEE Robotics& Automation Magazine, 2012, 19(3): 80-91.

[2]PARK Y, LEPETIT V,WOO W. ESM-Blur: Handling & rendering blur in 3D trackingand augmentation[C]// Proceedings of the IEEE International Symposium onMixed and Augmented Reality(ISMAR). IEEE, 2009: 163-166.

[3]STRASDAT H, MONTIEL J M M, DAVISON A J. Real-time monocular SLAM: Why filter?[J]. Proceedings of the IEEE International Conference on Robotics andAutomation(ICRA). 2010: 2657-2664.

[4]WHELAN T, KAESS M, JOHANNSSON H, et al. Real-time large-scale dense RGB-DSLAM with volumetric fusion[J]. The International Journal of Robotics Research, 2015, 34(4/5): 598-626.

(本文編輯：楊林青)

Research on Component Recognition and Attitude Estimation Method Based on Point Cloud

WU Haoran1, YU Hai2, PENG Lin2, LI Chunlong2

(1.State Grid Wuxi power supply company, Jiangsu Wuxi 210096, China; 2. Global energy Internet Research Institute, Nanjing 210003, China)

To identify and understand real object objects in an assembly environment, a registration based 3D perception method is proposed. To reduce the influence of noise on registration results, a density based adaptive matching algorithm is proposed to perceive real objects in a scene using the similarity of point cloud. The algorithm can adaptively adjust the density of point clouds from different sources, and realize the recognition of real object by affine invariance of coplanar four point feature. This method can reduce the amount of cloud data and the computational complexity, and also improve the matching accuracy. The experimental results show that the proposed method can identify components accurately in the assembly environment point cloud and calculate their pose. It can be used for augmented reality interactive applications under noisy conditions.

augmented reality; scene reconstruction; augmented reality assembly

10.11973/dlyny201704016

吳浩然(1964—)，男，高級工程師，總經(jīng)理，從事電力系統(tǒng)運(yùn)行檢修管理工作。

TP391

：A

：2095-1256(2017)04-0431-04

2017-06-03