胡麗娜 曹政
摘要:未知環(huán)境下的地圖構(gòu)建技術(shù)是智能機(jī)器人實(shí)現(xiàn)路徑規(guī)劃、智能導(dǎo)航的前提與關(guān)鍵??紤]到目前傳感器領(lǐng)域的發(fā)展態(tài)勢,文章主要針對固態(tài)雷達(dá)的SLAM算法進(jìn)行探究,使用覽沃Hap雷達(dá)搭建平臺采集數(shù)據(jù)并驗(yàn)證算法。首先,文章對研究背景以及近年來相關(guān)傳感器、算法進(jìn)行了介紹。其次,這篇文章對算法的整體流程進(jìn)行了概述,并對基于主成分分析(Principal Component Analysis,PCA) 和點(diǎn)云分布的特征提取、基于因子圖的位姿估計(jì)技術(shù)細(xì)節(jié)進(jìn)行了介紹。最后,使用實(shí)車采集了實(shí)驗(yàn)數(shù)據(jù),并以效果圖和相對位姿誤差(Relative Pose Error,RPE) 定性、定量地對結(jié)果進(jìn)行了分析。結(jié)果表明,此文提出的方法具備一定的實(shí)用價(jià)值。
關(guān)鍵詞:機(jī)器人;自動駕駛;同步定位與建圖;固態(tài)雷達(dá);前端里程計(jì)
中圖分類號:TP391? ? ? 文獻(xiàn)標(biāo)識碼:A
文章編號:1009-3044(2023)31-0125-03
開放科學(xué)(資源服務(wù))標(biāo)識碼(OSID)
0 引言
同時(shí)定位與地圖構(gòu)建,即Simultaneous Localization and Mapping (簡稱SLAM) ,是一種在機(jī)器人、計(jì)算機(jī)視覺領(lǐng)域廣泛應(yīng)用的關(guān)鍵技術(shù)[1]。該算法的目標(biāo)是讓機(jī)器人在位置環(huán)境中估計(jì)自身位置并構(gòu)建環(huán)境地圖,無須事先知曉所處環(huán)境的結(jié)構(gòu)或地圖信息。它是無人駕駛車輛、無人飛行器、工業(yè)自動化等領(lǐng)域智能系統(tǒng)實(shí)現(xiàn)自主感知、決策與行動的核心問題[2]。在實(shí)現(xiàn)該技術(shù)的基礎(chǔ)之上,才能使機(jī)器、智能系統(tǒng)能夠理解并與復(fù)雜的現(xiàn)實(shí)世界互動,才能為各個(gè)領(lǐng)域帶來更高效、更安全、更智能化的解決方案。使用單個(gè)傳感器實(shí)現(xiàn)SLAM大體上可分為兩個(gè)方向:基于相機(jī)(視覺)的SLAM技術(shù)[3]與基于激光雷達(dá)SLAM技術(shù)[4]。一般來說,視覺SLAM成本較低,但是易受到環(huán)境光照、天氣等因素影響導(dǎo)致建圖精度不高。激光雷達(dá)因其能夠感知距離、不受環(huán)境因素干擾的特性以及逐年降低的價(jià)格,使其成為當(dāng)前SLAM框架中主流的傳感選擇。
當(dāng)前基于激光雷達(dá)的SLAM技術(shù)多采用旋轉(zhuǎn)式激光雷達(dá),它通過旋轉(zhuǎn)機(jī)構(gòu)在不同的方向上進(jìn)行多次掃描,形成一個(gè)水平或垂直切片的激光點(diǎn)云?;谶@一特性,LOAM[5]通過在點(diǎn)云數(shù)據(jù)每一個(gè)水平環(huán)線上提取角點(diǎn)、面點(diǎn),并構(gòu)建點(diǎn)到線與點(diǎn)到面距離來估計(jì)相鄰幀間的位姿變化,從而完成地圖的構(gòu)建。LeGO-LOAM[6]在前者的基礎(chǔ)上增加了地面約束以獲取更高的建圖精度。近年來,固態(tài)雷達(dá)相對于旋轉(zhuǎn)式雷達(dá),以其可靠性高、體積小、功耗低等優(yōu)勢引起了從業(yè)人員的廣泛關(guān)注?;诠虘B(tài)雷達(dá)的相關(guān)研究尚處于起步階段,本文使用覽沃Hap固態(tài)激光雷達(dá)采集實(shí)驗(yàn)數(shù)據(jù),并利用主成分分析法(Principal Component Analysis, PCA) [7]和點(diǎn)云分布特性提取特征實(shí)現(xiàn)地圖構(gòu)建,隨后與慣導(dǎo)設(shè)備實(shí)測數(shù)據(jù)對比進(jìn)行誤差分析,最后對誤差和建圖效果進(jìn)行分析。
1 算法設(shè)計(jì)
1.1 算法整體框架
基于固態(tài)雷達(dá)的建圖系統(tǒng)框架如圖1所示,算法部分主要包括特征提取與特征篩選、前端里程計(jì)、IMU(慣性測量單元)預(yù)積分[8]、GTSAM優(yōu)化[9]與全局地圖五個(gè)部分。
1) 特征提取與特征篩選:Hap雷達(dá)采集的點(diǎn)云信息,經(jīng)由該算法提取點(diǎn)云特征,并對所提取的特征進(jìn)行篩選、過濾。
2) 前端里程計(jì):特征提取階段獲取的特征,經(jīng)由該算法,解算出Hap雷達(dá)點(diǎn)云相鄰兩幀相對位姿[Rp=x y z θr θp θy]。
3) IMU預(yù)積分:慣導(dǎo)IMU單元測得的加速度、角速度信息,經(jīng)由該算法估算機(jī)器人相對移動,輔助前端里程計(jì)以獲取更高精度位姿。
4) GTSAM優(yōu)化:前端里程計(jì)及IMU預(yù)積分計(jì)算得到的位姿,經(jīng)由該算法進(jìn)行圖優(yōu)化,解算全局最優(yōu)位姿。
5) 全局地圖:點(diǎn)云數(shù)據(jù)按求得的最優(yōu)位姿進(jìn)行堆疊形成全局地圖(如圖1) 。
1.2 硬件設(shè)備
實(shí)現(xiàn)數(shù)據(jù)采集及算法部署的設(shè)備配置如下:使用覽沃Hap固態(tài)雷達(dá)生成點(diǎn)云數(shù)據(jù),如圖2所示;使用英特爾志強(qiáng)E52680 v2處理器、8G內(nèi)存、操作系統(tǒng)為ubuntu 18.04的電腦,完成點(diǎn)云、IMU、GNSS(真值)數(shù)據(jù)的采集及建圖算法的運(yùn)行。
1.3 基于PCA與點(diǎn)云分布的特征提取
由于Hap固態(tài)雷達(dá)旋鏡式成像特點(diǎn),相比旋轉(zhuǎn)式機(jī)械雷達(dá)缺少傳統(tǒng)的幀與環(huán)線的概念。因此,本文選擇使用PCA完成點(diǎn)云特征的提取。首先,對于點(diǎn)云P中一個(gè)點(diǎn)[pi],以距離L將點(diǎn)云劃分為k個(gè)簇,并對每個(gè)簇計(jì)算其中心,公式如下(1) :
[pki=1ninpki] (1)
隨后,對于每一個(gè)簇計(jì)算協(xié)方差矩陣Cov及其特征值,公式如下(2) :
[Cov=1ni=1n(pki-pki)?(pki-pki)T,Cov?eij=λij?eij, j∈{0,1,2}.]? (2)
當(dāng)[λi0-λi1λi0]與[λi1-λi2λi0]滿足設(shè)定閾值時(shí),這些點(diǎn)被分別選為角、面特征點(diǎn)。此外,將所有提取特征送入前端里程計(jì)算會增加計(jì)算時(shí)長、降低算法效率。因此,出于點(diǎn)云近處稠密,不需要過多特征點(diǎn),遠(yuǎn)處稀疏,點(diǎn)云發(fā)散不適宜用作特征的考慮,本文對距離雷達(dá)60米以內(nèi)以及100米以外的特征點(diǎn)進(jìn)行隨機(jī)濾除,對該區(qū)域以內(nèi)的特征點(diǎn)進(jìn)行保留。通過上述方式,完成了特征的提取與篩選。
1.4 基于GTSAM的地圖構(gòu)建
特征提取完成之后,則根據(jù)角點(diǎn)特征和面點(diǎn)特征分別構(gòu)建線到線距離與面到面距離,來求解一個(gè)合適的相對位姿使得兩幀點(diǎn)云之間的上述距離達(dá)到最小。此外,慣導(dǎo)采集得到的加速度與角速度數(shù)據(jù),在一段時(shí)間內(nèi)做積分處理,可以分別得到該時(shí)間段內(nèi)機(jī)器人在位移、歐拉角上的變化程度。最后,本文采用因子圖模型,并使用增量平滑優(yōu)化庫GTSAM,將上述由里程計(jì)算法求得的幀間相對位姿和IMU預(yù)積分得到的位姿進(jìn)行融合求解最優(yōu)結(jié)果。該過程的基本思想是將多狀態(tài)的聯(lián)合估計(jì),轉(zhuǎn)化為一個(gè)求解聯(lián)合概率分布函數(shù)的最大后驗(yàn)概率分布值的問題,可以用數(shù)學(xué)公式表達(dá)為(3) :
[P(X,O,U)=iLP(oi|xk,xk-1)jMPuj|xk,xk-1] (3)
其中,X、O、U分別代表待優(yōu)化的位姿、里程計(jì)因子和預(yù)積分因子。在計(jì)算出每一時(shí)刻最優(yōu)位姿之后,將當(dāng)前固態(tài)雷達(dá)獲取的點(diǎn)云與上一時(shí)刻獲取點(diǎn)云進(jìn)行拼接,可累加式地得到點(diǎn)云地圖。
2 實(shí)驗(yàn)與評估
為了驗(yàn)證提出方法的建圖效果,本文在乘用車車頂架設(shè)固態(tài)雷達(dá),并在校園內(nèi)采集了一段路徑總長487m的數(shù)據(jù)。采集區(qū)域?yàn)閷W(xué)生宿舍區(qū),包含宿舍樓、小廣場、樹叢等特征較為復(fù)雜的場景。最終的建圖效果如圖3所示。
圖3中,圖a與圖b分別是建圖整體效果的俯視視角和側(cè)視視角。從整體上來看,生成的點(diǎn)云地圖未產(chǎn)生扭曲、形變的情況,未出現(xiàn)明顯的漂移現(xiàn)象。圖c是對廣場區(qū)域和宿舍區(qū)域的特寫,從該圖中可以看出不管是廣場中的燈柱還是宿舍立柱,其輪廓均橫平豎直,無任何重疊。圖d是地圖中樹林的特寫,它們同樣外形清晰易辨別,無重影、鬼影的情況。上述可視化結(jié)果均表明,本文提出的方法具備較高的建圖精度。
此外,除了使用可視化手段對結(jié)果進(jìn)行定性展示,我們還使用相對位姿誤差(Relative Pose Error,RPE) [10]對結(jié)果進(jìn)行了定量分析。其計(jì)算方式為(4) :
[rpei=T-1i?Ti-1-1E-1i?Ei-1] (4)
它的含義是相鄰位置真值T間相對位移和相鄰估計(jì)位置E間相對位移之間的誤差,反映的是位姿估計(jì)的準(zhǔn)確程度。根據(jù)這一準(zhǔn)則,在該段數(shù)據(jù)上本文算法的各項(xiàng)指標(biāo)如表1所示:
從表1中可以看出,幀間平均RPE為0.483m,即物體在這段路徑長487m的點(diǎn)云地圖中的位置與實(shí)際位置的平均誤差在半米以內(nèi)。最小誤差為0.076m,即建圖的最小誤差為7cm,這反映出本文提出的前端里程計(jì)的有效性。最大誤差0.844m,這體現(xiàn)出前端里程計(jì)自身存在的累積誤差問題。由于里程計(jì)的機(jī)理是計(jì)算相鄰幀間的相對位置變化,每一時(shí)刻估計(jì)位置與實(shí)際位置都存在出入,導(dǎo)致誤差隨軌跡長度累積。
3 結(jié)束語
本文基于覽沃Hap雷達(dá)設(shè)計(jì)了一種建圖方法,通過PCA方法和Hap雷達(dá)點(diǎn)云分布特點(diǎn)提取特征,并實(shí)車采集數(shù)據(jù)對提出方法進(jìn)行了驗(yàn)證。定性、定量的實(shí)驗(yàn)結(jié)果表明,該算法具備較高質(zhì)量的建圖性能,建圖效果清晰、無重影,在一定距離范圍內(nèi)能保證較高的建圖精度。實(shí)驗(yàn)也表明該算法同樣存在其他激光雷達(dá)里程計(jì)所存在的不足,即長距離建圖時(shí)地圖精度會受累積誤差的影響。在未來,可以通過設(shè)計(jì)新的適用于固態(tài)雷達(dá)的回環(huán)算法,通過構(gòu)建跨幀間的位置關(guān)系修正地圖,減小誤差。
參考文獻(xiàn):
[1] GRISETTI G,KüMMERLE R,STACHNISS C,et al.A tutorial on graph-based SLAM[J].IEEE Intelligent Transportation Systems Magazine,2010,2(4):31-43.
[2] 趙夢成,黎昱宏,張宏宇.ROS的服務(wù)類移動機(jī)器人SLAM導(dǎo)航的研究[J].電腦知識與技術(shù),2020,16(9):274-276.
[3] ABASPUR K,AZEROUNI I,F(xiàn)ITZGERALD L,DOOLY G,et al.A survey of state-of-the-art on visual SLAM[J].Expert Systems with Applications,2022,205:117734.
[4] 袁國帥,齊詠生,劉利強(qiáng),等.一種基于因子圖消元優(yōu)化的激光雷達(dá)視覺慣性融合SLAM方法[J].電子學(xué)報(bào),2023(8):1-11.
[5] ZHANG J,SINGH S.LOAM:lidar odometry and mapping in real-time[C]//Robotics:Science and Systems X.Robotics:Science and Systems Foundation,2014.
[6] SHAN T X,ENGLOT B.LeGO-LOAM:lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).ACM,2018:4758-4765.
[7] 劉亞洲,鄧安健.基于激光雷達(dá)的智慧交通信息采集算法[J].測繪與空間地理信息,2023,46(8):93-97,102.
[8] 賈曉雪,趙冬青,肖國銳,等.基于精化預(yù)積分的GNSS/IMU/視覺多源融合定位方法[J].北京航空航天大學(xué)學(xué)報(bào),2023(9):1-10.
[9] ZHENG T T,WANG F,XU Z.An improved gtsam-based nonlinear optimization algorithm in ORBSLAM3[C]//2022 International Conference on Intelligent Transportation,Big Data & Smart City (ICITBS).IEEE,2022.
[10] WU J Y,HUANG S H,YANG Y X,et al.Evaluation of 3D LiDAR SLAM algorithms based on the KITTI dataset[J].The Journal of Supercomputing,2023,79(14):15760-15772.
【通聯(lián)編輯:光文玲】