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

謝小鵬,古家威

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

1 引 言

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

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

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

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

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

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

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

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

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

(1)

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

(2)

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

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

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

(3)

(4)

(5)

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

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

(6)

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

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

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

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

圖1 當(dāng)目標(biāo)點(diǎn)集與參考點(diǎn)集以多對(duì)一方式匹配時(shí)

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

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

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

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

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

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

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

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

圖3 目標(biāo)點(diǎn)集打亂順序前后對(duì)比

(7)

(8)

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

4 實(shí) 驗(yàn)

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

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

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

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

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

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

5 結(jié) 論

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