基于固態(tài)雷達(dá)的SLAM研究與實(shí)現(xiàn)

胡麗娜 曹政

摘要：未知環(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)云分布的特征提取、基于因子圖的位姿估計技術(shù)細(xì)節(jié)進(jìn)行了介紹。最后，使用實(shí)車采集了實(shí)驗(yàn)數(shù)據(jù)，并以效果圖和相對位姿誤差（Relative Pose Error，RPE） 定性、定量地對結(jié)果進(jìn)行了分析。結(jié)果表明，此文提出的方法具備一定的實(shí)用價值。

關(guān)鍵詞：機(jī)器人；自動駕駛；同步定位與建圖；固態(tài)雷達(dá)；前端里程計

中圖分類號：TP391? ? ? 文獻(xiàn)標(biāo)識碼：A

文章編號：1009-3044（2023）31-0125-03

開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）

0 引言

同時定位與地圖構(gòu)建，即Simultaneous Localization and Mapping （簡稱SLAM） ，是一種在機(jī)器人、計算機(jī)視覺領(lǐng)域廣泛應(yīng)用的關(guān)鍵技術(shù)[1]。該算法的目標(biāo)是讓機(jī)器人在位置環(huán)境中估計自身位置并構(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í)世界互動，才能為各個領(lǐng)域帶來更高效、更安全、更智能化的解決方案。使用單個傳感器實(shí)現(xiàn)SLAM大體上可分為兩個方向：基于相機(jī)（視覺）的SLAM技術(shù)[3]與基于激光雷達(dá)SLAM技術(shù)[4]。一般來說，視覺SLAM成本較低，但是易受到環(huán)境光照、天氣等因素影響導(dǎo)致建圖精度不高。激光雷達(dá)因其能夠感知距離、不受環(huán)境因素干擾的特性以及逐年降低的價格，使其成為當(dāng)前SLAM框架中主流的傳感選擇。

當(dāng)前基于激光雷達(dá)的SLAM技術(shù)多采用旋轉(zhuǎn)式激光雷達(dá)，它通過旋轉(zhuǎn)機(jī)構(gòu)在不同的方向上進(jìn)行多次掃描，形成一個水平或垂直切片的激光點(diǎn)云。基于這一特性，LOAM[5]通過在點(diǎn)云數(shù)據(jù)每一個水平環(huán)線上提取角點(diǎn)、面點(diǎn)，并構(gòu)建點(diǎn)到線與點(diǎn)到面距離來估計相鄰幀間的位姿變化，從而完成地圖的構(gòu)建。LeGO-LOAM[6]在前者的基礎(chǔ)上增加了地面約束以獲取更高的建圖精度。近年來，固態(tài)雷達(dá)相對于旋轉(zhuǎn)式雷達(dá)，以其可靠性高、體積小、功耗低等優(yōu)勢引起了從業(yè)人員的廣泛關(guān)注。基于固態(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è)計

1.1 算法整體框架

基于固態(tài)雷達(dá)的建圖系統(tǒng)框架如圖1所示，算法部分主要包括特征提取與特征篩選、前端里程計、IMU（慣性測量單元）預(yù)積分[8]、GTSAM優(yōu)化[9]與全局地圖五個部分。

1） 特征提取與特征篩選：Hap雷達(dá)采集的點(diǎn)云信息，經(jīng)由該算法提取點(diǎn)云特征，并對所提取的特征進(jìn)行篩選、過濾。

2） 前端里程計：特征提取階段獲取的特征，經(jīng)由該算法，解算出Hap雷達(dá)點(diǎn)云相鄰兩幀相對位姿[Rp=x y z θr θp θy]。

3） IMU預(yù)積分：慣導(dǎo)IMU單元測得的加速度、角速度信息，經(jīng)由該算法估算機(jī)器人相對移動，輔助前端里程計以獲取更高精度位姿。

4） GTSAM優(yōu)化：前端里程計及IMU預(yù)積分計算得到的位姿，經(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中一個點(diǎn)[pi]，以距離L將點(diǎn)云劃分為k個簇，并對每個簇計算其中心，公式如下（1） ：

[pki=1ninpki] （1）

隨后，對于每一個簇計算協(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è)定閾值時，這些點(diǎn)被分別選為角、面特征點(diǎn)。此外，將所有提取特征送入前端里程計算會增加計算時長、降低算法效率。因此，出于點(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)建線到線距離與面到面距離，來求解一個合適的相對位姿使得兩幀點(diǎn)云之間的上述距離達(dá)到最小。此外，慣導(dǎo)采集得到的加速度與角速度數(shù)據(jù)，在一段時間內(nèi)做積分處理，可以分別得到該時間段內(nèi)機(jī)器人在位移、歐拉角上的變化程度。最后，本文采用因子圖模型，并使用增量平滑優(yōu)化庫GTSAM，將上述由里程計算法求得的幀間相對位姿和IMU預(yù)積分得到的位姿進(jìn)行融合求解最優(yōu)結(jié)果。該過程的基本思想是將多狀態(tài)的聯(lián)合估計，轉(zhuǎn)化為一個求解聯(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)化的位姿、里程計因子和預(yù)積分因子。在計算出每一時刻最優(yōu)位姿之后，將當(dāng)前固態(tài)雷達(dá)獲取的點(diǎn)云與上一時刻獲取點(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)行了定量分析。其計算方式為（4） ：

[rpei=T-1i?Ti-1-1E-1i?Ei-1] （4）

它的含義是相鄰位置真值T間相對位移和相鄰估計位置E間相對位移之間的誤差，反映的是位姿估計的準(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，這反映出本文提出的前端里程計的有效性。最大誤差0.844m，這體現(xiàn)出前端里程計自身存在的累積誤差問題。由于里程計的機(jī)理是計算相鄰幀間的相對位置變化，每一時刻估計位置與實(shí)際位置都存在出入，導(dǎo)致誤差隨軌跡長度累積。

3 結(jié)束語

本文基于覽沃Hap雷達(dá)設(shè)計了一種建圖方法，通過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á)里程計所存在的不足，即長距離建圖時地圖精度會受累積誤差的影響。在未來，可以通過設(shè)計新的適用于固態(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é)報，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é)報，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)編輯：光文玲】