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

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

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

0 引言

近年來，隨著無人車技術(shù)[1]的發(fā)展，同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)[2]成為重要課題和研究熱點。由于傳感器的不同，SLAM主要分為視覺SLAM[3]和激光SLAM[4]。相較于RGB-D相機傳感器，激光雷達傳感器具有精度高、速度快，對于光照環(huán)境要求低的特點。目前，高分辨率多線激光雷達傳感器廣泛應用于機器人自主導航和三維場景重建兩大領(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)域已廣泛應用于全局場景重建的配準。由于ICP原始算法中存在很多缺陷和不足，后來就引發(fā)了對其算法的各種改進和各類變種算法的出現(xiàn)[6]。

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

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

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

1.1 ICP算法原理介紹

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

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

(2)

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

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

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對應的任一點。

然后構(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]以及利用各種描述子的相似性，例如浙江大學戴靜蘭提出的利用曲率特征提高ICP算法的效率和算法的穩(wěn)定性[11]。

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

2.2 ICP配準的評價指標

ICP算法的配準實則是一種優(yōu)化策略的配準過程，算法要求兩幀點云數(shù)據(jù)都存在彼此之間的對應點，即一個點集是另一個點集的嚴格子集。尋找對應點對的過程是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配準效果的影響。設置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配準算法的影響效果，分別設定單個變量去評估ICP算法的配準精度。

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

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

實驗三：選取圖2所用的房間數(shù)據(jù)集作為測試數(shù)據(jù)集，得到不同旋轉(zhuǎn)和平移參數(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ù)集中相應拐點處的得分指標。

表1 拐點處ICP配準得分

參與配準的點對數(shù)量和點對質(zhì)量會直接影響ICP的配準效果，本文以算法每次迭代中正確配準的點對數(shù)量變化來代表點對質(zhì)量對ICP配準效果的影響，計算配準真值點與ICP搜索到的配準點的距離差值，在一定差值范圍內(nèi)，則認為該點對配準成功，實驗中閾值設定為0.5。經(jīng)典ICP算法中使用KD-Tree算法關(guān)聯(lián)對應點對，實驗三中根據(jù)經(jīng)驗值設置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過程中的軌跡恢復[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)驗閾值(一般設為傳感器精度)，則ICP配準失效；或者根據(jù)每次迭代的正確配準點對的變化趨勢來提前預判ICP的結(jié)果。這對研究SLAM技術(shù)中關(guān)鍵幀的篩選具有一定的參考價值。目前國內(nèi)外學者都對ICP算法在對應點對的關(guān)聯(lián)性和配準的收斂速度上作了改進，提高了ICP算法的配準精度和配準效率[15]，但在SLAM應用上仍需要對ICP配準算法的魯棒性和配準效率做進一步提升。