二維點云配準的交換截取迭代最近點

南開大學統計與數據科學學院 裴曉淞

為解決傳統ICP（迭代最近點Iterative Closest Point）[1]算法的存在容易陷入局部最優、誤差大、計算量大等問題，本文分析了傳統ICP算法缺點的成因，針對問題，提出了截取機制和交換機制，在兩者協同作用下，經過大量模擬實驗將平均誤差減小了83.6%，計算時間減小了42.4%，且由數據顯示，能有效地減少一部分局部最優的情況。

點云是通過測量儀器掃描物體而生成的點數據的集合。點云的配準在生活生產中，可以產生許多有價值的應用如：數字自動化生產、自動駕駛、醫學圖像配準等。點云的配準問題分為粗配準和精確配準兩部分[2]，最常見的、工業中使用最廣泛的精確配準算法是由Rusu等人提出的ICP[1]算法。二維ICP算法在激光領域的圖像處理也產生了很多應用[3]，但是配準的效果還有一定的改進空間。

謝小鵬等人對ICP算法進行了改進，加入了亂序一對一匹配和動態閾值的機制，一定程度上提高了ICP算法的性能[4]，但是也存在動態閾值運行較為不穩定等問題。所以本文針對上述算法的問題，研發出截取機制和交換機制，給出了一個理論說明更加清晰，匹配效果更好的改進算法。

1 基本原理

1.1 傳統ICP算法

Input：目標點云A= {Ai}、待配準點云B= {Bi}、終止最小誤差δ、最大迭代次數N ；

Step1：對任意點bi=Bi，在點云A找距離最近的點進行匹配，記為{ai}，得到匹配后的一對點云{ai}和{bi}；

Step2：由公式找到變換R和T，使得{ai}和R× {bi} +T的誤差err達到最小；

Step3：對B進行變換R×B+T得到新的點云；

Step4：若err沒有達到最小誤差δ和最大迭代次數N ，將上述步驟中B換成Step3中得到的新的點云回到Step1，重復迭代；否則結束并輸出結果；

Output：完成配準的點云，配準的變換矩陣，最終誤差，迭代次數。

其中Step2中誤差err的定義如式（1）所示：

變換R和T的求取公式推導如下：

旋轉矩陣用旋轉角度φ表示如式（2）所示：

平移矩陣T表示如式（3）所示：

其中ΔxΔy分別為點云在x軸和y軸上的位移。

記Ca和Cb是點集{ai}和的中心點，如式（4）所示：

令ai′ =ai-Ca，bi′=bi-Cb， 結 合err的 定 義，可以得到新的誤差函數如式（5）所示：

這里已經消去了變量T，只需求解旋轉矩陣R[3]，對上式進行分解得到如式（6）所示：

對f（φ）求導，如式（7）所示：

進而得到如式（9）所示：

由R的定義即可得到旋轉矩陣R。

平移矩陣由公式（10）得到：

1.2 交換截取ICP算法

Alternate and truncated Iterative Closest Point Algorithm簡稱ATICP。

傳統ICP算法有以下幾個問題：

（1）迭代容易陷入局部最優解。在面對點云較為對稱的情況下，不同旋轉方向的梯度可能會近似抵消，迭代有可能會陷入局部最優。

（2）每次迭代的運算量較大。在傳統ICP算法中，對于n個點的點云，每個點都需要計算到另一個點云所有點的距離，最近點的距離需要進行6n2次運算，邊際計算成本高，收益低，點數越多，模型的運算越復雜。

（3）迭代方向錯誤。點云可能在中心有一部分點數據，由于點云數據有一定的誤差，這樣的點更容易導致迭代方向的錯誤。

針對以上問題，這里給出兩點改進：

（1）交換機制（Alternate在后文圖標簡寫為A）。

在上文介紹的傳統ICP算法的Step1進行修改：在奇數次迭代中，與傳統ICP相同，在偶數次迭代中交換點云A、B的操作，對任意點ai=Ai，在點云B找距離最近的點進行匹配，記為{bi}，得到匹配后的點云{ai}和{bi}。

交換機制對ICP算法的影響：當誤差較大時，如果匹配的點云圖像較為對稱，傳統ICP算法的旋轉梯度可能會相互抵消，從而陷入局部最優；交換機制從另一個對稱匹配的角度，有可能讓點云進行正確的變換，從而幫助算法跳出局部最優。 而誤差較小時，無論是否交換，都傾向于一對一的正確匹配，交換機制對算法沒有影響。

如圖1所示，傳統ICP算法的旋轉梯度抵消，陷入局部最優；而交換機制使得算法跳出局部最優。

圖1 對ATICP左為奇數次匹配，右為偶數次匹配；對傳統ICP只進行左圖的匹配Fig.1 For ATICP, the left is an odd number of matches, and the right is an even number of matches; for traditional ICP,only the left image

（2）截取機制（Truncated在后文圖標簡寫為T）。

在上文介紹的傳統ICP算法Step1前加入一步預處理，將點云A、B中距離中心點較小的點按比例去除一部分，再進入算法。

截取機制對ICP算法的影響：當點數較少時，越多的點意味著越多的信息，可以幫助更快更好的配準，但是點數足夠時，在相同的點分布的情況下，更多的點數只能徒增計算量。而且由于點云本身的誤差，部分距離中心較近的點還會產生錯誤的梯度信息，對配準不利。

2 實驗

編程環境如表1所示。

表1 編程環境Tab.1 Programming environment

隨機生成點云數據集的生成規則如式（11）、式（12）所示：

對任意i∈ { 1 ， 2 …50}

這樣生成的隨機點云較為對稱，配準難度大，更容易體現模型的優劣。

最大迭代次數N=10，終止誤差δ=3，在截取步驟中截取40%的點云進行迭代。

一次實驗具有偶然性，這里進行1000次模擬實驗對比，如圖3所示展示為誤差大于100的模擬。圖例中T指截取機制，A指交換機制，前綴表示在ICP算法上的使用對應的技術。

如圖2所示，誤差大于100的模擬實驗中，ICP算法出現的次數最多且誤差較大，加入兩種機制的ATICP算法出現次數最少且誤差較小。兩種機制加入后的單獨實驗也顯示對ICP算法有不同程度的優化。

圖2 4種算法1000次模擬實驗的誤差大于100的散點圖Fig.2 Scatter plot with errors bigger than 100 for 1000 simulation experiments of four algorithms

err為前文定義的誤差均值，Time表示運行的平均時間，T指截取機制，A指交換機制。

如表2所示，分別加入交換機制和截取都對算法的誤差和計算量有一定程度的優化，且二者協同后，起到了疊加作用，使得平均誤差減小了83.6%，計算時間減小了42.4%。

表2 4種算法的模擬測試誤差和時間的均值Tab.2 The mean value of simulated test error and time for 4 algorithms

3 結論

本文在傳統ICP算法上進行改進，針對容易陷入局部最優、誤差大、計算量大的成因進行分析，提出了截取機制和交換機制。加入算法改進后，平均誤差減小了83.6%，計算時間減小了42.4%，提高了算法的性能。

引用

[1]BESL P J,MCKAY H D.A Method for Registration of 3-D Shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256.

[2]解則曉,徐尚.三維點云數據拼接中ICP及其改進算法綜述[J].中國海洋大學學報(自然科學版),2010,40(1):99-103.

[3]渠瀛.基于激光測距儀的移動機器人二維地圖創建問題研究[D].湖南:中國人民解放軍國防科技大學,2011.

[4]謝小鵬,古家威.一種改進的二維ICP點云配準算法[J].激光與紅外,2021,21(7):951-955.