基于天牛須改進粒子群算法的點云配準方法

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

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

引 言

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

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

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

1 點云分布熵原理

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

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

Fig.1 Specific steps for calculating point cloud distribution entropy

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

(1)

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

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

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

(2)

式中，N為兩組點云的總點云數量。點云分布熵的表達形式為：

(3)

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

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

如圖3所示，SDE與MSE都能很好地反映兩組點云的重合程度，當旋轉角度為0°時，SDE與MSE均為最小值，適合作為點云配準的評價標準。

Fig.2 SDE and MSE curve with rotation angle α

Fig.3 SDE curve with rotation angle α and β

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

Table 1 Compare between SDE and MSE calculation time

2 基于BAS-PSO算法的點云配準方法

2.1 點云配準

針對點云配準問題，其實質就是尋找能使兩組點云數據盡可能重合的旋轉矩陣[21]。由激光雷達采集獲得的激光點云數據因采集視角不同，同一物體的點云數據在空間上存在著剛性變換關系，即兩組點云數據中對應點都可通過同一種空間坐標變換方式使之盡可能重合，數學表達式如下式所示：

(4)

式中，p表示目標點云數據，q表示待配準點云數據，R為旋轉矩陣，T為平移矩陣。在點云粗配準中，平移矩陣可通過中心化的方式，將兩組點云數據的質心移至坐標原點來盡可能的消除平移誤差，即：

(5)

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

(6)

尋找點云配準的最優旋轉矩陣即尋找最優的旋轉角度α,β,γ。

2.2 BAS-PSO算法

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

粒子群優化算法中粒子速度v與位置x更新的數學表達如下式所示：

(7)

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

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

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

天牛須搜索算法的數學表達如下：

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

s=cd0

(8)

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

d=rands(n,1)

(9)

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

xl-xr=d0d/‖d‖

(10)

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

(11)

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

(12)

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

(13)

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

s=θs

(14)

式中，θ為設置的比例系數，一般取值為0.95，循環(2)步、(3)步即可獲得最優解。

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

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

Fig.4 The flowchart of BAS-PSO

3 實驗結果與分析

為驗證算法可行性及優化效果，作者在Intel Core-i7，8 GB內存的Windows 10操作系統上使用MATLAB R2018a軟件對斯坦福大學點云數據庫中的bunny,horse,dragon點云進行點云配準實驗，以不同視角下的點云數據P,Q為操作對象，使用以點云分布熵為優化目標，基于BAS-PSO算法進行點云粗配準。基于經驗對算法中的參量設置如表2所示。

為觀測BAS-PSO算法中每一代更新后SDE的優化效果，提取出每一代配準后點云的SDE，以bunny模型為例，構建了SDE隨進化次數變化的曲線圖，如圖5所示。

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

Table 2 Algorithm parameter setting

Fig.5 Evolutionary curve with SDE as the optimization goal

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

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

Table 3 Registration time of different point cloud coarse registration method

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

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 結 論

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

針對算法中關鍵參量如搜索步長和學習參量的設定為基于經驗的人工手動設定，無法確定是否達到算法最優，尋找一種自動化設置關鍵參量的方法，保證算法執行效率，提升點云配準效率是下一步需要繼續改進的方向。