基于NDT與ICP結合的點云配準算法

王慶閃，張 軍，劉元盛，張鑫晨

1.北京聯(lián)合大學 北京市信息服務工程重點實驗室，北京100101

2.北京聯(lián)合大學 機器人學院，北京100101

1 引言

SLAM（Simultaneous Localization and Mapping，時定位與地圖構建）技術，指的是將無人車放在一個完全未知的環(huán)境中，從一個未知位置開始移動并對環(huán)境進行增量式地圖創(chuàng)建，同時利用創(chuàng)建的地圖進行無人車自身的定位和導航。SLAM 問題的復雜性在于定位與構圖的相互依賴性。無人車的精確定位依賴于周圍環(huán)境地圖的一致性，同樣，高度一致性環(huán)境地圖的構建也以準確的定位為前提[1-2]。

由于無人車技術的發(fā)展，激光雷達作為無人車感知領域重要的傳感器之一，在無人車基于激光雷達的實時定位與建圖技術中具有重要的研究價值。由于激光雷達可以提供高頻率范圍測量，所以誤差相對恒定并與測量的距離無關。在激光雷達的唯一運動是旋轉(zhuǎn)激光束的情況下，可以實現(xiàn)點云的配準。另外使用激光雷達的一個關鍵優(yōu)勢是它對環(huán)境光線和場景中的光學文理不敏感。至今基于激光雷達的實時定位與建圖在無人車技術中仍然是一種流行的技術[3-5]。本文利用激光雷達來改善最小化測距估計中的漂移相關的問題。因此，文中重點介紹使用激光雷達來實現(xiàn)實時定位與地圖的構建。

基于激光雷達掃描相鄰幀，利用大量重復的點云信息求出幀與幀之間的轉(zhuǎn)換關系，使得兩幀點云之間的距離無限接近，此過程被稱為點云配準。ICP（Iterative Closest Point）算法[6]是點云配準的經(jīng)典算法，核心思想是求出相鄰幀點云之間的對應點，基于對應點構造旋轉(zhuǎn)與平移矩陣，求得幀間誤差函數(shù)，如誤差函數(shù)大于設置的閾值，則迭代找出最近的對應點距離，直到滿足給定的誤差為止。但是當激光雷達掃描的點云包含大量的點時，ICP 算法則消耗的時間成本太大，而且點云的初始位姿決定ICP算法的精確度。因此，許多學者構建了基于改進的ICP 算法[7]提高其算法的效率及準確性；文獻[8]提出了一種基于點到面的ICP算法，求點到局部平面之間的最小距離；文獻[9]提出了一種面到面的ICP算法，求出面到面之間的最短距離。除此之外，還有很多基于改進的ICP算法[10-13]來提高計算效率。通過以上算法對ICP 的改進，雖然提高了點云配準效率，但是也損失了配準精度。

由于激光雷達掃描中包含大量的點云數(shù)據(jù)，為了提高點云配準效率，文獻[14]提出了一種二維NDT（Normal Distribution Transform）點云配準算法，核心思想是一種快速空間模型技術，首先將點云投放在二維的單元格中，將每個二維的單元格內(nèi)的點云數(shù)據(jù)轉(zhuǎn)換成一個連續(xù)可微的概率密度分布函數(shù)，然后求出點云之間的配準。由于目前激光雷達掃描的數(shù)據(jù)是三維點云，文獻[15]提出了一種改進的NDT 算法并運用到三維空間中。NDT算法使用點云的密度函數(shù)進行點云配準，雖然該方法提高了計算效率，同樣損失了點云配準的精度。

基于特征的匹配方法因其從環(huán)境中提取有代表性的特征所需的計算資源較少而受到越來越多關注。文獻[16]和[17]提出了一種低漂移實時激光雷達建圖（Lidar Odometry and Mapping in real-time，LOAM）方法，核心思想是從點云中提取尖銳的邊和平整表面特征點，然后進行特征配準，基于特征的點到面的ICP 算法；文獻[18]提出了一種基于分割的匹配算法，首先將分段應用于點云，然后，根據(jù)特征值和形狀直方圖，計算出每個分段的特征向量進行點云匹配；文獻[19]提出了一種輕量級和地面優(yōu)化的激光雷達定位和建圖。雖然以上算法在某些場景中能夠提取有效的特征信息，但是經(jīng)過大量的測試，在非結構化復雜場景下或者單一場景中由于沒有明顯的特征，導致不準確的點云配準和較大的漂移。

通過以上算法的分析與實車的大量測試，Point-to-PointICP（ICP）點到點最近迭代、Point-to-LineICP（PL-ICP）點到線最近迭代、Point-to-PlaneICP（P-ICP）點到面最近迭代進行相鄰幀點云之間配準時精度相對較高，但是損失了算法計算效率；NDT 算法配準精度僅次于ICP、PL-ICP、P-ICP 算法的點云配準精度，但是NDT 算法計算效率相對較高。因此，本文構建了一種快速的二步式點云配準算法，經(jīng)過點云數(shù)據(jù)預處理之后，首先利用NDT算法對進行相鄰兩幀點云配準，為ICP最近點迭代提供了良好的初始位姿狀態(tài)，然后利用ICP最近點迭代進行兩幀點云位姿的校正，實現(xiàn)無人車的位姿精確估計，進而完成點云地圖的構建，如圖1 所示。文中構建的算法在北京聯(lián)合大學場景中以小旋風無人車為平臺進行了驗證，結果表明了算法是精確、有效的。

圖1 系統(tǒng)概述

2 相關點云配準算法描述

本文算法結合了NDT與ICP各自的優(yōu)點，構建了兩者相結合的快速點云配準與建圖，其中本文采用ICP最近點迭代點云配準方法進行相鄰幀點云之間的配準，以下分別介紹NDT 算法與ICP 最近點迭代算法的基本思想與流程。

2.1 NDT點云配準算法描述

NDT算法是把一個立方體內(nèi)大量離散點的數(shù)據(jù)集表示為一個分段連續(xù)可微的概率密度函數(shù)。首先將點云空間分為相同的若干個立方體，并滿足立方體內(nèi)至少有5個點云，分別求出每個立方體內(nèi)點的均值q 和協(xié)方差矩陣Σ：

其中，Xi=1,2,…,n 為點云集合，n 為點云個數(shù)。

然后以概率密度的形式對離散的點云進行分段連續(xù)可微表示，則通過NDT 算法表示立方體每個點的概率密度：

通過使用Hessian矩陣法解決車載激光雷達掃描的點云數(shù)據(jù)相鄰幀之間的配準，其點云的配準的本質(zhì)就是根據(jù)無人車在不同位置采集的點云數(shù)據(jù)來獲取坐標變換的關系。NDT點云配準具體算法如下：

（1）計算第一幀激光雷達掃描點云集的NDT；

（2）初始化坐標變換參數(shù)T：

其中R 是3×3的旋轉(zhuǎn)矩陣，t 為3×1的平移矩陣。

（3）將第二幀激光掃描點云集根據(jù)坐標轉(zhuǎn)換參數(shù)映射到第一幀坐標系中，并得到映射后的點云集合

（4）求每個點映射變換后的正態(tài)分布；

（5）將每個點的概率密度相加，去評估坐標的變換參數(shù)：

（6）使用Hessian矩陣法優(yōu)化s(p)。

（7）跳轉(zhuǎn)到第（3）步繼續(xù)執(zhí)行，直到滿足收斂條件為之。

2.2 ICP點云配準算法描述

ICP 算法本質(zhì)就是求解兩組點云數(shù)據(jù)P 與Q 之間的空間變換，使得兩點云模型之間的距離最小。在待配準的兩組點云數(shù)據(jù)的重疊區(qū)域內(nèi)，分別選取兩個點集來表示源點集和目標點集，其中P 為源點集，Q 為目標點集，n 代表兩個點集中的點云個數(shù)。設旋轉(zhuǎn)矩陣為R，平移矩陣為t，用E(R,t)來表示源點集P 在變換矩陣(R,t)下與目標點集Q 之間的誤差。則求解最優(yōu)變換矩陣的問題可以轉(zhuǎn)化為滿足min E(R,t)的最優(yōu)解(R,t)。

其中E(R,t)稱為目標函數(shù)，它表示的是兩個點集之間的差異程度。該目標函數(shù)可以通過下列公式來表示：

通過最小化目標函數(shù)來求解最優(yōu)變換矩陣，即R和t。

（1）計算目標點云P 中的每一個點在Q 點集中的對應最近點。

（2）求得使對應點對平均距離最小的變換，利用SVD分解求得旋轉(zhuǎn)參數(shù)R 和平移參數(shù)t，使得E(R,t)最小。

（3）對P 使用上一步求得的旋轉(zhuǎn)參數(shù)R 和平移參數(shù)t，得到新的變換點集P′。

（4）如果變換后的點集P′與Q 點集滿足目標函數(shù)要求，即兩點集的平均距離小于給定的閾值，則停止迭代計算；否則重新計算新的P′作為新的P 繼續(xù)迭代，直到滿足收斂條件為止。

3 基于NDT與ICP結合的快速點云配準與建圖

3.1 數(shù)據(jù)預處理

點云數(shù)據(jù)對無人車的定位與建圖至關重要，點云初始位姿狀態(tài)決定著點云配準算法的效率與精確度。在激光雷達進行環(huán)境數(shù)據(jù)采集時，由于激光雷達本身和環(huán)境等因素的干擾，激光雷達返回的數(shù)據(jù)具有數(shù)據(jù)量大、包含噪點、數(shù)據(jù)密度不均勻等特點。為了提高點云配準算法的實時性和準確性，對原始傳感器點云數(shù)據(jù)進行預處理。首先通過無人車搭載激光雷達進行數(shù)據(jù)收集，然后進行離群點移除、Voxel Grid 濾波[20]等處理。最后將處理后的數(shù)據(jù)輸入到點云配準算法進行后續(xù)處理。

3.2 基于NDT 與ICP 結合的快速點云配準實現(xiàn)無人車的精確定位

本文使二步法進行相鄰幀之間的點云配準，首先使用預處理之后的點云數(shù)據(jù)利用NDT 算法進行點云配準，對無人車的位姿進行粗估計，然后使用經(jīng)ICP 算法對NDT 算法進行校正點云配準的誤差，使得幀間誤差函數(shù)最優(yōu)，并完成對無人車位姿的精確估計。設經(jīng)過點云預處理之后t 時刻與t+1 時刻周圍環(huán)境點云分別為Xt和Xt+1，然后用Xt+1的點云與Xt的點云進行配準，算法如圖2所示。

圖2 NDT-ICP算法流程圖

NDT 點云配準是在Xt+1點云與Xt點云完全不知道任何初始相對位置的情況下，所進行的配準方法。主要目的是在初始條件未知的情況下，通過NDT 算法快速估算一個粗略的點云配準矩陣T。經(jīng)過NDT算法配準后輸出的點云分別為Xt和X′t+1，以及輸出最優(yōu)解坐標系變換參數(shù)T′。

ICP算法進行點云配準對點云初始位姿要求嚴格，然而NDT 算法可以為ICP 算法提供良好的點云位姿狀態(tài)。故ICP 精確配準算法是利用NDT 算法配準后得到的變換矩陣T′為位姿變換關系，然后利用ICP 算法得到較為精確的解。ICP算法通過計算X′t+1與X 對應點距離，構造新的旋轉(zhuǎn)平移矩陣T″，通過T″對X′t+1變換，計算變換之后的均方差。若均方差滿足閾值條件，則算法結束；否則繼續(xù)重復迭代直至誤差滿足閾值條件或者迭代次數(shù)終止。

通過以上點云粗配準、點云精確配準，實現(xiàn)了對無人車位姿的精確估計，進而完成對無人車的精確定位。

即便這只是一次“紙上談兵”，宴姝要做的案頭功課并不比實際操作輕松多少。她花了6個月的時間去閱讀文獻，了解青花瓷，并設計展覽環(huán)節(jié)。在一次次匯報交流中，這項展覽構思漸漸成型，最終形成了一份包括文物信息、背景研究、策展思路、運輸流程、展臺設計方案在內(nèi)的9000字報告。

3.3 基于NDT 與ICP 結合的快速點云配準實現(xiàn)地圖構建

通過車載傳感器返回的點云進行地圖構建，但以10 Hz 返回的點云數(shù)據(jù)量經(jīng)過長時間的累積是很龐大的，本文采用了一種以2 Hz 低頻率方式進行點云地圖的構建。除此之外，本文使用經(jīng)過預處理之后的點云進行地圖構建，包括噪聲點的移除以及voxelgrid濾波之后的點云，為地圖的構建提供了有效的點云數(shù)據(jù)。

經(jīng)過數(shù)據(jù)預處理之后得到的周圍環(huán)境的點云，設t時刻掃描周圍環(huán)境得到的點云為Xt，t+1 時刻掃描周圍環(huán)境得到的點云為Xt+1。t 時刻保存的有效地圖為Qt，本文采用NDT算法進行Xt與Xt+1的點云配準，經(jīng)過平移與旋轉(zhuǎn)得到了坐標的轉(zhuǎn)換，把Xt+1的點云坐標與Xt的點云坐標轉(zhuǎn)換為同一坐標系下，得到t+1 時刻的有效地圖為Qt+1，然后本文采用ICP 算法進行對Xt與Xt+1的點云之間的距離校正，校正后得到t+1 時刻的有效地圖為Q′t+1，經(jīng)過點云依次疊加，直至所有接收的點云進行坐標變換，最后得到點云地圖為Q。

本文同樣把無人車的定位與建圖分別來研究，首先通過點云配準算法求出無人車位姿變換，依次經(jīng)過時間的累加點云，最終完成點云地圖的構建。地圖構建完成為無人車自主導航提供了先驗地圖，無人車根據(jù)已構建好的先驗地圖進行精確定位。

4 實驗分析

為了驗證本文算法的有效性，進行了大量的實車測試。本文算法運行的硬件環(huán)境為Inteli7 處理器，16 GB內(nèi)存，系統(tǒng)環(huán)境為Linux 上的機器人操作系統(tǒng)（ROS）[21]的筆記本電腦，實驗平臺為北京聯(lián)合大學小旋風無人車，如圖3（a）所示，實驗環(huán)境分別為校園環(huán)境（實驗1-黃色路徑）、地下停車場（實驗2-紅色路徑），如圖3（b）所示。通過一系列的實驗，定性與定量地分析本文算法與LOAM算法、ICP算法、NDT算法。

圖3 實驗載體與實驗環(huán)境

4.1 數(shù)據(jù)預處理實驗分析

通過Voxel Grid濾波算法進行點云數(shù)據(jù)預處理，在保持點云原始形態(tài)的同時減少點云的冗余量，為本文算法提供良好的點云初始位姿狀態(tài)，提高了本文算法的計算效率。如表1 所示，實驗1 和實驗2 點云數(shù)據(jù)均來自車載激光雷達，實驗1總幀數(shù)約7 430幀，平均每幀原始點云數(shù)量約3 870個，經(jīng)過點云濾波之后，原始點云數(shù)量減少了70%；實驗2總幀數(shù)約5 627幀，平均每幀原始點云數(shù)量約2 460 個，由于實驗2 的測試環(huán)境結構比較單一，經(jīng)過點云濾波之后原始點云數(shù)量減少了75%，相比較實驗1 的點云濾波多了5%。經(jīng)過點云濾波后，在不改變原始點云形態(tài)的同時，大大減少了原始點云的數(shù)量，為點云配準提供了良好的有效點云數(shù)據(jù)。

表1 數(shù)據(jù)預處理實驗結果

圖4（a）是本文隨機選取了一幀點云原始數(shù)據(jù)，白色箭頭方向的點云是掃描場景中樹木形成的，點云數(shù)量大約為3 800；圖4（b）中白色的點云是當前時刻一幀點云原始數(shù)據(jù)經(jīng)過Voxel Grid 濾波算法計算出要刪除的點云數(shù)據(jù)；圖4（c）是數(shù)據(jù)預處理之后的結果，在保持了原有點云的形態(tài)同時減少了點云的數(shù)量。

4.2 相鄰幀之間點云配準實驗分析

本實驗無人車以2.0 m/s 恒定速度行駛，為保證數(shù)據(jù)有效性，選取相鄰幀之間的點云配準后得到平均變換矩陣。無人車雖然沒有依靠慣性導航（IMU）進行定位，但是本文算法、ICP算法及NDT算法與車載慣性導航數(shù)據(jù)進行對比分析，下面分別給出相鄰幀之間變換的平均矩陣，以IMU變換矩陣為參考進行數(shù)據(jù)分析：

圖4 點云數(shù)據(jù)預處理顯示圖

以上是ICP、NDT、NDT-ICP 進行幀間點云配準時得到的平均變換矩陣，包括旋轉(zhuǎn)矩陣R 和平移矩陣T。本文以IMU 提供的有效數(shù)據(jù)為基準，并與IMU 矩陣的值進行對比，本文算法無論是旋轉(zhuǎn)矩陣R 還是平移矩陣T 都更接近于IMU的值。

表2 變換參數(shù)誤差分析

本文統(tǒng)計了實驗1環(huán)境中的前500幀點云配準實驗數(shù)據(jù)，對幀間點云配準的迭代次數(shù)，以及幀間點云配準完成的時間進行分析。其中本文對NDT算法中的迭代次數(shù)及優(yōu)化的s(p)函數(shù)作為幀間點云配準的終止條件，其目的是對幀間點云進行粗配準，為ICP算法提供良好的點云初始位姿狀態(tài)，然后采用ICP點云配準算法完成最終的點云位姿的糾正，實現(xiàn)幀間點云精確配準。

如圖5 所示，本文分別使用ICP 點云配準算法以及本文構建的點云配準算法進行對比，在NDT 算法中設置的最大迭代次數(shù)為10 以及優(yōu)化函數(shù)s(p)是實驗2 環(huán)境中的經(jīng)驗值作為終止條件。從統(tǒng)計實驗數(shù)據(jù)中可以得出，使用ICP算法進行幀間點云配準時迭代的次數(shù)均大于19，采用NDT算法進行幀間點云粗配準時，迭代次數(shù)均小于4，使用NDT 算法配準后，NDT-ICP 的迭代次數(shù)均小于11。

圖5 幀間點云配準迭代次數(shù)示意圖

表3 是ICP、NDT、NDT-ICP 算法的500 幀點云配準的平均迭代次數(shù)，可以得出使用ICP算法的迭代次數(shù)為19.462 次，使用NDT 算法的迭代次數(shù)為3.866 次，使用NDT-ICP 算法的迭代次數(shù)為10.848 次。經(jīng)過使用NDT算法進行幀間點云粗配準后，NDT-ICP算法的幀間點云配準的迭代次數(shù)比ICP算法均減少了8.614。

表3 幀間點云配準平均迭代次數(shù)

圖6是點云配準完成的時間實驗結果效果圖，從圖中可以得到使用ICP 點云配準算法完成時間均大于90 ms，使用NDT點云配準算法完成時間均小于16 ms，使用NDT-ICP點云配準完成時間均小于40 ms。

圖6 幀間點云配準完成時間示意圖

表4是ICP、NDT、NDT-ICP連續(xù)500幀點云配準完成的平均時間，使用ICP算法配準平均時間為92.774 18 ms，使用NDT 算法配準平均時間為15.943 99 ms，使用NDT-ICP算法配準平均時間為38.626 51 ms。經(jīng)過使用NDT算法進行幀間點云粗配準后，NDT-ICP算法的幀間點云配準的計算效率比ICP算法提高了近41.63%。

表4 幀間點云配準平均完成時間

通過以上實驗數(shù)據(jù)分析得出，本文算法進行幀間點云配準時，不僅提高了幀間點云配準算法的計算效率，而且提高了幀間點云配準的精確度。

4.3 定位與地圖構建的實驗分析

如圖7 所示，實驗1 環(huán)境為復雜的園區(qū)非結構化道路，全程道路比較平坦，道路兩旁多為高大建筑物、低矮的道路邊緣以及停放少許的車輛等靜態(tài)障礙物，全程約1.08 km。圖7（a）是本文算法點云配準后形成的點云地圖；圖7（b）是不同算法進行點云配準后定位的路線軌跡圖，其中綠色代表本文算法，紅色代表LOAM 算法，藍色代表ICP 算法，品紅代表NDT 算法；圖7（c）是實驗1無人車局部直線定位顯示圖；圖7（d）是實驗1無人車局部彎道定位顯示圖。

圖7（a）中顏色代表不同高度的障礙物，紅色代表高度大于0.5 m 的障礙物，綠色代表小于0.5 m 的障礙物。圖7（b）則是同一數(shù)據(jù)下采用LOAM、ICP、NDT、本文算法進行點云配準對無人車的定位軌跡，可以看出本文算法與經(jīng)典算法相比是有效的。圖7（c）、（d）是無人車定位的局部效果，可以得出局部直道無人車定位時，由于環(huán)境缺少足夠的特征LOAM 算法效果最為不理想，與本文算法的最大偏差為2.5 m，ICP穩(wěn)定性不足，而NDT與本文算法比較穩(wěn)定，但是本文算法精確度高。如圖7（d）在彎道處本文算法也比較穩(wěn)定。

圖7 實驗1測試效果圖

圖8 實驗2測試效果圖

實驗2的測試環(huán)境為一個地下停車場，入口與出口具有一定的坡度值（小于20°），全程大約0.55 km。地下停車場內(nèi)部結構比較單一，對于點云配準算法是一個大的挑戰(zhàn)。與實驗1類似，圖8（a）本文算法點云配準后形成的點云地圖；圖8（b）是不同算法進行點云配準后定位的路線軌跡圖；圖8（c）是實驗2無人車局部直線定位顯示圖；圖8（d）是實驗2無人車局部彎道定位顯示圖。

實驗2與實驗1相比最大的環(huán)境特點是結構單一且空間較小的地下停車場，特別是入口與出口是特征區(qū)別較小的隧道空間（高度2.5 m，寬度4 m），并且坡度值小于20°，實現(xiàn)無人車精確定位有一定的難度。從圖8（c）中得出，由于周圍環(huán)境相似性高，導致LOAM算法的配準效果最不理想，NTD 算法與ICP 算法的穩(wěn)定性比較差，但定位效果優(yōu)于LOAM 算法，而本文算法點云配準比較穩(wěn)定，定位的效果比較理想。如圖8（d）在彎道處，通過文中的4種算法比較，本文算法對無人車的定位相對精確。

5 結論

本文構建了一種快速點云配準算法，實現(xiàn)激光雷達的實時定位與建圖，并運用到無人車中進行大量的實驗。首先通過計算機接收車載激光雷達所掃描的點云數(shù)據(jù)，對點云數(shù)據(jù)進行Voxel Grid 濾波，去除噪聲點及大量冗余的點云，為本文點云配準算法提供有效的點云數(shù)據(jù)；然后采用本文構建快速點云配準算法進行幀間點云配準，第一步采用NDT算法進行點云配準，實現(xiàn)對無人車位姿的粗估計，第二步采用ICP算法進行點云配準后的點云位姿校正，使幀間點云平移距離與旋轉(zhuǎn)角度達到最優(yōu)的配準參數(shù)，實現(xiàn)對無人車位姿的精確估計；最后點云配準隨時間累積，點云信息不斷更新，完成對點云地圖的構建，為無人車的自主導航提供了先驗地圖，最終實現(xiàn)無人車的精確定位。

本文通過一系列的實驗驗證本文構建的點云配準算法的有效性。在實驗1 和實驗2 環(huán)境中，本文算法的配準精度均優(yōu)于經(jīng)典配準算法，實現(xiàn)了無人車的精確定位。由于實驗2的場景結構的特殊性，有效點云信息量較少，對本文點云配準算法及其他比較經(jīng)典算法是一個大的挑戰(zhàn)。綜上所述，本文構建的快速點云配準是有效的，今后的研究重點是類似實驗2場景空間小且信息單一的點云配準問題。