叢明 溫旭 王明昊 劉冬

（大連理工大學(xué) 機械工程學(xué)院，遼寧 大連 116024）

自從20世紀(jì)80年代同步定位與地圖構(gòu)建的概念被提出以來，其始終是機器人自主導(dǎo)航的核心技術(shù)，廣泛應(yīng)用于無人車、無人機等領(lǐng)域。SLAM（同步定位和地圖構(gòu)建）問題的核心內(nèi)容可以理解為移動機器人是否可以放置在未知環(huán)境中，然后逐步構(gòu)建該環(huán)境的地圖，同時使用地圖來確定其全局位姿［1］。經(jīng)過多年的努力，目前基于激光雷達和基于視覺的方案均能實現(xiàn)精確的定位和建圖功能。雖然視覺方案具有成本低、采集信息豐富和便于回環(huán)檢測的優(yōu)點，但是由于對初始化和光照變化敏感而且還要消耗資源計算深度信息，因此不適合于室外環(huán)境。激光雷達不僅沒有上述缺點，而且高分辨率的三維激光雷達對于環(huán)境信息的捕捉能力也越來越強，因此在室外環(huán)境中基于激光的算法是更合適的選擇。

激光SLAM的里程計算法通過匹配相鄰幀的點云數(shù)據(jù)解算位姿。Besl等［2］提出了迭代最近點算法（ICP），該算法成為了點云匹配發(fā)展的基礎(chǔ)。但是ICP忽視了點云數(shù)據(jù)中的幾何信息。為了克服這個問題，Segal等［3］提出了G-ICP算法，將殘差定義為點到對應(yīng)平面的距離。Zhang等［4］在G-ICP的基礎(chǔ)上提出了LOAM算法，增加了角點特征，進一步擴展了特征點的類型。LOAM的一系列衍生算法［5-8］延續(xù)了提取點云數(shù)據(jù)中的特征點并構(gòu)建特征點與線和平面之間的約束關(guān)系的方式，但是特征提取也增加了對算力的要求。雖然在一些場景中僅使用激光雷達可以實現(xiàn)建圖和定位功能，但是在劇烈運動或長廊等場景下算法精度會迅速下降。為了提高里程計的精度和魯棒性，激光通常和其他傳感器融合共同估計機器人狀態(tài)。

多傳感器融合設(shè)計方案通常有松耦合和緊耦合兩種方式。在LOAM［4］和LeGO-LOAM［5］中，因為慣性測量單元（IMU）僅被用來去除點云運動畸變和提供優(yōu)化的初值并不參與優(yōu)化過程，所以屬于松耦合方案。松耦合的另一種實現(xiàn)方式是基于濾波方法融合多個傳感器各自獨立的結(jié)果得到一個更加精確的機器人狀態(tài)。Lynen等［9］提出了基于擴展卡爾曼濾波器（EKF）的通用和模塊化的多傳感器融合框架。Zhen等［10］使用誤差狀態(tài)卡爾曼濾波器（ESKF）融合IMU測量和粒子濾波器輸出的激光里程計。雖然松耦合易于實現(xiàn)，但是緊耦合算法往往能夠得到精度更高的結(jié)果。Gao等［11］分別設(shè)計了GPS和激光的緊耦合算法和松耦合算法，實驗表明緊耦合系統(tǒng)的精度優(yōu)于松耦合系統(tǒng)。Ye等［12］提出了基于優(yōu)化方法的激光-IMU緊耦合算法，但由于計算量過大，對計算平臺要求很高。在LIO-SAM［6］中，將點云匹配得到的里程計因子、IMU預(yù)積分［13］因子、回環(huán)因子和GPS因子在Isam2［14］上進行聯(lián)合優(yōu)化，雖然可以得到準(zhǔn)確的結(jié)果，但是計算成本很高。在FASTLIO［15］中，采用迭代卡爾曼濾波器處理誤差狀態(tài)，聯(lián)合優(yōu)化IMU和激光數(shù)據(jù)。FAST-LIO2［16］在FASTLIO的基礎(chǔ)上使用了優(yōu)化庫［17］，提高了程序的易讀性。FAST-LIO2在時間效率方面優(yōu)于LIO-SAM，而且達到了與LIO-SAM相似或更好的精度。但是由于FAST-LIO2采用增量kd樹［18］管理地圖，不方便進行回環(huán)檢測。而且沒有融合GPS信息，在大尺度場景下會出現(xiàn)嚴(yán)重的誤差累積問題。

雖然目前被提出的算法在多種場景中已經(jīng)能夠很好地完成定位和建圖任務(wù)，但是在大尺度的室外場景下往往會遇到兩個主要的挑戰(zhàn)：①特征稀疏的場景會導(dǎo)致軌跡跟蹤失敗。激光雷達通過接收被障礙物反射回來的激光從而計算周圍環(huán)境中障礙物到自身的距離。當(dāng)障礙物距離激光超過150～200 m甚至沒有障礙物時點云數(shù)據(jù)不足以推算軌跡，導(dǎo)致里程計出現(xiàn)較大誤差。②累計誤差會對算法表現(xiàn)造成嚴(yán)重影響。雖然里程計在短時間內(nèi)精度很高，但是大尺度場景下累計誤差尤其是高程誤差會極大影響軌跡推算的結(jié)果。在無法構(gòu)建有效的回環(huán)約束的場景下，這種情況更加明顯。為了解決以上的問題，本文提出了一種GPS-激光-IMU融合建圖算法。

本文的主要貢獻有：①設(shè)計并實現(xiàn)了一種基于IKF（迭代卡爾曼濾波器）的GPS-激光-IMU緊耦合里程計算法，通過融合GPS的絕對位置信息可以糾正激光-IMU里程計的累計誤差，并且可以在特征稀疏的場景下提高算法的魯棒性；②將重力的優(yōu)化轉(zhuǎn)化為旋轉(zhuǎn)矩陣群上的優(yōu)化，解決了重力過參數(shù)化的問題。在ENU坐標(biāo)系下重力方向是沿著Z軸反方向而且在地區(qū)不變時重力的模長是穩(wěn)定的，因此可以將對重力變量的優(yōu)化視為東北天坐標(biāo)系（ENU）和世界坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系的優(yōu)化，將重力的優(yōu)化在旋轉(zhuǎn)矩陣群上解決；③在室外場景下對算法進行了充分的評估，并且驗證了算法在大尺度室外場景下的魯棒性和精度。

1 算法框架

本文提出的算法通過輸入三維激光點云、IMU數(shù)據(jù)和GPS數(shù)據(jù)估計機器人位姿并且構(gòu)建周圍環(huán)境地圖，如圖1所示，總體框架由數(shù)據(jù)預(yù)處理、狀態(tài)估計和后端優(yōu)化3部分組成。

圖1 算法結(jié)構(gòu)圖Fig.1 Structure of the algorithm

1）數(shù)據(jù)預(yù)處理。該模塊主要對點云和GPS數(shù)據(jù)進行操作。由于激光對各點的采樣存在時間差，因此需要使用IMU數(shù)據(jù)解算位姿將一幀激光中的所有點云數(shù)據(jù)都變換到采樣結(jié)束時刻。輸入系統(tǒng)的GPS數(shù)據(jù)是在LLA坐標(biāo)系下，因此需要將其轉(zhuǎn)換到ENU坐標(biāo)系下。

2）狀態(tài)估計。使用IKF算法融合IMU、激光和GPS數(shù)據(jù)，估計機器人狀態(tài)。首先使用IMU數(shù)據(jù)傳遞機器人狀態(tài)，然后計算激光點云數(shù)據(jù)和GPS數(shù)據(jù)的殘差并更新機器人狀態(tài)，輸出當(dāng)前時刻的位姿估計，同時也為后端優(yōu)化提供良好初值。

3）后端優(yōu)化。由于相鄰幀激光點云數(shù)據(jù)有很大一部分是重疊的，因此采用管理關(guān)鍵幀的方式管理點云數(shù)據(jù)可以極大提高算法效率。目前掃描上下文（SC）描述子［19］、強度掃描上下文（ISC）描述子［20］和方位直方圖強度特征（ISHOT）描述子［21］是廣泛使用的描述子，本文采用SC全局描述子檢測回環(huán)。通過Isam2算法融合回環(huán)因子與里程計因子，修正全局位姿并更新全局地圖，新的全局地圖作為下一次點云匹配的基準(zhǔn)。

本文所做的工作主要集中于狀態(tài)估計部分，以下內(nèi)容圍繞狀態(tài)估計模塊展開。

2 狀態(tài)估計

2.1 狀態(tài)轉(zhuǎn)移模型

如圖2所示，在本文中世界坐標(biāo)系記為G，以IMU坐標(biāo)系為機器人坐標(biāo)系記為I，初始時刻G坐標(biāo)系與I坐標(biāo)系重合。記ENU坐標(biāo)系為E，通過旋轉(zhuǎn)變換可以將G坐標(biāo)系變換到E坐標(biāo)系。機器人的狀態(tài)x、機器人的誤差狀態(tài)δx、IMU測量u和噪音w分別為

圖2 不同坐標(biāo)系之間關(guān)系Fig.2 Relation between different coordinate systems

2.1.1 重力優(yōu)化

Sola［22］設(shè)計了基于誤差卡爾曼濾波器的里程計框架。Sola將重力視為三維向量進行優(yōu)化，但是由于重力是二自由度向量，這將導(dǎo)致重力過參數(shù)化和協(xié)方差矩陣奇異性的問題。而且優(yōu)化后為了保證重力的模長不變需要歸一化處理。在FAST-LIO2算法中，將重力在流形空間S2中優(yōu)化從而保證重力模長穩(wěn)定。

如圖3所示，在東北天坐標(biāo)系（ENU）中重力的方向始終指向地心而且在某一區(qū)域運行時重力的大小不會發(fā)生明顯改變，因此在本文算法應(yīng)用的場景中可以將重力視為常量。對重力進行優(yōu)化是為了準(zhǔn)確去除IMU測量的加速度中的重力分量。由上文可知東北天坐標(biāo)系與世界坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系為，因此按照式（2）將世界坐標(biāo)系下的重力gG轉(zhuǎn)換為的形式：

圖3 東北天坐標(biāo)系示意圖Fig.3 Illustration of local Cartesian coordinate system

式中，gG、gE分別為世界坐標(biāo)系和ENU坐標(biāo)系中的重力向量。這樣處理重力能夠充分利用ENU坐標(biāo)系中重力的物理性質(zhì)，將重力的優(yōu)化問題視為ENU坐標(biāo)系和世界坐標(biāo)系之間的旋轉(zhuǎn)優(yōu)化問題，即將對gG的優(yōu)化轉(zhuǎn)化為對的優(yōu)化，繼而將重力的優(yōu)化在旋轉(zhuǎn)矩陣群上解決，避免了直接對重力的運算，從而保證了重力模長不變。而且這種優(yōu)化方式建立起ENU坐標(biāo)系和世界坐標(biāo)系之間的約束加深了機器人各個狀態(tài)之間的耦合程度，因此狀態(tài)估計的精度得到提高。與FAST-LIO2中的優(yōu)化算法相比，本文提出的算法更加簡潔、計算量更低。

2.1.2 誤差狀態(tài)轉(zhuǎn)移模型

機器人狀態(tài)的動力學(xué)模型為

[a]∧表示向量a∈R3的反對稱向量積矩陣，

記i為IMU測量的索引，誤差狀態(tài)的轉(zhuǎn)移模型［22］為

式中：Pi為δxi的協(xié)方差矩陣；Pna、Pnω、Pnba和Pnbω分別為噪音na，nω，nba和nbω的協(xié)方差矩陣；Fxi、Fwi和Qi按如下公式計算：

式中，R{}為旋轉(zhuǎn)向量對應(yīng)的旋轉(zhuǎn)矩陣。

2.2 觀測模型

在本文提出的迭代卡爾曼濾波器中包括兩種觀測，使用激光點云數(shù)據(jù)和GPS數(shù)據(jù)作為觀測更新機器人狀態(tài)。依靠激光點云數(shù)據(jù)推測的軌跡光滑而且在短時間內(nèi)精度很高，但是由于點云匹配是通過將當(dāng)前幀和歷史幀點云進行匹配，因此不可避免地帶入了誤差的累積。根據(jù)GPS數(shù)據(jù)推算的機器人位姿雖然沒有被歷史結(jié)果影響，但是整條軌跡并不連續(xù)。因此兩種觀測可以互相彌補各自的缺點。

2.2.1 GPS觀測

由于輸入系統(tǒng)的GPS數(shù)據(jù)是在LLA坐標(biāo)系下，因此首先將GPS數(shù)據(jù)轉(zhuǎn)換到ENU坐標(biāo)系下［23］。將ENU系下的機器人位置觀測記為ygps，觀測誤差記為vgps，則觀測方程和殘差rgps分別為

觀測方程對機器人狀態(tài)x的雅可比矩陣H：

2.2.2 激光觀測

由于激光一般是按順序?qū)c采樣，所以連續(xù)的激光點之間存在一定的時間差。因此伴隨機器人的運動，一組激光點云中的數(shù)據(jù)并不是在同一位姿下采集的，所以首先要對點云進行去畸變?；贗MU數(shù)據(jù)可以推算出每個激光點相對于當(dāng)前幀激光點云結(jié)束時刻的位姿關(guān)系，然后將所有的點變換到結(jié)束時刻。

歷史幀的激光點云數(shù)據(jù)有很大一部分是重疊的，因此只保存關(guān)鍵幀的數(shù)據(jù)不僅不會丟失數(shù)據(jù)而且會提高算法效率。通過最近鄰算法搜索到n個距離當(dāng)前位姿最近的關(guān)鍵幀{F1，F(xiàn)2，…，F(xiàn)n}，將n個關(guān)鍵幀的點云數(shù)據(jù)融合成點云地圖M：

記k為激光點云的索引，pL為第k幀激光中的點。qj(j=1，2，3，…，m)為點云地圖M中距離pL最近的m個激光點，如圖4所示，根據(jù)qj計算出pL對應(yīng)平面的方程為

圖4 激光觀測模型Fig.4 Laser measurement model

vlaser為觀測的誤差，以pL到對應(yīng)平面的距離為觀測，則觀測方程和殘差rlaser為

觀測方程對機器人的狀態(tài)x的雅可比矩陣H為

2.2.3 迭代卡爾曼濾波器

在線性高斯系統(tǒng)中卡爾曼濾波器可以得到狀態(tài)的最優(yōu)無偏估計，但是在非線性系統(tǒng)中應(yīng)用EKF（擴展卡爾曼濾波器）需要計算觀測方程在線性化點對狀態(tài)的雅可比矩陣，這樣便能將非線性系統(tǒng)線性化，繼而估計狀態(tài)。因此線性化點的選擇很大程度上影響了EKF的性能。線性化點越接近狀態(tài)的真值，狀態(tài)估計的結(jié)果就越精確。當(dāng)?shù)趉幀觀測被輸入系統(tǒng)時，根據(jù)狀態(tài)轉(zhuǎn)移方程估計的狀態(tài)為，在EKF中將視為線性化點計算觀測方程的雅可比矩陣H，更新狀態(tài)得到后驗估計（j為迭代次數(shù)）。IKF將每次得到的后驗估計作為線性化點重新估計狀態(tài)直到收斂。

本文采用針對誤差狀態(tài)的IKF，狀態(tài)轉(zhuǎn)移方程可以得到兩個狀態(tài)：和，分別為標(biāo)準(zhǔn)狀態(tài)和誤差狀態(tài)的先驗?！玁(0，)并且滿足關(guān)系xk=+。以GPS數(shù)據(jù)和激光點云數(shù)據(jù)為觀測計算雅可比矩陣H和卡爾曼增益K并更新然后以為線性化點重新線性化觀測方程。因為重新選擇了線性化點，因此誤差狀態(tài)也需要重新計算［16］，計算公式為

式中：θ為R對應(yīng)旋轉(zhuǎn)向量的模長；a為R對應(yīng)旋轉(zhuǎn)向量的單位向量；迭代次數(shù)j=0時，Jj為單位矩陣。采用FAST-LIO中的卡爾曼增益計算公式能夠避免高維矩陣求逆運算，降低計算成本，因此迭代卡爾曼濾波器的增益和更新方程為

式中，r為GPS和激光觀測的殘差，R為觀測誤差vgps和vlaser的協(xié)方差矩陣。在迭代卡爾曼濾波器中不斷迭代計算卡爾曼增益和雅可比矩陣并且更新機器人狀態(tài)直到收斂。

3 實驗

為驗證本文提出算法的有效性，本節(jié)介紹在普通場景和大尺度場景下的實驗及對應(yīng)的分析。實驗的計算平臺為配備了Ubuntu18.04 ROS Melodic，Intel（R）Core（TM）i7-11800H CPU以及16 G內(nèi)存的筆記本計算機。自建小車硬件平臺中搭載的激光雷達為速騰聚創(chuàng)公司的RS-LiDAR-16，IMU和GPS的型號為阿路比公司的LPMS-IG1P，上位機采用大疆公司的妙算Manifold 2-C，硬件的安裝位置如圖5所示。實驗包括：普通場景下對比其他算法與本文算法的建圖精度、室外大尺度場景下驗證本文算法的建圖效果等。普通場景下的實驗采用公開數(shù)據(jù)集，室外大尺度場景下建圖使用的數(shù)據(jù)由搭載16線激光雷達、九軸IMU和GPS的小車采集，硬件平臺如圖5所示。

圖5 自建移動平臺Fig.5 Platform of self-build mobile vehicle

3.1 普通場景下建圖結(jié)果對比

首先對比在融合GPS數(shù)據(jù)時各算法的精度。使用LIO-SAM文獻［6］中的開源數(shù)據(jù)集序列，數(shù)據(jù)包括16線激光雷達、IMU和GPS數(shù)據(jù)。表1為各算法的均方根誤差對比。本文算法表示完整的GPS-激光-IMU融合建圖算法。為了驗證GPS融合和重力優(yōu)化對算法精度的提升，分別關(guān)閉GPS融合和重力優(yōu)化功能進行對比試驗。本文算法-NoGrav為關(guān)閉重力優(yōu)化功能的建圖算法，本文算法-NoGPS為關(guān)閉GPS融合功能的建圖算法。本文算法與本文算法-NoGrav相比，均方根誤差有所降低，說明本文提出的重力優(yōu)化可以提高建圖精度。本文算法與本文算法-NoGPS、FAST-LIO2和LeGO-LOAM相比，精度有大幅度提升，證明了本文算法的有效性。由于FAST-LIO2在初始化部分出現(xiàn)問題，所以誤差相對較大。

表1 不同算法的均方根誤差Table 1 RMSE of different algorithms m

LIO-SAM-GPS表示融合GPS數(shù)據(jù)的LIO-SAM算法，本文算法的平移均方根誤差僅為LIO-SAMGPS的46%。圖6（a）和6（b）分別為本文算法和LIOSAM-GPS誤差曲線，其中APE為絕對位姿誤差，RMSE為均方根誤差，Median為誤差中位數(shù)，Mean為誤差平均數(shù)，STD為標(biāo)準(zhǔn)差。這些評估標(biāo)準(zhǔn)下，本文算法均優(yōu)于LIO-SAM-GPS。

圖6 絕對位姿誤差曲線Fig.6 Curves of absolute pose error

為了驗證本文算法在無法獲取GPS信號時的精度，本節(jié)進一步設(shè)計了在不融合GPS數(shù)據(jù)時各算法的精度對比試驗。采用M2DGR數(shù)據(jù)集［24］，該數(shù)據(jù)集中錄制了視覺傳感器數(shù)據(jù)、激光點云、IMU和GPS數(shù)據(jù)，本節(jié)實驗使用M2DGR的Street_03和Street_08序列。表2和表3分別為各算法的均方根誤差與最大平移誤差。通過對比可以發(fā)現(xiàn)，本文算法在不融合GPS數(shù)據(jù)時與開源算法的精度接近，保證了算法在GPS拒止場景下的魯棒性。

表2 不融合GPS數(shù)據(jù)時的均方根誤差Table 2 RMSE when GPS data is not fused m

表3 不融合GPS數(shù)據(jù)時的最大平移誤差Table 3 Maximum translation error when GPS is not fused m

3.2 大尺度場景下建圖結(jié)果對比

圖7中紅線為移動平臺的行駛軌跡，軌跡全長超過6 km，由于硬件平臺速度的限制，行駛時間約1.5 h。為了測試算法在大尺度場景下的建圖與定位精度，本文在如圖8所示場景下進行了對比試驗。圖8展示了本文算法在該場景下的建圖結(jié)果。為了能夠與其他開源算法進行充分對比，本文中分別從定性和定量兩個角度對實驗結(jié)果進行分析。

圖7 小車行駛軌跡Fig.7 Traveling path of the vehicle

圖8 大尺度場景建圖結(jié)果Fig.8 Mapping result of large-scale environment

在不融合GPS的情況下，本文算法-NoGPS、FAST-LIO2和LeGO-LOAM在空曠的場景中由于點云數(shù)量不足均無法完成建圖任務(wù)，因此不對建圖效果進行對比。從定性角度分析，圖8（a）圖表示建圖結(jié)果，圖8（b）中軌跡表示本文算法未構(gòu)成回環(huán)約束時在虛框處的軌跡，圖8（c）中軌跡表示LIO-SAMGPS未構(gòu)成回環(huán)約束時在虛框處的軌跡，在該處小車的軌跡形成了一個回環(huán)，可以明顯看出在未構(gòu)成回環(huán)約束之前本文算法的軌跡一致性較好而LIOSAM-GPS發(fā)生了嚴(yán)重的偏差。從定量角度分析，如表4所示，當(dāng)回到出發(fā)點時本文算法的起始誤差為0.11 m，精度高于LIO-SAM-GPS。

表4 起始平移誤差Table 4 End-to-end translation error m

由于該場景中的部分區(qū)域非常空曠導(dǎo)致點云數(shù)量不足，因此機器人運動到該區(qū)域時會出現(xiàn)相鄰幀匹配失敗的問題。而且由于后端無法構(gòu)建有效的約束，當(dāng)回到出發(fā)點時，已經(jīng)無法通過回環(huán)優(yōu)化修正累計誤差。因此沒有融合GPS數(shù)據(jù)的情況下，無法建立有效可用的地圖。GPS數(shù)據(jù)可以提供機器人的絕對位置信息，能夠及時修正誤差，因此融合GPS數(shù)據(jù)可以提高系統(tǒng)魯棒性，能夠防止里程計漂移的問題。

4 結(jié)論

本文提出了一種基于迭代卡爾曼濾波器的GPS-激光-IMU融合建圖算法，解決了室外大尺度場景下誤差累積的問題，提高了特征稀疏場景下的魯棒性，能夠應(yīng)用在空曠的室外環(huán)境中。本文將重力的優(yōu)化轉(zhuǎn)換為旋轉(zhuǎn)矩陣群上的問題，避免了重力過參數(shù)化和協(xié)方差矩陣奇異性的問題。實驗表明，本文算法的均方根誤差為0.089 m，與其他算法相比本文算法達到了更高的精度。后續(xù)將研究在GPS拒止場景下如何降低誤差累積對定位和建圖的影響。