吳振慧，王彩余

（1. 揚州市職業(yè)大學 電子工程學院， 江蘇 揚州 225009； 2. 揚州廣潤機械有限公司 技術部， 江蘇 揚州 225000）

在工業(yè)生產領域，尤其是自動化裝配和生產領域，受測量方式和被測物體集合形狀的限制，對于同一物體，需要在不同視角下掃描得到兩組或多組不同的點云數(shù)據。為了計算在此過程中物體或設備的運動，或是進行點云拼接，都需要找到兩個點云之間的對應空間位置關系，進行點集與點集坐標系間的轉換，實現(xiàn)配準，可見點云配準至關重要。

從物理學角度分析，點云配準即為計算源點云和目標點云之間的旋轉矩陣R 和平移矩陣T，通過這兩個矩陣即可描述三維空間中兩個點云的相對位置關系[1]。為了提高點云對于物體的描述精度，目前的掃描儀通常采用小點間距、小線間距的陣列式掃描。對于同一物體，越小的點、線間距意味著掃描出的點云中包含的點數(shù)據越多。然而，針對大型點云數(shù)據，其配準算法要在保證精度的基礎上，提高其速度，若采用一般的配準算法，計算耗時非常大，難以在工業(yè)界使用。因此，本文提出基于迭代最近點（Iterative Closest Point，ICP），結合預處理和K-D 樹的大型點云配準算法，以期在滿足精度要求的同時，提高計算速度。

1 迭代最近點算法

1.1 迭代最近點算法步驟

迭代最近點（ICP）算法[2]是目前使用最廣泛的點云精配準算法之一。ICP 算法的核心步驟為：在目標點云（Target）中尋找源點云（Source）的最近點，通過找到的對應最近點，使用最小二乘法構建目標函數(shù)，計算參數(shù)矩陣R 和T，經過一步計算后，將源點云進行R 和T 的位移變化，再次尋找最近點，如此迭代直到目標點云和源點云的距離小于閾值。

ICP 算法的目標函數(shù)如下：

ICP 算法的步驟如下：

第一步，在目標點云 Q 中計算源點云 P（k）中每一點的最近點，k 表示迭代次數(shù)。

第二步，計算能使得P（k）和其對應最近點標準誤差E 最小的旋轉矩陣R 和平移矩陣T，其標準誤差計算依據公式（1）。

第三步，使用第二步中計算得到的旋轉矩陣R 和平移矩陣 T，移動 P（k）使其成為 P（k+1）。

第四步：如果 P（k+1）和 Q 之間的標準誤差 E 小于閾值，則迭代停止，否則重復第一步至第三步。

1.2 迭代最近點算法計算方法

迭代最近點算法的主要問題在于：（1）如何在目標點云Q 中尋找源點云P（k）中某一點對應的最近點；（2）如何根據一一對應的最近點，計算得到旋轉矩陣R 和平移矩陣T；（3）如何將每一步計算得到的旋轉矩陣和平移矩陣合成為一個參數(shù)矩陣。

1.2.1 最近點的計算

針對問題（1），如果點云數(shù)據量較小，可以采用暴力搜索的方式尋找最近點。然而，針對大型點云該方法會消耗大量時間，并不適用。針對此，本算法采用了去質心和降采樣的預處理方式。去質心即在計算開始時，將點云的坐標分別減去其質心坐標，保留相對位置，以大大減少搜尋最近點的時間。過多的對應點對于計算準確性的提高作用甚微，只需采集少部分的特征點即可進行計算，故本文采用讀取點云后即進行30 %的降采樣操作方式，隨機均勻刪除70 %的點，在進行計算時，還可人為或隨機選取更小一部分的特征點作為迭代依據。因此，公式（1）可轉化為：

其中：p 表示源點云P 中被選擇作為特征點的點云，稱為特征源點云；q 表示目標點云Q 中與p 對應的最近點，稱為特征目標點云。

要注意的是，在計算旋轉矩陣R 和平移矩陣T 時，可以僅使用很少一部分的特征點，但在判斷迭代是否停止時，需要對所有點求其平均標準誤差E。公式（2）中的N 不是源點云所有點的個數(shù)，而是特征源點云所有點的個數(shù)。

1.2.2 參數(shù)矩陣的求解

奇異值分解算法計算效率高，計算結果準確，是針對問題（2）的經典計算方法。因此，本算法采用奇異值分解的方式并依據公式（2）求解目標函數(shù)，具體計算過程如下：

第一步，計算兩部分特征點云的質心：

第二步，去質心化：

第三步，構造協(xié)方差矩陣H：

第四步，使用奇異值分解矩陣H，得到U、S、V：

第五步，計算旋轉矩陣R：

第六步，計算平移矩陣T：

然后，更新特征源點云p 和源點云P 中所有點的坐標：

至此，完成一步迭代。當標準誤差E 小于閾值時，迭代停止，配準結束。根據工業(yè)現(xiàn)場定位要求，閾值設置為0.5 mm。

1.2.3 點云坐標的齊次化

針對問題（3），參考運動學原理[3]，可以引入齊次坐標，通過齊次變換矩陣M 統(tǒng)一表示旋轉矩陣R 和平移矩陣T，

此時，一個三維空間的坐標表示為齊次坐標的形式，

可以發(fā)現(xiàn)齊次坐標變化公式（15）與笛卡爾坐標變化公式（12）等價，

根據點云繞固定坐標系的齊次變換是齊次變換矩陣的順序左乘的定理，可以得到最終的齊次變換矩陣為：

這里Mi表示第i 次迭代計算得到的齊次變換矩陣。

2 最近點搜索加速方法

針對大型點云配準的場景，傳統(tǒng)ICP 算法的弊端在于，即使采用了去質心、降采樣的預處理方式，如果對于每一個特征源點云中的點，都采用暴力搜索的方式尋找最近點，也需要多次遍歷目標點云的所有點。針對這一弊端，楊秋翔等人基于法矢夾角對ICP 算法進行了改進[4]，楊小青等人基于法向量進行了改進[5]，朱玉梅等人提出基于DNSS 與點到平面的ICP 結合的方法[6]。考慮大型點云配準這一場景，本文采用K-D 樹方法加速最近點的搜索。

2.1 K-D 樹算法原理

K-D 樹是K-Dimensional 樹的簡稱，即為K維空間的樹，是一種對k 維空間的點儲存一遍從而可以進行快速搜索的樹形數(shù)據結構[7]。K-D 樹主要應用于多維空間中的最鄰近搜索和范圍搜索。K-D 樹中每個結點數(shù)據類型描述見表1。

表1 K-D 樹結點數(shù)據類型

2.1.1 K-D 樹的構建

K-D 樹的構建過程如下：

輸入：k 維空間數(shù)據集 T={x1，x2，…，xN}，其中

輸出：K-D 樹

（1）開始：構造根結點，根結點對應于包含T的k 維空間的超矩形區(qū)域。選擇x（1）作為切分的坐標軸，以T 中所有數(shù)據x（1）坐標的中位數(shù)作為切分點。分割超平面通過切分點并與坐標軸x（1）垂直，將根結點對應的超矩形區(qū)域切分為兩個子區(qū)域，即根結點的左樹Left 和右樹Right。左樹對應坐標x（1）小于切分點的左子空間，右樹對應于坐標 x（1）大于切分點的右子空間。

（2）重復：對深度為 j 的結點，選擇 x（l）作為切分的坐標軸，l=j（modk）+1 即為Split，以該結點的區(qū)域中所有數(shù)據x（l）坐標的中位數(shù)作為切分點。分割超平面通過切分點并與坐標軸x（l）垂直，將該結點對應的超矩形區(qū)域切分為兩個子區(qū)域，即該結點的左樹Left 和右樹Right。左樹對應坐標x（l）小于切分點的左子空間，右樹對應于坐標x（l）大于切分點的右子空間。

（3）停止：直到結點的左樹和右樹沒有數(shù)據時，分割停止，K-D 樹形成。

K-D 樹構建的時間復雜度為 O（log2n），并且對于迭代最近點算法，僅需要在第一次迭代之前構建K-D 樹即可，無須在每次迭代中反復構建，因此其構建時間對于算法的整體時間影響很小。

2.1.2 K-D 樹查找最鄰近結點

K-D 樹查找最鄰近結點，即完成ICP 算法中在目標點云中查找源點云中的點對應最近點的過程，其查找過程如下：

（1）從根結點開始，判斷輸入點（即源點云中的點）與該根結點的位置關系，如果其x（l）坐標小于根結點的x（l）坐標，說明輸入點在根結點的左樹，則進入左子結點；如果其x（l）坐標大于根結點的x（l）坐標，說明輸入點在根結點的右樹，則進入右子結點。

（2）重復步驟（1）不斷下移，直至移動到葉結點，將該葉結點作為“當前最近點” 。

（3）從“當前最近點” 上移，對于每個經過的結點，進行如下操作：①如果該結點比“當前最近點” 更接近輸入點，則將其變?yōu)椤爱斍白罱c” ；②如果該結點另一側子結點有更近的點，則進入其另一側子結點并不斷向下尋找。

（4）當搜索回到根結點時，搜索完成，找到最鄰近結點。

2.2 K-D 樹算法舉例

為了直觀地展示K-D 樹搜索臨近點的過程，圖1 給出了一個二維平面上K-D 樹的構建實例，其中包含 6 個點 （2，3），（5，4），（9，6），（4，7），（8，1），（7，2）。

圖1 K-D 樹的構造

假設輸入的點為（2，4.5），該點標注如圖2，需要在上述6 個點中尋找與其最近的點。

圖2 輸入點空間位置（星號位置）

首先，從根結點（7，2）開始搜索輸入點（2，4.5），其過程是：

（1）輸入點X 值2 小于根結點X 值7，因此訪問左樹，當前結點是（5，4）。

（2）輸入點Y 值 4.5 大于當前結點 Y 值 4，因此訪問右樹，當前結點是（4，7）。

（3）當前結點已經是葉結點，向下搜索過程結束。

此時記錄搜索路徑為＜（7，2），（5，4），（4，7）＞，然后需要上移回溯搜索路徑：

（1）回溯到（4，7），“當前最近點” 設置為（4，7）。計算（4，7）到輸入點（2，4.5）的距離為 d1，圖 2 中圓圈的半徑即為d1。

（2）回溯到（5，4），由于（5，4）到輸入點（2，4.5）的距離d2小于d1，因此“當前最近點” 更新為（5，4）；另外，由于輸入點到（5，4）分割線的距離小于 d1，說明（5，4）的另一側也需要查找，添加其另一側的點（2，3）后，搜索路徑變?yōu)椋迹?，2），（2，3）＞。

（3）回溯到（2，3），由于（2，3）到輸入點（2，4.5）的距離 d3小于 d2，因此“當前最近點” 更新為（2，3）。

（4）回溯到（7，2），由于（7，2）到輸入點（2，4.5）的距離d4大于d3，因此“當前最近點” 無須更新；另外，由于輸入點到（7，2）分割線的距離大于d3，說明（7，2）的另一側無須查找。

（5）所有搜索路徑上的點均被回溯，回至根結點結束，（2，4.5）的最近點輸出為（2，3）。

最近點搜索結果如圖3 所示。

圖3 輸入點和其最近點

3 實驗結果與分析

本文算法采用C++語言編寫，在Visual Studio 2015 中編譯和運行，采用OpenGL 三維圖像庫進行可視化交互。在此界面中，可以自由調整視角、縮放物體及框選物體上的點作為特征點等，并且程序可判斷所選取點是否可見。本算法認為框選區(qū)域中僅正面可見的點為選擇的特征點。

本文選擇工業(yè)零部件的掃描點云作為實驗數(shù)據，目標點云和源點云均以txt 文件形式存儲，存儲形式為：以v 開頭的行表示一個點（Vertex）的三維坐標，以f 開頭的行表示一個三角面片（Face）頂點的編號。點云文件的存儲示例如圖4所示。

圖4 點云文件存儲形式

讀取點云文件后，以計算機圖形學中典型的Half-Edge 結構進行點云模型的存儲。在Half-Edge 結構下，可以很方便地遍歷一個點的臨近點和臨近邊，Half-Edge 結構如圖5 所示，其中，每個“半邊” 有對應的start（起始點）、twin（孿生邊）、next（下一條邊）、previous（前一條邊）、left face（左側邊）。通過這種數(shù)據結構即可構造完整的點云模型。

圖5 Half-Edge 數(shù)據結構

讀取并顯示目標點云和源點云模型如圖6 所示。圖6 顯示其形狀幾乎一致，但位置相差甚遠。

圖6 目標點云和源點云模型

源點云信息輸出如圖7 所示，其中包括頂點個數(shù)（453 089）、邊個數(shù)（1 350 160）、面?zhèn)€數(shù)（895 959）等。由于其頂點個數(shù)多達45 萬余，屬于大型點云，符合本算法的使用范圍。

圖7 源點云相關信息

由系統(tǒng)隨機選取 3 000、1 000、500 個點作為特征點進行計算，或手動選取如圖8 所示的特征點，其實驗結果如表2 所示。表2 中序號4、5 對應的特征點即為圖8 中左右選擇的區(qū)域。

圖8 手動選取特征點（白色區(qū)域）

表2 實驗結果

從表2 不難發(fā)現(xiàn)，采用系統(tǒng)隨機選擇特征點的方式，可以在使用更少的特征點，減少計算時間的前提下，保證配準誤差小于0.1 mm。若采用手動選取特征點，由于點云數(shù)目眾多，容易選取較多的特征點，因此計算時間較長。同時，因為選取的特征點分布較集中，所以誤差比隨機選擇特征點時大，與本算法原理相符。但不論是隨機還是手動選擇特征點，都能保證在30 s 內完成誤差小于0.2 mm 的配準，速度和精度均滿足工業(yè)要求。

為驗證采用K-D 樹加速的優(yōu)越性，本文將其與傳統(tǒng)的一一暴力搜索最近點的方法進行精度和速度比較。原點云數(shù)目高達45 萬多，本文降采樣后在僅有1 萬點左右的稀疏點云上進行傳統(tǒng)的ICP 配準。選取所有點作特征點，完成配準的計算時間為24.94 s，配準誤差為0.075 mm。由此可見，基于K-D 樹加速的ICP 配準算法，能夠在保證配準精度的前提下，大大提高ICP 算法的配準速度，是適用于大型點云配準的可靠方法。

4 結 論

本文基于ICP 算法，結合各種預處理方式和K-D 樹數(shù)據結構，提出了針對大型點云的配準算法。該算法解決了傳統(tǒng)ICP 算法不適用于大量數(shù)據計算的弊端，在保證計算精度的前提下，大大提高了計算速度，并且可視化界面交互性強，可以實時顯示點云模型和配準結果。大型點云數(shù)據的配準實驗表明，該算法魯棒性強，速度靈敏，精確度高，可用于工業(yè)現(xiàn)場。