基于天牛須改進(jìn)粒子群算法的點云配準(zhǔn)方法

陳斯祺，張海洋，趙長明，張子龍，王文鑫，張 明

(北京理工大學(xué) 光電學(xué)院 光電成像技術(shù)與系統(tǒng)教育部重點實驗室，北京 100081)

引 言

激光雷達(dá)近年來在3-D場景感知及重建方面得到了廣泛的應(yīng)用，例如地形勘探、遺跡保護(hù)、城市建模、智能駕駛以及電力巡線等[1-6]，由激光雷達(dá)掃描獲取的3-D點云數(shù)據(jù)經(jīng)過點云濾波、特征點匹配、點云配準(zhǔn)等步驟后獲得,用于還原3-D場景[7-9]。點云配準(zhǔn)是3維場景重建中至關(guān)重要的環(huán)節(jié)，配準(zhǔn)方法可分為：手動配準(zhǔn)、依賴于儀器的配準(zhǔn)以及自動配準(zhǔn)[10]。由于場景重建所需要的數(shù)據(jù)量較大，點云數(shù)量較多，手動配準(zhǔn)與依賴于儀器的配準(zhǔn)效率太低，一般采用自動配準(zhǔn)方法[11]。自動配準(zhǔn)方法所使用的算法不同，如何優(yōu)化配準(zhǔn)時間與精度成為了點云配準(zhǔn)的主要研究方向。

最經(jīng)典的點云配準(zhǔn)方法是由BESL和McKAY提出的最近迭代點(iterative closest point,ICP)算法[12]，它能精度較高地實現(xiàn)點云配準(zhǔn)，但計算量大，對點云數(shù)據(jù)的初始條件要求較高，且容易陷入局部最優(yōu)。在ICP算法的基礎(chǔ)上，眾多學(xué)者對ICP算法進(jìn)行了改進(jìn)：BOUAZIZ等人提出的稀疏 ICP(sparse iterative closest point,SICP)算法[13]，削弱了離群點對配準(zhǔn)效果的影響，提高了配準(zhǔn)精度；RUSINKIEWICZ等人提出利用點-面對應(yīng)來代替點-點對應(yīng)[14]，可以大大提高算法的穩(wěn)健性和收斂精度。在優(yōu)化算法方面，WU等人嘗試使用二進(jìn)制編碼的標(biāo)準(zhǔn)遺傳算法(standard genetic algorithm,SGA)進(jìn)行配準(zhǔn),但這一算法搜索空間大,匹配時間長[15]；DUAN等人使用粒子群優(yōu)化算法尋找對應(yīng)點后使用ICP算法實現(xiàn)散亂點云配準(zhǔn)[16]，速度較快但易陷入局部最優(yōu)。

現(xiàn)階段點云自動配準(zhǔn)方法多數(shù)采用尋找特征點的方式進(jìn)行匹配，優(yōu)化特征點間的對應(yīng)關(guān)系來實現(xiàn)點云配準(zhǔn)，此方法計算量較大且僅在特征點較明顯的場景下可以獲得較好的結(jié)果。在優(yōu)化算法層面，粒子群算法(particle swarm optimization,PSO)搜索速度快、效率高，算法簡單，但對于離散優(yōu)化問題處理效率不高，容易陷入局部最優(yōu)[17]。天牛須搜索算法(beetle antennae search,BAS)在全局搜索最優(yōu)解方面具有優(yōu)勢，可彌補(bǔ)粒子群算法易陷入局部最優(yōu)的問題[18]。為解決以上問題提出一種基于天牛須改進(jìn)的粒子群算法(particle swarm optimization algorithm improved by beetle antennae algorithm,BAS-PSO)，以優(yōu)化點云分布熵(spatial distribution entropy,SDE)為目標(biāo)，尋找能夠使點云分布熵最小的空間坐標(biāo)變化關(guān)系，對于任意未知對應(yīng)關(guān)系的兩組點云數(shù)據(jù)，獲得其旋轉(zhuǎn)矩陣以實現(xiàn)點云粗配準(zhǔn)，為精配準(zhǔn)提供良好的初始值。

1 點云分布熵原理

本文中使用八叉樹建立3-D立方柵格。將初始兩組點云數(shù)據(jù)放至同一空間中,對兩組點云數(shù)據(jù)分別進(jìn)行中心化處理，將質(zhì)心移至坐標(biāo)原點后對合成的點云數(shù)據(jù)建立3-D立方柵格。由于每個柵格大小一致，而包含在柵格中的點云數(shù)量不同，即可通過同一柵格中點云數(shù)量的大小來表示在該區(qū)域內(nèi)點云的稀疏程度,點云數(shù)量大表明該區(qū)域點云密集，數(shù)量小則表明該區(qū)域點云稀疏。

點云分布熵可用于描述點云在空間中的位置關(guān)系，通過密集程度反映點云的分布情況[19]。點云數(shù)據(jù)的采集往往是在不同視角的條件下進(jìn)行的，點云數(shù)據(jù)之間并沒有明顯的對應(yīng)關(guān)系[20]，在經(jīng)歷配準(zhǔn)操作后，點云數(shù)據(jù)會相對集中而非表現(xiàn)出配準(zhǔn)前的不確定狀態(tài)，這表現(xiàn)在兩組點云數(shù)據(jù)相對距離最小，且空間中的位置關(guān)系唯一。點云信息熵可準(zhǔn)確反映兩組點云在配準(zhǔn)過程中位置變換關(guān)系，故適用于點云配準(zhǔn)過程中的優(yōu)化對象。求解點云分布熵的具體步驟如圖1所示。

Fig.1 Specific steps for calculating point cloud distribution entropy

在點云空間中，按照柵格數(shù)量切分，將同一空間下的兩個點云數(shù)據(jù)切分為2M×2M×2M個柵格，即每個柵格的大小為：

(1)

式中,Xmax,Ymax,Zmax為點云數(shù)據(jù)坐標(biāo)軸方向的最大值，Xmin,Ymin,Zmin為最小值，x,y,z為每一個立體柵格沿坐標(biāo)軸方向的長度。

將點云數(shù)據(jù)按照邊界以及柵格數(shù)量進(jìn)行柵格化后，統(tǒng)計落入每一個柵格中點云數(shù)量n(i,j,k) ，求得其分布概率p(i,j,k) :

p(i,j,k)=n(i,j,k)/N

(2)

式中，N為兩組點云的總點云數(shù)量。點云分布熵的表達(dá)形式為：

(3)

相比ICP算法通過求算均值平方差(mean square error,MSE)來評價點云配準(zhǔn)的精度，點云分布熵的求算方式時間復(fù)雜度更小。MSE的求算方法需要為待配準(zhǔn)點云中每一個點在目標(biāo)點云中找尋與之距離最小的點，這需要多次全局遍歷，而點云分布熵的評價方式只需進(jìn)行一次遍歷，運(yùn)算速度有較大提高。

對完全重合的兩組bunny點云數(shù)據(jù)P,Q進(jìn)行點云分布熵計算操作。在-180°～180°范圍內(nèi)將Q繞x軸旋轉(zhuǎn),每1°生成一個新的點云數(shù)據(jù)Qm，Qm與P共同組成新的點云數(shù)據(jù)Tm，計算Tm的點云分布熵ESDE和均值平方差EMSE。下標(biāo)m表示旋轉(zhuǎn)角α,β,γ的角度。以旋轉(zhuǎn)角度α為橫坐標(biāo)，ESDE與EMSE為縱坐標(biāo)建立與旋轉(zhuǎn)角度的對應(yīng)關(guān)系，如圖2所示。Q繞x軸與y軸旋轉(zhuǎn)計算兩個維度變換的點云分布熵。

如圖3所示，SDE與MSE都能很好地反映兩組點云的重合程度，當(dāng)旋轉(zhuǎn)角度為0°時，SDE與MSE均為最小值，適合作為點云配準(zhǔn)的評價標(biāo)準(zhǔn)。

Fig.2 SDE and MSE curve with rotation angle α

Fig.3 SDE curve with rotation angle α and β

在運(yùn)算速度方面，對不同大小的點云數(shù)據(jù)，點云分布熵的計算方式都明顯優(yōu)于均值平方差的計算方式，實驗數(shù)據(jù)如表1所示。

Table 1 Compare between SDE and MSE calculation time

2 基于BAS-PSO算法的點云配準(zhǔn)方法

2.1 點云配準(zhǔn)

針對點云配準(zhǔn)問題，其實質(zhì)就是尋找能使兩組點云數(shù)據(jù)盡可能重合的旋轉(zhuǎn)矩陣[21]。由激光雷達(dá)采集獲得的激光點云數(shù)據(jù)因采集視角不同，同一物體的點云數(shù)據(jù)在空間上存在著剛性變換關(guān)系，即兩組點云數(shù)據(jù)中對應(yīng)點都可通過同一種空間坐標(biāo)變換方式使之盡可能重合，數(shù)學(xué)表達(dá)式如下式所示：

(4)

式中，p表示目標(biāo)點云數(shù)據(jù)，q表示待配準(zhǔn)點云數(shù)據(jù)，R為旋轉(zhuǎn)矩陣，T為平移矩陣。在點云粗配準(zhǔn)中，平移矩陣可通過中心化的方式，將兩組點云數(shù)據(jù)的質(zhì)心移至坐標(biāo)原點來盡可能的消除平移誤差，即：

(5)

旋轉(zhuǎn)矩陣R可通過點繞x軸、y軸、z軸旋轉(zhuǎn)的角度α,β,γ確定，其表達(dá)方式為：

(6)

尋找點云配準(zhǔn)的最優(yōu)旋轉(zhuǎn)矩陣即尋找最優(yōu)的旋轉(zhuǎn)角度α,β,γ。

2.2 BAS-PSO算法

粒子群優(yōu)化算法是模擬鳥群捕食行為的一種尋優(yōu)算法[22]，它的基本思想是將每一個個體視為n維空間中沒有質(zhì)量和體積的粒子，粒子包含位置與飛行速度兩個屬性，每一個粒子用一個n維矢量表示，可以視為n維空間中的一個尋優(yōu)解，在尋優(yōu)過程中，每個粒子以自身的飛行速度更新自己的位置，通過記錄每個位置的適應(yīng)度來確定個體極值pbest和粒子群體的群體極值gbest，找到全局最優(yōu)解并由此來調(diào)整自己的位置與速度，適應(yīng)度由目標(biāo)函數(shù)決定。粒子群優(yōu)化算法通過群體尋優(yōu)的正反饋機(jī)制，根據(jù)個體與群體的對目標(biāo)函數(shù)的適應(yīng)度不斷調(diào)整個體狀態(tài)，將個體逐步遷移至較優(yōu)區(qū)域，從而獲得目標(biāo)函數(shù)的最優(yōu)解[23]。

粒子群優(yōu)化算法中粒子速度v與位置x更新的數(shù)學(xué)表達(dá)如下式所示：

(7)

式中,c1和c2為非負(fù)的學(xué)習(xí)參量;r1,r2為取值范圍在(0,1)之間的隨機(jī)數(shù)，以保證群體的多樣性;t表示迭代次數(shù);pi,best為粒子群中第i個粒子所記錄的個體適應(yīng)度極值;gbest為當(dāng)前整個粒子群中記錄的適應(yīng)度極值，通過設(shè)置適合的學(xué)習(xí)參量并不斷迭代逐步獲得問題的最優(yōu)解。

粒子群優(yōu)化算法雖然在收斂速度上具有優(yōu)勢，但由于缺乏粒子速度的動態(tài)調(diào)節(jié)，容易陷入局部最優(yōu)，導(dǎo)致在收斂后期的收斂精度低和不易收斂[24]。針對以上問題，可使用天牛須搜索算法為粒子速度調(diào)整提供自主尋優(yōu)的能力。

天牛須搜索算法是2017年提出的一種新型仿生優(yōu)化算法，在搜索速度和全局搜索方面有一定的優(yōu)勢，可用于粒子群算法粒子速度調(diào)節(jié)優(yōu)化[24]，其仿生學(xué)原理為：自然界里天牛在覓食的過程中，由于不知道食物位置，只能通過氣味來尋找食物的大致方向。天牛有左右兩個觸須，它可通過左右兩觸須所探測到的氣味強(qiáng)度來判斷食物在自身位置的左右方位，例如當(dāng)左須探測到的氣味比右觸須探測到的氣味更強(qiáng)時，天牛向左觸須方向移動， 移動一段距離后再次使用左右觸須探測當(dāng)前位置食物氣味直至找到氣味最強(qiáng)即食物的位置。在天牛須搜索算法中，食物的具體位置即為尋優(yōu)的極值點，食物氣味相當(dāng)于尋優(yōu)函數(shù)本身，通過逐步逼近的方式獲得最優(yōu)解。

天牛須搜索算法的數(shù)學(xué)表達(dá)如下：

(1)在n維空間中設(shè)置質(zhì)心位置為x，其左觸須位置為xl，右觸須位置為xr，用d0表示兩須之間的距離，根據(jù)步長與兩須間距離成正比這一特點，可設(shè)置步長為：

s=cd0

(8)

式中，c是一個設(shè)定比例。由于質(zhì)心的方向是隨機(jī)的，即右觸須指向左觸須的向量也是隨機(jī)的，所以用一個隨機(jī)n維向量來表示右觸須指向左觸須的方向，即:

d=rands(n,1)

(9)

(2)左右觸須長度相同，且連線方向的法向量指向質(zhì)心，則可以根據(jù)質(zhì)心位置x，兩須間距d0以及右觸須指向左觸須的向量d表達(dá)左右觸須的位置。將d歸一化處理后可以得到：

xl-xr=d0d/‖d‖

(10)

xl=x+d0d/(‖d‖2)

(11)

xr=x-d0d/(‖d‖2)

(12)

(3)對于目標(biāo)函數(shù)f，分別求得xl和xr兩個位置的值fl和fr,并比較兩個值的大小，為尋求最小值，則當(dāng)flfr時，質(zhì)心向右須方向移動距離s，可表示為：

(13)

質(zhì)心移動后，按照比例改變步長大小：

s=θs

(14)

式中，θ為設(shè)置的比例系數(shù)，一般取值為0.95，循環(huán)(2)步、(3)步即可獲得最優(yōu)解。

基于天牛須改進(jìn)的粒子群算法的基本思路是將粒子群中個體最優(yōu)適應(yīng)度值的比較過程改為天牛須搜索算法尋優(yōu)，通過這種方式更新個體與群體的最優(yōu)適應(yīng)度值。使用BAS-PSO算法，以點云分布熵作為優(yōu)化目標(biāo)求解獲得最佳的空間變換關(guān)系，設(shè)置旋轉(zhuǎn)角度[α,β,γ]為目標(biāo)解，通過尋找點云分布熵值最小時對應(yīng)的解[α,β,γ] 來獲得點云配準(zhǔn)時最優(yōu)的旋轉(zhuǎn)矩陣實現(xiàn)點云粗配準(zhǔn)。

該算法的操作步驟如圖4所示。

Fig.4 The flowchart of BAS-PSO

3 實驗結(jié)果與分析

為驗證算法可行性及優(yōu)化效果，作者在Intel Core-i7，8 GB內(nèi)存的Windows 10操作系統(tǒng)上使用MATLAB R2018a軟件對斯坦福大學(xué)點云數(shù)據(jù)庫中的bunny,horse,dragon點云進(jìn)行點云配準(zhǔn)實驗，以不同視角下的點云數(shù)據(jù)P,Q為操作對象，使用以點云分布熵為優(yōu)化目標(biāo)，基于BAS-PSO算法進(jìn)行點云粗配準(zhǔn)?；诮?jīng)驗對算法中的參量設(shè)置如表2所示。

為觀測BAS-PSO算法中每一代更新后SDE的優(yōu)化效果，提取出每一代配準(zhǔn)后點云的SDE，以bunny模型為例，構(gòu)建了SDE隨進(jìn)化次數(shù)變化的曲線圖，如圖5所示。

從圖5中可以看出，隨著粒子種群的進(jìn)化，SDE不斷減小并向優(yōu)化方向進(jìn)行，在第30次更新種群后，SDE趨于平穩(wěn)，其值接近于兩點云重合時計算獲得的SDE值，證明BAS-PSO算法是一種有效的優(yōu)化算法。

Table 2 Algorithm parameter setting

Fig.5 Evolutionary curve with SDE as the optimization goal

針對部分缺失的點云數(shù)據(jù)以及含有噪聲的點云數(shù)據(jù)，本文中的算法依舊有較好的魯棒性，配準(zhǔn)效果如圖6所示。

使用BAS-PSO算法配準(zhǔn)效果與主成分分析法(principal component analysis,PCA)、四點集法(4-point congruent sets,4PCS)以及基于遺傳算法的粗配準(zhǔn)算法進(jìn)行對比，配準(zhǔn)后的模型如圖7所示。對比實驗結(jié)果如表3所示。

Table 3 Registration time of different point cloud coarse registration method

由仿真結(jié)果可知，在粗配準(zhǔn)效果上，BAS-PSO算法與4PCS算法以及基于遺傳算法的粗配準(zhǔn)方法均明顯優(yōu)于PCA算法，在配準(zhǔn)速度上，雖然PCA算法速度快，但在考慮配準(zhǔn)效果的前提下，BAS-PSO算法優(yōu)于4PCS算法與基于遺傳算法的粗配準(zhǔn)方法，如圖8所示。針對不同大小的點云數(shù)據(jù)，BAS-PSO算法在配準(zhǔn)速度上同樣具有優(yōu)勢，且針對數(shù)據(jù)量大的點云數(shù)據(jù)，配準(zhǔn)速度優(yōu)勢越大。

Fig.6 Abnormal point cloud data registration effect

Fig.7 Registration effect of different point cloud coarse registration method

Fig.8 The curve of the registration time with the number of point clouds

4 結(jié) 論

提出了一種以點云分布熵為優(yōu)化目標(biāo)，基于天牛須改進(jìn)的粒子群算法用于激光點云數(shù)據(jù)的粗配準(zhǔn)。在優(yōu)化目標(biāo)上，點云分布熵計算量明顯小于傳統(tǒng)ICP算法所使用的均值平方差，且點云分布熵可以準(zhǔn)確反映點云配準(zhǔn)效果，可作為配準(zhǔn)算法中的尋優(yōu)目標(biāo)。在尋優(yōu)算法方面，BAS-PSO算法可以有效彌補(bǔ)天牛須搜索算法個體單一，尋優(yōu)步長收斂過快，以及粒子群算法易陷入局部最優(yōu)的問題，實現(xiàn)既精準(zhǔn)又快速的點云配準(zhǔn)。本文中通過對基于BAS-PSO算法的點云粗配準(zhǔn)方法與PCA算法、4PCS算法以及基于遺傳算法的點云粗配準(zhǔn)方法進(jìn)行了實驗對比，證實了BAS-PSO算法在配準(zhǔn)速度上的優(yōu)勢，在點云數(shù)據(jù)量較大的條件下，BAS-PSO算法也能實現(xiàn)點云的粗配準(zhǔn)，為ICP算法提供良好的初始狀態(tài)。

針對算法中關(guān)鍵參量如搜索步長和學(xué)習(xí)參量的設(shè)定為基于經(jīng)驗的人工手動設(shè)定，無法確定是否達(dá)到算法最優(yōu)，尋找一種自動化設(shè)置關(guān)鍵參量的方法，保證算法執(zhí)行效率，提升點云配準(zhǔn)效率是下一步需要繼續(xù)改進(jìn)的方向。