趙宇軒,賈克斌*,陳嘉平

(1.北京工業(yè)大學(xué) 信息學(xué)部,北京 100124; 2.先進(jìn)信息網(wǎng)絡(luò)北京實(shí)驗(yàn)室,北京 100124)

隨著同步定位與建圖(Simultaneous Localization and Mappi-ng,SLAM)技術(shù)的不斷發(fā)展,其已經(jīng)廣泛應(yīng)用于2D/3D建圖、智能機(jī)器人導(dǎo)航等領(lǐng)域[1]。多數(shù)時(shí)間,機(jī)器人可以通過GPS、GNSS等設(shè)備獲取位置信息進(jìn)行定位。但當(dāng)機(jī)器人處在地下停車場(chǎng)、隧道等封閉環(huán)境時(shí),GPS、GNSS設(shè)備定位便會(huì)失效。機(jī)器人無法獲得自身位置定位從而無法進(jìn)行導(dǎo)航任務(wù),而SLAM技術(shù)為此需求提供了一種解決方法[2]。

激光SLAM通過激光雷達(dá)直接獲取距離信息,提供三維點(diǎn)云。激光雷達(dá)在有著高測(cè)量精度的同時(shí),受周邊環(huán)境影響較小,無論白天黑夜都能正常工作[3]。激光雷達(dá)視野更廣,直接獲取點(diǎn)云也使建立地圖變得更加方便[4]。但激光雷達(dá)多部署于車輛載具,這導(dǎo)致其使用門檻較高、便攜性差,無法快速進(jìn)行數(shù)據(jù)采集。隨著激光雷達(dá)產(chǎn)業(yè)的不斷發(fā)展,手持旋軸式激光雷達(dá)掃描設(shè)備也逐漸走入人們的視野,滿足了人們對(duì)于便攜性的需求[5],手持旋軸式激光雷達(dá)不僅輕便易用,還有著更大的掃描范圍,如圖1所示。

圖1 建圖范圍對(duì)比

LOAM(LiDAR Odometry and Mapping in Real-time)算法[6]提取邊緣特征點(diǎn)與平面特征點(diǎn)分別進(jìn)行配準(zhǔn),屬于基于特征的方法,這使得LOAM可以實(shí)時(shí)快速地進(jìn)行定位與地圖構(gòu)建。同時(shí),隨著研究的不斷深入,如今已有多種基于LOAM的改進(jìn)算法。文獻(xiàn)[7]提出,在LOAM的基礎(chǔ)上引入附加特征點(diǎn),增強(qiáng)SLAM系統(tǒng)在結(jié)構(gòu)化場(chǎng)景中的配準(zhǔn)能力。文獻(xiàn)[8]提出一個(gè)根據(jù)當(dāng)前激光幀內(nèi)點(diǎn)云距離分布信息,采用一種模糊算法,調(diào)整系統(tǒng)參數(shù),增強(qiáng)SLAM系統(tǒng)對(duì)環(huán)境空間變化適應(yīng)性的方法。當(dāng)今應(yīng)用較為廣泛的主流LOAM改進(jìn)算法有對(duì)地面點(diǎn)分割聚類的輕量級(jí)SLAM算法LeGO-LOAM[9]和通過緊耦合慣性測(cè)量單元(Inertial Measurement unit,IMU)實(shí)現(xiàn)更好定位建圖效果的SLAM算法LIO-SAM[10]。

主流激光SLAM算法可以滿足大多數(shù)定位建圖需求,但均未考慮手持旋軸激光雷達(dá)的旋轉(zhuǎn)定位建圖場(chǎng)景。LOAM算法假設(shè)雷達(dá)運(yùn)動(dòng)為勻速模型,通過雷達(dá)里程計(jì)對(duì)點(diǎn)云進(jìn)行畸變?nèi)コ?在激光雷達(dá)水平放置且低速運(yùn)動(dòng)時(shí)尚可達(dá)成較好效果,但在激光雷達(dá)旋轉(zhuǎn)場(chǎng)景下不足以滿足需求。LeGO-LOAM算法針對(duì)載具設(shè)備上水平放置的雷達(dá),通過提取地面點(diǎn)聚類的方法定位建圖,對(duì)地面點(diǎn)的要求同樣不滿足手持旋軸激光雷達(dá)的旋轉(zhuǎn)過程。LIO-SAM算法通過遠(yuǎn)高于激光雷達(dá)掃描頻率的IMU傳感器提供數(shù)據(jù),對(duì)激光雷達(dá)點(diǎn)云進(jìn)行畸變?nèi)コ?可以滿足激光雷達(dá)旋轉(zhuǎn)的應(yīng)用場(chǎng)景。然而設(shè)備運(yùn)行過程中,在一個(gè)旋轉(zhuǎn)周期的部分時(shí)間內(nèi),激光雷達(dá)的部分掃描區(qū)間會(huì)朝向上空開闊區(qū)域。此區(qū)域基本無法接收返回的激光點(diǎn),易導(dǎo)致特征點(diǎn)稀疏問題的發(fā)生,這對(duì)基于特征的SLAM算法的穩(wěn)定性有很大影響。為提升手持旋軸激光雷達(dá)使用的穩(wěn)定性,受上述工作啟發(fā),本文將一種基于點(diǎn)云強(qiáng)度信息的特征點(diǎn)引入LIO-SAM算法中,增加參與配準(zhǔn)的特征點(diǎn)數(shù)量,并實(shí)時(shí)判斷系統(tǒng)所處環(huán)境是否存在退化風(fēng)險(xiǎn),動(dòng)態(tài)調(diào)整濾波數(shù)值設(shè)置,保證SLAM系統(tǒng)的穩(wěn)定性。

1 算法流程

本文的算法流程路線如圖2所示,輸入為10 Hz的激光雷達(dá)數(shù)據(jù)與200 Hz的IMU數(shù)據(jù)。

圖2 算法流程路線

主要流程分為以下3部分。

① 首先對(duì)IMU與激光雷達(dá)數(shù)據(jù)進(jìn)行處理,對(duì)IMU進(jìn)行預(yù)積分操作,根據(jù)IMU信息對(duì)激光雷達(dá)進(jìn)行點(diǎn)云畸變矯正。

② 隨后對(duì)矯正過的點(diǎn)云進(jìn)行特征點(diǎn)提取,分別提取邊緣特征點(diǎn)、強(qiáng)度特征點(diǎn)以及平面點(diǎn),動(dòng)態(tài)調(diào)整濾波數(shù)值設(shè)置并構(gòu)建局部地圖。

③ 最后進(jìn)行配準(zhǔn)優(yōu)化和回環(huán)檢測(cè)與圖優(yōu)化,發(fā)布激光里程計(jì)。

1.1 數(shù)據(jù)處理

W代表世界坐標(biāo)系,I代表IMU坐標(biāo)系,T21代表從坐標(biāo)系1到坐標(biāo)系2的變換矩陣。則可以規(guī)定IMU坐標(biāo)系到世界坐標(biāo)系變換矩陣為TWI=(RWI,PWI),若RT表示旋轉(zhuǎn)狀態(tài),PT表示位置向量,VT表示速度,bT表示IMU偏置,則狀態(tài)x可以被表示如下:

x=[RT,PT,VT,bT]T

(1)

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

IMU在SLAM系統(tǒng)運(yùn)行中發(fā)揮重要作用,IMU以遠(yuǎn)高于雷達(dá)頻率的速度來提供位姿、加速度、角速度等數(shù)據(jù)來更好地推算雷達(dá)的狀態(tài)。IMU的角速度與加速度計(jì)算公式如下:

(2)

(3)

(4)

(5)

(6)

隨后使用文獻(xiàn)[11]的方法進(jìn)行IMU預(yù)積分,提升運(yùn)算效率。

1.1.2 激光雷達(dá)點(diǎn)云去畸變

在實(shí)際采集數(shù)據(jù)過程中,激光雷達(dá)在運(yùn)動(dòng),且點(diǎn)云數(shù)據(jù)并非瞬時(shí)獲取。這會(huì)導(dǎo)致一幀內(nèi)的點(diǎn)云數(shù)據(jù)產(chǎn)生運(yùn)動(dòng)畸變,需要將一幀內(nèi)所有點(diǎn)云投影至掃描幀開始時(shí)刻,完成畸變的去除。通過插值的方法來確定點(diǎn)的位姿:

(7)

(8)

tcur=treal+tScanStart

(9)

式中:tcur為當(dāng)前點(diǎn)在全局中的時(shí)間;tb與tf分別為此雷達(dá)幀前后最近的兩幀IMU時(shí)間;Rb與Rf為計(jì)算的旋轉(zhuǎn)角度;treal為一點(diǎn)p相對(duì)于掃描幀開始時(shí)刻的時(shí)間;tScanStart與tScanEnd分別為激光雷達(dá)掃描幀開始與結(jié)束時(shí)刻;Podm為位移增量。后經(jīng)插值得到當(dāng)前點(diǎn)關(guān)于掃描開始時(shí)刻的角度變換Rcur與位移Pcur。根據(jù)所求的三軸旋轉(zhuǎn)角度與三位移量構(gòu)成六自由度數(shù)據(jù)后,即可得出此點(diǎn)相對(duì)于開始時(shí)刻的變換矩陣Tcur。從而可將點(diǎn)變換至掃描開始時(shí)刻:

TstartΔT=Tcur

(10)

(11)

pori=ΔRpcur+Δp

(12)

式中:pcur為當(dāng)前點(diǎn)未去畸變坐標(biāo);Tstart為開始時(shí)刻點(diǎn)位姿,經(jīng)ΔT=(ΔR,Δp)變換至開始時(shí)刻位置pori完成畸變?nèi)コ?/p>

1.2 特征提取

改進(jìn)算法將一幀內(nèi)的點(diǎn)云分為邊緣特征點(diǎn)、平面點(diǎn)的同時(shí),額外提取強(qiáng)度特征點(diǎn)來增加參與配準(zhǔn)的激光點(diǎn)數(shù)量。激光雷達(dá)接收的強(qiáng)度返回值會(huì)受到多種因素的影響而變化,但在很短的雷達(dá)幀間時(shí)間內(nèi),假定強(qiáng)度信息變化趨勢(shì)的整體一致性,提取出周圍強(qiáng)度值變化較大的點(diǎn)作為強(qiáng)度特征點(diǎn)參與到點(diǎn)云配準(zhǔn)。對(duì)一幀點(diǎn)云Pk內(nèi)的點(diǎn)進(jìn)行曲率c與強(qiáng)度變化率m的計(jì)算:

(13)

(14)

式中:i為Pk中一點(diǎn);S為與i在同一掃描線的周圍連續(xù)多個(gè)點(diǎn);j為S中一點(diǎn);r為掃描點(diǎn)的深度值;I為點(diǎn)的強(qiáng)度值;k為第k幀。將每根掃描線分為6個(gè)區(qū)域,每個(gè)區(qū)域劃定內(nèi)分別根據(jù)強(qiáng)度變化率與曲率進(jìn)行排序。設(shè)置mth與cth分別作為強(qiáng)度特征點(diǎn)與邊緣特征點(diǎn)的閾值,將數(shù)據(jù)大于閾值的幾個(gè)點(diǎn)提取作為強(qiáng)度特征點(diǎn)、邊緣特征點(diǎn),并根據(jù)相鄰兩點(diǎn)列索引差,剔除雷達(dá)由于射向地面導(dǎo)致線束中斷而被誤判為邊緣特征點(diǎn)的點(diǎn)。為保證特征點(diǎn)不過于密集,每選取一個(gè)邊緣特征點(diǎn)或強(qiáng)度特征點(diǎn)時(shí),會(huì)將其同一掃描線周圍點(diǎn)標(biāo)識(shí)為不可選取。強(qiáng)度特征點(diǎn)與邊緣特征點(diǎn)的選取原理不同,不會(huì)影響它們周圍點(diǎn)的選取與否。例如將點(diǎn)i選取為強(qiáng)度特征點(diǎn)后,點(diǎn)i周圍點(diǎn)不能被選為強(qiáng)度特征點(diǎn),但點(diǎn)i周圍點(diǎn)依舊可選為邊緣特征點(diǎn)。提取的強(qiáng)度特征點(diǎn)主要為不同材質(zhì)的邊界等周邊環(huán)境復(fù)雜的點(diǎn),如圖3所示。圖3(a)顯示了激光雷達(dá)掃描到地面上一條涂有漆面的標(biāo)識(shí)線,由于表面反射強(qiáng)度值不同,可以從中提取出強(qiáng)度特征點(diǎn),如圖3(b)所示。然而,有些邊緣特征點(diǎn)同樣滿足周圍反射強(qiáng)度值變化大的條件,但將邊緣特征點(diǎn)改提取為新的強(qiáng)度特征點(diǎn)并無意義,所以選擇提取的強(qiáng)度特征點(diǎn)是強(qiáng)度變化率mi滿足閾值mth且曲率ci不滿足邊緣特征點(diǎn)閾值cth的點(diǎn)。且為防止掃描時(shí)由于距離過近,強(qiáng)度值跳變較大,被誤判為強(qiáng)度特征點(diǎn),被選取點(diǎn)的深度ri應(yīng)大于深度閾值rth,選取的強(qiáng)度特征點(diǎn)應(yīng)滿足式(15):

圖3 強(qiáng)度特征點(diǎn)選取

mi>mth∩cirth

(15)

1.3 動(dòng)態(tài)濾波數(shù)值設(shè)置與局部地圖構(gòu)建

在數(shù)據(jù)采集過程提取特征點(diǎn)后,系統(tǒng)會(huì)對(duì)特征點(diǎn)進(jìn)行一次降采樣濾波。然而周圍的場(chǎng)景總是在變化的,單一濾波數(shù)值設(shè)置只滿足部分場(chǎng)景。例如適用于戶外特征點(diǎn)較多的濾波數(shù)值,在進(jìn)入通道等退化環(huán)境時(shí)會(huì)容易導(dǎo)致建圖失敗,且激光雷達(dá)處于旋轉(zhuǎn)過程時(shí),特征點(diǎn)數(shù)量變化更加頻繁。這就需要在運(yùn)行過程中實(shí)時(shí)調(diào)節(jié)濾波數(shù)值保證系統(tǒng)運(yùn)行穩(wěn)定。本文使用一種簡(jiǎn)潔快速的方法實(shí)時(shí)調(diào)整濾波數(shù)值,達(dá)到防止退化的目的并保持系統(tǒng)運(yùn)行的效率。

在系統(tǒng)提取特征點(diǎn)后,根據(jù)提取邊緣特征點(diǎn)數(shù)量Nf對(duì)比設(shè)置正常特征點(diǎn)數(shù)量Nnor與有退化風(fēng)險(xiǎn)特征點(diǎn)數(shù)量Ndeg,判斷系統(tǒng)所處狀態(tài)從而實(shí)時(shí)調(diào)整相應(yīng)的數(shù)值,判斷方式如下:

(16)

根據(jù)以上3種不同情況,實(shí)時(shí)動(dòng)態(tài)地調(diào)整濾波數(shù)值。當(dāng)特征點(diǎn)數(shù)量較少時(shí),認(rèn)為系統(tǒng)有退化風(fēng)險(xiǎn),則選取小數(shù)值,多保留特征點(diǎn),當(dāng)特征點(diǎn)數(shù)量足夠時(shí),選取較大數(shù)值保證系統(tǒng)運(yùn)算效率,通過此方法提升SLAM系統(tǒng)的穩(wěn)定性。

隨后,通過提取關(guān)鍵幀,在當(dāng)前掃描幀周圍進(jìn)行局部地圖構(gòu)建,構(gòu)成的局部地圖包含強(qiáng)度特征點(diǎn)局部地圖Mi、邊緣特征點(diǎn)局部地圖Me和平面點(diǎn)局部地圖Mp,為配準(zhǔn)優(yōu)化構(gòu)建條件。同時(shí)判斷此幀內(nèi)強(qiáng)度特征點(diǎn)數(shù)量,若數(shù)量較少,則認(rèn)為此幀為強(qiáng)度特征退化環(huán)境,此幀內(nèi)的強(qiáng)度特征點(diǎn)將不參與后續(xù)配準(zhǔn)優(yōu)化,以防對(duì)配準(zhǔn)優(yōu)化產(chǎn)生負(fù)面作用。

1.4 配準(zhǔn)優(yōu)化

將參與配準(zhǔn)的特征點(diǎn)與局部地圖進(jìn)行配準(zhǔn),對(duì)于強(qiáng)度特征點(diǎn)與邊緣特征點(diǎn)進(jìn)行點(diǎn)-線最小距離優(yōu)化:

(17)

對(duì)于面點(diǎn)進(jìn)行點(diǎn)-面最小距離優(yōu)化:

(18)

式中:Pfeature為提取出的點(diǎn);p1、p2、p3分別為在局部地圖中搜索出的與Pfeature同標(biāo)簽的點(diǎn)。隨后使用優(yōu)化方法求解,使殘差最小化,當(dāng)優(yōu)化問題收斂時(shí),可以得到激光里程計(jì)從而建立地圖。

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

2.1 實(shí)驗(yàn)系統(tǒng)概述

使用便攜式手持旋軸激光掃描儀進(jìn)行實(shí)驗(yàn),如圖4所示。

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

其中激光雷達(dá)為16線,垂直視角30°,水平分辨率0.2°,采集頻率為10 Hz,IMU采樣頻率200 Hz,在Ubuntu18.04操作系統(tǒng)下使用ROS melodic機(jī)器人操作系統(tǒng),使用手持旋軸激光掃描儀器繞建筑物行走一周,在此之間經(jīng)過一個(gè)封閉通道,如圖5所示。

圖5 實(shí)驗(yàn)環(huán)境

2.2 特征點(diǎn)提取對(duì)比

在實(shí)驗(yàn)過程中,將改進(jìn)算法與LIO-SAM算法提取的特征點(diǎn)進(jìn)行對(duì)比,結(jié)果如圖6所示。在圖6(a)采用的LIO-SAM算法中,綠色代表曲率較大的邊緣特征點(diǎn),粉色點(diǎn)代表提取的面點(diǎn)。在圖6(b)采用的改進(jìn)算法中,不僅提取綠色邊緣特征點(diǎn)、粉色的平面點(diǎn),還有額外的藍(lán)色強(qiáng)度特征點(diǎn)。從圖6(a)與圖6(b)的對(duì)比中可知兩種算法提取特征點(diǎn)不同,改進(jìn)算法可以在運(yùn)行過程中為SLAM系統(tǒng)提取更多數(shù)量的特征點(diǎn)。

2.3 退化環(huán)境效果對(duì)比

在實(shí)驗(yàn)過程中,在易于退化封閉通道環(huán)境中,兩種算法結(jié)果對(duì)比如圖7所示。

圖7 局部場(chǎng)景

圖7(a)中,LIO-SAM算法在進(jìn)入封閉環(huán)境時(shí),未判斷是否有退化風(fēng)險(xiǎn),沒有實(shí)時(shí)調(diào)整濾波數(shù)值。進(jìn)入通道時(shí)發(fā)生退化問題,發(fā)生定位建圖錯(cuò)誤。

圖7(b)中,改進(jìn)算法檢測(cè)到進(jìn)入易退化環(huán)境特征點(diǎn)變少后,改變?yōu)V波數(shù)值設(shè)置以應(yīng)對(duì)退化環(huán)境,保證系統(tǒng)的穩(wěn)定性,平穩(wěn)通過通道場(chǎng)景。

2.4 定位軌跡對(duì)比

整體軌跡效果對(duì)比如圖8所示,手持旋軸激光雷達(dá)繞建筑物行走一周。圖8(a)采用的LIO-SAM算法的里程計(jì)軌跡終點(diǎn)相對(duì)于起點(diǎn)有較大偏移。圖8(c)中,改進(jìn)算法的軌跡終點(diǎn)與出發(fā)點(diǎn)接近。使用EVO工具對(duì)里程計(jì)軌跡數(shù)據(jù)進(jìn)行繪制,圖8(b)中,LIO-SAM算法軌跡已經(jīng)產(chǎn)生偏差,圖8 (d)中,改進(jìn)算法軌跡繞建筑物一圈,與圖5相似。為明確對(duì)比實(shí)驗(yàn)結(jié)果,對(duì)軌跡數(shù)據(jù)起止坐標(biāo)進(jìn)行對(duì)比分析,結(jié)果如表1所示。

表1 定位軌跡對(duì)比 單位:m

表1中,Δx、Δy、Δz、Δd分別為終點(diǎn)坐標(biāo)與起點(diǎn)坐標(biāo)的x、y、z軸的坐標(biāo)差值以及相對(duì)于起點(diǎn)的距離差值。相比于水平方向上的差值Δx與Δy由于人工采集數(shù)據(jù)時(shí)可能造成的誤差,垂直方向上的偏移量Δz更加可觀。由表1 LIO-SAM算法與改進(jìn)算法對(duì)比可知,改進(jìn)算法在垂直偏移量Δz與相對(duì)于起點(diǎn)的距離差值Δd均優(yōu)于LIO-SAM算法。結(jié)合圖8與表1,改進(jìn)算法在手持旋軸激光雷達(dá)的定位軌跡應(yīng)用上優(yōu)于LIO-SAM算法。

2.5 建圖效果對(duì)比

對(duì)比手持旋軸激光雷達(dá)使用LIO-SAM算法與改進(jìn)算法的建圖效果如圖9所示。

圖9 建圖效果對(duì)比

圖9(a)中,LIO-SAM算法由于里程計(jì)軌跡錯(cuò)誤估計(jì),導(dǎo)致掃描中激光雷達(dá)建圖出現(xiàn)較大偏差,產(chǎn)生圖9(a)中圈出的重影問題。圖9(b)中,改進(jìn)算法則有著較好的表現(xiàn)。綜上,改進(jìn)算法在手持旋軸激光雷達(dá)的建圖效果上優(yōu)于LIO-SAM算法。

2.6 長(zhǎng)距離定位建圖對(duì)比

SLAM系統(tǒng)的定位建圖誤差會(huì)在運(yùn)行過程中逐漸累積,目前實(shí)驗(yàn)使用的IMU精度尚有較大提升空間,誤差在旋轉(zhuǎn)且長(zhǎng)距離場(chǎng)景下會(huì)相對(duì)明顯。使用EVO工具對(duì)里程計(jì)軌跡數(shù)據(jù)進(jìn)行繪制,圖10(a)與圖10(b)分別代表定位軌跡與定位建圖過程中x、y、z三軸的坐標(biāo)值。其中,藍(lán)色代表LIO-SAM算法,綠色代表改進(jìn)算法。結(jié)合圖10(a)與圖10(b)可知,兩算法在定位建圖過程中x、y軸的數(shù)據(jù)大體一致,但LIO-SAM算法的z軸數(shù)據(jù)隨著系統(tǒng)運(yùn)行,出現(xiàn)相對(duì)更多偏移,如圖10(b)所示。圖10(c)中藍(lán)色框處出現(xiàn)相對(duì)較明顯的傾斜。而圖10(d)中改進(jìn)算法的地圖相對(duì)較為平坦。對(duì)比建圖效果與坐標(biāo)偏移,改進(jìn)算法在手持旋軸激光雷達(dá)的長(zhǎng)距離定位建圖效果較LIO-SAM算法也有一定提升。

圖10 長(zhǎng)距離定位建圖實(shí)驗(yàn)結(jié)果對(duì)比

3 結(jié)束語

本文在LIO-SAM算法提取邊緣特征點(diǎn)與平面點(diǎn)的基礎(chǔ)上,計(jì)算出一種基于點(diǎn)云強(qiáng)度信息的強(qiáng)度特征點(diǎn),增加特征點(diǎn)數(shù)量,并在定位建圖過程中,實(shí)時(shí)動(dòng)態(tài)判斷SLAM系統(tǒng)所處情況是否存在退化風(fēng)險(xiǎn),動(dòng)態(tài)調(diào)整濾波數(shù)值設(shè)置,保證系統(tǒng)穩(wěn)定運(yùn)行。實(shí)驗(yàn)結(jié)果表明,改進(jìn)算法在手持旋軸激光雷達(dá)應(yīng)用中,相比LIO-SAM算法有更好的定位建圖效果。但較快速轉(zhuǎn)彎場(chǎng)景與出現(xiàn)的動(dòng)態(tài)物體對(duì)系統(tǒng)仍有一定影響,后續(xù)需進(jìn)一步改進(jìn)。