一種融合IMU的手持旋軸激光雷達定位建圖方法

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

(1.北京工業(yè)大學 信息學部,北京 100124; 2.先進信息網絡北京實驗室,北京 100124)

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

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

圖1 建圖范圍對比

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

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

1 算法流程

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

圖2 算法流程路線

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

① 首先對IMU與激光雷達數據進行處理,對IMU進行預積分操作,根據IMU信息對激光雷達進行點云畸變矯正。

② 隨后對矯正過的點云進行特征點提取,分別提取邊緣特征點、強度特征點以及平面點,動態(tài)調整濾波數值設置并構建局部地圖。

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

1.1 數據處理

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

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

(1)

1.1.1 IMU數據處理

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

(2)

(3)

(4)

(5)

(6)

隨后使用文獻[11]的方法進行IMU預積分,提升運算效率。

1.1.2 激光雷達點云去畸變

在實際采集數據過程中,激光雷達在運動,且點云數據并非瞬時獲取。這會導致一幀內的點云數據產生運動畸變,需要將一幀內所有點云投影至掃描幀開始時刻,完成畸變的去除。通過插值的方法來確定點的位姿:

(7)

(8)

tcur=treal+tScanStart

(9)

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

TstartΔT=Tcur

(10)

(11)

pori=ΔRpcur+Δp

(12)

式中:pcur為當前點未去畸變坐標;Tstart為開始時刻點位姿,經ΔT=(ΔR,Δp)變換至開始時刻位置pori完成畸變去除。

1.2 特征提取

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

(13)

(14)

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

圖3 強度特征點選取

mi>mth∩cirth

(15)

1.3 動態(tài)濾波數值設置與局部地圖構建

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

在系統(tǒng)提取特征點后,根據提取邊緣特征點數量Nf對比設置正常特征點數量Nnor與有退化風險特征點數量Ndeg,判斷系統(tǒng)所處狀態(tài)從而實時調整相應的數值,判斷方式如下:

(16)

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

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

1.4 配準優(yōu)化

將參與配準的特征點與局部地圖進行配準,對于強度特征點與邊緣特征點進行點-線最小距離優(yōu)化:

(17)

對于面點進行點-面最小距離優(yōu)化:

(18)

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

2 實驗結果與分析

2.1 實驗系統(tǒng)概述

使用便攜式手持旋軸激光掃描儀進行實驗,如圖4所示。

圖4 實驗設備

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

圖5 實驗環(huán)境

2.2 特征點提取對比

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

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

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

圖7 局部場景

圖7(a)中,LIO-SAM算法在進入封閉環(huán)境時,未判斷是否有退化風險,沒有實時調整濾波數值。進入通道時發(fā)生退化問題,發(fā)生定位建圖錯誤。

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

2.4 定位軌跡對比

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

表1 定位軌跡對比 單位:m

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

2.5 建圖效果對比

對比手持旋軸激光雷達使用LIO-SAM算法與改進算法的建圖效果如圖9所示。

圖9 建圖效果對比

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

2.6 長距離定位建圖對比

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

圖10 長距離定位建圖實驗結果對比

3 結束語

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