基于局部塊特征優(yōu)化的室內(nèi)高精度激光SLAM方法

蔡 睿,章國寶*,朱宏偉

(1.東南大學(xué) 自動(dòng)化學(xué)院,江蘇 南京 211189; 2.南京深地智能建筑技術(shù)研究院,江蘇 南京 210019)

近年來,隨著傳感器硬件技術(shù)的發(fā)展完善,多線三維激光雷達(dá)、慣性測(cè)量單元(Inertial Measurement Unit,IMU)和全球?qū)Ш叫l(wèi)星系統(tǒng)等傳感器在精度方面已經(jīng)達(dá)到了較高的水準(zhǔn)[1]。然而,實(shí)際測(cè)繪工程所需的并非簡(jiǎn)單對(duì)單個(gè)傳感器采集信息的直接輸出,往往是對(duì)多個(gè)不同傳感器在實(shí)時(shí)運(yùn)動(dòng)狀態(tài)下采集的多種信息的融合處理[2-3]。在這一背景下,激光SLAM(Simultaneous Localization and Mapping,即時(shí)定位與地圖構(gòu)建)算法即是以解決上述問題為目的,利用一種或多種傳感器[4]的數(shù)據(jù),同時(shí)進(jìn)行測(cè)繪平臺(tái)的定位和全局點(diǎn)云地圖生成的技術(shù)。當(dāng)前多數(shù)解決方案僅采用三維激光雷達(dá)數(shù)據(jù)作為傳感器,或同時(shí)融合IMU等數(shù)據(jù)作為輔助[5-6],解決GNSS(Global Navigation Satellite System,全球?qū)Ш叫l(wèi)星系統(tǒng))在部分極端環(huán)境下無信號(hào)或信號(hào)弱、無法精準(zhǔn)采集軌跡信息的難題。

目前,激光SLAM仍然是以三維激光雷達(dá)數(shù)據(jù)為主要信息來源,部分以IMU數(shù)據(jù)作為輔助提升平臺(tái)位姿計(jì)算精度。與視覺相機(jī)相比,三維激光雷達(dá)可以避免受到環(huán)境亮度的影響,而IMU輔助位姿計(jì)算可以減弱激光點(diǎn)云數(shù)據(jù)在運(yùn)動(dòng)中產(chǎn)生的累積誤差帶來的影響。二十世紀(jì)以來,SLAM技術(shù)逐漸實(shí)際應(yīng)用于室內(nèi)掃地機(jī)器人、自主駕駛系統(tǒng)、測(cè)繪建圖機(jī)器人等多個(gè)領(lǐng)域的產(chǎn)品中[7],且國內(nèi)外涌現(xiàn)出一批優(yōu)秀的解決方案,其中包含了Gmapping[8-9]、Cartographer[10]、LOAM(LiDAR Odometry and Mapping)[11]以及LeGO-LOAM(Lightweight and Ground-Optimized LOAM)[12]等。然而對(duì)于部分特殊環(huán)境,現(xiàn)有解決方案建圖效果失真的情況時(shí)有發(fā)生。

針對(duì)幾何特征匱乏的室內(nèi)環(huán)境提出融合IMU數(shù)據(jù)的激光SLAM方法,分割地面點(diǎn)與非地面點(diǎn)后在點(diǎn)云位置信息和強(qiáng)度信息的深度圖中劃分局部塊,在局部塊內(nèi)進(jìn)行離群點(diǎn)篩除、幾何光滑度梯度和強(qiáng)度梯度計(jì)算,作為此局部塊的特征向量。IMU積分值作為求解位姿的初值,以兩幀特征之間的差值作為目標(biāo)函數(shù)迭代求解。方法融合了三維激光點(diǎn)云的幾何信息、強(qiáng)度信息和IMU數(shù)據(jù),提升了對(duì)室內(nèi)幾何特征匱乏環(huán)境的定位與建圖系統(tǒng)的精度。

1 系統(tǒng)概述

本文設(shè)計(jì)系統(tǒng)接收三維激光點(diǎn)云原始數(shù)據(jù)和IMU數(shù)據(jù),通過設(shè)計(jì)算法實(shí)現(xiàn)對(duì)室內(nèi)環(huán)境的高精度定位與建圖功能。系統(tǒng)主要組成部分包含如下模塊。

① 數(shù)據(jù)預(yù)處理和分割。根據(jù)原始點(diǎn)云的坐標(biāo)數(shù)據(jù)將其分割為地面點(diǎn)與非地面點(diǎn)兩類,并對(duì)點(diǎn)云的坐標(biāo)數(shù)據(jù)和強(qiáng)度數(shù)據(jù)分別投影至柱面形成深度圖以便特征計(jì)算。

② 去除離群值和特征提取。對(duì)非地面范圍內(nèi)的坐標(biāo)深度圖和強(qiáng)度深度圖分別劃分局部塊,對(duì)局部塊內(nèi)離群值進(jìn)行剔除,并針對(duì)每個(gè)局部塊分別計(jì)算子區(qū)域的特征值梯度值作為該局部塊的特征向量。而對(duì)于地面點(diǎn)區(qū)域的坐標(biāo)信息采用直接計(jì)算法向量的方式處理。

③ IMU數(shù)據(jù)積分。IMU的數(shù)據(jù)的采集頻率是高于點(diǎn)云數(shù)據(jù)一個(gè)數(shù)量級(jí)的,對(duì)每兩幀點(diǎn)云之間的IMU數(shù)據(jù)進(jìn)行積分處理。這一模塊和以上兩個(gè)模塊是并行的。

④ 特征匹配和幀間位姿求解。采用IMU數(shù)據(jù)積分模塊的結(jié)果作為初值,以幀間特征差構(gòu)建目標(biāo)函數(shù),利用L-M(Levenberg-Marquardt)方法迭代求解。

⑤ 地圖構(gòu)建。利用當(dāng)前幀提取的地面法向量和全局地圖地面法向量配準(zhǔn)修正,最終更新地圖。

系統(tǒng)框架如圖1所示。

2 算法設(shè)計(jì)

2.1 數(shù)據(jù)預(yù)處理

2.1.1 地面點(diǎn)集分割

由于實(shí)際環(huán)境中地面環(huán)境較為單一,一般呈現(xiàn)類固定平面的狀態(tài),包含信息較少且相對(duì)變化較少。因此對(duì)初始點(diǎn)云先采用文獻(xiàn)[13]中擬合地面平面并進(jìn)行標(biāo)記的方法分割為地面點(diǎn)集與非地面點(diǎn)集,非地面點(diǎn)用于提取特征,地面點(diǎn)用于提取平面法向量,地面點(diǎn)集分割效果如圖2所示。

圖2 地面點(diǎn)集分割效果

2.1.2 點(diǎn)云坐標(biāo)與強(qiáng)度深度圖生成

對(duì)于非地面點(diǎn)Vk=Pk-Gk,由于原始點(diǎn)云數(shù)據(jù)量較大,ICP(Iterative Closest Point,迭代最近點(diǎn))等現(xiàn)有點(diǎn)云匹配方法對(duì)每個(gè)點(diǎn)都進(jìn)行匹配處理導(dǎo)致運(yùn)算量大,對(duì)運(yùn)行設(shè)備造成較重的負(fù)擔(dān),因此本方法采取了投影到柱面得到深度圖后再將其劃分為若干局部塊,對(duì)每個(gè)局部塊單獨(dú)計(jì)算特征用于匹配的方法。

(1)

(2)

圖3 深度圖生成示意圖

2.1.3 特征提取

受參考文獻(xiàn)[14]啟發(fā),在非地面點(diǎn)深度圖上劃分固定尺寸局部塊并提取局部塊的主要特征,而在地面點(diǎn)集合上采用提取法向量的方法,能夠?qū)μ卣餍畔⒇S富的非地面點(diǎn)和特征信息較少的非地面點(diǎn)分別進(jìn)行處理。特征提取的具體步驟如下。

① 對(duì)非地面點(diǎn)深度圖進(jìn)行局部塊的劃分,每個(gè)局部塊塊寬為m,塊高為n,通過對(duì)每個(gè)局部塊計(jì)算特征來描述該局部塊的主要特征,深度圖劃分局部塊示意圖如圖4所示(其中被劃分為地面點(diǎn)的像素點(diǎn)被置為無效值)。

圖4 局部塊劃分示意圖

② 對(duì)每個(gè)局部塊的所有深度像素判斷離群像素點(diǎn)(即該局部塊中與其余像素點(diǎn)的深度值相差過大)并剔除,不進(jìn)入后續(xù)的計(jì)算過程。

③ 對(duì)每個(gè)局部塊選取中心點(diǎn),以高為m/4,寬為n/4為界在該中心點(diǎn)周圍建立4個(gè)子區(qū)域,每個(gè)子區(qū)域?yàn)橐粋€(gè)同心塊,如圖5所示。

圖5 局部塊子區(qū)域示意圖

④ 對(duì)中心點(diǎn)分別將4個(gè)子區(qū)域作為領(lǐng)域從內(nèi)至外依次計(jì)算該點(diǎn)光滑度c1、c2、c3、c4。

(3)

⑤ 對(duì)每個(gè)子區(qū)域內(nèi)的中心點(diǎn)以外的點(diǎn),以其周邊高為m/4、寬為n/4的鄰域內(nèi)的點(diǎn)計(jì)算c值,并對(duì)每個(gè)子區(qū)域q計(jì)算以其作為鄰域的中心點(diǎn)的二維梯度(其中x和y并非原始點(diǎn)云數(shù)據(jù)的三維坐標(biāo)的x值和y值,而是經(jīng)過處理到深度圖后點(diǎn)在圖上的像素坐標(biāo)):

(4)

經(jīng)過式(4)計(jì)算后得到4個(gè)二維梯度值,作為該局部塊B(i,j)的特征。

2.1.4 IMU數(shù)據(jù)積分

(5)

(6)

(7)

(8)

(9)

采用參考文獻(xiàn)[15]中提出的幀間積分方法,得到相鄰兩點(diǎn)云幀的時(shí)刻k和k+1之間的相對(duì)運(yùn)動(dòng)增量為

(10)

(11)

(12)

2.2 雷達(dá)里程計(jì)

2.2.1 相鄰幀點(diǎn)云配準(zhǔn)

(13)

(14)

得到兩個(gè)深度圖每個(gè)局部塊B(i,j)的特征向量之間的差值函數(shù),由兩個(gè)差值函數(shù)構(gòu)建目標(biāo)函數(shù)為

(15)

2.2.2 當(dāng)前幀位姿估算

(16)

2.3 地圖構(gòu)建

2.3.1 地圖配準(zhǔn)與修正

在點(diǎn)云配準(zhǔn)時(shí),地面點(diǎn)可以提供俯仰角、橫滾角和z軸方向的約束,而非地面點(diǎn)可以提供偏航角、x軸和y軸方向的約束[13]。非地面點(diǎn)的約束實(shí)現(xiàn)已經(jīng)在雷達(dá)里程計(jì)部分完成了,而在每幀點(diǎn)云數(shù)據(jù)中均會(huì)進(jìn)行地面點(diǎn)的篩選并提取地面點(diǎn)的法向量,地圖配準(zhǔn)與修正模塊對(duì)當(dāng)前幀的法向量與已構(gòu)建地圖的地面法向量進(jìn)行匹配并優(yōu)化當(dāng)前幀位姿。

2.3.2 更新地圖

(17)

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

3.1 實(shí)驗(yàn)設(shè)備與數(shù)據(jù)

為測(cè)試本文方法的實(shí)際運(yùn)行效果,采用公開數(shù)據(jù)集和設(shè)備實(shí)測(cè)數(shù)據(jù)集共同驗(yàn)證的方式,并與主流算法LOAM、LeGO-LOAM的效果進(jìn)行比對(duì)。

公開數(shù)據(jù)集選用了具有完備傳感器數(shù)據(jù)和真值的KITTI數(shù)據(jù)集,便于比對(duì)3種方法的結(jié)果與真值之間的絕對(duì)位姿誤差(Absolute Pose Error,APE)。使用到的傳感器數(shù)據(jù)包括Velodyne HDL-64E LiDAR和OXTS RT 3003 IMU,采集場(chǎng)景包括住宅區(qū)、公路等,特征較為豐富。

實(shí)測(cè)數(shù)據(jù)集采用設(shè)備為RS-LiDAR-16混合固態(tài)激光16線雷達(dá)和HWT901B-485高精度10軸姿態(tài)角度傳感器,如圖6所示。由于實(shí)驗(yàn)設(shè)備限制無法提供ground-truth基準(zhǔn)值進(jìn)行數(shù)值分析,因此僅用作特殊場(chǎng)景的全局地圖生成效果比對(duì)。采集場(chǎng)景包括幾何特征信息單一且稀少的走廊環(huán)境和適用于測(cè)試提取點(diǎn)云強(qiáng)度特征影響的地下停車場(chǎng),如圖7所示。

圖6 實(shí)測(cè)數(shù)據(jù)所用設(shè)備

圖7 實(shí)測(cè)數(shù)據(jù)場(chǎng)景

3.2 幾何特征匹配效果

由于KITTI數(shù)據(jù)集中不具備點(diǎn)云強(qiáng)度域信息,僅包含點(diǎn)云位置信息,但具備ground-truth基準(zhǔn)值參考,可以有效進(jìn)行不同算法之間的具體數(shù)值比對(duì),因此選擇其中的00、05、07、09序列數(shù)據(jù),對(duì)本文提出的點(diǎn)云坐標(biāo)強(qiáng)度圖局部塊特征向量提取與匹配方法進(jìn)行可行性驗(yàn)證,并與主流算法的運(yùn)行效果進(jìn)行比對(duì)。

在運(yùn)行數(shù)據(jù)集并分析效果的過程中,發(fā)現(xiàn)LOAM由于幾何特征約束的單一性和缺少閉環(huán)環(huán)節(jié)容易逐漸累積誤差,而LeGO-LOAM由于其閉環(huán)環(huán)節(jié)的存在,檢測(cè)到閉環(huán)時(shí)對(duì)位姿的調(diào)整會(huì)導(dǎo)致位姿數(shù)據(jù)的突變,這一點(diǎn)在高度項(xiàng)上體現(xiàn)得尤其明顯,如圖8(a)中軌跡所示。

而本文方法在加強(qiáng)地面約束的基礎(chǔ)上采用局部塊的方法劃定子區(qū)域并計(jì)算其光滑度梯度值作為表述該子區(qū)域的特征向量的方法,增強(qiáng)了對(duì)小范圍區(qū)域的描述性,與原始LOAM中劃分角特征點(diǎn)與面特征點(diǎn)并進(jìn)行幀間匹配的方法相比,考慮了對(duì)特征點(diǎn)周邊環(huán)境的描述,在KITTI數(shù)據(jù)集上的實(shí)踐效果較好,減小了累積誤差的影響,且不存在位姿數(shù)據(jù)突變的問題。圖8(a)～圖8(d)分別為LOAM、LeGO-LOAM和本文方法在KITTI00、05、07、09數(shù)據(jù)集上的運(yùn)行軌跡與真值的比對(duì),其中x軸、z軸分別為水平方向的橫縱軸。

圖9(a)～圖9(d)分別為LOAM、LeGO-LOAM和本文方法在KITTI00、05、07、09數(shù)據(jù)集上的APE值比對(duì)圖,表1為公共數(shù)據(jù)集軌跡APE結(jié)果。由此可以看出,本文方法在精度上獲得了提升,且避免了位姿的突變。

表1 公共數(shù)據(jù)集軌跡APE結(jié)果 單位:m

圖9 公共數(shù)據(jù)集軌跡誤差比對(duì)

3.3 強(qiáng)度特征匹配結(jié)果

實(shí)地采集的數(shù)據(jù)中包含了點(diǎn)云位置信息、強(qiáng)度信息和IMU數(shù)據(jù),因此用于測(cè)試附加強(qiáng)度特征匹配模塊的效果。在使用LeGO-LOAM測(cè)試實(shí)地?cái)?shù)據(jù)集的過程中發(fā)現(xiàn),對(duì)于特征單一導(dǎo)致匹配較為困難的走廊環(huán)境,LeGO-LOAM會(huì)出現(xiàn)“跑飛”現(xiàn)象,即生成地圖結(jié)果與實(shí)際相差較大。通過對(duì)比走廊環(huán)境與所用KITTI數(shù)據(jù)集的環(huán)境,得出出現(xiàn)這一情況的原因是走廊環(huán)境的地面較狹窄且壁面環(huán)境相似度高,導(dǎo)致了劃分地面點(diǎn)和提取特征的困難。而附加了強(qiáng)度特征匹配模塊的本文方法則會(huì)在這一環(huán)境下結(jié)合強(qiáng)度特征和IMU數(shù)據(jù)進(jìn)行修正。對(duì)兩種方法的效果進(jìn)行了比對(duì),如圖10所示。

由圖10可以看出,本文方法通過強(qiáng)度特征匹配和融合IMU數(shù)據(jù)有效改善了“跑飛”現(xiàn)象,增強(qiáng)了算法的穩(wěn)定性。

而對(duì)于地下停車場(chǎng)場(chǎng)景,本文方法增加的強(qiáng)度特征約束項(xiàng)提高了幀間匹配的準(zhǔn)確性,建圖中墻壁、拐角等的“重影”現(xiàn)象有所減少,如圖11所示。

圖11 實(shí)測(cè)地下停車場(chǎng)環(huán)境效果比對(duì)

圖11(a)和圖11(b)分別為兩種算法對(duì)同一場(chǎng)景的整體效果對(duì)比(圖中紅框標(biāo)注區(qū)分別為圖11(c)和圖11(d)所在位置),圖11(c)和圖11(d)中紅色橢圓標(biāo)注區(qū)域分別為LOAM運(yùn)行出現(xiàn)“重影”現(xiàn)象和本文方法“重影”改善效果。

3.4 結(jié)果分析

所設(shè)計(jì)的實(shí)驗(yàn)采用了公共數(shù)據(jù)集和實(shí)測(cè)數(shù)據(jù)集對(duì)本文方法與已有的LOAM和LeGO-LOAM算法的運(yùn)行結(jié)果進(jìn)行了比對(duì)。

通過對(duì)KITTI公共數(shù)據(jù)集00、05、07、09序列的建圖并比對(duì)真值和計(jì)算誤差,可見幾何特征提取和匹配模塊的改進(jìn)提升了特征對(duì)環(huán)境的表述能力,進(jìn)一步提升了匹配的精度,此外也避免了位姿突變的發(fā)生。而附加了強(qiáng)度特征匹配模塊的本文方法在實(shí)測(cè)數(shù)據(jù)集上的結(jié)果也較已有算法獲得了較大的提升,改善了兩種已有算法運(yùn)行結(jié)果中出現(xiàn)的“跑飛”和“重影”現(xiàn)象。

4 結(jié)束語

針對(duì)幾何特征稀少、幀間匹配難度較大的環(huán)境,提出了一種基于深度圖劃分局部塊并篩除離群點(diǎn)、提取塊內(nèi)點(diǎn)云光滑度特征信息和強(qiáng)度特征信息,同時(shí)融合IMU數(shù)據(jù)求解幀間位姿的定位建圖方法。經(jīng)過KITTI公共數(shù)據(jù)集的驗(yàn)證,幾何特征提取與匹配模塊的改進(jìn)有效地提升了位姿估算的精度,降低了匹配誤差的累積,提升了定位與建圖的準(zhǔn)確性。經(jīng)實(shí)測(cè)數(shù)據(jù)集實(shí)驗(yàn),強(qiáng)度特征與幾何特征的提取和結(jié)合可以有效地改善已有算法中會(huì)出現(xiàn)的“跑飛”和“重影”現(xiàn)象,即對(duì)于幾何信息較為匱乏的環(huán)境,融合點(diǎn)云幾何信息與強(qiáng)度信息的方法比使用單一幾何信息的匹配方法效果更好。