胡麗娜 曹政

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

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

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

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

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

0 引言

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

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

1 算法設計

1.1 算法整體框架

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

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

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

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

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

5） 全局地圖：點云數(shù)據(jù)按求得的最優(yōu)位姿進行堆疊形成全局地圖（如圖1） 。

1.2 硬件設備

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

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

由于Hap固態(tài)雷達旋鏡式成像特點，相比旋轉(zhuǎn)式機械雷達缺少傳統(tǒng)的幀與環(huán)線的概念。因此，本文選擇使用PCA完成點云特征的提取。首先，對于點云P中一個點[pi]，以距離L將點云劃分為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）

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

1.4 基于GTSAM的地圖構建

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

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

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

2 實驗與評估

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

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

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

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

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

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

3 結束語

本文基于覽沃Hap雷達設計了一種建圖方法，通過PCA方法和Hap雷達點云分布特點提取特征，并實車采集數(shù)據(jù)對提出方法進行了驗證。定性、定量的實驗結果表明，該算法具備較高質(zhì)量的建圖性能，建圖效果清晰、無重影，在一定距離范圍內(nèi)能保證較高的建圖精度。實驗也表明該算法同樣存在其他激光雷達里程計所存在的不足，即長距離建圖時地圖精度會受累積誤差的影響。在未來，可以通過設計新的適用于固態(tài)雷達的回環(huá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的服務類移動機器人SLAM導航的研究[J].電腦知識與技術，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] 袁國帥，齊詠生，劉利強，等.一種基于因子圖消元優(yōu)化的激光雷達視覺慣性融合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.

【通聯(lián)編輯：光文玲】