基于固態雷達的SLAM研究與實現

胡麗娜 曹政

摘要：未知環境下的地圖構建技術是智能機器人實現路徑規劃、智能導航的前提與關鍵。考慮到目前傳感器領域的發展態勢，文章主要針對固態雷達的SLAM算法進行探究，使用覽沃Hap雷達搭建平臺采集數據并驗證算法。首先，文章對研究背景以及近年來相關傳感器、算法進行了介紹。其次，這篇文章對算法的整體流程進行了概述，并對基于主成分分析（Principal Component Analysis，PCA） 和點云分布的特征提取、基于因子圖的位姿估計技術細節進行了介紹。最后，使用實車采集了實驗數據，并以效果圖和相對位姿誤差（Relative Pose Error，RPE） 定性、定量地對結果進行了分析。結果表明，此文提出的方法具備一定的實用價值。

關鍵詞：機器人；自動駕駛；同步定位與建圖；固態雷達；前端里程計

中圖分類號：TP391? ? ? 文獻標識碼：A

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

開放科學（資源服務）標識碼（OSID）

0 引言

同時定位與地圖構建，即Simultaneous Localization and Mapping （簡稱SLAM） ，是一種在機器人、計算機視覺領域廣泛應用的關鍵技術[1]。該算法的目標是讓機器人在位置環境中估計自身位置并構建環境地圖，無須事先知曉所處環境的結構或地圖信息。它是無人駕駛車輛、無人飛行器、工業自動化等領域智能系統實現自主感知、決策與行動的核心問題[2]。在實現該技術的基礎之上，才能使機器、智能系統能夠理解并與復雜的現實世界互動，才能為各個領域帶來更高效、更安全、更智能化的解決方案。使用單個傳感器實現SLAM大體上可分為兩個方向：基于相機（視覺）的SLAM技術[3]與基于激光雷達SLAM技術[4]。一般來說，視覺SLAM成本較低，但是易受到環境光照、天氣等因素影響導致建圖精度不高。激光雷達因其能夠感知距離、不受環境因素干擾的特性以及逐年降低的價格，使其成為當前SLAM框架中主流的傳感選擇。

當前基于激光雷達的SLAM技術多采用旋轉式激光雷達，它通過旋轉機構在不同的方向上進行多次掃描，形成一個水平或垂直切片的激光點云。基于這一特性，LOAM[5]通過在點云數據每一個水平環線上提取角點、面點，并構建點到線與點到面距離來估計相鄰幀間的位姿變化，從而完成地圖的構建。LeGO-LOAM[6]在前者的基礎上增加了地面約束以獲取更高的建圖精度。近年來，固態雷達相對于旋轉式雷達，以其可靠性高、體積小、功耗低等優勢引起了從業人員的廣泛關注。基于固態雷達的相關研究尚處于起步階段，本文使用覽沃Hap固態激光雷達采集實驗數據，并利用主成分分析法（Principal Component Analysis， PCA） [7]和點云分布特性提取特征實現地圖構建，隨后與慣導設備實測數據對比進行誤差分析，最后對誤差和建圖效果進行分析。

1 算法設計

1.1 算法整體框架

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

1） 特征提取與特征篩選：Hap雷達采集的點云信息，經由該算法提取點云特征，并對所提取的特征進行篩選、過濾。

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

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

4） GTSAM優化：前端里程計及IMU預積分計算得到的位姿，經由該算法進行圖優化，解算全局最優位姿。

5） 全局地圖：點云數據按求得的最優位姿進行堆疊形成全局地圖（如圖1） 。

1.2 硬件設備

實現數據采集及算法部署的設備配置如下：使用覽沃Hap固態雷達生成點云數據，如圖2所示；使用英特爾志強E52680 v2處理器、8G內存、操作系統為ubuntu 18.04的電腦，完成點云、IMU、GNSS（真值）數據的采集及建圖算法的運行。

1.3 基于PCA與點云分布的特征提取

由于Hap固態雷達旋鏡式成像特點，相比旋轉式機械雷達缺少傳統的幀與環線的概念。因此，本文選擇使用PCA完成點云特征的提取。首先，對于點云P中一個點[pi]，以距離L將點云劃分為k個簇，并對每個簇計算其中心，公式如下（1） ：

[pki=1ninpki] （1）

隨后，對于每一個簇計算協方差矩陣Cov及其特征值，公式如下（2） ：

[Cov=1ni=1n（pki-pki）?（pki-pki）T，Cov?eij=λij?eij， j∈{0，1，2}.]? （2）

當[λi0-λi1λi0]與[λi1-λi2λi0]滿足設定閾值時，這些點被分別選為角、面特征點。此外，將所有提取特征送入前端里程計算會增加計算時長、降低算法效率。因此，出于點云近處稠密，不需要過多特征點，遠處稀疏，點云發散不適宜用作特征的考慮，本文對距離雷達60米以內以及100米以外的特征點進行隨機濾除，對該區域以內的特征點進行保留。通過上述方式，完成了特征的提取與篩選。

1.4 基于GTSAM的地圖構建

特征提取完成之后，則根據角點特征和面點特征分別構建線到線距離與面到面距離，來求解一個合適的相對位姿使得兩幀點云之間的上述距離達到最小。此外，慣導采集得到的加速度與角速度數據，在一段時間內做積分處理，可以分別得到該時間段內機器人在位移、歐拉角上的變化程度。最后，本文采用因子圖模型，并使用增量平滑優化庫GTSAM，將上述由里程計算法求得的幀間相對位姿和IMU預積分得到的位姿進行融合求解最優結果。該過程的基本思想是將多狀態的聯合估計，轉化為一個求解聯合概率分布函數的最大后驗概率分布值的問題，可以用數學公式表達為（3） ：

[P（X，O，U）=iLP（oi|xk，xk-1）jMPuj|xk，xk-1] （3）

其中，X、O、U分別代表待優化的位姿、里程計因子和預積分因子。在計算出每一時刻最優位姿之后，將當前固態雷達獲取的點云與上一時刻獲取點云進行拼接，可累加式地得到點云地圖。

2 實驗與評估

為了驗證提出方法的建圖效果，本文在乘用車車頂架設固態雷達，并在校園內采集了一段路徑總長487m的數據。采集區域為學生宿舍區，包含宿舍樓、小廣場、樹叢等特征較為復雜的場景。最終的建圖效果如圖3所示。

圖3中，圖a與圖b分別是建圖整體效果的俯視視角和側視視角。從整體上來看，生成的點云地圖未產生扭曲、形變的情況，未出現明顯的漂移現象。圖c是對廣場區域和宿舍區域的特寫，從該圖中可以看出不管是廣場中的燈柱還是宿舍立柱，其輪廓均橫平豎直，無任何重疊。圖d是地圖中樹林的特寫，它們同樣外形清晰易辨別，無重影、鬼影的情況。上述可視化結果均表明，本文提出的方法具備較高的建圖精度。

此外，除了使用可視化手段對結果進行定性展示，我們還使用相對位姿誤差（Relative Pose Error，RPE） [10]對結果進行了定量分析。其計算方式為（4） ：

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

它的含義是相鄰位置真值T間相對位移和相鄰估計位置E間相對位移之間的誤差，反映的是位姿估計的準確程度。根據這一準則，在該段數據上本文算法的各項指標如表1所示：

從表1中可以看出，幀間平均RPE為0.483m，即物體在這段路徑長487m的點云地圖中的位置與實際位置的平均誤差在半米以內。最小誤差為0.076m，即建圖的最小誤差為7cm，這反映出本文提出的前端里程計的有效性。最大誤差0.844m，這體現出前端里程計自身存在的累積誤差問題。由于里程計的機理是計算相鄰幀間的相對位置變化，每一時刻估計位置與實際位置都存在出入，導致誤差隨軌跡長度累積。

3 結束語

本文基于覽沃Hap雷達設計了一種建圖方法，通過PCA方法和Hap雷達點云分布特點提取特征，并實車采集數據對提出方法進行了驗證。定性、定量的實驗結果表明，該算法具備較高質量的建圖性能，建圖效果清晰、無重影，在一定距離范圍內能保證較高的建圖精度。實驗也表明該算法同樣存在其他激光雷達里程計所存在的不足，即長距離建圖時地圖精度會受累積誤差的影響。在未來，可以通過設計新的適用于固態雷達的回環算法，通過構建跨幀間的位置關系修正地圖，減小誤差。

參考文獻：

[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的服務類移動機器人SLAM導航的研究[J].電腦知識與技術，2020，16（9）：274-276.

[3] ABASPUR K，AZEROUNI I，FITZGERALD L，DOOLY G，et al.A survey of state-of-the-art on visual SLAM[J].Expert Systems with Applications，2022，205：117734.

[4] 袁國帥，齊詠生，劉利強，等.一種基于因子圖消元優化的激光雷達視覺慣性融合SLAM方法[J].電子學報，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] 劉亞洲，鄧安健.基于激光雷達的智慧交通信息采集算法[J].測繪與空間地理信息，2023，46（8）：93-97，102.

[8] 賈曉雪，趙冬青，肖國銳，等.基于精化預積分的GNSS/IMU/視覺多源融合定位方法[J].北京航空航天大學學報，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.

【通聯編輯：光文玲】