林俊義，吳 雷，楊梅英，張雪楓，江開(kāi)勇

(華僑大學(xué) 廈門(mén)市數(shù)字化視覺(jué)測(cè)量重點(diǎn)實(shí)驗(yàn)室/福建省特種能場(chǎng)制造重點(diǎn)實(shí)驗(yàn)室，福建 廈門(mén) 361021)

0 引言

大型自由曲面類(lèi)零件在汽車(chē)、輪船、航空等領(lǐng)域的應(yīng)用越來(lái)越廣泛，如汽車(chē)覆蓋件、沖壓件、螺旋槳、大型葉片等。但是這類(lèi)工件往往缺少規(guī)則特征，表面幾乎全部為自由曲面，定位極其困難。目前主要采用專(zhuān)用夾具進(jìn)行人工定位，成本高、效率低且定位精度難以保證。近年來(lái)，基于3D點(diǎn)云模型的配準(zhǔn)技術(shù)被廣泛應(yīng)用于三維重建[1]、智能抓取[2]、誤差檢測(cè)與評(píng)估[3-5]、目標(biāo)識(shí)別與定位[6]等領(lǐng)域。其中，最經(jīng)典的是由BESL等[7]提出的迭代最近點(diǎn)(Iterative Closest Point, ICP)算法,但該算法收斂速度慢，對(duì)初值敏感、易陷入局部最優(yōu)，故常作為兩步配準(zhǔn)法中的精配準(zhǔn)使用[8]。

在粗配準(zhǔn)階段，由于點(diǎn)云數(shù)據(jù)量大，粗配準(zhǔn)時(shí)間長(zhǎng)且配準(zhǔn)精度差。為了稀疏點(diǎn)云以減小計(jì)算量，需要對(duì)點(diǎn)云提取穩(wěn)健的特征關(guān)鍵點(diǎn)，因此尋找一種好的關(guān)鍵點(diǎn)提取方法成為粗配準(zhǔn)的關(guān)鍵。李仁忠等[9]提出基于內(nèi)部形態(tài)描述子關(guān)鍵點(diǎn)(Intrinsic Shape Signatures, ISS)的點(diǎn)云配準(zhǔn)方法，提高了配準(zhǔn)效率，但僅結(jié)合改進(jìn)的ICP算法，配準(zhǔn)精度無(wú)明顯提升[10]；陸軍等[11]提出以鄰域半徑內(nèi)法向量的變化信息設(shè)定約束條件提取關(guān)鍵點(diǎn)，該方法僅適用于細(xì)節(jié)特征豐富的模型;ZHANG等[12]采用尺度不變特征變換(3D-Scale Invariant Feature Transform, 3D-SIFT)關(guān)鍵點(diǎn)提取算法，該算法需要首先對(duì)圖像進(jìn)行Harris特征提取，同樣適用于特征豐富的零件；THEILER等[13]提出結(jié)合高斯差分和Harris算法的關(guān)鍵點(diǎn)提取方法，該方法可以顯著提高配準(zhǔn)效率，但對(duì)大型自由曲面提取效果不佳；KLEPPE等[14]提出考慮形狀因子和局部協(xié)方差矩陣的關(guān)鍵點(diǎn)提取方法，該方法提高了關(guān)鍵點(diǎn)提取的穩(wěn)定性，但不適用于密度差異大的點(diǎn)云配準(zhǔn)。

基于散亂點(diǎn)云提取的關(guān)鍵點(diǎn)往往不具備唯一性，而特征描述子包含豐富的局部特征信息，具有較好的區(qū)分度，但對(duì)于稠密的點(diǎn)云數(shù)據(jù)，計(jì)算每個(gè)點(diǎn)的特征描述子會(huì)嚴(yán)重影響點(diǎn)云配準(zhǔn)的效率，且稠密點(diǎn)云包含大量特征相近的點(diǎn)。因此，將關(guān)鍵點(diǎn)與特征描述子相結(jié)合已成為特征配準(zhǔn)的研究熱點(diǎn)。常用的特征描述子包括：快速特征點(diǎn)直方圖(Fast Point Feature Histograms, FPFH)[15]、方向特征直方圖(Signature of Histograms of Orien Tations, SHOT)[16]、歸一化對(duì)齊徑向特征(Normal Aligned Radial Feature, NARF)[17]、局部特征直方圖(Local Feature Statistics Histogram, LFSH)[18]等。其中，F(xiàn)PFH具有描述性強(qiáng)、特征維度低、計(jì)算速度快等特點(diǎn)，廣泛應(yīng)用于點(diǎn)云配準(zhǔn)中。如彭真等[19]利用體素格濾波，再結(jié)合法向距離提取關(guān)鍵點(diǎn)，并以FPFH進(jìn)行特征配準(zhǔn)，顯著提升了配準(zhǔn)效率；劉劍等[20]提出一種結(jié)合FPFH與Delaunay三角剖分的點(diǎn)云配準(zhǔn)算法，通過(guò)三角網(wǎng)格建立局部關(guān)聯(lián)性，縮小點(diǎn)對(duì)匹配的搜索范圍，提高了配準(zhǔn)效率；李新春等[21]提出以k鄰域特征參數(shù)(法向量、距離等)和曲率參數(shù)提取關(guān)鍵點(diǎn)，結(jié)合FPFH的點(diǎn)云配準(zhǔn)算法，提高了噪聲干擾和數(shù)據(jù)缺失情況下的算法魯棒性和配準(zhǔn)精度。

在精配準(zhǔn)階段，目前存在各種改進(jìn)的ICP算法，其中全局優(yōu)化可以獲得較高的配準(zhǔn)精度，但往往降低了配準(zhǔn)效率。如PARRA等[22]通過(guò)嵌套分支定界(Branch and Bound, BNB)算法和ICP算法對(duì)旋轉(zhuǎn)矩陣進(jìn)行全局優(yōu)化，但降低了配準(zhǔn)速度；YANG等[23]通過(guò)結(jié)合BNB和ICP算法對(duì)轉(zhuǎn)換矩陣實(shí)現(xiàn)了全局優(yōu)化，克服了ICP算法陷入局部最優(yōu)的限制，但增加了計(jì)算復(fù)雜度。

本文針對(duì)大型自由曲面零件缺乏顯著特征，難以采用現(xiàn)有方法提取關(guān)鍵點(diǎn)的問(wèn)題，提出一種基于自適應(yīng)分辨率的聚類(lèi)分塊，并以分塊質(zhì)心作為關(guān)鍵點(diǎn)的新方法，利用質(zhì)心點(diǎn)的曲率、夾角和FPFH特征信息高效解決稠密點(diǎn)云的粗配準(zhǔn)問(wèn)題。進(jìn)一步，基于ISS關(guān)鍵點(diǎn)結(jié)合BNB嵌套ICP算法進(jìn)行精配準(zhǔn)，提高了配準(zhǔn)效率，實(shí)現(xiàn)大型自由曲面零件的快速視覺(jué)定位。

1 算法流程

本文將三維點(diǎn)云配準(zhǔn)技術(shù)擴(kuò)展應(yīng)用于零件視覺(jué)定位領(lǐng)域，試圖通過(guò)機(jī)器人視覺(jué)系統(tǒng)對(duì)零件進(jìn)行單次測(cè)量，將局部測(cè)量點(diǎn)云與完整CAD模型點(diǎn)云進(jìn)行配準(zhǔn)，從而求解被測(cè)零件在機(jī)器人坐標(biāo)系下的空間位姿。算法流程如圖1所示，主要包括3個(gè)方面：自適應(yīng)分辨率超體素聚類(lèi)分塊、基于質(zhì)心點(diǎn)的重疊區(qū)域識(shí)別與配準(zhǔn)和基于關(guān)鍵點(diǎn)的精配準(zhǔn)。

2 快速定位方法

2.1 自適應(yīng)分辨率超體素聚類(lèi)分塊

采用面結(jié)構(gòu)光視覺(jué)系統(tǒng)測(cè)量得到的3D數(shù)據(jù)往往存在噪聲且數(shù)據(jù)量大，基于CAD模型的離散點(diǎn)云數(shù)據(jù)，為了保持特征細(xì)節(jié)也存在數(shù)據(jù)量大的問(wèn)題。因此，首先采用統(tǒng)計(jì)濾波算法[24]進(jìn)行點(diǎn)云預(yù)處理。

對(duì)數(shù)據(jù)量龐大的點(diǎn)云分割后再進(jìn)行后續(xù)處理，不僅可以提高運(yùn)算效率，還可以增強(qiáng)點(diǎn)云的局部一致性，避免陷入局部?jī)?yōu)化。超體素聚類(lèi)分塊[25]本質(zhì)上是將點(diǎn)云分割為很多小塊，并研究每個(gè)小塊之間的關(guān)系，找到具有一致性的點(diǎn)云塊作為整體進(jìn)行求解。然而采用現(xiàn)有的超體素聚類(lèi)算法直接對(duì)不同模型的點(diǎn)云數(shù)據(jù)進(jìn)行分塊，會(huì)造成不同模型分塊大小差異明顯，影響算法的穩(wěn)定性。為此，本文提出自適應(yīng)分辨率的超體素分塊算法，主要包括自適應(yīng)分辨率的確定和區(qū)域生長(zhǎng)規(guī)則兩個(gè)步驟。具體如下：

(1)自適應(yīng)分辨率的確定

聚類(lèi)分辨率對(duì)點(diǎn)云分塊結(jié)果具有直接的影響，包括體素分辨率Rv和種子分辨率Rs，自適應(yīng)分辨率考慮點(diǎn)云的大小與面積比例兩個(gè)因素。點(diǎn)云大小利用AABB包圍盒算法[26]計(jì)算點(diǎn)云的最小包圍盒，以最小包圍盒對(duì)角線長(zhǎng)的一半(Cd)作為參數(shù)；面積比例通過(guò)實(shí)驗(yàn)調(diào)整分割后的點(diǎn)云塊面積與原點(diǎn)云面積的比值來(lái)確定。如在比值取0.015時(shí)，利用測(cè)試的數(shù)據(jù)擬合出自適應(yīng)分辨率的確定公式，如式(1)所示：

Rv=0.01×Cd+0.8,

Rs=0.037×Cd+3.0。

(1)

(2)區(qū)域生長(zhǎng)規(guī)則

區(qū)域生長(zhǎng)規(guī)則是一個(gè)由色彩信息、空間信息和法向信息構(gòu)成的度量，具有39維特征，記為：

F=[x,y,z,R,G,B,FPFH1.33]。

(2)

式中：x,y,z為超體素種子點(diǎn)的3D坐標(biāo);R,G,B為色彩空間的顏色信息;FPFH1.33為文獻(xiàn)[15]提出的快速點(diǎn)特征直方圖的33維特征描述子。種子點(diǎn)的特征距離可由下式得出：

(3)

式中：Dc為點(diǎn)云數(shù)據(jù)色彩空間的歐式距離;Ds為被種子分辨率歸一化后的空間歐式距離;Dn為法向量之間的夾角;Rs為種子點(diǎn)分辨率的大小;wc、ws、wn分別為調(diào)整顏色、空間、法向量的權(quán)重。設(shè)定距離閾值即可對(duì)點(diǎn)云進(jìn)行分割。

2.2 基于質(zhì)心點(diǎn)的重疊區(qū)域識(shí)別與配準(zhǔn)

提取每塊點(diǎn)云的質(zhì)心點(diǎn)，并以質(zhì)心點(diǎn)為對(duì)象計(jì)算質(zhì)心點(diǎn)的幾何形狀特征，識(shí)別出與測(cè)量點(diǎn)云對(duì)應(yīng)的CAD模型質(zhì)心點(diǎn)集。反求質(zhì)心點(diǎn)在原始CAD模型上的點(diǎn)云，預(yù)估出重疊區(qū)域。主要步驟如下：

(1)提取CAD模型點(diǎn)云的各點(diǎn)云塊質(zhì)心點(diǎn)集P{m1,m2,…,mn}以及測(cè)量點(diǎn)云的各點(diǎn)云塊質(zhì)心點(diǎn)集Q{n1,n2,…,nn}。

(2)計(jì)算質(zhì)心點(diǎn)集P、Q中每個(gè)質(zhì)心點(diǎn)的主曲率。

(3)計(jì)算質(zhì)心點(diǎn)集P、Q中每個(gè)質(zhì)心點(diǎn)的法向量與查詢(xún)質(zhì)心點(diǎn)到鄰域重心的連線之間的夾角。

首先計(jì)算k鄰域的重心點(diǎn)Pc、Qc。點(diǎn)集P、Q中查詢(xún)質(zhì)心點(diǎn)到其重心的連線分別為‖Pc-Pi‖、‖Qc-Qj‖;nPi、nQi為點(diǎn)集P、Q中各查詢(xún)點(diǎn)的法向量，它們之間的夾角記為αP和αQ，其值分別為：

αP=accos〈nPi,‖Pc-Pj‖〉,

(4)

αQ=accos〈nQi,‖Qc-Qj‖〉。

(5)

(4)由步驟(2)可以得到質(zhì)心點(diǎn)集P、Q中每個(gè)查詢(xún)質(zhì)心點(diǎn)的最大主曲率分別記為Pi_k1、Qj_k1；最小主曲率分別記為Pi_k2、Qj_k2。因此，判別準(zhǔn)則不等式為：

‖Pi_k1-Qj_k1‖≤ε1,

‖Pi_k2-Qj_k2‖≤ε2,

‖αP-αQ‖≤ε3。

(6)

其中ε1、ε2、ε3為各不等式設(shè)定的閾值。

以判別準(zhǔn)則篩選出符合要求的查詢(xún)質(zhì)心點(diǎn)，計(jì)算出查詢(xún)質(zhì)心點(diǎn)的FPFH特征描述子，具體步驟如下：

(1)對(duì)于任意查詢(xún)質(zhì)心點(diǎn)，計(jì)算其與鄰域點(diǎn)的位置關(guān)系，以特征組{α,φ,θ,d}表示，如圖2所示。其中npq和npk分別為查詢(xún)質(zhì)心點(diǎn)Pq與其鄰域點(diǎn)Pk的法向量。各特征定義如下：

α=V·npk,

(7)

(8)

θ=arctan(W·npk,U·npq),

(9)

d=‖Pk-Pq‖。

(10)

(2)以鄰近的簡(jiǎn)化點(diǎn)特征直方圖(Simplified Point Feature Histograms, SPFH)來(lái)計(jì)算FPFH特征值：

FPFH(Pq)=SPFH(Pq)+

(11)

式中ωk表示權(quán)重，在一定的度量空間中表示查詢(xún)質(zhì)心點(diǎn)與其鄰域點(diǎn)的距離。

基于FPFH特征，采用KD樹(shù)最近鄰搜索算法，先從局部測(cè)量點(diǎn)云質(zhì)心點(diǎn)集P到CAD模型質(zhì)心點(diǎn)集Q查找對(duì)應(yīng)點(diǎn)。由于噪聲等因素的影響，存在錯(cuò)誤匹配點(diǎn)對(duì)，而隨機(jī)采樣一致性算法常被用來(lái)去除錯(cuò)誤匹配點(diǎn)，具有良好的穩(wěn)健性和抗噪性[27]。運(yùn)用該算法建立正確的對(duì)應(yīng)匹配點(diǎn)，并求解粗配準(zhǔn)轉(zhuǎn)換矩陣。

2.3 基于關(guān)鍵點(diǎn)的精配準(zhǔn)

由于ICP算法存在局部收斂問(wèn)題，且直接利用全部點(diǎn)云數(shù)據(jù)進(jìn)行精配準(zhǔn)將非常耗時(shí)，受文獻(xiàn)[23]啟發(fā)，提出一種基于關(guān)鍵點(diǎn)的分支定界(BNB)嵌套ICP精配準(zhǔn)算法來(lái)提高精配準(zhǔn)的效率。本算法關(guān)鍵點(diǎn)提取主要考慮以下幾個(gè)因素：①關(guān)鍵點(diǎn)應(yīng)該在比較穩(wěn)定的區(qū)域，以方便法向的提??；②關(guān)鍵點(diǎn)的分布要求比較均勻，且不造成數(shù)據(jù)缺失。分析比較ISS、Harris、3D-SIFT三種關(guān)鍵點(diǎn)提取效果后，本文采用ISS關(guān)鍵點(diǎn)提取算法。

3 算法結(jié)果分析與定位實(shí)驗(yàn)

機(jī)器人視覺(jué)定位實(shí)驗(yàn)平臺(tái)如圖3所示，其中硬件主要包括：減震工作臺(tái)、UR5機(jī)器人、視覺(jué)測(cè)量系統(tǒng)(兩個(gè)CCD工業(yè)相機(jī)(DMK51BU02)、一臺(tái)DLP投影儀(伊萊斯柯)、兩個(gè)Computar工業(yè)鏡頭(MP1614))、計(jì)算機(jī)(CPU:Intel(R)Core(TM)i5-4200 H,2.80 GHz,內(nèi)存：4 GB)。在Windows 7平臺(tái)下，基于VC++2013編程環(huán)境，采用MFC(microsoft foundation classes)結(jié)合OpenCV開(kāi)元庫(kù)等實(shí)現(xiàn)本文算法。

3.1 算法結(jié)果分析

基于自適應(yīng)分辨率的超體素聚類(lèi)分塊實(shí)驗(yàn)：首先以固定分辨率對(duì)沖壓件A和沖壓件B兩模型進(jìn)行分塊，設(shè)置Rv=5.0和Rs=6.0，點(diǎn)云分塊效果如圖4所示，而依據(jù)式(1)進(jìn)行自適應(yīng)分辨率取值，分割結(jié)果如圖5所示?？梢钥闯觯瑢?duì)不同模型，自適應(yīng)分辨率聚類(lèi)分塊算法分割差異小、算法更穩(wěn)定。

對(duì)沖壓件A的局部測(cè)量點(diǎn)云與CAD模型點(diǎn)云進(jìn)行點(diǎn)云塊的質(zhì)心點(diǎn)提取，結(jié)果如圖6所示，其中深色標(biāo)志點(diǎn)即為各點(diǎn)云塊的質(zhì)心，可以看出質(zhì)心點(diǎn)分布稀疏、均勻，數(shù)據(jù)量大大減少，有利于提高算法的效率。

基于質(zhì)心點(diǎn)特征對(duì)沖壓件B進(jìn)行重疊區(qū)域識(shí)別，結(jié)果如圖7所示，右圖上半部分為提取出與測(cè)量點(diǎn)云重疊的區(qū)域?？梢钥闯?，通過(guò)本文設(shè)定的判別準(zhǔn)則可以有效地提取出重疊區(qū)域。

根據(jù)質(zhì)心點(diǎn)及其特征參數(shù)，對(duì)點(diǎn)云粗配準(zhǔn)進(jìn)行測(cè)試，結(jié)果如圖8所示。其中深色代表CAD模型點(diǎn)云，淺色代表測(cè)量點(diǎn)云。從圖中可以看出算法達(dá)到了粗配準(zhǔn)的要求。

分別采用ISS、Harris、3D-SIFT三種提取算法對(duì)沖壓件A的局部測(cè)量點(diǎn)云(淺色)進(jìn)行提取，實(shí)驗(yàn)結(jié)果如圖9所示?？梢钥闯?，ISS算法提取到的關(guān)鍵點(diǎn)分布均勻、合理。該算法既可保留具有顯著特征的點(diǎn)，又可大大減少數(shù)據(jù)量，可提高精配準(zhǔn)的效率。

基于ISS關(guān)鍵點(diǎn)，采用BNB分支定界嵌套ICP算法實(shí)現(xiàn)點(diǎn)云的精配準(zhǔn)，配準(zhǔn)結(jié)果如圖10所示。

為了進(jìn)一步闡明分塊的優(yōu)勢(shì)，將本文算法與不分塊直接基于FPFH的特征點(diǎn)云迭代配準(zhǔn)算法(D_FPFH)進(jìn)行對(duì)比分析。兩種算法的效果如圖11所示，其中圖11a為D_FPFH算法效果圖，圖11b為本文算法效果圖。從兩圖中可以看出：對(duì)于完整模型中特征變化明顯的模型，兩種算法都可以滿(mǎn)足要求，如沖壓件B；而對(duì)于完整模型中特征幾乎一致的模型，D_FPFH算法失效，而本文算法可以滿(mǎn)足要求。因此對(duì)于大部分區(qū)域結(jié)構(gòu)相似，只存在大小區(qū)別的模型，本文算法具有較高的穩(wěn)定性。在效率方面，本文雖然增加了基于點(diǎn)云分塊的重疊區(qū)域提取的時(shí)間(約1 s)，但采用稀疏質(zhì)心點(diǎn)進(jìn)行粗匹配，模型A和B整個(gè)粗配準(zhǔn)時(shí)間僅為2.958 s和3.202 s(如表1和表2)，效率得到了顯著提高。

表1 模型A不同算法比較結(jié)果

表2 模型B不同算法比較結(jié)果

同樣，在精配準(zhǔn)階段，將基于關(guān)鍵點(diǎn)的BNB嵌套ICP算法與D_FPFH算法和直接ICP算法(D_ICP)作對(duì)比，結(jié)果如表1和表2所示。從表中可以看出，對(duì)兩個(gè)不同的模型，本方法的精度得到了顯著的提高；雖然采用BNB嵌套ICP算法比D_ICP算法耗時(shí)較長(zhǎng)，但基于關(guān)鍵點(diǎn)的匹配算法提高了配準(zhǔn)效率，節(jié)省了超過(guò)30%的配準(zhǔn)時(shí)間。

3.2 定位實(shí)驗(yàn)

以沖壓件B為實(shí)驗(yàn)對(duì)象，對(duì)視覺(jué)定位的準(zhǔn)確性進(jìn)行實(shí)驗(yàn)驗(yàn)證。具體實(shí)驗(yàn)步驟如下：

(1)通過(guò)眼在手上的視覺(jué)系統(tǒng)對(duì)沖壓件B進(jìn)行測(cè)量，得到?jīng)_壓件B的局部測(cè)量點(diǎn)云如圖12所示。

(2)將局部測(cè)量點(diǎn)云與CAD模型點(diǎn)云配準(zhǔn)，使CAD模型點(diǎn)云配準(zhǔn)到實(shí)際沖壓件B的位姿下。

(3)利用手眼關(guān)系將CAD模型點(diǎn)云轉(zhuǎn)換到機(jī)器人基座坐標(biāo)系下。

(4)利用機(jī)器人末端執(zhí)行器(探針)點(diǎn)觸標(biāo)記在沖壓件B上的4個(gè)標(biāo)志點(diǎn)(如圖13的圓周上)，計(jì)算獲得此4個(gè)點(diǎn)在機(jī)器人基座坐標(biāo)系下的三維坐標(biāo)。

將步驟(4)中的4個(gè)點(diǎn)映射到步驟(3)中的CAD模型點(diǎn)云上，如圖14所示，比較分析4個(gè)標(biāo)記點(diǎn)的位置誤差。

經(jīng)多次重復(fù)試驗(yàn)，位置誤差保持在1 mm之內(nèi)，驗(yàn)證了本文方法對(duì)實(shí)現(xiàn)大型自由曲面零件視覺(jué)定位的有效性。

4 結(jié)束語(yǔ)

本文提出一種面向大型自由曲面類(lèi)零件的機(jī)器人視覺(jué)快速定位方法。通過(guò)自適應(yīng)分辨率聚類(lèi)算法對(duì)大量點(diǎn)云數(shù)據(jù)進(jìn)行了有效分割，并利用分塊質(zhì)心的幾何特征信息與FPFH特征信息，實(shí)現(xiàn)了局部測(cè)量點(diǎn)云數(shù)據(jù)與CAD模型點(diǎn)云數(shù)據(jù)的重疊區(qū)域識(shí)別與粗配準(zhǔn)；采用基于關(guān)鍵點(diǎn)的分支定界(BNB)嵌套ICP算法進(jìn)一步提高了精配準(zhǔn)的效率。通過(guò)算法對(duì)比分析與沖壓件視覺(jué)定位實(shí)驗(yàn)對(duì)算法的有效性進(jìn)行了驗(yàn)證，結(jié)果表明本文算法能夠?qū)崿F(xiàn)大型自由曲面零件的快速定位，定位誤差小于1 mm，可滿(mǎn)足沖壓件機(jī)器人切割、下料等需求。下一步將研究多方位視覺(jué)定位方法，進(jìn)一步提高大型件的定位精度。