一種改進(jìn)的二維ICP點云配準(zhǔn)算法

謝小鵬,古家威

(1.東莞理工學(xué)院城市學(xué)院,廣東 東莞 523419；2.季華實驗室,廣東 佛山 528000)

1 引 言

點云配準(zhǔn)技術(shù)在逆向工程、機(jī)器人導(dǎo)航及無人駕駛等領(lǐng)域具有廣泛的應(yīng)用,點云配準(zhǔn)一般來說包括兩個步驟,粗配準(zhǔn)和精配準(zhǔn),粗配準(zhǔn)主要實現(xiàn)目標(biāo)點集與參考點集之間的快速配準(zhǔn),但是精度不高,常用的方法有NDT(Normal Distribution Transform)方法[1],王慶閃等實現(xiàn)了NDT與ICP結(jié)合的點云配準(zhǔn)方法[2],精配準(zhǔn)的實現(xiàn)方法主要是迭代最近點(ICP,Iterative Closest Point)算法[3],由Besl于1992年提出。近些年來,很多學(xué)者對ICP算法進(jìn)行了改進(jìn),Censi采用點到線的度量方式進(jìn)而提出了PL-ICP(Point-to-Line ICP)方法[4],該方法提高了ICP算法的精度；解則曉改進(jìn)了點到面ICP即P-ICP(Point-to-Plain ICP)[5]； Chetverikov提出了Trimmed ICP[6],該方法將n個匹配點對的歐氏距離進(jìn)行排序,去除距離最大的ηn(0<η<1)個匹配點對,該方法需要對η值做出平衡,過大過小都會影響最終結(jié)果的精度；孫翌將[7]k鄰域搜索方法應(yīng)用到ICP算法中,提高了查找最近點的效率,進(jìn)而加快了ICP算法的處理速度。

本文針對傳統(tǒng)ICP 算法存在的問題進(jìn)行了改進(jìn),首先介紹了傳統(tǒng)ICP的主要步驟,發(fā)現(xiàn)傳統(tǒng)ICP算法有以下幾種問題:迭代方向錯誤,迭代優(yōu)化過程中易出現(xiàn)局部最優(yōu)的情況,迭代次數(shù)過多,增加了算法的處理時間。本文提出了改進(jìn)方案,首先打亂目標(biāo)點集中點序號的順序,然后在接下來的尋找最近點過程中采用一對一的方式進(jìn)行,增強(qiáng)了算法的穩(wěn)定性,對每個匹配點對進(jìn)行篩選,使用動態(tài)閾值去過濾那些歐氏距離過大的一些匹配點對,加快了算法收斂速度。

2 傳統(tǒng)二維ICP算法

傳統(tǒng)的二維ICP算法的一般步驟如下:

步驟一:選取參考點集和目標(biāo)點集。

步驟二:遍歷目標(biāo)點集中所有的點,在參考點集中選擇歐氏距離最小的一個點。

步驟三:建立目標(biāo)函數(shù),對目標(biāo)函數(shù)進(jìn)行優(yōu)化求解,得到目標(biāo)點集的旋轉(zhuǎn)矩陣Rj和平移矩陣Tj,進(jìn)而得到新的目標(biāo)點集。

步驟四:判斷是否達(dá)到了最大迭代次數(shù)或者誤差條件,是則停止迭代,輸出最終的結(jié)果,否則轉(zhuǎn)到步驟一,繼續(xù)迭代。

在步驟三中,需要建立目標(biāo)函數(shù)并優(yōu)化求解,下面是目標(biāo)函數(shù)的建立和優(yōu)化過程:

(1)

將目標(biāo)點集與參考點集中所有對應(yīng)點對的歐氏距離的平方和累加起來得到目標(biāo)函數(shù),并使之達(dá)到最小,于是目標(biāo)函數(shù)為:

(2)

接下來對目標(biāo)函數(shù)進(jìn)行求解:

對目標(biāo)函數(shù)中的3個待定系數(shù)分別求偏導(dǎo)并令其為0:

-sinθyi-t1)(sinθxi-cosθyi)=0

(3)

(4)

(5)

對上述3個方程聯(lián)立即可得到3個待定系數(shù)的值,進(jìn)而得到本次迭代的旋轉(zhuǎn)矩陣Rj和平移矩陣Tj,將目標(biāo)點集中所有的點代入到式(1)中作旋轉(zhuǎn)平移變換即可得到新的目標(biāo)點集。

當(dāng)算法結(jié)束后,最后一次迭代得到的目標(biāo)點集即為最終的目標(biāo)點集,最終的旋轉(zhuǎn)矩陣和平移矩陣由公式(6)得到:

(6)

其中,m是迭代次數(shù)。

3 改進(jìn)的二維ICP算法

傳統(tǒng)的二維ICP算法容易出現(xiàn)以下一些問題:

(1)在尋找最近點的過程中,會出現(xiàn)目標(biāo)點集與參考點集多對一的情況,進(jìn)而使得算法的迭代方向發(fā)生錯誤,最終的誤差無法收斂。在圖1中,箭頭所指圓圈指的是目標(biāo)點集,其它圓圈指的是參考點集,綠色帶箭頭線段指的是目標(biāo)點集與最近點的配對關(guān)系,可以發(fā)現(xiàn)目標(biāo)點集中所有的點都指向了同一個最近點。

圖1 當(dāng)目標(biāo)點集與參考點集以多對一方式匹配時

(2)本文將目標(biāo)點集中的某個點與其找到的最近點稱作匹配點對,尋找到最近點后,有些匹配點對的歐式距離過大,會影響算法的迭代方向,增加算法的迭代次數(shù)。

在圖2中黑色連線的匹配點對間的歐氏距離遠(yuǎn)大于其他匹配點對的歐氏距離,這將大大地改變ICP算法的迭代方向。

圖2 當(dāng)出現(xiàn)匹配點對間歐氏距離過大的情況

(3)傳統(tǒng)的二維ICP算法在優(yōu)化過程中容易陷入局部最優(yōu)的情況,導(dǎo)致求解出來的結(jié)果錯誤。

本文提出的一種改進(jìn)的二維ICP掃描匹配算法針對傳統(tǒng)的二維ICP算法進(jìn)行了改進(jìn)。

(1)為避免出現(xiàn)目標(biāo)點集與參考點集多對一的情況,采取目標(biāo)點集與參考點集一對一的方式,具體實施方法則是當(dāng)目標(biāo)點集中的某個點在參考點集中找到最近點后,將參考點集中的這個最近點排除在參考點集之外。

(2)打亂目標(biāo)點集的順序。一般來說,用于ICP匹配的點集的點序號存在著某種規(guī)律性,比如從上到下或者從左到右排列,目標(biāo)點集中序號最前的點能夠在參考點集中正確找到最近點,由于序號靠前的點具有優(yōu)先選擇權(quán),目標(biāo)點集中序號靠后的點找到的最近點有很大的可能性不是真實的最近點。由于點集客觀存在的排列規(guī)律性,會導(dǎo)致大多數(shù)點都找不到真實的最近點,為了讓目標(biāo)點集中大多數(shù)點都能找到真實的最近點,本文將目標(biāo)點集中的點序號打亂,增加了目標(biāo)點集中能找到真實最近點的個數(shù),如圖3所示。

在圖3(a)中目標(biāo)點集的點序號按照從上往下的順序排列,由此形成了圖中的匹配點對,可以發(fā)現(xiàn)這些匹配點對間的歐氏距離都相對要更大,對于這些歐氏距離過大的匹配點對,算法會對其進(jìn)行篩選,進(jìn)而發(fā)生大范圍誤過濾的情況,這將影響算法的最終效果。在圖3(b)中目標(biāo)點集的點序號經(jīng)過了打亂,此時目標(biāo)點集中有更多的點找到了真實最近點。

圖3 目標(biāo)點集打亂順序前后對比

(7)

(8)

圖4 改進(jìn)的二維ICP算法流程圖

4 實 驗

為驗證本文改進(jìn)算法的優(yōu)點,進(jìn)行對比實驗。實驗中點集數(shù)據(jù)來自于一款旋轉(zhuǎn)二維激光掃描器在實際環(huán)境中所采集的數(shù)據(jù),該款激光掃描器每轉(zhuǎn)一圈獲取360個數(shù)據(jù)。實驗環(huán)境的測試環(huán)境如下:CPU為Intel(R)Core(TM)i5-6500U,8G內(nèi)存,Windows10系統(tǒng),編程環(huán)境是MATLAB7.0。

圖5是傳統(tǒng)二維ICP算法的ICP迭代過程,圖中的點集數(shù)據(jù)來源于二維激光掃描器,圖中的點集數(shù)據(jù)來源于二維激光掃描器,圖中有兩簇點集,分別是目標(biāo)點集及參考點集,其中參考點集由目標(biāo)點集經(jīng)過旋轉(zhuǎn)平移變換所得到,連線表示表示匹配點對間的匹配關(guān)系。迭代過程中,目標(biāo)點集與參考點集不斷靠近,最終幾乎完全重合。圖6是本文改進(jìn)的二維ICP算法的ICP迭代過程,采用的數(shù)據(jù)與圖5中的數(shù)據(jù)相同,都是從初始狀態(tài)到最終目標(biāo)點集與參考點集的無限接近,可以發(fā)現(xiàn)本文算法所需的迭代次數(shù)明顯要更少,特別是在無限接近階段,本文算法能夠迅速地收斂。

圖5 傳統(tǒng)二維ICP算法的迭代過程

圖6 本文改進(jìn)的二維ICP算法的迭代過程

為具體量化本文改進(jìn)ICP算法的優(yōu)勢,在獲得多組點集數(shù)據(jù)之后,使用這些數(shù)據(jù)在傳統(tǒng)ICP算法與本文改進(jìn)ICP算法上分別處理,表1是傳統(tǒng)ICP算法與本文改進(jìn)ICP算法在平均迭代次數(shù)與平均處理時間上的對比,可以發(fā)現(xiàn)本文所需的迭代次數(shù)要更少,處理時間也要更短。本文改進(jìn)ICP算法處理較快的原因在于算法的后續(xù)收斂階段,正常的匹配點對的歐氏距離趨于0,因此一些誤匹配點對的出現(xiàn)會極大地影響算法的收斂,而本文算法設(shè)有自適應(yīng)變化的閾值用來過濾一些誤匹配點對,從而加快了算法收斂,減少了迭代次數(shù)。

表1 傳統(tǒng)ICP算法與本文改進(jìn)ICP算法的迭代次數(shù)與處理時間

5 結(jié) 論

本文針對傳統(tǒng)ICP算法出現(xiàn)的一些問題,對其進(jìn)行了改進(jìn)。本文采用目標(biāo)點集與參考點集一對一的方式進(jìn)行最近點配對,并對目標(biāo)點集的點序號進(jìn)行打亂,增強(qiáng)了ICP算法的穩(wěn)定性；在ICP算法每次迭代過后,設(shè)置一個新的閾值用于過濾一些誤匹配點對,進(jìn)而減小算法的迭代次數(shù),加快了算法處理速度,實驗結(jié)果表明,本文改進(jìn)的ICP算法在處理速度具有明顯地提升。