基于迭代卡爾曼濾波器的GPS-激光-IMU融合建圖算法

叢明 溫旭 王明昊 劉冬

（大連理工大學 機械工程學院，遼寧 大連 116024）

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

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

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

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

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

1 算法框架

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

圖1 算法結構圖Fig.1 Structure of the algorithm

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

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

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

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

2 狀態估計

2.1 狀態轉移模型

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

圖2 不同坐標系之間關系Fig.2 Relation between different coordinate systems

2.1.1 重力優化

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

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

圖3 東北天坐標系示意圖Fig.3 Illustration of local Cartesian coordinate system

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

2.1.2 誤差狀態轉移模型

機器人狀態的動力學模型為

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

記i為IMU測量的索引，誤差狀態的轉移模型［22］為

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

式中，R{}為旋轉向量對應的旋轉矩陣。

2.2 觀測模型

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

2.2.1 GPS觀測

由于輸入系統的GPS數據是在LLA坐標系下，因此首先將GPS數據轉換到ENU坐標系下［23］。將ENU系下的機器人位置觀測記為ygps，觀測誤差記為vgps，則觀測方程和殘差rgps分別為

觀測方程對機器人狀態x的雅可比矩陣H：

2.2.2 激光觀測

由于激光一般是按順序對點采樣，所以連續的激光點之間存在一定的時間差。因此伴隨機器人的運動，一組激光點云中的數據并不是在同一位姿下采集的，所以首先要對點云進行去畸變。基于IMU數據可以推算出每個激光點相對于當前幀激光點云結束時刻的位姿關系，然后將所有的點變換到結束時刻。

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

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

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

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

觀測方程對機器人的狀態x的雅可比矩陣H為

2.2.3 迭代卡爾曼濾波器

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

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

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

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

3 實驗

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

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

3.1 普通場景下建圖結果對比

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

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

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

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

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

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

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

3.2 大尺度場景下建圖結果對比

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

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

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

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

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

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

4 結論

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