基于ICP和NDT的激光點(diǎn)云匹配方法研究

朱 鋒，吳長(zhǎng)水，茅 健

（上海工程技術(shù)大學(xué) 機(jī)械與汽車(chē)工程學(xué)院，上海 201620）

0 引 言

自動(dòng)駕駛技術(shù)是當(dāng)前的研究熱點(diǎn)，車(chē)輛定位技術(shù)是自動(dòng)駕駛不可或缺的部分，良好的定位可以幫助車(chē)輛實(shí)現(xiàn)自動(dòng)駕駛功能。同時(shí)定位與地圖構(gòu)建（SLAM）技術(shù)是車(chē)輛定位的關(guān)鍵技術(shù)之一，指的是搭載特定傳感器的機(jī)器人或者車(chē)輛，在沒(méi)有先驗(yàn)信息的環(huán)境中，通過(guò)自身運(yùn)動(dòng)過(guò)程建立環(huán)境地圖，并估計(jì)自身的運(yùn)動(dòng)。根據(jù)傳感器的不同，SLAM技術(shù)又可分為視覺(jué)SLAM和激光SLAM兩大類(lèi)。近年來(lái)，這2種SLAM技術(shù)取得了長(zhǎng)足的進(jìn)步，并逐漸在產(chǎn)品應(yīng)用上落地。尤其是在自動(dòng)駕駛領(lǐng)域，激光雷達(dá)的應(yīng)用逐漸普及起來(lái)，三維激光SLAM技術(shù)將起到舉足輕重的作用。

激光SLAM框架可以大致分為5個(gè)模塊：傳感器數(shù)據(jù)讀取、前端里程計(jì)、后端優(yōu)化、回環(huán)檢測(cè)、建圖。其中，前端里程計(jì)是激光SLAM的重要步驟，其功能是將激光雷達(dá)采集到的點(diǎn)云數(shù)據(jù)做相鄰幀之間的匹配，從而得出相鄰幀間位姿關(guān)系，再根據(jù)上一幀的位姿，來(lái)估計(jì)出當(dāng)前幀的位姿。三維激光前端里程計(jì)幀間匹配方法主要分為直接匹配和和特征匹配兩類(lèi)。直接匹配法又可細(xì)分為迭代最近點(diǎn)ICP和正態(tài)分布變換NDT。

Chen等人提出的ICP算法，通過(guò)待匹配的2幀點(diǎn)云建立歐氏距離并使其最小化，不斷迭代直至滿足設(shè)定的終止條件，從而得到相對(duì)位姿變化。但激光點(diǎn)云數(shù)據(jù)往往較大，導(dǎo)致ICP算法耗時(shí)長(zhǎng)；而且ICP算法需要提供一個(gè)較好的初值，否則最終迭代結(jié)果可能會(huì)陷入局部最優(yōu)。Censi提出了PL-ICP算法（point-to-line ICP），該法精度較高，適用于2D激光SLAM，缺點(diǎn)是對(duì)初值更敏感，容易陷入局部極值。Low提出了PP-ICP算法（point-toplane ICP），該算法精度高且適用于3D激光SLAM。

Biber等人提出一種二維NDT的匹配方法，Magnusson等人在此基礎(chǔ)上將二維NDT推廣到三維匹配中，其核心思想是將點(diǎn)云數(shù)據(jù)投放到由小立方體組成的網(wǎng)格中，每個(gè)立方體中的點(diǎn)云轉(zhuǎn)換成一個(gè)概率密度函數(shù)，然后求出點(diǎn)云之間的匹配關(guān)系。三維NDT算法初始化效果好，運(yùn)行速度快，魯棒性高，在3D激光SLAM使用較多。

Zhang等人提出的特征匹配算法，其思想是先計(jì)算曲率，并按曲率提取線特征和面特征，再和上一幀做匹配，最后求出相對(duì)位姿。LOAM方案在純激光匹配算法中很具有代表性，長(zhǎng)期霸榜于KITTI數(shù)據(jù)集前三位。LeGO-LOAM在LOAM的基礎(chǔ)上對(duì)地面特征做了分割，減小了特征搜索范圍，從而節(jié)省了計(jì)算資源；并加入了回環(huán)修正，精度也有所提升。LIO-mapping提出了一種LiDAR與IMU緊耦合融合方法，在LOAM的基礎(chǔ)上加入IMU，基于滑動(dòng)窗口方法，把雷達(dá)線／面特征、IMU預(yù)積分等的約束放在一起進(jìn)行優(yōu)化，得到良好的建圖效果。LIO-SAM在LeGO-LOAM的基礎(chǔ)上，先通過(guò)點(diǎn)云特征計(jì)算出相對(duì)位姿，再利用相對(duì)位姿、IMU預(yù)積分和GPS做融合，分2步完成，相比于直接一步做緊耦合，大大提高了效率。上述列舉的方案中，前端激光里程計(jì)模塊都是基于特征匹配得到的。

本文的研究對(duì)象是三維激光SLAM，針對(duì)前端激光里程計(jì)，用2種不同的點(diǎn)云匹配方法：迭代最近點(diǎn)ICP和正態(tài)分布變換NDT，在KITTI數(shù)據(jù)集上分別進(jìn)行點(diǎn)云配準(zhǔn)，同時(shí)也評(píng)估了這2種點(diǎn)云匹配方法的效率和精度。

1 前端激光里程計(jì)匹配方法

1.1 迭代最近點(diǎn)

迭代最近點(diǎn)（ICP）算法的核心思想，即通過(guò)待匹配的2幀點(diǎn)云建立歐氏距離并使其最小化，不斷迭代直至滿足設(shè)定的終止條件，從而得到相對(duì)位姿變化。圖1為擬合示意圖。

圖1 擬合示意圖Fig.1 Fitting schematic diagram

然后構(gòu)建最小二乘問(wèn)題，求，使得誤差平方和達(dá)到最?。?/p>

觀察上式等號(hào)右側(cè)2項(xiàng)，第一項(xiàng)只和有關(guān)，第二項(xiàng)和，都有關(guān)。只要求得旋轉(zhuǎn)矩陣，再令第二項(xiàng)為零即可求得平移。由2組點(diǎn)集的質(zhì)心，，令：

計(jì)算有關(guān)的誤差項(xiàng)，得：

對(duì)3×3矩陣進(jìn)行SVD分解（奇異值分解），得：

當(dāng)滿秩時(shí)，解得：

1.2 正態(tài)分布變換

正態(tài)分布變換（NDT）核心思想是將點(diǎn)云數(shù)據(jù)投放到由小立方體組成的網(wǎng)格中，每個(gè)立方體中的點(diǎn)云轉(zhuǎn)換成一個(gè)概率密度函數(shù)，然后通過(guò)數(shù)學(xué)手段求出點(diǎn)云之間的匹配關(guān)系。圖2為通過(guò)概率刻畫(huà)點(diǎn)云，左邊為原始點(diǎn)云，右邊為每個(gè)方塊內(nèi)點(diǎn)的概率。

圖2 概率刻畫(huà)點(diǎn)云Fig.2 Point cloud description with probability

根據(jù)預(yù)測(cè)的位姿，對(duì)點(diǎn)進(jìn)行旋轉(zhuǎn)和平移：

旋轉(zhuǎn)和平移后的點(diǎn)與目標(biāo)點(diǎn)集中的點(diǎn)在同一坐標(biāo)系下，此時(shí)可計(jì)算各點(diǎn)的聯(lián)合概率：

所有點(diǎn)的聯(lián)合概率：

取對(duì)數(shù)，去除常數(shù)項(xiàng)，簡(jiǎn)化問(wèn)題，則目標(biāo)函數(shù)變?yōu)椋?/p>

按照高斯牛頓法的求解流程，計(jì)算殘差函數(shù)關(guān)于代求參數(shù)的雅可比，便可迭代優(yōu)化。

2 基于數(shù)據(jù)集的實(shí)現(xiàn)

本文是基于KITTI數(shù)據(jù)集實(shí)現(xiàn)的，使用的數(shù)據(jù)包為kitti_2011_10_03_drive_0027_synced.bag，對(duì)其中的激光雷達(dá)點(diǎn)云數(shù)據(jù)，分別添加了前端里程計(jì)的算法ICP和NDT，算法流程見(jiàn)圖3和圖4，測(cè)試算法并得到了里程計(jì)的軌跡，最后和gnss軌跡做數(shù)據(jù)對(duì)比分析。

圖3 ICP算法流程Fig.3 The flowchart of ICP algorithm

圖4 NDT算法流程圖Fig.4 The flowchart of NDT algorithm

里程計(jì)子模塊功能設(shè)計(jì)如下：

（1）點(diǎn)云匹配。用pcl庫(kù)設(shè)置匹配參數(shù)，從接收到一幀點(diǎn)云開(kāi)始，和地圖進(jìn)行匹配，如果是第一幀數(shù)據(jù)，那么就認(rèn)為是地圖，供下一幀匹配使用。本文要把里程計(jì)軌跡和gnss軌跡做對(duì)比，所以把初始時(shí)刻gnss和imu給出的位姿作為里程計(jì)的初始位姿。

（2）提取關(guān)鍵幀。如果把每一幀匹配好的點(diǎn)云都加入地圖，會(huì)使地圖太大，所以要提取關(guān)鍵幀，即每隔一段距離取一幀點(diǎn)云，用關(guān)鍵幀來(lái)拼接成地圖。

（3）滑窗。關(guān)鍵幀會(huì)不斷增加，地圖會(huì)一直累加，那么會(huì)導(dǎo)致很多不必要的計(jì)算量，所以把時(shí)間靠前的關(guān)鍵幀給剔除，把和當(dāng)前幀一定距離范圍內(nèi)的關(guān)鍵幀找出來(lái)拼接即可。

（4）點(diǎn)云濾波。在匹配之前需要濾波，對(duì)點(diǎn)云稀疏化。這里點(diǎn)云濾波是直接采用了pcl庫(kù)中的voxel_filter，其基本原理就是把三維空間劃分成等尺寸的立方體格子，在一個(gè)立方體格子內(nèi)最多只留一個(gè)點(diǎn)。濾波格子大小決定了匹配的效率和精度，格子越小，點(diǎn)越多，精度越高，但是速度越慢，反之速度加快，精度下降；可適當(dāng)調(diào)整參數(shù)，對(duì)比效果。

（5）位姿預(yù)測(cè)。點(diǎn)云匹配有一個(gè)特性，對(duì)位姿的預(yù)測(cè)值比較敏感，所以在載體運(yùn)動(dòng)時(shí)，不能以其上一幀的位姿作為這一幀的預(yù)測(cè)值，可以使用IMU預(yù)測(cè)，也可以使用運(yùn)動(dòng)模型預(yù)測(cè)，此處采用運(yùn)動(dòng)模型來(lái)做位姿預(yù)測(cè)。假如當(dāng)前幀是第幀，那么用第2幀位姿和第1幀位姿就可以計(jì)算一個(gè)位姿變化量，在1幀位姿基礎(chǔ)上累加這個(gè)位姿變化量，就可認(rèn)為是第幀的預(yù)測(cè)值。

3 實(shí)驗(yàn)結(jié)果分析

本文中用到的計(jì)算機(jī)配置：Intel i7-9750H處理器，16 G內(nèi)存，系統(tǒng)環(huán)境為Ubuntu18.04和ROS Melodic1.14。使用精度評(píng)估工具evo，把對(duì)應(yīng)的gnss數(shù)據(jù)和激光里程計(jì)數(shù)據(jù)進(jìn)行分析處理。采用的評(píng)價(jià)指標(biāo)如下：

（1）絕對(duì)位姿誤差（absolute pose error，）。用于比較估計(jì)軌跡和參考軌跡并計(jì)算整個(gè)軌跡的統(tǒng)計(jì)數(shù)據(jù)，此處估計(jì)軌跡為激光里程計(jì)，參考軌跡為gnss。

（2）相對(duì)位姿誤差（relative pose error，）。相對(duì)位姿誤差比較運(yùn)動(dòng)過(guò)程中的姿態(tài)增量，可以給出局部精度，如slam系統(tǒng)每米的平移或者旋轉(zhuǎn)漂移量。

將ICP算法和NDT算法分別在KITTI數(shù)據(jù)集上進(jìn)行仿真實(shí)驗(yàn)，得出了的效果如圖5所示。圖5（a）、圖5（b）中的藍(lán)色軌跡是gnss軌跡，綠色軌跡為激光里程計(jì)。對(duì)比2種算法，可以發(fā)現(xiàn)激光里程計(jì)均出現(xiàn)了一定程度上的漂移。具體來(lái)說(shuō)，圖5（a）中的漂移比較嚴(yán)重，圖5（b）中的漂移比較輕。下面將給出數(shù)據(jù)做定量分析。

圖5 KITTI測(cè)試效果圖Fig.5 KITTI test effect diagram

圖6和圖7分別展示了ICP和NDT的位姿誤差分析圖，將圖中數(shù)據(jù)整理到表1和表2中。在表1、表2中，max表示最大誤差，表示平均誤差，表示誤差中位數(shù)，min表示最小誤差，表示均方根誤差，表示標(biāo)準(zhǔn)差。

圖6 ICP方法的絕對(duì)位姿誤差和相對(duì)位姿誤差Fig.6 APE and RPE of ICP

圖7 NDT方法的絕對(duì)位姿誤差和相對(duì)位姿誤差Fig.7 APE and RPE of NDT

表1 絕對(duì)位姿誤差對(duì)比Tab.1 The comparison of APE

表2 相對(duì)位姿誤差對(duì)比Tab.2 The comparison of RPE

在絕對(duì)位姿誤差中，ICP比NDT的最大誤差和均方根誤差都要大很多；在相對(duì)位姿誤差對(duì)比中，ICP的誤差比NDT的誤差也大很多。根據(jù)圖5（a）和圖5（b），在運(yùn)動(dòng)開(kāi)始時(shí)沒(méi)有旋轉(zhuǎn)，對(duì)于平移，ICP和NDT精度幾乎相當(dāng)；在第三個(gè)轉(zhuǎn)彎處，ICP出現(xiàn)了較大的漂移誤差；在第七個(gè)轉(zhuǎn)彎處，ICP累積漂移更大，此處由于沒(méi)有較好的初值，最終迭代結(jié)果陷入了局部最優(yōu)，因而軌跡出現(xiàn)了失真。由上可以得出：在角度變化時(shí)，NDT的魯棒性明顯優(yōu)于ICP；且ICP需要提供一個(gè)較好的初值，否則最終迭代結(jié)果可能會(huì)陷入局部極值。

4 結(jié)束語(yǔ)

本文利用三維激光點(diǎn)云中2種匹配算法：迭代最近點(diǎn)（ICP）和正態(tài)分布變換（NDT）算法，實(shí)現(xiàn)了前端激光里程計(jì)的功能。為了比較ICP和NDT算法的效率和精度，在KITTI數(shù)據(jù)集上進(jìn)行了仿真實(shí)驗(yàn)。實(shí)驗(yàn)中，用gnss軌跡作為真值，激光里程計(jì)作為估計(jì)值，給出了2種算法的絕對(duì)位姿誤差和相對(duì)位姿誤差，結(jié)果表明NDT算法具有更好的效率和精度，具體表現(xiàn)為如下2點(diǎn)：

（1）對(duì)于前端激光里程計(jì)，NDT的匹配精度優(yōu)于ICP。

（2）由于ICP需要較好的初值且易受旋轉(zhuǎn)影響，ICP更適用于精匹配，NDT則更適用于粗匹配。

本文實(shí)現(xiàn)了基本的激光里程計(jì)功能，但是對(duì)里程計(jì)來(lái)講，漂移是不可避免的。后續(xù)的工作，是增加其他約束來(lái)消除累計(jì)誤差，從而構(gòu)建高精地圖。