宋學佳，敖銀輝，王文杰

(廣東工業(yè)大學機電工程學院，廣東 廣州 510006)

在未知且復雜的環(huán)境中，需要實時精準地估計無人駕駛車輛的位姿與速度，并實時感知環(huán)境物體，構建環(huán)境地圖，以作為軌跡規(guī)劃、車輛反饋控制和障礙物躲避等功能的信息輸入，因此，優(yōu)秀的同時定位與地圖構建(SLAM)方法對于無人駕駛車輛(UGV)至關重要。 根據(jù)傳感器的不同，一般分為基于視覺SLAM 與基于雷達SLAM 方法。 基于視覺的方法通常使用相機傳感器，包括單目、雙目和深度相機，通過對連續(xù)圖像信息的處理以確定相機的運動狀態(tài)。由于相機體積小、成本較低和易于安裝等優(yōu)點，相機被廣泛應用于SLAM，并提出了許多方法，例如基于單目相機設計的魯棒性強且實時性高的ORBSLAM[1]，基于深度相機以解決深度估計問題的RGB-D SLAM[2]，以及基于視覺和慣性測量單元(IMU)信息融合的視覺慣性SLAM 方法[3-5]，進一步提高了視覺SLAM 方法的定位精度與魯棒性。 盡管視覺SLAM 方法有許多優(yōu)點，但是其對光照強度、視角范圍以及系統(tǒng)初始化的敏感性，影響了視覺SLAM 方法的可靠性。

相反，由于激光雷達主動感知的特性，其受光照強度的影響很小，具備全時段工作的優(yōu)點，并且隨著激光雷達傳感器技術的發(fā)展，其更加適用于長距離且環(huán)境復雜的場景，可以感知環(huán)境物體的詳細特征，因此，本文將基于激光雷達的SLAM 方法作為重點研究方向。 經(jīng)過多年的發(fā)展，許多優(yōu)秀的方法被提出。 論文[6-7]提出了一種融合了激光雷達與IMU的測量信息的方法LOAM，獲得了低漂移且實時性高的狀態(tài)估計與建圖效果，并作為經(jīng)典的雷達SLAM 方法被廣泛使用，長期在自動駕駛KITTI 數(shù)據(jù)集解析排名中保持前列。 論文[8]基于LOAM 做改進，獲得了更好的效果。 但上述方法仍存在一些問題，例如沒有融合類似GNSS 等具有絕對位置測量的傳感器數(shù)據(jù)、在點云特征稀疏或特別稠密的場景時誤差大與效率低、回環(huán)檢測失效以及無法構建賽道樁桶地圖等。

針對上述問題，我們提出了一種基于iEKF 理論的多傳感器緊耦合SLAM 方法，首先利用IMU 預測車輛的先驗狀態(tài)與消除點云畸變，并使用滑動窗口方法保存點云特征以提高點云匹配精度；然后，在GNSS 測量信息可用時，通過對GNSS 信息作初始化與坐標系變換，分別完成濾波更新得到后驗狀態(tài)。最后在后端環(huán)節(jié)選擇性地引入關鍵幀進行狀態(tài)優(yōu)化與反饋更新，構建全局環(huán)境點云地圖。 此外，我們還開發(fā)了多目標檢測與跟蹤算法，利用Camera-LiDAR聯(lián)合檢測，實現(xiàn)了多個樁桶的目標檢測、跟蹤與全局樁桶地圖的構建，豐富了本文方法的適用場景。 因此，我們工作的主要貢獻可以概括如下:

①基于迭代擴展卡爾曼濾波設計了多傳感器緊耦合SLAM 方法，融合了GNSS 測量信息，具有更高的定位精度與冗余度；

②使用點云滑動窗口方法保存點云特征以提高點云匹配精度，使得在開闊場景中仍具有很好的效果；

③提出一種有效的多目標檢測與跟蹤算法，實現(xiàn)了多個賽道樁桶的目標檢測、跟蹤與全局樁桶地圖的構建。

1 相關研究

由于激光雷達機械結構的旋轉(zhuǎn)掃描機制與傳感器自身的運動，會導致激光雷達的測量出現(xiàn)運動畸變，所以只使用單一激光雷達傳感器估計車輛狀態(tài)信息，畸變會導致估計結果出現(xiàn)較大的漂移，特別是在非勻速行駛工況。 因此，激光雷達通常需要融合其他傳感器信息，例如IMU、運動傳感器以及GNSS等，以獲得理想的狀態(tài)估計結果。 通情況下，多傳感器信息融合的方法可以分為松耦合方法和緊耦合方法。

在松耦合方法中，LOAM[6]通過計算點云平滑度來提取邊緣和平面特征，提高了匹配效率，并利用IMU 預測幀間運動，以去除點云運動畸變，但勻速運動模型假設與回環(huán)檢測的缺點，影響了其定位與建圖效果。 LEGO-LOAM[8]在LOAM 的基礎上進行改進，通過點云分割、地面提取與協(xié)助優(yōu)化，使得其在樹木與草地等噪聲覆蓋的場景中具有較好的效果。 論文[9]利用EKF 融合雷達與IMU 測量，但只適用于二維場景。 Zhen 等[10]先通過高斯粒子濾波器匹配雷達與先驗地圖得到狀態(tài)估計，然后再用IMU 協(xié)助優(yōu)化。

雖然松耦合方法在計算上是有效的，但是容易丟失傳感器信息[11]，相反緊耦合方法具有更高的估計精度與更強的魯棒性，成為了重要的研究方向[12]，許多相關方法被提出。 Hemann 等[13]基于誤差狀態(tài)卡爾曼濾波(ESKF)理論緊耦合IMU 與雷達高度圖信息。 同樣，基于ESKF 設計的緊耦合方法LINS[14]被用于估計機器人狀態(tài)，但LINS 沒有正確推導狀態(tài)協(xié)方差的傳遞方程，只適用于特定環(huán)境。對于聯(lián)合優(yōu)化的方法，MC2SLAM[15]為基于雷達連續(xù)掃描與局部地圖非剛性配準的運動補償方法，通過構建因子圖優(yōu)化結構緊耦合IMU 測量。 Park等[16]提出了最小化雷達與IMU 約束來優(yōu)化局部軌跡的方法。 Geneva 等[17]提出了圖優(yōu)化結構的LIPS，以融合IMU 的預積分結果與從雷達點云中提取的平面約束。 Ye 等[18]提出一種緊耦合的方法LIO-Mapping，其通過一種新的旋轉(zhuǎn)約束建圖的方法來優(yōu)化車輛位姿與地圖，但其基于圖優(yōu)化的方式來處理所有的傳感器測量，無法滿足實時性要求。 此外，在GNSS 可用時，融合GNSS 的方法可以獲得更好定位效果，郭安等[19]基于KF 理論融合GNSS、IMU、氣壓計與空速計信息，實現(xiàn)對無人機狀態(tài)估計的仿真，但由于其三級串聯(lián)的思路會丟失高頻率傳感器數(shù)據(jù)，導致冗余性低。 彭文正等[20]基于iEKF理論融合了IMU、GNSS 與雷達等傳感器，實現(xiàn)了二維的車輛狀態(tài)估計，但對先驗地圖的依賴較高且沒有建圖功能。 Zhu 等[21]通過GNSS 約束實現(xiàn)閉環(huán)，以融合GNSS 與雷達的測量。

2 系統(tǒng)框架

2.1 系統(tǒng)定義

首先定義具有12 自由度的車輛狀態(tài)量為X＝[Pvqg]T，分別表示平移P、速度v、三維旋轉(zhuǎn)對應的四元數(shù)q以及重力加速度g。 在實驗平臺的傳感器組合中，IMU 的測量頻率最高，因此，以IMU的時間戳為基準，將雷達與GNSS 的時間戳與IMU時間戳對齊，使車輛狀態(tài)量可以被正確地預測與校正更新。 Ligom 的系統(tǒng)框架如圖1 所示。

圖1 Ligom 的系統(tǒng)框架圖

為了方便理解，本文使用的符號與定義如表1所示。

表1 本文使用的符號與定義

2.2 濾波方法

作為EKF 的改進算法，iEKF 同樣分為預測和更新兩個環(huán)節(jié)，其中預測環(huán)節(jié)是結合傳感器測量與預測方程，由上一時刻的后驗狀態(tài)遞推得到當前時刻的先驗狀態(tài)并完成正確的協(xié)方差傳遞。更新環(huán)節(jié)是通過計算狀態(tài)的期望觀測與傳感器的實際測量的殘差，從而更新先驗狀態(tài)，得到后驗狀態(tài)。 預測方程如式(1)所示:

更新方程如式(2)所示:

式中:op為更新環(huán)節(jié)的迭代次數(shù)，α為迭代步長。 本文的收斂條件為狀態(tài)的更新量足夠小或者達到了最大的迭代次數(shù)，假如滿足收斂條件，結束迭代完成更新環(huán)節(jié)，否則使，繼續(xù)進行迭代更新。

2.3 狀態(tài)預測環(huán)節(jié)

本文設定IMU 的加速度計測量值為am、陀螺儀角速度測量值為ωm，以及IMU 的隨機游走為ab與ωb。 在IMU 的連續(xù)測量時間t-1 至t時刻，有時間增量Δt，根據(jù)IMU 的積分模型，將車輛t-1 時刻的后驗狀態(tài)遞推預測到t時刻先驗狀態(tài)，狀態(tài)量的預測方程如式(3)所示:

式中:符號?為四元數(shù)q之間的乘法運算。

式中:

對于時刻tIMU 的測量數(shù)據(jù)輸入ut＝[am ωm]T，會引入其自身的協(xié)方差矩陣Qt，如式(8)所示:

式中:Λ和Θ分別是加速度a與角速度ω的高斯白噪聲，可以通過IMU 的離線標定得到。 因此，根據(jù)式(1)完成車輛先驗狀態(tài)的預測。

2.4 雷達點云校正環(huán)節(jié)

2.4.1 地面點分割

車輛在行駛過程中，雷達實時感知周圍環(huán)境，得到地面點云與非地面點云，其中，地面點云屬于特征明顯的平面特征點云集合，可以通過簡單計算將地面點從原始點云中分割出來。 分割地面點主要有兩個目的，其一提高了點云特征提取的計算效率，因為去地面后，剩余的非地面點云數(shù)量比原始點云減少很多；其二提高了目標檢測環(huán)節(jié)的準確率和效率，避免了地面點云的干擾。

本文采用角度法和深度法相結合，如圖2 所示，首先通過標定確定雷達的安裝高度，當雷達線束對環(huán)境物體掃描時，雷達高度、掃描線束以及地面形成三角形，從而確定地面深度[22]。 假設雷達掃描得到的地面點為P1與P2，角度β1為P1P2與水平方向形成的角度，而當點P3為非地面點時，P2P3與水平方向形成的角度β2明顯大于β1，因此通過設置角度閾值并結合深度法，能夠準確高效地分割地面點云。

圖2 點云分割方法示意圖

2.4.2 特征提取與坐標轉(zhuǎn)換

參考論文LeGo-LOAM 的理論，計算局部曲面的平滑度，根據(jù)平滑度的大小，將非地面點云劃分為邊緣特征~eL和平面特征~sL。 本文車輛平臺的運行工況是非勻速運動，所以雷達點云的測量信息存在運動畸變，使得測量點不同于真實位置，需要去除當前時刻t的雷達測量畸變。 傳統(tǒng)的LIO 算法，其雷達測量的幀間運動信息是通過IMU 預積分得到，相比之下，本文融合了GNSS 測量，得到更準確的車輛狀態(tài)，根據(jù)式(9)求得更加準確的雷達t-1 至t時刻的幀間變換矩陣，從而通過去除點云的運動畸變。

2.4.3 雷達點云信息融合

如2.4.2 所述，本文在地圖坐標系M中進行點云配準，所以，當獲取到t時刻測量點云時，首先去除當前幀點云運動畸變，然后根據(jù)t時刻車輛狀態(tài)與IMU-Lidar 的外參，將點云投影到地圖坐標系M，得到，如式(10)所示:

根據(jù)特征點類型的不同，計算觀測殘差，其殘差的定義為:平面點計算點到平面的距離、邊緣點計算點到直線的距離，如式(11)所示:

為了便于公式的書寫，此處設q＝，根據(jù)矩陣運算鏈式法則，求解點云校正環(huán)節(jié)的雅克比矩陣Ht如式(12)所示:

式中:

因此，矩陣Ht的最終表達式為:

在校正環(huán)節(jié)中，通過定義權重值ξ＝1-1.8 ｜rest｜，從而隨著t時刻觀測殘差rest數(shù)值的不同，動態(tài)調(diào)整校正環(huán)節(jié)中Ht起到的作用。 最后根據(jù)式(2)完成狀態(tài)更新。

2.5 GNSS 校正環(huán)節(jié)

由車輛的先驗狀態(tài)求得R()，結合已知傳感器外參變換角度對應的旋轉(zhuǎn)矩陣，計算初始變換關系。 根據(jù)GNSS 坐標系G的z軸與重力坐標系g方向平行的關系，修正三個方向的數(shù)值，得到實時的變換關系，并通過式(17)，初始化相關參數(shù):

在t時刻，系統(tǒng)的GNSS 的測量值計算實時的。 本文通過式(18)將車輛的先驗狀態(tài)信息投影到地心系W，并計算觀測殘差rest:

計算GNSS 校正環(huán)節(jié)的雅克比矩陣Ht如式(19)所示:

式中:

由于hφq表達式比較復雜，通過數(shù)值計算得到。最后根據(jù)式(2)完成狀態(tài)更新。

2.6 多目標檢測與跟蹤

在賽道工況中，多個樁桶組成的封閉賽道地圖信息是未知的，需要車輛在行駛過程中實時感知樁桶信息，并對多幀信息的同一樁桶進行目標跟蹤，為建圖算法提供樁桶的位置、類別以及ID 信息。 因此設計了Camera-Lidar 聯(lián)合檢測的多目標檢測與跟蹤算法，框架圖如圖3 所示，具體步驟為:

圖3 多目標檢測與跟蹤算法框架圖

①使用YoloV4-tiny 對圖像進行目標檢測，得到檢測框的類別與位置信息；

②以點云的時間戳為基準，將圖像檢測框與非地面點云進行時間戳對齊；

③通過雷達與相機離線標定[23]的外參數(shù)據(jù)，將非地面點云重投影到圖像中，使用點云聚類算法對檢測框內(nèi)的點云聚類與坐標解算，得到樁桶的三維坐標與類別信息；

④結合車輛狀態(tài)與樁桶信息，采用EKF 預測和更新樁桶位置，并使用匈牙利算法完成匹配，賦予每個樁桶獨立的ID 信息。

2.7 后端優(yōu)化與建圖

本文后端算法主要有三個作用:①優(yōu)化前端里程計信息，得到車輛最優(yōu)后驗狀態(tài)；②構建高精度的環(huán)境點云地圖，本文基于LEGO-LOAM 的建圖算法進行改進，優(yōu)化了建圖的精度與效率。 ③車輛在探索行駛中，構建封閉的賽道樁桶地圖。

對于樁桶地圖建圖算法，本文使用GTSAM 構建兩種因子類型，分別為位姿變換因子與樁桶觀測因子。 對于位姿變換因子，提取關鍵幀i和j時刻的車輛位姿，分別為，根據(jù)式(21a)求解幀間車輛位姿的期望觀測；對于樁桶觀測因子，由2.6 節(jié)已知樁桶的位置、類別以及ID信息，假設樁桶的ID 為k，根據(jù)實時的車輛里程計信息，將其投影到地圖系M得到，并結合j時刻的車輛位姿與式(21b)求解樁桶的期望觀測。

最后，根據(jù)式(24)構建成本函數(shù)，通過LM 算法求解此非線性優(yōu)化問題，優(yōu)化所有位姿關鍵幀與樁桶信息。 本算法的優(yōu)點是多目標跟蹤算法，可以在一段時間內(nèi)，對同一樁桶進行連續(xù)跟蹤并賦予獨立的ID，并根據(jù)樁桶的同一ID 去索引不同時刻的位置與類別信息，以準確地構建樁桶觀測因子，得到準確的賽道樁桶地圖。

3 實驗

3.1 實驗平臺

如圖4 所示為一輛大學生無人駕駛方程式車輛平臺，其配備了多種傳感器，包括元生IMU YIS-510V 的IMU、16 線激光雷達RS-LIDAR-16、華測P3-DU GNSS 接收機與單目相機MV-CE013-50GC，可為車輛提供實時的自身狀態(tài)與環(huán)境的測量信息。 車輛配備了研華MIC-7700 工控機、NVIDIA jetson TX2 開發(fā)板以及華海RapidECU-E3 等計算單元硬件，并基于機器人操作系統(tǒng)(ROS)與C++搭建系統(tǒng)，軟硬件協(xié)同實現(xiàn)傳感器數(shù)據(jù)處理、車輛狀態(tài)估計與地圖構建的功能。

圖4 實驗平臺與傳感器位置示意圖

3.2 實驗工況與結果分析

為了測試驗證Ligom 的效果，選擇開源算法LEGO-LOAM 與LIO-Mapping 進行對比，并在上述的實驗平臺中配置對應的算法運行環(huán)境，以執(zhí)行在不同工況中定位與建圖效果對比。

本文主要在四個不同平臺與環(huán)境采集的數(shù)據(jù)集上進行驗證，分別為Steven Campus、公園、KITTI 街道以及GDUT-Tracking 數(shù)據(jù)集。 其中前三種是開源數(shù)據(jù)集，而GDUT-Tracking 數(shù)據(jù)集是上述的自研實驗平臺，在開闊的環(huán)境中，記錄車輛在封閉賽道行駛過程的測量數(shù)據(jù)，用于驗證算法的定位與樁桶地圖建圖的精度。

3.2.1 Steven Campus 數(shù)據(jù)集

此數(shù)據(jù)集主要為Stevens 理工學院校園的樹木、建筑、道路以及人行道的測量，存在大量的噪聲數(shù)據(jù)，并且整個數(shù)據(jù)軌跡較長，容易造成誤差累積，對算法的測試壓力較大。 由于Steven Campus 是LEGO-LOAM 算法中的示例測試集，LEGO-LOAM 運行效果優(yōu)異，所以在此小節(jié)中，以LEGO-LOAM 的結果作為參考，對比分析LIO-Mapping 與Ligom 的定位與建圖效果。

如圖5 所示，在前半段中，LEGO-LOAM、Ligom與LIO-Mapping 都可以平穩(wěn)地運行并輸出定位與建圖結果，并且三個系統(tǒng)的里程計估計基本重合，但是，隨著行駛路程的增長，系統(tǒng)的誤差在累計，特別是UGV 行駛在框圖所在的位置時，地面的高度有較大的變化，此時，Ligom 相比LIO-Mapping，定位軌跡更加接近LEGO-LOAM，因為Ligom 具有有效的回環(huán)檢測機制，考慮了歷史幀的點云信息，以修正車輛狀態(tài)，具備更強的魯棒性，在長時間和長距離的運行中漂移較小。 并且如圖6 虛線框所示，Ligom 的建圖結果更加精細和準確。

圖5 Steven Campus 數(shù)據(jù)集中三種方法的定位軌跡對比

圖6 Steven Campus 數(shù)據(jù)集中三種方法的建圖結果

3.2.2 公園數(shù)據(jù)集

在此測試中，UGV 行駛在公園的行人小道，環(huán)境的特征主要包括公園及道路周圍的建筑、樹木與低矮草叢，并且部分路段僅有少量樹木與草叢，此時比較難以獲得準確的平面或邊緣特征點云，對于點云特征提取與配準算法要求較高。

如圖7 的虛線框所示，Ligom 可以將相同樹木的點云正確地聚類，得到準確的樹木點云特征，而此處LEGO-LOAM 與LIO-Mapping 得到的樹木點云出現(xiàn)了不重合的結果，其原因在于這兩個方法的回環(huán)檢測結果不準確，車輛回到起點附近時軌跡出現(xiàn)了較大的漂移，從而導致了建圖結果的誤差。

3.2.3 KITTI 街道數(shù)據(jù)集

選擇KITTI_2011_09_30_drive_0027 作為本小節(jié)的測試數(shù)據(jù)集，以模擬商用車輛的行駛工況，驗證算法在車速較快、路況較復雜的街道場景中的定位與建圖效果。

如圖8 所示，Ligom 與LEGO-LOAM 的定位軌跡接近，都可以正確估計行駛過程中的路面高度的變化，并實現(xiàn)正確的軌跡回環(huán)，而LIO-Mapping 在估計車輛z方向位置中出現(xiàn)了非常大的誤差，定位軌跡嚴重偏離，起始點與終點的z方向高度差達到約1.4 m，無法為車輛提供準確的車輛狀態(tài)信息。 表2 顯示了車輛回到起點時三種方法的相對平移誤差。

表2 車輛回到起點時三種方法的相對平移誤差單位:m

圖8 KITTI 街道數(shù)據(jù)集中三種方法的定位軌跡對比

由圖9 可知，由于算法對于距離的設定，LIOMapping 可以處理更遠處的點云信息，但因為遠距離物體的測量與其車輛定位信息的誤差，LIOMapping 的建圖結果中，物體位置出現(xiàn)較大的偏離，特別是起始位置附近的地圖由許多模糊結構組成。對比LEGO-LOAM，Ligom 能夠更加準確地識別并構建物體的輪廓，例如街道旁的車輛與綠化帶。 因此，在復雜街道的場景中，Ligom 的定位與建圖算法效果優(yōu)于其他兩種方法，能夠為車輛提供高精度的實時定位與建圖信息。

圖9 KITTI 街道數(shù)據(jù)集三種方法的建圖結果

3.2.4 GDUT-Tracking 數(shù)據(jù)集

此小節(jié)的測試目的是驗證融合GNSS 測量對車輛定位精度的提升效果，以及賽道樁桶地圖的建圖精度。 因此，在圖10(a)虛線框所示的開闊場景中，隨機擺放兩種不同軌跡的閉環(huán)樁桶賽道。 在此場景中，獲得的點云特征稀疏，對于傳統(tǒng)的LIO 算法具有很大的挑戰(zhàn)性，并且遮擋物較少，可以接收到準確可用的GNSS 測量，因此可以針對性地驗證測試目的。

圖10 Tracking 數(shù)據(jù)集中環(huán)境與定位軌跡對比

由于獲得的點云特征稀疏，松耦合的LEGOLOAM 算法無法得到有意義的結果，所以只對比LIO-Mapping。 運行Ligom 與LIO-Mapping 得到定位軌跡如圖10(b)、圖10(c)所示，以GNSS 測量作為參考軌跡，從測試結果可知，Ligom 得到的定位軌跡更接近GNSS 測量，特別是場景2，LIO-Mapping 在x方向中出現(xiàn)了較大的偏移，最大偏差達到了約2.5 m，由此可知，Ligom 融合了GNSS 測量，可以得到更加準確的車輛狀態(tài)。

在車輛行駛過程中，實時構建樁桶地圖如圖11所示，Ligom 可以準確地識別不同樁桶的顏色、位置以及ID 信息，然后根據(jù)同一ID 將多幀樁桶信息求解最優(yōu)后驗估計，完成回環(huán)檢測構建樁桶地圖。 由于樁桶是隨機擺放的，沒有先驗地圖信息作為參考，所以，為了驗證樁桶地圖的建圖精度，如圖11 所示，本文選擇六個樁桶點，對比地圖結果與實際測量的距離誤差。 由表3 和表4 可知，樁桶之間的距離誤差很小，樁桶建圖算法可實現(xiàn)高精度建圖。

表3 樁桶A、B、C 的地圖結果與實際測量的間距

表4 樁桶D、E、F 的地圖結果與實際測量的間距

圖11 Tracking 數(shù)據(jù)集實際環(huán)境與樁桶地圖結果

4 結論

本文提出了一種多傳感器信息緊耦合的無人駕駛車輛SLAM 方法，可以在復雜的環(huán)境中，實現(xiàn)無人駕駛車輛狀態(tài)的實時估計與環(huán)境地圖的構建。Ligom 基于iEKF 理論設計，適用于融合多傳感器信息，特別是融合了GNSS 傳感器，可以消除長時間積累的激光雷達慣性里程計漂移，并使用滑動窗口方法保存點云特征，提高了點云匹配精度，在點云特征比較稀疏的開闊環(huán)境中仍具有很好的效果。 本文所提出的方法在四個不同平臺與環(huán)境采集的數(shù)據(jù)集下進行了充分驗證，結果表明，相比LEGO-LOAM 與LIO-Mapping，Ligom 方法可以達到更高的精度與具備更強的魯棒性。