吳慶華，蔡瓊捷思，黎志昂，劉嘉程

（1. 湖北省現(xiàn)代制造質(zhì)量工程重點(diǎn)實(shí)驗(yàn)室，湖北武漢430064；2. 湖北工業(yè)大學(xué)機(jī)械工程學(xué)院，湖北武漢430064）

1 引 言

點(diǎn)云配準(zhǔn)是虛擬現(xiàn)實(shí)、三維測(cè)量、機(jī)器人視覺(jué)引導(dǎo)等領(lǐng)域中的一項(xiàng)關(guān)鍵技術(shù)［1］。配準(zhǔn)主要分為手動(dòng)和自動(dòng)兩大類。前者是通過(guò)貼上標(biāo)記，計(jì)算標(biāo)記點(diǎn)在不同視角下的位置關(guān)系矩陣；后者是通過(guò)描述子或者形狀關(guān)系，進(jìn)行迭代查詢最優(yōu)位置關(guān)系矩陣［2］。目前，自動(dòng)配準(zhǔn)需求廣泛，是點(diǎn)云配準(zhǔn)的研究熱點(diǎn)。然而，受物體表面形貌和紋理特性等因素影響，所獲取的三維點(diǎn)云往往存在局部數(shù)據(jù)丟失、噪聲和亂序等問(wèn)題。這類問(wèn)題會(huì)影響點(diǎn)云的配準(zhǔn)精度和時(shí)效，對(duì)此許多學(xué)者開(kāi)展了大量研究工作。

自動(dòng)配準(zhǔn)算法中運(yùn)用最廣泛的是由Besl 等提出的最近點(diǎn)迭代算法（Iterative Closest Point，ICP）［3］，該算法通過(guò)迭代求解出最小誤差時(shí)的對(duì)應(yīng)點(diǎn)對(duì)之間的變換關(guān)系。雖然該算法精度很高，但需要良好的初值來(lái)保證收斂性。所以，自動(dòng)配準(zhǔn)一般先采用粗配準(zhǔn)獲得初值，再用ICP 進(jìn)行精配準(zhǔn)，因此，如何將點(diǎn)云變換到ICP 的收斂域成為解決問(wèn)題的關(guān)鍵［4］。Radu Bogdan Rusu 等［5］提出了快速點(diǎn)特征直方圖（Fast Point Feature Histograms，F(xiàn)PFH）的特征量，以點(diǎn)與其鄰域點(diǎn)法向量夾角等信息為特征構(gòu)建直方圖，并實(shí)現(xiàn)了三維點(diǎn)云的粗配準(zhǔn)。該方法對(duì)大多數(shù)點(diǎn)云具有較高的配準(zhǔn)精度，但耗時(shí)長(zhǎng)，特別是對(duì)于存在異常法向量夾角等信息的缺失點(diǎn)云更難以收斂；Salti S［6］提出基于方向直方圖的編碼統(tǒng)計(jì)特征（Signature of Histogram of Orientation，SHOT）點(diǎn)云配準(zhǔn)算法，該算法同時(shí)具有直方圖特征的抗噪性和基于標(biāo)簽特征的鑒別性，且對(duì)旋轉(zhuǎn)和尺度具有不變性，其局限性在于僅適用于含有大量不同曲率特征的缺失點(diǎn)云；王欣［7］使用邊界生長(zhǎng)法提取點(diǎn)云的邊界點(diǎn)云，通過(guò)匹配邊界點(diǎn)云實(shí)現(xiàn)粗配準(zhǔn)，對(duì)于缺失點(diǎn)云，由于其邊界并不完整容易使配準(zhǔn)失效；Magnusson M 提出的正態(tài)分布變換（Normal Distributions Transform，NDT）算法［8］，該算法耗時(shí)穩(wěn)定，并且初值誤差大也能很好的糾正，但對(duì)缺失點(diǎn)云，由于局部區(qū)域統(tǒng)計(jì)結(jié)果會(huì)產(chǎn)生異常，導(dǎo)致配準(zhǔn)效果不佳。

缺失點(diǎn)云的局部數(shù)據(jù)存在丟失，會(huì)造成其局部法向特征、邊界特征和重疊度等發(fā)生一定程度的改變。這些改變會(huì)導(dǎo)致大部分算法的收斂速度、配準(zhǔn)精度和適用性產(chǎn)生不同程度的影響。擴(kuò)展高斯圖像聚類可以有效地提取出法向特征相似且重疊度較大的子點(diǎn)云，且對(duì)局部點(diǎn)云缺失并不敏感［9］。為此，本文提出了基于擴(kuò)展高斯圖像聚類（Clustering Extended Gaussian Image，CEGI）的缺失點(diǎn)云配準(zhǔn)算法。該算法對(duì)于缺失點(diǎn)云具有較高的配準(zhǔn)精度，可以有效的避免上述問(wèn)題帶來(lái)的影響。

2 C-EGI 點(diǎn)云配準(zhǔn)算法原理

C-EGI 算法的基本思想是將點(diǎn)云通過(guò)高斯映射得到擴(kuò)展高斯圖像（Extended Gaussian Image，EGI）；然后，對(duì)EGI 聚類分割，并對(duì)分割后的聚類進(jìn)行匹配；將匹配結(jié)果逆高斯映射后，進(jìn)行粗配準(zhǔn)；最后，使用粗配準(zhǔn)結(jié)果更新源點(diǎn)云，再和模板點(diǎn)云進(jìn)行精配準(zhǔn)，如圖1 所示。其關(guān)鍵在于分割后的聚類匹配和粗配準(zhǔn)關(guān)系求取。本文采用體積-模長(zhǎng)算子（Volume-norm Operator，VN Operator）對(duì)分割后的聚類進(jìn)行匹配；使用基于距離-曲率描述子（Distance-curvature Descriptor，DC Descriptor）查詢對(duì)應(yīng)點(diǎn)對(duì)，實(shí)現(xiàn)粗配準(zhǔn)。

2.1 基于VN 的聚類匹配

基于VN 的聚類匹配分為兩個(gè)部分，EGI聚類分割和基于VN 匹配分割后的聚類。EGI 是將三維物體的一點(diǎn)通過(guò)高斯映射至單位球面上的一點(diǎn)，并在該點(diǎn)上添加一個(gè)向量，向量的方向與該點(diǎn)的法向量方向一致，模長(zhǎng)為該點(diǎn)所在面的面積［9］。并且，法向相近的點(diǎn)會(huì)區(qū)域性集結(jié)，如圖2所示，（a）表示長(zhǎng)方體與其擴(kuò)展高斯圖像的對(duì)應(yīng)關(guān)系，（b）表示空間曲面與其擴(kuò)展高斯圖像的對(duì)應(yīng)關(guān)系。

圖2 三維物體映射擴(kuò)展高斯圖像示意圖Fig. 2 Diagram of 3D object mapping to EGI

在獲得EGI 之前，需要估算點(diǎn)云中每個(gè)點(diǎn)的法向量。取點(diǎn)Pi(xi，yi，zi)與鄰域點(diǎn)云集N{Pi}，采用最小二乘法擬合出Pi點(diǎn)的平面方程z=Ax+By+C［10］，利用公式（1）求出該平面方程的偏差平方和d：

取d最小值時(shí)所對(duì)應(yīng)的向量n(A，B，C)作為點(diǎn)Pi(xi，yi，zi)的法向量。然后，進(jìn)行高斯映射，獲得對(duì)應(yīng)EGI，并對(duì)EGI 使用歐式聚類。

為確保聚類后源點(diǎn)云和模板點(diǎn)云的高斯點(diǎn)云簇是相對(duì)應(yīng)的，本文采用基于VN 算子進(jìn)行匹配。其定義函數(shù)為：

其中：Vi表示每個(gè)簇的體積，Li表示每個(gè)簇的向量總模長(zhǎng)，α和β為自定義權(quán)重系數(shù)。

2.2 基于DC 的粗配準(zhǔn)

由于EGI 只能反映三維物體的旋轉(zhuǎn)姿態(tài)關(guān)系，無(wú)法反映出三維物體的空間位置關(guān)系［11］。本文對(duì)最優(yōu)高斯點(diǎn)云簇對(duì)使用索引查找的方式進(jìn)行逆高斯映射，獲取對(duì)應(yīng)的實(shí)體點(diǎn)云對(duì)，進(jìn)行對(duì)應(yīng)點(diǎn)對(duì)求解。由于此時(shí)點(diǎn)云的法向特征相似，重疊度較高，并且數(shù)據(jù)集大幅下降，使用傳統(tǒng)算法進(jìn)行對(duì)應(yīng)點(diǎn)對(duì)的求解優(yōu)勢(shì)并不明顯，所以本文提出一種基于DC 描述子的粗配準(zhǔn)方法。

首先，通過(guò)DC 描述子進(jìn)行對(duì)應(yīng)點(diǎn)對(duì)的查詢，其定義函數(shù)為：

圖3 對(duì)應(yīng)點(diǎn)對(duì)查詢示意圖Fig. 3 Diagram of querying corresponding point

3 C-EGI 配準(zhǔn)算法設(shè)計(jì)

本文算法總體流程如圖4 所示。主要分為兩部分：

3.1 粗配準(zhǔn)矩陣求解

首先，對(duì)實(shí)際掃描點(diǎn)云S和模板點(diǎn)云T使用高斯映射獲得EGI，對(duì)EGI 進(jìn)行聚類，通過(guò)基于VN 的匹配獲得最優(yōu)簇對(duì)；其次，在逆高斯映射后，通過(guò)DC 描述子求解對(duì)應(yīng)點(diǎn)對(duì)，并使用RANSAC 算法剔除異常點(diǎn)對(duì)；最后，采用SVD 算法計(jì)算出粗配準(zhǔn)矩陣RcTc。

3.2 最終配準(zhǔn)矩陣求解RcTc

將源點(diǎn)云S 進(jìn)行矩陣變換乘以后獲得點(diǎn)云S′，對(duì)S′ 采用ICP 算法計(jì)算出精配準(zhǔn)矩陣RfTf。利用公式（5）求解最終配準(zhǔn)矩陣RT。

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

實(shí)驗(yàn)運(yùn)行環(huán)境為IntelCore 2.4GHz CPU，PCL 1.8.0 平臺(tái)，實(shí)驗(yàn)數(shù)據(jù)來(lái)自于斯坦福大學(xué)激光掃描數(shù)據(jù)庫(kù)中的bunny（35947）模型和實(shí)際采集點(diǎn)云。分別進(jìn)行了一般點(diǎn)云配準(zhǔn)實(shí)驗(yàn)、缺失點(diǎn)云配準(zhǔn)實(shí)驗(yàn)和散放棒料抓取中的點(diǎn)云配準(zhǔn)實(shí)驗(yàn)。

圖4 算法流程圖Fig 4 The algorithm flowchart

實(shí)驗(yàn)分別使用FPFH，NDT，SHOT 和本文算法，結(jié)合ICP 精配準(zhǔn)算法進(jìn)行配準(zhǔn)實(shí)驗(yàn)比對(duì)，為了使實(shí)驗(yàn)更有說(shuō)服力，設(shè)置各實(shí)驗(yàn)?zāi)Ｐ驮键c(diǎn)云（模板點(diǎn)云）密度的10 倍作為各算法搜索半徑；NDT 網(wǎng)格內(nèi)點(diǎn)云數(shù)量設(shè)為10，步長(zhǎng)設(shè)置為0.1；為減小ICP 算法受到數(shù)據(jù)局部缺失的影響，采用縮小ICP 算法的最近點(diǎn)搜索范圍的方法，設(shè)置ICP 算法的搜索半徑為模型原始點(diǎn)云密度的5倍。采用配準(zhǔn)后對(duì)應(yīng)點(diǎn)之間的距離d的均方誤差（Mean Square Error，MSE）作為配準(zhǔn)評(píng)價(jià)標(biāo)準(zhǔn)，如式（6）所示：

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

對(duì)Bunny（35 947）點(diǎn)云進(jìn)行旋轉(zhuǎn)變換，獲取不同姿態(tài)的點(diǎn)云，任取3 個(gè)姿態(tài)分別使用C-EGI，F(xiàn)PFH，NDT，SHOT 進(jìn)行配準(zhǔn)測(cè)試。

圖5 為各算法配準(zhǔn)效果，表1 和表2 為各算法的配準(zhǔn)精度與耗時(shí)。從表1 和表2 中可知，F(xiàn)PFH算法擁有較高的配準(zhǔn)精度，但是耗時(shí)長(zhǎng)。SHOT算法相比FPFH 精度稍有下降，但速度優(yōu)勢(shì)明顯。NDT 算法相對(duì)SHOT 算法精度相近，但耗時(shí)相對(duì)較高。本文算法精度與SHOT 算法相近，但速度提升近31.1%。

圖5 各算法配準(zhǔn)結(jié)果Fig. 5 Registration result of different algorithms

表1 配準(zhǔn)精度實(shí)驗(yàn)誤差對(duì)比表Tab. 1 MSE of registration experiment （mm2）

表2 配準(zhǔn)精度實(shí)驗(yàn)耗時(shí)對(duì)比表Tab. 2 Running time of registration experiment （s）

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

本實(shí)驗(yàn)分兩步驗(yàn)證，首先驗(yàn)證本文算法的精度和時(shí)效性，然后驗(yàn)證本文算法的穩(wěn)定性。4.2.1 缺失點(diǎn)云配準(zhǔn)精度和時(shí)效性實(shí)驗(yàn)

由于物體表面材質(zhì)、紋理和光線遮擋，獲取的點(diǎn)云數(shù)據(jù)常常存在數(shù)據(jù)丟失。故設(shè)計(jì)缺失點(diǎn)云實(shí)驗(yàn)，對(duì)各算法進(jìn)行配準(zhǔn)測(cè)試。分別采用統(tǒng)計(jì)閾值T=0.1 和T=0.5 的統(tǒng)計(jì)離群點(diǎn)濾波過(guò)濾相對(duì)稀疏部分，模擬物體自身形貌遮擋或表面反光導(dǎo)致的數(shù)據(jù)丟失；分別使用X軸方向和Z軸方向直通濾波過(guò)濾40%左右的截面數(shù)據(jù)，模擬光線受到遮擋導(dǎo)致的數(shù)據(jù)丟失。

圖6 為各算法對(duì)缺失點(diǎn)云的實(shí)際配準(zhǔn)效果，表3 和表4 為各算法對(duì)缺失點(diǎn)云的配準(zhǔn)精度與耗時(shí)。從表3 和表4 中可知，NDT 算法對(duì)缺失點(diǎn)云已不能有效的配準(zhǔn)。FPFH 算法對(duì)于缺失點(diǎn)云配準(zhǔn)精度依舊穩(wěn)定，但是耗時(shí)更長(zhǎng)。對(duì)于SHOT 算法而言，精度相對(duì)FPFH 算法較差，但耗時(shí)縮小了近70 倍。本文算法與FPFH 算法精度相當(dāng)，并且相對(duì)SHOT 算法速度提升了近

38.7%。

圖6 缺失點(diǎn)云配準(zhǔn)實(shí)驗(yàn)各算法結(jié)果Fig. 6 Registration result of different algorithms with losing point cloud

表3 缺失點(diǎn)云配準(zhǔn)實(shí)驗(yàn)誤差對(duì)比表Tab. 3 MSE of registration experiment with losing point cloud（mm2）

4.2.2 缺失點(diǎn)云配準(zhǔn)穩(wěn)定性實(shí)驗(yàn)

本文算法是利用基于擴(kuò)展高斯圖像聚類提取相同特征的子點(diǎn)云進(jìn)行配準(zhǔn)，如圖7 所示，（a）為模擬孔洞缺失時(shí)的擴(kuò)展高斯圖像聚類提取的子點(diǎn)云示意圖，（b）為模擬截面缺失時(shí)的擴(kuò)展高斯圖像聚類提取的子點(diǎn)云示意圖。

為驗(yàn)證其可以穩(wěn)定有效地規(guī)避點(diǎn)云局部缺失帶來(lái)的影響。本實(shí)驗(yàn)采用與精度實(shí)驗(yàn)相同方法，通過(guò)統(tǒng)計(jì)濾波和直通濾波模擬孔洞缺失和截面缺失。

表4 缺失點(diǎn)云配準(zhǔn)實(shí)驗(yàn)耗時(shí)對(duì)比表Tab. 4 Running time of registration experiment with losing point cloud（s）

圖7 擴(kuò)展高斯圖像聚類結(jié)果Fig. 7 Clustering result of Extended Gaussian Image

分別對(duì)源點(diǎn)云進(jìn)行9 組不同閾值的統(tǒng)計(jì)濾波和X 軸直通濾波后與同一模板點(diǎn)云進(jìn)行配準(zhǔn)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖8 和9 所示，采用缺失點(diǎn)數(shù)的占比區(qū)分各組數(shù)據(jù)。從圖8 和9 可知，本文算法在9組不同孔洞缺失和截面缺失下的進(jìn)行配準(zhǔn)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果都顯示本文算法能獲得良好的配準(zhǔn)效果。因此，該算法針對(duì)局部孔洞缺失和局部截面缺失兩種情況均具有良好的魯棒性。

圖8 孔洞缺失點(diǎn)云配準(zhǔn)穩(wěn)定性實(shí)驗(yàn)結(jié)果Fig. 8 Robust result of registration experiment with hole losing point clouds

圖9 截面缺失配準(zhǔn)穩(wěn)定性實(shí)驗(yàn)結(jié)果Fig. 9 Robust result of registration experiment with section losing

4.3 散放棒料抓取中的點(diǎn)云配準(zhǔn)實(shí)驗(yàn)

本實(shí)驗(yàn)采用3D 掃描儀和散放棒料零件進(jìn)行測(cè)試，如圖10 所示，從實(shí)際掃描點(diǎn)云中任取五個(gè)姿態(tài)不同棒料作為實(shí)驗(yàn)對(duì)象進(jìn)行配準(zhǔn)測(cè)試。

圖10 實(shí)驗(yàn)儀器及掃描結(jié)果圖Fig. 10 Experiment instrument and the scanned result

圖11 為各算法對(duì)散放棒料抓取中的點(diǎn)云的配準(zhǔn)效果，表5 和表6 為各算法對(duì)散放棒料抓取中的點(diǎn)云的配準(zhǔn)精度和耗時(shí)。從表5 和表6 中可知，NDT 算法配準(zhǔn)精度較差且耗時(shí)較長(zhǎng)。FPFH算法由于該點(diǎn)云特征單一且存在缺失，使得在時(shí)效性和精度兩個(gè)方面結(jié)果都不佳。SHOT 算法相比NDT 和FPFH 精度較優(yōu)，并在耗時(shí)上具有明顯優(yōu)勢(shì)。本文算法在精度方面優(yōu)于這三種算法，在評(píng)價(jià)誤差上優(yōu)于FPFH 算法17.9%，并且在平均耗時(shí)方面相比SHOT 算法提速近26.4%，可見(jiàn)本算法能滿足實(shí)際缺失點(diǎn)云配準(zhǔn)要求。

圖11 實(shí)際點(diǎn)云配準(zhǔn)實(shí)驗(yàn)各算法結(jié)果Fig. 11 Registration result of different algorithms with actual point cloud

表5 實(shí)際點(diǎn)云配準(zhǔn)實(shí)驗(yàn)誤差對(duì)比表Tab. 5 MSE of registration experiment with actual point cloud（mm2）

表6 實(shí)際點(diǎn)云配準(zhǔn)實(shí)驗(yàn)耗時(shí)對(duì)比表Tab. 6 Running time of registration experiment with actual point cloud（s）

5 結(jié) 論

本文針對(duì)缺失點(diǎn)云提出了一種基于擴(kuò)展高斯圖像聚類的配準(zhǔn)算法，實(shí)現(xiàn)了對(duì)缺失點(diǎn)云的快速高效配準(zhǔn)，可以滿足一般物體掃描時(shí)存在數(shù)據(jù)丟失的點(diǎn)云配準(zhǔn)要求。通過(guò)把點(diǎn)云映射到EGI中聚類再逆映射回實(shí)體點(diǎn)云的方法進(jìn)行配準(zhǔn)，根據(jù)實(shí)驗(yàn)結(jié)果證明該方法可以有效的避免點(diǎn)云由于掃描儀視角、物體材質(zhì)和掃描環(huán)境等因素帶來(lái)的影響。并且，與傳統(tǒng)配準(zhǔn)算法相比，具有更好的精度和時(shí)效性。

但由于本算法運(yùn)用了較多的參數(shù)進(jìn)行計(jì)算，并且利用EGI 進(jìn)行聚類，所以本算法的參數(shù)選取和法向方向判斷是一個(gè)重要因素，對(duì)于如何自動(dòng)調(diào)節(jié)最優(yōu)參數(shù)和法向統(tǒng)一是本算法未來(lái)研究的重要方向。