祁 敏 宋勇磊

（南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）

1 引言

機(jī)器人同時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM），指的是機(jī)器人在未知環(huán)境中，通過(guò)傳感器連續(xù)感知周圍環(huán)境，完成自身定位與環(huán)境地圖創(chuàng)建的過(guò)程，是機(jī)器人領(lǐng)域研究的熱點(diǎn)和難點(diǎn)。目前，根據(jù)傳感器，SLAM 系統(tǒng)又可分為基于視覺(jué)、基于雷達(dá)以及基于多傳感融合等。由于視覺(jué)具有豐富的紋理和色彩信息，基于視覺(jué)的SLAM 系統(tǒng)在定位方面很有優(yōu)勢(shì)，但由于相機(jī)對(duì)光照變化敏感，將它作為唯一傳感器用于SLAM是不可靠的。

激光雷達(dá)能夠穩(wěn)定捕捉環(huán)境的精確信息的優(yōu)勢(shì)，但如果機(jī)器人劇烈運(yùn)動(dòng)，激光數(shù)據(jù)會(huì)發(fā)生畸變，并且由于輸出數(shù)據(jù)頻率較低，隨著運(yùn)動(dòng)劇烈的增加，會(huì)產(chǎn)生較大的累積誤差，不能滿足機(jī)器人定位和導(dǎo)航的需求。慣性測(cè)量單元（Inertial Measurement Unit，IMU）是一種不受外界干擾的位姿估計(jì)設(shè)備，可以用于估計(jì)機(jī)器人姿態(tài)的六個(gè)自由度：位置信息（x，y，z）和三個(gè)方向（橫滾、偏航、俯仰），具有較高的測(cè)量精度，并且數(shù)據(jù)的輸出頻率很高，通常在100Hz～1kHz，能夠提供激光掃描之間的穩(wěn)健狀態(tài)變化。將雷達(dá)與慣導(dǎo)能夠彌補(bǔ)激光雷達(dá)在劇烈運(yùn)動(dòng)時(shí)產(chǎn)生的數(shù)據(jù)畸變，并且可以用于因子圖的優(yōu)化以累積誤差的減少，兩者結(jié)合是實(shí)現(xiàn)6 自由度位姿估計(jì)并進(jìn)行增量式地圖構(gòu)建的有效手段［1～2］。

現(xiàn)有的激光雷達(dá)/IMU 融合的工作主要分為兩類：松耦合和緊耦合。松耦合通過(guò)分別處理兩個(gè)傳感器，以推斷它們的運(yùn)動(dòng)約束。與松散耦合方法相比，緊耦合方法［3］則通過(guò)聯(lián)合優(yōu)化直接融合激光雷達(dá)和慣性測(cè)量，表現(xiàn)出較高的魯棒性和準(zhǔn)確性，因此近年來(lái)引起了越來(lái)越多的研究興趣。LOAM［4］和Lego-LOAM［5］都使用了IMU 數(shù)據(jù)對(duì)激光獲得的數(shù)據(jù)進(jìn)行畸變矯正，在一定程度上使得激光數(shù)據(jù)更可靠；LIOM［6］介紹了一種緊密耦合的激光雷達(dá)和IMU融合的方法，使用原始IMU 測(cè)量和Lidar 測(cè)量數(shù)據(jù)優(yōu)化系統(tǒng)狀態(tài)，在激光退化和運(yùn)動(dòng)快速的情況能有效優(yōu)化系統(tǒng)狀態(tài)。LIO-SAM［7］對(duì)IMU 數(shù)據(jù)進(jìn)行預(yù)積分用于消除點(diǎn)云的畸變，并在圖優(yōu)化階段將預(yù)積分信息作為圖優(yōu)化的約束條件，進(jìn)一步提高了IMU數(shù)據(jù)的利用率并且提高了地圖構(gòu)建的精度。R-LINS［8］使用誤差狀態(tài)卡爾曼濾波模型（ESKF）將雷達(dá)數(shù)據(jù)和IMU數(shù)據(jù)進(jìn)行緊密耦合。

對(duì)于3D SLAM 技術(shù)，目前主要研究集中在估計(jì)機(jī)器人位姿［9］。SLAM 的整體框架大致可以分為激光里程計(jì)和后端優(yōu)化。在激光里程計(jì)部分，通常采用提取整幀點(diǎn)云中較為穩(wěn)定的點(diǎn)進(jìn)行匹配，并從點(diǎn)與點(diǎn)的匹配上升到點(diǎn)與線、點(diǎn)與面的匹配。例如：LOAM、Lego-LOAM、LIO-SAM 等主流算法通過(guò)提取點(diǎn)云中結(jié)構(gòu)性較強(qiáng)的點(diǎn)進(jìn)行幀間匹配，從而實(shí)現(xiàn)實(shí)時(shí)性較高的SLAM 系統(tǒng)。在后端優(yōu)化時(shí)，通過(guò)回環(huán)檢測(cè)以及因子圖［13～14］構(gòu)建點(diǎn)云幀之間的因子約束。針對(duì)提高6 自由度的地圖構(gòu)建的精度，本文提出兩點(diǎn)創(chuàng)新：

1）利用IMU 預(yù)積分值對(duì)激光數(shù)據(jù)進(jìn)行畸變矯正，并提出一種自適應(yīng)參數(shù)RANSAC平面擬合算法進(jìn)行面特征點(diǎn)的精提取。

2）提出自適應(yīng)閾值的回環(huán)檢測(cè)方法，摒棄固定閾值的回環(huán)檢測(cè)算法，利用機(jī)器人位姿變換自適應(yīng)閾值。

2 整體算法流程

整體算法框架如圖1，分為兩部分：激光Lidar/IMU 數(shù)據(jù)融合以及后端優(yōu)化。激光雷達(dá)以10Hz 的速率輸入點(diǎn)云幀，慣性測(cè)量單元以2kHz 的速度輸入傳感器的角速度和線加速度。第一步進(jìn)行數(shù)據(jù)的預(yù)處理，對(duì)IMU 獲得數(shù)據(jù)進(jìn)行預(yù)積分，將激光的噪聲點(diǎn)去除，然后利用IMU數(shù)據(jù)的預(yù)積分結(jié)果對(duì)激光點(diǎn)云進(jìn)行畸變矯正；第二步使用自適應(yīng)參數(shù)RANSAC 擬合算法對(duì)矯正后的點(diǎn)云進(jìn)行幾何特征提取并根據(jù)幀間關(guān)聯(lián)進(jìn)行位姿估計(jì)；后端優(yōu)化部分使用因子圖進(jìn)行地圖的全局優(yōu)化，此外，通過(guò)一些關(guān)鍵幀選取策略進(jìn)行關(guān)鍵幀的篩選，并在對(duì)每一幀關(guān)鍵幀利用提出的自適應(yīng)閾值的回環(huán)檢測(cè)方法進(jìn)行回環(huán)檢測(cè)，最后將激光里程計(jì)因子、回環(huán)因子與IMU 預(yù)積分因子作為因子圖的約束進(jìn)行位姿的優(yōu)化。

圖1 算法總體架構(gòu)

第三步將重點(diǎn)描述激光雷達(dá)、IMU 數(shù)據(jù)融合方法以及特征提取算法。第四步重點(diǎn)講述自適應(yīng)閾值的回環(huán)檢測(cè)方法。

3 基于Lidar/IMU的數(shù)據(jù)融合

激光點(diǎn)云數(shù)據(jù)通過(guò)機(jī)械雷達(dá)獲取的顯著缺點(diǎn)是在橫向的數(shù)據(jù)采集時(shí)，由于時(shí)間的差異，存在一定程度的誤差，具體表現(xiàn)在激光獲取數(shù)據(jù)的頻率為10Hz，因此同一幀點(diǎn)云數(shù)據(jù)在激光初始旋轉(zhuǎn)獲得的數(shù)據(jù)點(diǎn)與當(dāng)前幀結(jié)束獲得的點(diǎn)存在0.1s 的誤差。因此在進(jìn)行點(diǎn)云特征提取之前，需要利用IMU數(shù)據(jù)對(duì)激光點(diǎn)云進(jìn)行去畸變操作，以提高激光點(diǎn)的準(zhǔn)確性，然后對(duì)激光點(diǎn)云進(jìn)行特征提取，以在保留較多信息的基礎(chǔ)上降低數(shù)據(jù)的規(guī)模。

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

3.1.1 IMU模型

對(duì)于IMU測(cè)量的角速度和加速度數(shù)據(jù)，構(gòu)建其測(cè)量模型。其角速度（陀螺儀）測(cè)量模型和加速度（加速度計(jì)）測(cè)量模型為［10］

3.1.2 IMU預(yù)積分

根據(jù)IMU 動(dòng)力學(xué)模型以及傳感器獲取的角速度和加速度，在t+?t內(nèi)機(jī)器人運(yùn)動(dòng)的線速度、空間中的位置以及狀態(tài)可以估計(jì)為［10］

相比于激光雷達(dá)，由于IMU 的輸出頻率很高，通常在100Hz～1kHz，為保持兩者數(shù)據(jù)的一致性，采用IMU預(yù)積分對(duì)相對(duì)運(yùn)動(dòng)約束進(jìn)行再參數(shù)化，將高頻的加速度和角速度轉(zhuǎn)換為位姿變換，用于關(guān)鍵幀的位姿變換。我們通過(guò)［1］提出的IMU 預(yù)積分方法獲得時(shí)i 和時(shí)間j 獲得的采樣數(shù)據(jù)之間的預(yù)積分［10］測(cè)量值：

如圖2 所示，利用式（7）和式（8）能夠?qū)MU 的離散信息轉(zhuǎn)換為連續(xù)變化的函數(shù)信息。

圖2 IMU預(yù)積分模型

3.2 激光里程計(jì)

利用IMU 預(yù)積分可以獲取任意i、j時(shí)刻的平移和旋轉(zhuǎn)變換信息。通過(guò)激光的獲取頻率f以及水平方向的分辨率r，能夠計(jì)算每一個(gè)激光點(diǎn)pi獲取的時(shí)刻ti：

其中t0為當(dāng)前幀第一列點(diǎn)云獲取時(shí)間，通常為點(diǎn)云的時(shí)間戳，col(i)為激光點(diǎn)在水平方向上的坐標(biāo)偏移。由式（7）和式（9），pi去畸變后的坐標(biāo)為

將式（10）應(yīng)用于點(diǎn)云的每個(gè)點(diǎn)，即可獲得去畸變點(diǎn)云信息，相比于原始點(diǎn)云，去畸變后的點(diǎn)云空間位置更精確，有利于后續(xù)的特征檢測(cè)。

進(jìn)行特征檢測(cè)前，首先計(jì)算點(diǎn)云中點(diǎn)的曲率值，并通過(guò)曲率的大小對(duì)點(diǎn)進(jìn)行線面特征點(diǎn)的分類，獲取粗糙的線面特征點(diǎn)信息，詳細(xì)步驟參照文獻(xiàn)［4］。

對(duì)于上述獲取的粗糙的面特征點(diǎn)，本文提出一種自適應(yīng)參數(shù)RANSAC 擬合算法進(jìn)一步篩選面特征點(diǎn)并保存面特征點(diǎn)的法向量用于幀間估計(jì)，步驟如下：

1）對(duì)去畸變后的點(diǎn)云進(jìn)行地面點(diǎn)的去除，提出一種高效地面去除算法，首先將三維空間的點(diǎn)云信息壓縮到二維平面，并將二維空間劃分為小顆粒的柵格圖，統(tǒng)計(jì)柵格內(nèi)點(diǎn)的數(shù)量信息，對(duì)于小于閾值n的柵格加入地面的候選點(diǎn)；

2）將步驟1）獲得的兩類點(diǎn)云集：地面候選點(diǎn)PG和PNG，分別進(jìn)行歐式聚類，將點(diǎn)云轉(zhuǎn)換為空間內(nèi)離散的點(diǎn)云類；

3）此時(shí)步驟2）獲得的聚類由兩類可能：該類別內(nèi)只含一個(gè)平面和該類別含有多個(gè)平面。由于無(wú)法判斷聚類所屬類別，因此提出一種通用的平面檢測(cè)方法，如圖3，首先使用RANSAC 方法擬合其中的一個(gè)平面特征，包含的點(diǎn)云數(shù)量為nx，然后動(dòng)態(tài)縮小擬合平面所需的點(diǎn)云閾值，m 為自定義參數(shù)，進(jìn)行迭代的平面擬合。

圖3 平面特征檢測(cè)

至此，存儲(chǔ)每個(gè)聚類的平面特征點(diǎn)以及對(duì)應(yīng)的法向量，用于后續(xù)幀間關(guān)聯(lián)。

在點(diǎn)云匹配階段，利用IMU 預(yù)積分的先驗(yàn)數(shù)據(jù)，將第k 幀點(diǎn)云特征變換到k+1 幀，假設(shè)，則有：

對(duì)于線特征點(diǎn)i?，特征點(diǎn)j、k?為i點(diǎn)最近的兩個(gè)點(diǎn)，有：

對(duì)于面特征點(diǎn)i?，特征點(diǎn)j?為距離點(diǎn)i最近的點(diǎn)，有：

在進(jìn)行位姿估計(jì)之前，首先通過(guò)前一幀點(diǎn)云位姿結(jié)果與IMU預(yù)積分得到初始化當(dāng)前幀位姿，將其作為初始位姿輸入Levenberg-Maquardt 算法最小化線、面特征點(diǎn)之間的距離以獲得機(jī)器人位姿ξ=(R,t)。

4 后端優(yōu)化

4.1 關(guān)鍵幀選取策略

將雷達(dá)的每一幀用于因子圖優(yōu)化［11～14］是不現(xiàn)實(shí)的。因此，可以采用關(guān)鍵幀思想［7，15］選取因子圖的待優(yōu)化節(jié)點(diǎn)，以減少待優(yōu)化的幀數(shù)。

本篇文章對(duì)關(guān)鍵幀選取策略制定了標(biāo)準(zhǔn)：

1）關(guān)鍵幀位姿之間的歐式距離需要滿足大于dε且兩幀獲取時(shí)間滿足不小于tε。

2）當(dāng)前幀跟蹤到的參考關(guān)鍵幀特征點(diǎn)數(shù)量小于75%。

3）當(dāng)前幀的線、面特征點(diǎn)數(shù)量不少于pline、pedge。

其中，1）、2）兩點(diǎn)主要解決關(guān)鍵幀之間需要有一定的差異性，3）主要用于保證當(dāng)前幀的質(zhì)量，當(dāng)前幀的特征點(diǎn)應(yīng)該是充足的且分布均勻的。

4.2 自適應(yīng)閾值的回環(huán)檢測(cè)

在SLAM 問(wèn)題中，位姿估計(jì)往往是一個(gè)遞推的過(guò)程，即由上一幀的位姿解算當(dāng)前幀位姿，因此存在將上一幀的位姿累計(jì)到下一幀的過(guò)程即累計(jì)誤差。回環(huán)的主要思想是通過(guò)減少中間過(guò)程的約束達(dá)到降低累積誤差的效果。如圖4，由于環(huán)境的復(fù)雜性，機(jī)器人建圖過(guò)程中往往會(huì)回到之前經(jīng)過(guò)的地方，通過(guò)將當(dāng)前幀與累計(jì)誤差較少的幀進(jìn)行位姿的推算從而達(dá)到減少累計(jì)誤差的效果。

圖4 回環(huán)示意圖

回環(huán)幀往往是機(jī)器人曾經(jīng)走過(guò)，并且再次經(jīng)過(guò)的幀，只有兩幀點(diǎn)云之間的位姿足夠近并且能夠相互關(guān)聯(lián)（重疊（關(guān)鍵點(diǎn)）），才能夠進(jìn)行回環(huán)檢測(cè)。本文進(jìn)行回環(huán)檢測(cè)的步驟為如圖5。

假設(shè)歷史關(guān)鍵幀為Ki(i?(1,2,…c-1))，當(dāng)檢測(cè)到某一幀為關(guān)鍵幀Kc后，以空間位置Pi(xi,yi,zi)(i=1,2,…c-1)構(gòu)建KDTree 并篩選距離最近的k 幀作為候選回環(huán)幀，針對(duì)計(jì)算當(dāng)前幀Kc與候選回環(huán)幀K?(? ?(1...k))之間的距離dc?：

當(dāng)且僅當(dāng)距離和時(shí)間滿足式（15）時(shí)，將K?作為候選回環(huán)幀。

此時(shí)使用的閾值dmax為固定值，一般為預(yù)設(shè)的，實(shí)際上存在隨著建圖規(guī)模的增加，回環(huán)幀檢測(cè)兩幀之間差距非常大的情況，此時(shí)需要人工對(duì)dmax的選取進(jìn)行地圖規(guī)模的干預(yù)。針對(duì)這一問(wèn)題，本文提出自適應(yīng)閾值的回環(huán)檢測(cè)方法，利用幀間關(guān)聯(lián)計(jì)算機(jī)器人的t時(shí)刻累積運(yùn)動(dòng)距離Dt：

其中，d0為回環(huán)距離的最小值，Dmin為預(yù)設(shè)的檢測(cè)回環(huán)幀的最短距離。最后，當(dāng)檢測(cè)到回環(huán)時(shí)，將當(dāng)前dmax折半以補(bǔ)充回環(huán)修正時(shí)修正的精度。

對(duì)于候選回環(huán)幀K?，將當(dāng)前幀Kc與候選幀K?進(jìn)行精配準(zhǔn)。即找到兩幀的局部點(diǎn)云地圖Kc-k∪…∪Kc-1∪Kc∪Kc+1∪Kc+k與K?-k∪…∪K?-1∪K?∪K?+1∪K?+k，并使用ICP 算法進(jìn)行精配準(zhǔn)，并將最后得到的結(jié)果加入因子圖進(jìn)行優(yōu)化［15］。

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

5.1 實(shí)驗(yàn)設(shè)備

如圖6，搭建一種手持式建圖設(shè)備進(jìn)行實(shí)驗(yàn)驗(yàn)證。該設(shè)備搭建了Velodyne 16 線激光雷達(dá)獲取周圍環(huán)境的點(diǎn)云信息，inertialLabs KERNEL-100 IMU獲取設(shè)備運(yùn)動(dòng)過(guò)程中的角速度和加速度。激光雷達(dá)以10Hz 的頻率采集點(diǎn)云信息，IMU 以1kHz 的頻率采樣傳感器的運(yùn)動(dòng)。

圖6 實(shí)驗(yàn)設(shè)備

5.2 校園環(huán)境實(shí)驗(yàn)評(píng)估

使用上述實(shí)驗(yàn)設(shè)備在校園環(huán)境內(nèi)采集室內(nèi)走廊以及室外教學(xué)樓周圍的環(huán)境的激光和IMU數(shù)據(jù)，并使用上述算法進(jìn)行地圖構(gòu)建，圖7 和圖8（a）展示了本文提出的算法在室內(nèi)和室外環(huán)境建地圖的效果圖。

圖7 室內(nèi)建圖效果

圖8 室外建圖效果

圖8（b）為室外環(huán)境建圖時(shí)回環(huán)檢測(cè)的效果，分析可得，提出的自適應(yīng)閾值回環(huán)策略通過(guò)能夠有效檢測(cè)回環(huán)幀。

5.3 KITTI數(shù)據(jù)集評(píng)估

通過(guò)KITTI 數(shù)據(jù)集［16］對(duì)提出的激光/IMU 緊耦合的SLAM 算法（以下簡(jiǎn)稱為L(zhǎng)I-SAM）進(jìn)行了評(píng)估。KITTI數(shù)據(jù)集是目前國(guó)際上最大的自動(dòng)駕駛場(chǎng)景下的激光雷達(dá)SLAM 算法評(píng)測(cè)數(shù)據(jù)集。KITTI包含市區(qū)、鄉(xiāng)村和高速公路等場(chǎng)景采集的真實(shí)城市與公路的激光數(shù)據(jù)，并提供了平均平移誤差以及平均旋轉(zhuǎn)誤差評(píng)工具。采用KITTI 數(shù)據(jù)集下的Raw Data［16］對(duì)應(yīng)的序列對(duì)LOAM、Lego-LOAM 和LI-SLAM進(jìn)行評(píng)估比較。圖9（a）為使用LI-SLAM 算法運(yùn)行KITII 05 序列的建圖效果圖，圖9（b）為三種SLAM算法運(yùn)行KITTI 05序列數(shù)據(jù)集的軌跡，由軌跡預(yù)測(cè)圖可得，本文提出的LI-SALM 算法在大規(guī)模建圖且?guī)в谢丨h(huán)的軌跡預(yù)測(cè)精度優(yōu)于LOAM 和Lego-LOAM。

圖9 KITTI 05序列建圖與軌跡對(duì)比

表2 為本文提出的自適應(yīng)閾值的回環(huán)算法在KITTI 05 序列上的準(zhǔn)確率評(píng)估效果，通過(guò)評(píng)估三個(gè)固定的dmax與自適應(yīng)dmax回環(huán)準(zhǔn)確率，驗(yàn)證本文提出的算法的有效性。由實(shí)驗(yàn)可得，相比于固定閾值，自適應(yīng)dmax算法在通用的基礎(chǔ)上，回環(huán)檢測(cè)的準(zhǔn)確率提升至95%左右。

表1 KITTI 05回環(huán)效果評(píng)估

表2 KITTI數(shù)據(jù)集評(píng)估結(jié)果

表2 為序列04～07 序列下的KITTI 數(shù)據(jù)集評(píng)估結(jié)果，由實(shí)驗(yàn)數(shù)據(jù)可得，本文提出的LI-SLAM 算法在多數(shù)情況下對(duì)軌跡預(yù)測(cè)的精度提升起到了積極作用，由于KITTI 數(shù)據(jù)集IMU 數(shù)據(jù)參數(shù)未知，且06數(shù)據(jù)集不存在回環(huán)場(chǎng)景，因此位姿預(yù)測(cè)存在較大誤差。

6 結(jié)語(yǔ)

本文提出了一種Lidar/IMU 緊耦合的建圖方法，利用IMU 高頻率的特性對(duì)低頻Lidar 獲取到的點(diǎn)云進(jìn)行去畸變，以提高激光原始點(diǎn)云的精度，在激光特征選取階段，對(duì)曲率檢測(cè)點(diǎn)云的線面特征點(diǎn)，提出自適應(yīng)閾值的RANSAC平面擬合算法獲取面特征點(diǎn)的法向量以提高激光里程計(jì)的精度。在后端優(yōu)化部分，提出自適應(yīng)閾值的回環(huán)檢測(cè)方法，消除累積誤差的影響。通過(guò)在校園環(huán)境內(nèi)建圖以及KITTI 數(shù)據(jù)集的評(píng)估，驗(yàn)證了所提出的SLAM 算法的精確度。

本文提出的建圖方法采用融合了Lidar 和IMU的信息以提高建圖精度。但在結(jié)構(gòu)化不明顯的場(chǎng)景下難以進(jìn)行地圖的精確構(gòu)建，后續(xù)將加入視覺(jué)傳感器，融合Lidar、IMU、視覺(jué)信息進(jìn)行地圖構(gòu)建。