基于點(diǎn)云配準(zhǔn)的曲面三維差異檢測(cè)*

羅錦鴻，陳新度，吳 磊

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

0 引言

在視覺引導(dǎo)機(jī)器人的智能化自動(dòng)化加工任務(wù)中，工件的形狀誤差分析和三維差異數(shù)據(jù)提取是一個(gè)關(guān)鍵研究問題。本文針對(duì)陶瓷素胚工件的機(jī)器人自動(dòng)化打磨任務(wù)，需要對(duì)陶瓷素胚工件的部分表面進(jìn)行差異比對(duì)以識(shí)別定位加工區(qū)域。為了獲取待加工工件的點(diǎn)云數(shù)據(jù)，如圖1a所示，利用了三維掃描儀來掃描待加工工件的表面，如圖1b所示，有標(biāo)準(zhǔn)模型曲面點(diǎn)云(白色)和通過掃描得到的待檢測(cè)曲面點(diǎn)云(藍(lán)色)。

近年來，研究人員通過對(duì)三維視覺方法獲取的實(shí)測(cè)曲面的三維數(shù)據(jù)進(jìn)行分析，來實(shí)現(xiàn)曲面差異檢測(cè)，而使用點(diǎn)云配準(zhǔn)的方法來讓標(biāo)準(zhǔn)模型數(shù)據(jù)與實(shí)測(cè)數(shù)據(jù)對(duì)齊用來實(shí)現(xiàn)差異檢測(cè)逐漸成為趨勢(shì)，比如李宇萌等[1]采用了ICP精確配準(zhǔn)的方法進(jìn)行模型與實(shí)測(cè)數(shù)據(jù)對(duì)齊，在102 s內(nèi)將2萬左右點(diǎn)云的精確對(duì)齊，實(shí)現(xiàn)了轉(zhuǎn)子表面的缺陷檢測(cè)。馬維康等[2]通過手持式智能激光掃描儀對(duì)試驗(yàn)件不同工序段進(jìn)行掃描測(cè)量,將掃描的三維點(diǎn)云數(shù)據(jù)與設(shè)計(jì)的理論數(shù)模進(jìn)行對(duì)比分析,得出型面精度的偏差情況,以此來檢驗(yàn)試驗(yàn)件是否合格。

但是前人的方法大部分是針對(duì)全局配準(zhǔn)或者位姿容易確定的情形，可以使用ICP算法直接進(jìn)行數(shù)據(jù)對(duì)齊。在面向待配準(zhǔn)點(diǎn)云數(shù)據(jù)是模型的局部實(shí)測(cè)數(shù)據(jù)的情形時(shí)，這些方法并不適用的。本文針對(duì)這種局部曲面配準(zhǔn)情況，測(cè)試了兩種基于局部特征的配準(zhǔn)方法，實(shí)現(xiàn)了快速粗配準(zhǔn)。在精確配準(zhǔn)階段，由于ICP配準(zhǔn)算法在面對(duì)大型點(diǎn)云數(shù)據(jù)時(shí)耗時(shí)長(zhǎng)，滿足不了本文的實(shí)時(shí)性的要求。本文實(shí)現(xiàn)了一種由CUDA加速的ICP算法，實(shí)現(xiàn)了大型曲面點(diǎn)云的快速對(duì)齊。而且在曲面對(duì)齊后，通過本文設(shè)計(jì)的引入Octree預(yù)檢測(cè)的差異提取方法可以快速地提取曲面的三維差異。

通過本文設(shè)計(jì)的實(shí)驗(yàn)證明，在面對(duì)要求實(shí)時(shí)性比對(duì)檢測(cè)目標(biāo)曲面與標(biāo)準(zhǔn)模型曲面的任務(wù)時(shí)，本文的方法具有精度高、可靠、快速的特點(diǎn)。

(a) 得到工件單幀掃描點(diǎn)云 (b) 掃描點(diǎn)云(藍(lán))和模型點(diǎn)云(白)

1 基于Hough投票的粗配準(zhǔn)

點(diǎn)云配準(zhǔn)是指利用兩幅點(diǎn)云的匹配點(diǎn)，利用匹配關(guān)系來估算兩幅點(diǎn)云的旋轉(zhuǎn)平移關(guān)系，從而使兩幅點(diǎn)云正確地重合在一起。首先要解決的是精確配準(zhǔn)的初值問題，即解決粗配準(zhǔn)問題。

點(diǎn)云的粗配準(zhǔn)方法有很多，研究人員針對(duì)工件點(diǎn)云的粗配準(zhǔn)問題上進(jìn)行過很多研究，比如陳巍等[3]、周欣康[4]和項(xiàng)輝宇等[5]分別在校正葉片工件的裝夾誤差研究中、航空葉片型面檢測(cè)中以及板金屬?zèng)_壓零件表面模型與實(shí)測(cè)點(diǎn)云配準(zhǔn)研究中，都使用了PCA(主成分分析)的全局配準(zhǔn)算法，對(duì)實(shí)測(cè)點(diǎn)云與模型點(diǎn)云進(jìn)行粗對(duì)齊。但是由于PCA算法要求實(shí)測(cè)點(diǎn)云形狀與模型相近，這就需要在獲取數(shù)據(jù)時(shí)，完整地掃描工件。

本文要對(duì)工件的待加工處的部分掃描點(diǎn)云與整體模型點(diǎn)云進(jìn)行配準(zhǔn)，顯然PCA全局配準(zhǔn)方法不合適，文章考慮了基于局部特征的粗配準(zhǔn)算法，基于局部特征的配準(zhǔn)方法主要有SAC-IA[6](Sample Consensus Initial Alignment)、RANSAC[7]和三維霍夫投票[8](Hough voting)。本文分別對(duì)三維霍夫投票法和SAC-IA兩種算法進(jìn)行了測(cè)試。

本文總算法流程如圖2所示。有模型點(diǎn)云M={m1,…,mi}和通過實(shí)測(cè)掃描點(diǎn)云S={s1,…,sj}，分別對(duì)M和S進(jìn)行濾波得到稀疏的下采樣點(diǎn)云M′={m1′,…,mk′ }和S′={s1′,…,sl′ }。計(jì)算M′和S′中的每個(gè)點(diǎn)的局部特征描述子F(M′)={fm1′,…,fmk′ }，F(xiàn)(S′)={fs1′,…,fsl′}和局部參考坐標(biāo)系LRF(M′)={rm1′,…,rmk′ }，F(xiàn)RF(S′)={rs1′,…,rsl′}。使用Hough投票法或采樣一致性初始對(duì)齊(SAC-IA)算法得到粗對(duì)齊矩陣T1，使用T1變換S中的每個(gè)點(diǎn)，得到粗對(duì)齊點(diǎn)云S_initial，然后用CUDA加速的ICP算法完成S_initial與M的精確對(duì)齊，得到最終配準(zhǔn)完成的掃描點(diǎn)云S_dest，在提取差異點(diǎn)云步驟中，遍歷S_dest點(diǎn)云中的所有點(diǎn)，當(dāng)查詢點(diǎn)與其在M中最近的點(diǎn)之間的距離大于設(shè)定閾值時(shí)，將該查詢點(diǎn)看作是差異點(diǎn)，加入到差異點(diǎn)集合S_d，遍歷完成后，最終的集合S_d是差異點(diǎn)云。

圖2 三維差異檢測(cè)算法流程圖

1.1 體素柵格采樣

體素柵格采樣是點(diǎn)云數(shù)據(jù)精簡(jiǎn)的一種非常實(shí)用的方法，它首先創(chuàng)建一個(gè)三維的體素柵格，輸入點(diǎn)云將包含在柵格中的三維立方體內(nèi)，然后用三維立方體中所有的點(diǎn)計(jì)算平均值得到重心點(diǎn)，用來近似表示體素中其他點(diǎn)，統(tǒng)計(jì)重心點(diǎn)集合將得到下采樣點(diǎn)云。

實(shí)驗(yàn)表明，對(duì)于以本文中的工件為代表的典型平滑曲面點(diǎn)云，由于點(diǎn)云密集以及曲面光滑，三維局部特征十分相似，如果直接使用原始數(shù)據(jù)進(jìn)行粗配準(zhǔn)會(huì)導(dǎo)致計(jì)算量非常大而且失敗。于是本文采用了柵格濾波算法對(duì)模型點(diǎn)云M和掃描點(diǎn)云S進(jìn)行濾波。數(shù)據(jù)量在210 000左右的模型點(diǎn)云和數(shù)據(jù)量在34 000左右掃描點(diǎn)云下柵格濾波后的數(shù)據(jù)量為8 200和1 300左右。輸入點(diǎn)云柵格下采樣后的點(diǎn)云如圖3所示。點(diǎn)云濾波后，在空間分布上具有規(guī)律性，在計(jì)算三維局部特征時(shí)，通過采用柵格濾波后的點(diǎn)作為關(guān)鍵點(diǎn)將有效地去冗余點(diǎn)云，以及減少了計(jì)算特征描述子要考慮的近鄰點(diǎn)數(shù)，加快了粗配準(zhǔn)的速度以及成功率。

圖3 柵格采樣后的模型點(diǎn)云和掃描點(diǎn)云的狀態(tài)

1.2 3D-Hough投票

3D-Hough[8]中算法具體步驟如下：

(1)輸入模型的下采樣曲面點(diǎn)云M′={m1′,…,mk′ }和下采樣掃描點(diǎn)云S′={s1′,…,sl′}。

(2)計(jì)算M′和S′中的每個(gè)點(diǎn)的局部三維特征描述子F(M′)={fm1′,…,fmk′ }，F(xiàn)(S′)={fs1′,…,fsl′}和局部參考坐標(biāo)系LRF(M′)={rm1′,…,rmk′ }，F(xiàn)RF(S′)={rs1′,…,rsl′}。

(3)在本實(shí)驗(yàn)中，將重心點(diǎn)作為Hough投票的參考點(diǎn)。于是先計(jì)算M′的重心點(diǎn)c_m，再計(jì)算M′中每個(gè)點(diǎn)到重心點(diǎn)c_m的向量vmk=c_m-mk′，得到向量集合V_M={vm1,…,vmk}。

(4)通過局部三維特征描述子歐式距離作為匹配度來計(jì)算大小為n的匹配集。

(5)遍歷匹配集合，匹配上的對(duì)應(yīng)點(diǎn)mn和sn對(duì)應(yīng)的兩個(gè)參考坐標(biāo)系：rmk′到rsl′之間有變換矩陣Tn，于是有Tn*vmn=vsn，即估計(jì)的掃描點(diǎn)云的重心點(diǎn)為csn=Tn*vmn+sl′。

(6)通過步驟(5)的計(jì)算就會(huì)得到一個(gè)重心點(diǎn)集合C_S={cs1,…,csn}。集合C_S即是三維Hough空間中的投票。

(7)為集合C_S設(shè)置區(qū)間步長(zhǎng)劃分投票空間，當(dāng)投票數(shù)超過一定時(shí)，選取投票數(shù)最大的區(qū)間，該峰值區(qū)間中的所有投票重心求平均值即為掃描點(diǎn)云的參考點(diǎn)(掃描點(diǎn)云重心)。

(8)通過對(duì)峰值區(qū)間篩選的對(duì)應(yīng)關(guān)系，可以計(jì)算出模型點(diǎn)云與掃描點(diǎn)云之間的旋轉(zhuǎn)平移變換矩陣。

本文分別對(duì)6組在不同的位置拍攝的單幀掃描數(shù)據(jù)進(jìn)行了測(cè)試，粗配準(zhǔn)后狀態(tài)如圖4所示。

圖4 粗配準(zhǔn)后的模型點(diǎn)云和掃描點(diǎn)云的狀態(tài)

如表1所示，在同等效果的情況下，使用基于局部特征的的3D-Hough投票算法更快，平均運(yùn)行時(shí)間在0.3 s以內(nèi)。這是由于SAC-IA方法是在匹配點(diǎn)集中不斷抽取必要數(shù)目的匹配點(diǎn)去估算變換矩陣，所以要進(jìn)行多次抽取來選取最小誤差的估算變換參數(shù)，而3D-Hough投票算法，通過找出峰值的方式來剔除錯(cuò)誤匹配點(diǎn)對(duì)，一次估計(jì)就可以求解出變換矩陣，所以速度更快。

表1 測(cè)試SAC-IA和Hough投票法

續(xù)表

2 CUDA加速的點(diǎn)云精確配準(zhǔn)

2.1 ICP

在點(diǎn)云精確對(duì)齊領(lǐng)域，Besl P J等[9]提出了經(jīng)典的ICP(Iterative Closest Point)配準(zhǔn)算法，通過迭代最近點(diǎn)的方法實(shí)現(xiàn)了兩個(gè)點(diǎn)云的精確配準(zhǔn)。通過上面的粗配準(zhǔn)步驟后，再通過ICP算法進(jìn)行精確配準(zhǔn)可以使得兩個(gè)點(diǎn)云精確對(duì)齊。

ICP算法基本原理是：分別在待配準(zhǔn)的目標(biāo)點(diǎn)云P和源點(diǎn)云Q中，按照一定的約束條件比如鄰近關(guān)系，找到對(duì)應(yīng)點(diǎn)對(duì)最鄰近點(diǎn)對(duì)，構(gòu)建基于距離的誤差函數(shù)式(1)。不斷迭代計(jì)算出最優(yōu)匹配參數(shù)R和t，使得誤差函數(shù)最小，直至達(dá)到滿意的誤差要求，一般使用SVD或者非線性優(yōu)化方法求解。

(1)

2.2 CUDA-ICP

直到最近，幾乎所有的視覺實(shí)現(xiàn)都使用了GPU或FPGA等方法進(jìn)行加速[10-11]。由于本文模型以及點(diǎn)云點(diǎn)數(shù)較多，而且面對(duì)曲率變化小，特征不明顯的點(diǎn)云下采樣后在精確配準(zhǔn)時(shí)會(huì)損失精度甚至可能配準(zhǔn)失敗。本文利用了GPU的超高性能計(jì)算能力，以及CUDA架構(gòu)的簡(jiǎn)易開發(fā)性，編寫了CUDA程序來加速ICP算法，以實(shí)現(xiàn)實(shí)時(shí)配準(zhǔn)。

在實(shí)驗(yàn)中，首先利用了K-D tree(k-dimensional樹)數(shù)據(jù)結(jié)構(gòu)實(shí)現(xiàn)了快速的最近點(diǎn)搜索，對(duì)一個(gè)模型點(diǎn)云構(gòu)建一次K-D tree可以用于多次迭代中的最近鄰搜索。其次，通過在對(duì)應(yīng)估計(jì)的步驟中利用多線程來同時(shí)計(jì)算搜索近鄰。再者，在剛體變換掃描點(diǎn)云S步驟中，開啟多線程同時(shí)對(duì)每個(gè)點(diǎn)進(jìn)行矩陣運(yùn)算。最后，在需要規(guī)約計(jì)算的地方使用CUDA提供的Thrust庫的接口thrust::reduce實(shí)現(xiàn)快速規(guī)約運(yùn)算，這些方法都極大地加速了配準(zhǔn)過程。

圖5是本文中的CUDA-ICP算法計(jì)算和優(yōu)化的流程圖以及詳細(xì)步驟。

圖5 CUDA-ICP算法流程圖

(1)初始化：輸入M={m1,…,mm}和經(jīng)過粗對(duì)齊后的掃描點(diǎn)云S={s1,…,sn}。輸入收斂閾值τ和連續(xù)收斂次數(shù)閾值t以及最大近鄰閾值d；

(2)對(duì)模型點(diǎn)云M構(gòu)建K-D tree，用于最近鄰查詢；

(3)開啟大于n數(shù)量的線程并行：通過已經(jīng)構(gòu)建好的K-D tree實(shí)現(xiàn)最近鄰搜索，尋找到S中每個(gè)點(diǎn)sn在M中的最近點(diǎn)mm，當(dāng)對(duì)應(yīng)點(diǎn)之間的距離小于d時(shí)加入對(duì)應(yīng)點(diǎn)集合。最終得到對(duì)應(yīng)點(diǎn)集合M′和S′；

(4)使用thrust庫進(jìn)行規(guī)約：計(jì)算集合M′和S′之間的誤差et；

(5)計(jì)算迭代誤差前后變化Δe=|et-et-1|，當(dāng)Δe大于設(shè)定收斂閾值τ，則繼續(xù)執(zhí)行。當(dāng)Δe小于τ時(shí)則認(rèn)為本次迭代收斂，若連續(xù)收斂的次數(shù)大于設(shè)定的次數(shù)閾值t，就結(jié)束計(jì)算，本實(shí)驗(yàn)中t=5；

(6)使用thrust庫進(jìn)行規(guī)約：計(jì)算M′和S′的重心點(diǎn)M_c和S_c；

(7)開啟大于n數(shù)量的線程并行：計(jì)算去重心點(diǎn)集合M_2和S_2；

(8)開啟大于n數(shù)量的線程并行：計(jì)算去重心點(diǎn)集合M_2和S_2之間的協(xié)方差矩陣集合W；

(9)使用thrust庫進(jìn)行規(guī)約：對(duì)集合W進(jìn)行規(guī)約得到協(xié)方差矩陣w。使用SVD分解w得到本次迭代的變換矩陣T；

(10)開啟大于n數(shù)量的線程并行：使用變換矩陣T來旋轉(zhuǎn)平移掃描點(diǎn)集合S中的每個(gè)點(diǎn)，變換后的掃描點(diǎn)集合作為S跳到步驟(3)；

(11)結(jié)束計(jì)算。

表2 測(cè)試CUDA-ICP

續(xù)表

本文對(duì)6組實(shí)驗(yàn)數(shù)據(jù)進(jìn)行了測(cè)試，如表2結(jié)果所示，本文改進(jìn)的CUDA-ICP算法將原本運(yùn)行時(shí)間在7 s左右的基于K-D tree的標(biāo)準(zhǔn)ICP配準(zhǔn)算法加速到了0.5 s左右。本文配準(zhǔn)精度控制在RMS誤差小于0.5 mm，每次迭代時(shí)間控制在配準(zhǔn)耗時(shí)控制在1.5 ms以內(nèi)，滿足本文中的三維差異定位任務(wù)的準(zhǔn)確性和實(shí)時(shí)性的要求。

3 三維差異提取

本文認(rèn)為掃描點(diǎn)云S_dest={sd1,…,sdj}的當(dāng)前查詢點(diǎn)與其在模型點(diǎn)云M={m1,…,mi}上的最近點(diǎn)的距離大于某個(gè)設(shè)定的距離閾值時(shí)，就認(rèn)為該查詢點(diǎn)是差異點(diǎn)。這樣遍歷算法的時(shí)間復(fù)雜度為O(i*j)。通過八叉樹(Octree)數(shù)據(jù)結(jié)構(gòu)實(shí)現(xiàn)了三維差異的快速提取。

如圖6所示，八叉樹結(jié)構(gòu)通過對(duì)三維空間的集合實(shí)體進(jìn)行體元剖分，每個(gè)體元具有相同的時(shí)間和空間復(fù)雜度，通過循環(huán)遞歸的劃分方法對(duì)大小為2n×2n×2n的三維空間的幾何對(duì)象進(jìn)行剖分，從而構(gòu)成一個(gè)具有根節(jié)點(diǎn)的方向圖。通過遞歸地比較模型點(diǎn)云構(gòu)建的Octree的樹結(jié)構(gòu)和掃描點(diǎn)云構(gòu)建的Octree結(jié)構(gòu)，可以對(duì)比得到不同的差異節(jié)點(diǎn)，通過記錄新增節(jié)點(diǎn)和缺少節(jié)點(diǎn)并記錄其對(duì)應(yīng)點(diǎn)云，從而實(shí)現(xiàn)檢測(cè)差異。

本文通過Octree節(jié)點(diǎn)預(yù)先得到存在差異點(diǎn)云的體素，再通過有差異點(diǎn)的體素及其周圍體素中的點(diǎn)云對(duì)標(biāo)準(zhǔn)模型點(diǎn)云進(jìn)行近鄰搜索，得到最終的差異點(diǎn)云。Octree數(shù)據(jù)結(jié)構(gòu)示意圖如圖6所示。

圖6 Octree數(shù)據(jù)結(jié)構(gòu)示意圖

(1)設(shè)置距離閾值d_min，則Octree的最小子節(jié)點(diǎn)步長(zhǎng)度為V=d_min，輸入模型點(diǎn)云M構(gòu)建Octree，輸入掃描點(diǎn)云S_dest構(gòu)建Octree；

(2)通過比對(duì)兩個(gè)Octree得到新增的Octree節(jié)點(diǎn)，提取新增節(jié)點(diǎn)及其鄰近節(jié)點(diǎn)處的點(diǎn)云?？梢钥焖俚玫匠醪讲町慄c(diǎn)云S_d_initia={s1″,…,sn″}，如圖7a所示；

(3)遍歷S_d_initia，查詢點(diǎn)sq″與在模型點(diǎn)云M上的最近點(diǎn)mq之間的距離為dq=distance(mq,sq″)，當(dāng)dq>d_min時(shí)認(rèn)為當(dāng)前查詢點(diǎn)sq″是差異點(diǎn)，將該點(diǎn)加入到差異點(diǎn)集合S_d中。如圖7b～圖7d所示；

(4)輸出差異點(diǎn)云S_d，如圖7d所示。

通過上述計(jì)算，可以正確獲得需要提取的差異點(diǎn)云，差異提取步驟在CPU端中運(yùn)行時(shí)間為0.15 s。使用CUDA加速后耗時(shí)在1 ms以內(nèi)。

(a) 初步差異點(diǎn)云S_d_initia (b) 差異點(diǎn)云S_d距離示意圖

(c) 最終差異點(diǎn)云S_d(紅) (d) 點(diǎn)云S_d細(xì)節(jié)圖7 差異點(diǎn)云提取

4 結(jié)束語

本文針對(duì)初始位姿不確定的平滑點(diǎn)云數(shù)據(jù)的差異提取問題進(jìn)行了研究，利用曲面配準(zhǔn)，以及近鄰搜索等方法實(shí)現(xiàn)了三維差異檢測(cè)。而且針對(duì)算法運(yùn)行速度問題，測(cè)試了SAC-IA與3D-Hough投票兩種點(diǎn)云的粗配準(zhǔn)方法，使用了Octree數(shù)據(jù)結(jié)構(gòu)實(shí)現(xiàn)了差異的快速定位以及K-D tree實(shí)現(xiàn)快速近鄰搜索，并利用了CUDA來加速程序。最終在本課題馬桶陶瓷素胚的曲面形狀差異檢測(cè)上，將點(diǎn)對(duì)誤差控制在0.5 mm以內(nèi)，運(yùn)行時(shí)間在控制在1 s以內(nèi)，實(shí)現(xiàn)了快速精確的工件三維差異檢測(cè)與定位。