謝德芳，陳叢桂，周 聰，馬亮華，黎鑫澤

（廣州大學(xué)機(jī)械與電氣工程學(xué)院，廣州 510006）

0 引言

圖1 算法流程圖

1 濾波去噪

點(diǎn)云在特征提取、配準(zhǔn)、曲面重建之前要進(jìn)行預(yù)處理，預(yù)處理對(duì)點(diǎn)云處理有著重要意義，主要采用濾波去噪。

VoxelGrid[9]（體素網(wǎng)格）濾波有著很好的濾波效果，使用此濾波不僅能達(dá)到減少點(diǎn)云點(diǎn)集數(shù)目，也能保持點(diǎn)云的幾何形狀，維持原始形狀特征，對(duì)于點(diǎn)云配準(zhǔn)是一種理想的濾波方式。

VoxelGrid 濾波工作原理：三維點(diǎn)云中的體素是三維空間中的最小分割單位，即相當(dāng)于二維圖像中的像素。在輸入點(diǎn)云數(shù)據(jù)上創(chuàng)建一個(gè)個(gè)體素網(wǎng)格（將體素網(wǎng)格視為一組空間中的微小三維空間）。然后，在每個(gè)體素中，所有存在的點(diǎn)將用它們的質(zhì)心近似。因此VoxelGrid濾波可以保持三維點(diǎn)云的宏觀幾何形狀。

2 RANSAC擬合

RANSAC 算法從一組數(shù)據(jù)集中，通過(guò)反復(fù)隨機(jī)選擇點(diǎn)集中的子集，達(dá)到擬合目標(biāo)數(shù)學(xué)模型的效果。RANSAC 算法基本思想如下：假定目標(biāo)數(shù)學(xué)模型，隨機(jī)選擇n個(gè)點(diǎn)，通過(guò)這n個(gè)點(diǎn)確定數(shù)學(xué)模型方程；在數(shù)據(jù)集選取點(diǎn)代入此數(shù)學(xué)模型方程，并計(jì)算誤差；找出所有在誤差范圍內(nèi)的點(diǎn)——局內(nèi)點(diǎn)，剔除局外點(diǎn)。在設(shè)定的迭代次數(shù)命令下重復(fù)上述過(guò)程，局內(nèi)點(diǎn)最多的模型為最優(yōu)數(shù)學(xué)模型。

迭代次數(shù)推導(dǎo)如下：

式中：n為假定模型需要選取點(diǎn)的數(shù)目；p為迭代過(guò)程中從數(shù)據(jù)集內(nèi)隨機(jī)選取的點(diǎn)都為局內(nèi)點(diǎn)的概率；ω為每次從數(shù)據(jù)集中選取一個(gè)局內(nèi)點(diǎn)的概率，ω= 局內(nèi)點(diǎn)數(shù)目/ 數(shù)據(jù)集數(shù)目，1-ωn是n個(gè)點(diǎn)中至少有1個(gè)點(diǎn)為局外點(diǎn)的概率；k為迭代次數(shù)。

形容詞的意動(dòng)用法，是指主語(yǔ)主觀上斷定賓語(yǔ)擁有某種狀況，可以按照“認(rèn)為賓語(yǔ)謂語(yǔ)”的格式來(lái)解釋。如:“不恥下問(wèn)”的意思是一個(gè)人不認(rèn)為請(qǐng)教比自己地位低下的人是可恥的?！皭u”是形容詞的意動(dòng)用法，解釋為:認(rèn)為……是恥辱的事情。

對(duì)式（1）左右兩邊取對(duì)數(shù)得：

考慮到迭代過(guò)程中每個(gè)點(diǎn)的選取都是獨(dú)立的，某個(gè)點(diǎn)被選取之后，也可能會(huì)被選定，因此修正式（2），得修正后的迭代次數(shù)

3 粗配準(zhǔn)

3.1 PFH和FPFH特征描述子

點(diǎn)云粗配準(zhǔn)通過(guò)點(diǎn)的幾何特征進(jìn)行，如法向量、曲率等。但點(diǎn)周圍的幾何特征數(shù)量多且相似度高，無(wú)法得到點(diǎn)云的全局特征信息，因此有了點(diǎn)特征直方圖PFH[10]（point feature histogram）。PFH 通過(guò)點(diǎn)和臨近點(diǎn)的空間差異作出幾何描述，PFH 提供的信息具有旋轉(zhuǎn)不變性，對(duì)于點(diǎn)云而言十分穩(wěn)健。

如圖2 所示，Pq的PFH計(jì)算的影響區(qū)域，Pq用深色標(biāo)注并放在圓球的中間位置，半徑為r，(Pq)的所有k鄰元素（即與點(diǎn)Pq的距離小于半徑r 的所有點(diǎn)）全部互相連接在一個(gè)網(wǎng)絡(luò)中。

FPFH（fast point feature histograms）在保持了PFH大部分特性的前提下，降低了算法的時(shí)間復(fù)雜度，提高了計(jì)算效率，本質(zhì)上是PFH 的快速簡(jiǎn)化模型。只需要計(jì)算Pq（查詢點(diǎn)）和緊鄰點(diǎn)（圖3）之間的特征元素?？芍獜?fù)雜度有所降低，稱之為SPFH（simple point feature histograms）。

圖2 查詢點(diǎn)Pq的計(jì)算PFH的影響區(qū)域

圖3 查詢點(diǎn)Pq的計(jì)算FPFH的影響區(qū)域

確定點(diǎn)的k領(lǐng)域，得出最終Pq直方圖公式如下：

3.2 SAC-IA配準(zhǔn)

SAC-IA 配準(zhǔn)（采樣一致性初始配準(zhǔn)：Sample Consensus Initial Aligment,SAC-IA），通過(guò)FPFH特征配準(zhǔn)點(diǎn)云可得到一個(gè)大致的位姿，達(dá)到粗配準(zhǔn)的效果。

配準(zhǔn)算法步驟如下。

（1）源點(diǎn)云B中選取n個(gè)點(diǎn)，為了保證選取的點(diǎn)具備不同的FPFH特征，選取點(diǎn)的距離必須小于給定的最小閾值。

（2）目標(biāo)點(diǎn)云A查找與源點(diǎn)云B滿足相似條件的點(diǎn)，并保持一一對(duì)應(yīng)關(guān)系。

（3）計(jì)算對(duì)應(yīng)點(diǎn)的旋轉(zhuǎn)矩陣和平移矩陣，并根據(jù)Huber函數(shù)進(jìn)行評(píng)判：

式中：m為給定閾值；li為第i組對(duì)應(yīng)點(diǎn)變換后的距離差。

重復(fù)上述步驟直至結(jié)果最優(yōu)，即誤差函數(shù)取最小值，得到最終的平移矩陣和旋轉(zhuǎn)矩陣。

4 精配準(zhǔn)

粗配準(zhǔn)后僅僅得到一個(gè)較好的位姿，為了使兩期點(diǎn)云盡可能重合，需要進(jìn)行精配準(zhǔn)。ICP算法的基本原理如下：兩期點(diǎn)云 A 和 B，點(diǎn)集為 A={a1, a2, a3, …, an}、B={b1, b2, b3…, bm}，通過(guò)旋轉(zhuǎn)平移變換后，點(diǎn)云A、B中的點(diǎn)一一對(duì)應(yīng)。

式中：R為旋轉(zhuǎn)矩陣；T為平移矩陣。

ICP配準(zhǔn)的步驟如下。

（1）目標(biāo)點(diǎn)云A中取點(diǎn)集ai，并在源點(diǎn)云B中找到對(duì)應(yīng)點(diǎn)bi，使。

（2）計(jì)算旋轉(zhuǎn)矩陣和平移矩陣，使目標(biāo)函數(shù)取最小值。

（3）對(duì)目標(biāo)點(diǎn)云A 進(jìn)行旋轉(zhuǎn)平移變換，更新得到新點(diǎn)云數(shù)據(jù)集A′。

（4）計(jì)算已更新點(diǎn)云A′和源點(diǎn)云B 中所有對(duì)應(yīng)點(diǎn)的距離，做歸一化處理，得。

（5）給定閾值，若平均距離d 小于給定的閾值，重復(fù)以上步驟，否則視為收斂。

通過(guò)上述步驟得到的旋轉(zhuǎn)矩陣和平移矩陣，用于原點(diǎn)云坐標(biāo)轉(zhuǎn)換，完成配準(zhǔn)過(guò)程。

5 實(shí)驗(yàn)結(jié)果和分析

本文實(shí)驗(yàn)在cpu 主頻2.4 GHz， 內(nèi) 存 為 4 G 的windows10系統(tǒng)下進(jìn)行實(shí)驗(yàn)平臺(tái)的搭建，使用C++進(jìn)行編程，實(shí)驗(yàn)中選用的是PCL 開(kāi)源庫(kù)中的milk_cartoon_all_small_clorox 數(shù)據(jù)文件，點(diǎn)云數(shù)據(jù)中大約有240 000個(gè)點(diǎn)，實(shí)驗(yàn)結(jié)果如圖4所示。

圖4 配準(zhǔn)結(jié)果

由實(shí)驗(yàn)結(jié)果可知，本文提出的配準(zhǔn)優(yōu)化算法可以滿足點(diǎn)云配準(zhǔn)的重合精度。此外，本文提出的配準(zhǔn)優(yōu)化算法和傳統(tǒng)的配準(zhǔn)算法相比較，配準(zhǔn)速度有明顯的提升，在保證配準(zhǔn)精度的前提下，算法效率提高了29.5%。實(shí)驗(yàn)使用改進(jìn)的配準(zhǔn)算法和傳統(tǒng)配準(zhǔn)算法所消耗的時(shí)間如表1所示。

表1 傳統(tǒng)算法和改進(jìn)算法的比較

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

針對(duì)傳統(tǒng)配準(zhǔn)算法耗時(shí)不足的問(wèn)題，本文提出了一種基于RANSAC算法的點(diǎn)云配準(zhǔn)算法。首先，對(duì)點(diǎn)云使用RANSAC算法提取可以代替原點(diǎn)云的關(guān)鍵面，接著，使用FPFH特征進(jìn)行粗配準(zhǔn)，在粗配準(zhǔn)的基礎(chǔ)上使用ICP算法得到旋轉(zhuǎn)矩陣和平移矩陣，達(dá)到精配準(zhǔn)的效果。經(jīng)實(shí)驗(yàn)證明，該方法可應(yīng)用于點(diǎn)云的配準(zhǔn)過(guò)程。與傳統(tǒng)配準(zhǔn)算法相比，收斂穩(wěn)定，速度快，具有很好的實(shí)用價(jià)值。