ICP配準算法的影響因素及評價指標分析

陳春旭，漆鈺暉，朱一帆，裴 凌，徐昌慶

(1.上海交通大學(xué) 上海市北斗導(dǎo)航與位置服務(wù)重點實驗室，上海 200240；2.南昌大學(xué) 信息工程學(xué)院，南昌 330031)

0 引言

近年來，隨著無人車技術(shù)[1]的發(fā)展，同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)[2]成為重要課題和研究熱點。由于傳感器的不同，SLAM主要分為視覺SLAM[3]和激光SLAM[4]。相較于RGB-D相機傳感器，激光雷達傳感器具有精度高、速度快，對于光照環(huán)境要求低的特點。目前，高分辨率多線激光雷達傳感器廣泛應(yīng)用于機器人自主導(dǎo)航和三維場景重建兩大領(lǐng)域，基于高分辨率多線激光雷達傳感器的SLAM技術(shù)為測繪行業(yè)提供了更加成熟可靠的解決方案，其技術(shù)的關(guān)鍵環(huán)節(jié)之一即局部三維激光點云配準問題，其點云配準的精度和效率會對移動機器人后續(xù)的定位與建圖產(chǎn)生關(guān)鍵性的影響。

目前常用的三維空間點云配準方法多采用迭代最鄰近點(Iterative Closest Point，ICP)[5]算法，該算法是采用點點(點面)距離尋找鄰近點，再通過均方誤差函數(shù)求取多幀點云間的旋轉(zhuǎn)和平移矩陣以達到配準的目的，實質(zhì)是一種迭代收斂的算法。該算法最初提出是用于對象重建，但近年來在機器人領(lǐng)域已廣泛應(yīng)用于全局場景重建的配準。由于ICP原始算法中存在很多缺陷和不足，后來就引發(fā)了對其算法的各種改進和各類變種算法的出現(xiàn)[6]。

本文主要研究了ICP算法在三維激光點云配準中的應(yīng)用，首先對ICP算法的原理及其最小二乘求解過程進行了詳細分析；其次分析研究了影響ICP配準方法的各類因素，并提出了相應(yīng)的配準評價指標；最后通過實驗驗證的方法總結(jié)了不同因素對配準精度的影響，利用評價指標對實驗結(jié)果進行了分析總結(jié)。

1 面向三維激光點云的ICP配準算法

三維激光點云配準過程，就是求一個兩組點云之間的旋轉(zhuǎn)平移矩陣(剛性變換或歐式變換)，將源點云變換到目標點云相同的坐標系下[7]。三維點云配準原理可以表示為方程:Pt=RPs+t；其中，Pt、Ps就是目標點云與源點云中的一對對應(yīng)點。其中，R是旋轉(zhuǎn)矩陣，t是平移矩陣。圖1所示為三維點云配準的算法流程圖。

1.1 ICP算法原理介紹

ICP算法是目前三維空間點云配準中應(yīng)用最為廣泛的配準算法，其目的在于尋找最小二乘逼近的三維空間坐標變換矩陣。ICP算法本質(zhì)上是基于最小二乘法的最優(yōu)配準方法，該算法通過選擇對應(yīng)關(guān)系點對并反復(fù)進行迭代，直到滿足算法的收斂條件，從而得到最優(yōu)的剛體變換。

定義2個給定的點云集合：源點云集P，目標點云集Q，pi∈P,qj∈Q,其中i=1,2,…,M,j=1,2,…,N，M和N分別為點集P和Q的大小。尋找一個變換矩陣T，使得源點云中的點經(jīng)過T變換，能夠與目標點云中的點距離誤差最小，目標函數(shù)如式(1)所示, 其中,pi∈P，qi為pi在Q點集中找到的配準點，L為合適的配準對的個數(shù)。

(1)

本文使用的ICP算法迭代收斂的約束條件主要有3個：最大迭代次數(shù)、兩次變換矩陣之間的差值(ΔT)和均方誤差(Mean Squared Error，MSE)。ICP算法的具體實現(xiàn)步驟如下：

1)獲取2個待配準的點云P和Q，變換矩陣為T(初始值默認為單位陣)，首先將點云集P使用變換矩陣T進行變換，得到新的點集P′；

2)建立新點集P′和Q的相關(guān)性，利用點云關(guān)聯(lián)算法，從Q中尋找pi的對應(yīng)最近點qi；

3)構(gòu)建最小二乘的約束條件

(2)

4)求解變換矩陣ΔT，得到新的變換矩陣T=ΔT·T；

5)返回步驟1)，重復(fù)以上步驟，直到滿足收斂條件。

1.2 目標函數(shù)求解分析

傳統(tǒng)ICP算法的目標函數(shù)求解常用奇異值分解(Singular Value Decomposition，SVD)的方法。根據(jù)1.1節(jié)中ICP算法的問題描述[8]，首先定義第i對點的誤差項ei：

ei=qi-(Rpi+t)

(3)

其中，pi是源點云集中的任一點，qi是目標點云集中與pi對應(yīng)的任一點。

然后構(gòu)建最小二乘式(4)，求使得誤差平方和達到極小的旋轉(zhuǎn)矩陣R和平移向量t：

(4)

定義源點集和目標點集的質(zhì)心：

(5)

將2組點集的質(zhì)心代入式(4)進行處理，則誤差函數(shù)可以簡化成如下形式

(6)

由式(6)可以看出，第一項只與旋轉(zhuǎn)矩陣R有關(guān)，而第二項既有R也有t，但只和質(zhì)心相關(guān)。因此先求得R，再令第二項為0就能得到t。

(7)

則求最優(yōu)變換矩陣T的問題轉(zhuǎn)為先求解式(8)中的最優(yōu)旋轉(zhuǎn)矩陣

(8)

對式(8)展開如下：

由上式可以看出，第一項和第二項均與R無關(guān)，那么問題轉(zhuǎn)化為：

(9)

W=UΣVT

(10)

其中，Σ為奇異值組成的對角矩陣，對角線元素從大到小排列，而U和V為對角矩陣。當W滿秩時，R=UVT為所求最優(yōu)值，根據(jù)R繼而求得t。

2.ICP配準的影響因素及評價指標

2.1 ICP配準的影響因素

ICP配準算法主要分為兩部分：三維點云關(guān)聯(lián)算法和約束條件構(gòu)建求解算法。三維點云關(guān)聯(lián)算法的目標在于盡可能地尋找相似的點對，并剔除異常點對。目前已有的關(guān)聯(lián)點對的算法有尋找近鄰點的KD-Tree算法[9]、八叉樹算法[10]以及利用各種描述子的相似性，例如浙江大學(xué)戴靜蘭提出的利用曲率特征提高ICP算法的效率和算法的穩(wěn)定性[11]。

目前ICP及其變種的配準方法構(gòu)建的約束算法都是基于最小二乘算法，基于最小二乘的迭代收斂算法是一個容易陷入局部優(yōu)化的算法。在實際實驗中，在不提供相對準確的初值情況下，當轉(zhuǎn)彎角度或者移動距離較大時，經(jīng)常會出現(xiàn)ICP算法陷入局部收斂而導(dǎo)致配準失敗的情況。為了提高配準效率和準確度，通常借助于其他傳感器如IMU[12]給ICP提供初值。本文主要選取旋轉(zhuǎn)角度和平移距離2個參數(shù)作為影響因素對ICP配準效果進行分析。

2.2 ICP配準的評價指標

ICP算法的配準實則是一種優(yōu)化策略的配準過程，算法要求兩幀點云數(shù)據(jù)都存在彼此之間的對應(yīng)點，即一個點集是另一個點集的嚴格子集。尋找對應(yīng)點對的過程是ICP算法計算代價最大的一步，點云數(shù)據(jù)間若只存在部分重疊區(qū)，則能夠找尋到的正確的配準點對的數(shù)量是有限的，同時正確配準點對的數(shù)量會直接影響ICP算法的配準精度。因此,本文將在實驗部分對不同平移和旋轉(zhuǎn)參數(shù)下的2組點云集計算正確配準的點對數(shù)量，并將迭代過程中配準點對數(shù)量的變化趨勢作為ICP配準的一個評價指標進行分析和比較。

(11)

S的值越小，則認為ICP的配準效果越好，本文測試實驗效果的得分低于0.01時，可以認為配準精度較高。

3 實驗驗證

3.1 實驗說明

本文的實驗平臺為Ubuntu16.04系統(tǒng)和Velodyne16線激光采集平臺，為保證實驗結(jié)果的準確性，選擇多組三維激光點云數(shù)據(jù)進行實驗測試。實驗數(shù)據(jù)包含2組室內(nèi)外場景的開源數(shù)據(jù)集和2組Velodyne16線實地采集的室內(nèi)外數(shù)據(jù)集。將原始數(shù)據(jù)作為源點云，對源點云進行旋轉(zhuǎn)平移，并加入高斯白噪聲，作為目標點云。測試算法使用經(jīng)典ICP算法，同時使用KD-Tree算法對最近鄰點搜索進行加速。

在實際機器人運動中，機器人主要是進行平面上的運動，因此考慮偏航角和平面位移對ICP配準效果的影響。設(shè)置ICP的迭代次數(shù)為100，確保足夠的迭代次數(shù)，ΔT收斂閾值為10-12，誤差均方差為10-12。

首先經(jīng)過數(shù)據(jù)測試，得到每組數(shù)據(jù)集的得分閾值為0.03，得分越高，則配準效果越差，得分超過0.03則判定為配準失效，結(jié)束進程。圖2～圖5所示為各個場景數(shù)據(jù)集的配準效果圖，紅色為源點云，綠色為目標點云。圖2所示為PCL庫[13]提供的室內(nèi)房間激光點云測試數(shù)據(jù)；圖3所示為KITTI數(shù)據(jù)集中提取的一幀十字路口的激光點云數(shù)據(jù)；圖4所示為Velodyne16線實地采集的實驗室場景；圖5所示為實地采集的室外場景。實驗測試前，在空間直角坐標系下，將源點云繞z軸旋轉(zhuǎn)30°后沿x軸和y軸方向均平移10m得到目標點云，并加入高斯白噪聲。

3.2 實驗結(jié)果及分析

為測試旋轉(zhuǎn)角度和平移距離2個參數(shù)對ICP配準算法的影響效果，分別設(shè)定單個變量去評估ICP算法的配準精度。

實驗一：固定x軸和y軸方向的平移距離為1m，從零開始逐步增加偏航角度，步長為1°，直到ICP配準失效，圖6所示為各個場景數(shù)據(jù)集的配準得分折線圖。

實驗二：根據(jù)實驗一得到的旋轉(zhuǎn)角度的變化范圍，除KITTI數(shù)據(jù)集點云的旋轉(zhuǎn)角度設(shè)置為20°，其他場景的旋轉(zhuǎn)角度固定為30°，y軸方向的固定位移均為1m，逐步增大x軸方向上的位移，步長為1m，得到ICP算法隨x軸方向位移增大的配準得分折線圖，實驗結(jié)果如圖7所示。

實驗三：選取圖2所用的房間數(shù)據(jù)集作為測試數(shù)據(jù)集，得到不同旋轉(zhuǎn)和平移參數(shù)下的正確配準點對數(shù)量的變化趨勢。設(shè)定2組點集之間x和y方向的位移均為1m，旋轉(zhuǎn)角度只考慮偏航角，從零開始依次增大到80°。實驗結(jié)果如圖8、圖9所示。

從圖6和圖7的實驗結(jié)果中可以看出，雖然各個測試數(shù)據(jù)集場景的折線拐點不同，但圖中折線的變化走勢基本一致。當旋轉(zhuǎn)角度或者平移距離在一定范圍內(nèi)變化時，ICP配準精度得分有所波動，但均能達到較好的配準效果。如圖2～圖5，配準得分低于0.01的實驗效果圖，四類場景數(shù)據(jù)均達到較好的配準效果。這說明ICP算法在一定的角度變換和位移內(nèi)，配準結(jié)果可靠，不易陷入局部優(yōu)化。

當超過一定的角度變換和位移時，ICP算法在沒有提供準確初值的情況下，即使參與配準的點云數(shù)量足夠多，配準也會失效，如圖6和圖7所示的得分會在拐點處急劇增加，表1所示為四類場景數(shù)據(jù)集中相應(yīng)拐點處的得分指標。

表1 拐點處ICP配準得分

參與配準的點對數(shù)量和點對質(zhì)量會直接影響ICP的配準效果，本文以算法每次迭代中正確配準的點對數(shù)量變化來代表點對質(zhì)量對ICP配準效果的影響，計算配準真值點與ICP搜索到的配準點的距離差值，在一定差值范圍內(nèi)，則認為該點對配準成功，實驗中閾值設(shè)定為0.5。經(jīng)典ICP算法中使用KD-Tree算法關(guān)聯(lián)對應(yīng)點對，實驗三中根據(jù)經(jīng)驗值設(shè)置KD-Tree的距離閾值以確保足夠的配準點對。圖8是偏航角分別為20°、40°、60°時每次迭代的正確匹配對個數(shù)，圖9是偏航角為65°、70°、75°時每次迭代的正確匹配對個數(shù)。當偏航角大于等于65°時，ICP配準失效。在65°偏航角時的ICP配準失效結(jié)果如圖10所示，此時得分指標為1.58575。

由圖8和圖9可以提出用正確配準點對數(shù)量的變化趨勢作為ICP配準效果的評價指標。

圖8為ICP配準成功情況下的實驗結(jié)果圖，從圖中可以看出，正確配準的點對數(shù)量呈增長趨勢，直到參與配準的點對全部完成配準從而收斂。其中，每段折線都存在一個正確配準點對急劇增長的中間過程，這說明ICP找尋配準點對的方向是正確的。圖9為ICP配準失效情況下每次迭代中正確配準點對數(shù)量，初始狀態(tài)下雖然正確配準點對的數(shù)量有所增加，但漲幅變化有限，到達峰值后點對數(shù)量下降，最終趨于穩(wěn)定且數(shù)量較少。因此，在多次迭代之后，若正確點對個數(shù)增長幅度不大且有下降趨勢，則可以認為ICP配準失效。

4 結(jié)束語

三維激光點云配準方法是激光SLAM研究中極其重要的內(nèi)容，局部點云配準精度的提高有利于整個SLAM過程中的軌跡恢復(fù)[14]。本文通過仿真移動距離參數(shù)和旋轉(zhuǎn)角度參數(shù)進行測試實驗，在此基礎(chǔ)上分析ICP的配準效果，研究ICP配準的影響因素；在對實驗結(jié)果進行分析的過程中，得到誤差距離和正確配準點對數(shù)量變化趨勢這2個配準的評價指標。實驗結(jié)果表明，ICP配準算法可以在一定角度和距離的變化范圍內(nèi)避免受到局部最優(yōu)的影響，同時，正確的配準點對數(shù)量對于ICP是否失效也有著重大影響。從實驗結(jié)果中可以得出2個評價ICP是否失效的評判標準，根據(jù)ICP的計算結(jié)果得分，若超過經(jīng)驗閾值(一般設(shè)為傳感器精度)，則ICP配準失效；或者根據(jù)每次迭代的正確配準點對的變化趨勢來提前預(yù)判ICP的結(jié)果。這對研究SLAM技術(shù)中關(guān)鍵幀的篩選具有一定的參考價值。目前國內(nèi)外學(xué)者都對ICP算法在對應(yīng)點對的關(guān)聯(lián)性和配準的收斂速度上作了改進，提高了ICP算法的配準精度和配準效率[15]，但在SLAM應(yīng)用上仍需要對ICP配準算法的魯棒性和配準效率做進一步提升。