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

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

0 引言

圖1 算法流程圖

1 濾波去噪

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

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

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

2 RANSAC擬合

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

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

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

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

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

考慮到迭代過程中每個點的選取都是獨立的，某個點被選取之后，也可能會被選定，因此修正式（2），得修正后的迭代次數(shù)

3 粗配準(zhǔn)

3.1 PFH和FPFH特征描述子

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

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

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

圖2 查詢點Pq的計算PFH的影響區(qū)域

圖3 查詢點Pq的計算FPFH的影響區(qū)域

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

3.2 SAC-IA配準(zhǔn)

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

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

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

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

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

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

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

4 精配準(zhǔn)

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

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

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

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

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

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

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

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

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

5 實驗結(jié)果和分析

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

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

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

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

6 結(jié)束語

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