基于因子圖的激光SLAM模型優化算法

摘 要：針對激光SLAM點云建圖模型研究，設計一種因子圖優化的SLAM模型方案。在前端模型中，激光里程計通過引入IESKF實現IMU與雷達點云數據的緊耦合構建IESKF-LIO。在SLAM后端模型中，為提高SLAM的魯棒性和實時精度，構建多種因子進行位姿約束與補償優化，同時在因子圖融合過程中提出關鍵幀和增量式平滑建圖，減少模型運算負擔。在KITTI數據集中進行建圖實驗，實驗結果驗證了該模型較傳統SLAM模型軌跡誤差更低，建圖效果更好。

關鍵詞：SLAM；因子圖；IESKF；回環檢測

中圖分類號：TP391.9" 文獻標志碼：A" 文章編號：1671-5276（2024）05-0167-04

Factor Graph-based Laser SLAM Model Optimization Algorithm

Abstract：For the study of laser SLAM point cloud mapping model， a factor graph optimized SLAM model scheme is designed. In the front-end module， the laser odometry realizes the tight coupling of IMU and radar point cloud data by introducing IESKF， thus IESKF-LIO being constructed. In the SLAM back-end model， a variety of factors are built for pose constraints and compensation optimization in order to improve the robustness and real-time accuracy of SLAM， meanwhile， key frames and incremental smooth mapping are proposed during the factor graph fusion process to reduce the computational burden of the model. Through the mapping experiments in KITTI data sets， the experimental results verify that the designed model has lower trajectory errors and better mapping effects than the traditional SLAM onel.

Keywords：SLAM；factor graph；IESKF；loopback testing

0 引言

室外場景下的語義類別較多，地面形體上各類別所呈現的高低起伏狀態不具有一般規律性。傳統激光SLAM獲取的室外點云地圖較為稀疏，獲取點云不均勻和無規律性較強，同時所獲點云信息僅能表示物體的一般特征，無法體現更深度的細節信息。

SLAM技術最早在1987年被提出［1］，經過30余年，SLAM技術得到了充分的發展。SMITH等［2］提出了EKF-SLAM模型，該模型在小規模和簡單場景下具有較高精度的建圖效果，但不適用于較大型場景下的建圖作業。FAST-LIO是香港大學開發的實時建圖模型，通過IESKF將激光點云特征與IMU數據進行融合，為了降低激光點云特征帶來的計算量，提出依賴維度的卡爾曼增益公式［3］。CHANG等［4］提出一種融入IMUamp;ODO預積分的SLAM系統，在前端使用三維概率圖，提高在特征有限的環境中點云匹配的效果。LeGO-LOAM［5］算法在LOAM算法框架的基礎上，在特征提取部分針對地面特征進行優化，并增加了回環檢測，但其仍然無法解決在結構化場景及空曠環境中雷達里程計失效的問題。為了解決激光雷達里程計失效的問題，研究者嘗試使用激光雷達與其他傳感器結合定位建圖。LIO-SAM［6］算法則是在圖優化的基礎上實現緊耦合的激光雷達里程計，采用關鍵幀的滑動窗口模式減小建圖計算量。付林等［7］提出結合慣性/高度傳感器的激光SLAM算法，在點云初始化時引入慣性/高度傳感器的姿態，有效提高了高程退化環境中定位精度。劉振宇等［8］提出一種融合激光雷達與慣性測量單元融合的算法，在算法中使用幀與局部地圖匹配所得的距離值和IMU數據構建出聯合優化函數，提高了原算法的精度。

針對室外環境的SLAM建圖研究，本文以激光雷達點云數據為中心搭建多傳感器數據融合的同步定位和建圖方案。通過構建激光慣性里程計優化幀間里程數據、因子圖融合等方式，實現對室外場景下的高效地形感知與建圖算法搭建；同時通過仿真與對比實驗，驗證了比傳統算法更為明顯的優化效果。

1 基于優化卡爾曼濾波的LIO設計

自卡爾曼濾波算法被提出后，被廣泛運用在控制系統和導航與定位等方面［9］。本文采用基于FAST-LIO框架的設計理念，使用緊密耦合的迭代擴展卡爾曼濾波器（IEKF）將LiDAR特征點與IMU數據融合在一起，引入誤差狀態卡爾曼濾波器實現對激光雷達和IMU融合位姿進行誤差補償。經過多次迭代，獲得高精度的后驗狀態參數與協方差參數，在快速運動或非結構化環境中實現穩健導航和建圖操作。

在融合IMU和激光雷達數據時，構建觀測方程將測量結果結合起來，進一步需要求解出觀測的協方差矩陣H，在得到觀測方程的協方差矩陣后需要對協方差矩陣進行狀態更新以減小誤差傳遞，最后是獲取增益矩陣Ki+1。不同于傳統卡爾曼濾波協方差更新方程，本文采用的是FAST-LIO模型中IESKF模型的增益矩陣更新方式。

2 關鍵因子構建

2.1 因子圖模型

因子圖模型是用因子描述多變量復雜函數的二維圖，屬于概率模型（PGM）中的一種，可以表征全局函數和局部函數之間的關系，同時也可以表示各個變量與局部函數之間的關聯，經常被用于多源數據融合的SLAM研究中。

在多源SLAM全局優化過程中，基于貝葉斯模型構建系統的狀態量和觀測量SLAM的后驗概率模型，假定各個因子都符合高斯分布，則其SLAM模型中求解最大后驗概率問題則可以轉化為非線性最小二乘問題。針對多種因子進行非線性求解的目標函數可以被定義為式（1）：

式中：hi表示第i個傳感器模塊的觀測數據；γi表示第i的狀態量下的傳感器測量數據；hi（X）－γi表示各個因子的約束殘差項；∑i f表示各個傳感器的測量方差；狀態變量X為待優化參數項。

2.2 改進最優描述子的快速回環因子設計

為有效解決大范圍定位下的誤差漂移問題，同時在保證回環精度前提下提升回環檢測速度，本文基于經典Scan-Context回環模型進行改進，設計了一種在保證精度的同時實現快速回環的優化方案，通過聯合Scan-Context描述子與Lidar-Iris特征圖的方式構建并確定回環檢測最優關鍵幀因子。整體的構建過程如下。

1）采用圓環分區的方式，將三維空間的點云信息轉換為二維空間的矩陣。以當前幀為中心將所獲點云轉換為鳥瞰圖，設置最大搜索距離為dmax，以dmax為圓半徑將掃描區域劃分為Nr個圓環和Ns個扇區，將每個環展開可以得到一個 Nr×Ns的二維圖像。每個像素點Pi，j是第i個環第j個扇面對應的Bin值。本文定義每個像素點Pi，j的Bin值為當前像素區域內點云最大高度，這樣利用Scan-Context就構成了一個Nr×Ns的二維矩陣，最終形式如圖1所示。

將Scan-Context獲取的環向量作為描述符，每個環向量都被用來構造KD-Tree，在KD-Tree中通過最近鄰搜索選取與當前關鍵幀環鍵相似的環鍵對應的關鍵幀作為回環候選關鍵幀。

2）根據距離原點由近及遠對矩陣的每一行向量ki計算環向量平均值，得到該幀下的描述符環向量K。環向量K是Nr×1維的矩陣，且具有旋轉不變性，故可以采用KD-Tree的結構來計算兩幀間環向量的相似度，從而快速地選取合格的候選回環幀。

3）在得到候選回環幀后，進一步利用Lidar-Iris對多個回環候選關鍵幀中的Bin值進行重新編碼，生成Lidar-Iris圖像。

4）將每個關鍵幀下得到的二值特征圖存于歷史數據庫中。在步驟2）中獲得候選回環關鍵幀后，通過計算當前關鍵幀和數據庫中候選關鍵幀下Lidar-Iris特征圖像之間的Hamming距離判斷最優回環幀。如果距離計算結果最小且小于閾值，則判定該時刻下形成回環，進一步以當前幀為中心將KD-Tree中的最優回環幀與當前幀進行配準，得到回環下的位姿變換矩陣，最終完成回環的精匹配。

回環檢測完成后，將得到的位姿變換矩陣作為新的約束傳入因子圖優化框架中，進一步更新傳感器的位姿估計，消除里程計累計誤差和漂移。

2.3 IMU預積分因子、 GPS環境因子構建

IMU預積分處理就是將在雷達兩幀間獲取的多組IMU加速度、角速度數據進行積分處理，從而得到機器在幀間的精準位姿變化信息，根據積分數據進一步得到IMU預積分因子。但兩幀間獲取的IMU數據為離散形式，不滿足連續積分，因此要對積分公式進行離散化處理，可得到關鍵幀之間的離散IMU預積分測量模型。

雖然通過增加IMU預積分因子、回環檢測因子和融合IMU位姿優化的激光慣性里程計因子可以使整體SLAM獲得穩定的位姿和狀態估計，但本研究主要是以室外環境為主，且多為較大場景結構或非結構化地形進行掃描，同時KITTI數據集下提供了較為精準的RTK-GPS軌跡真值。因此進一步考慮增加GPS約束，可以保證在室外環境下獲得絕對位置精度補償，更好地消除漂移問題。

3 基于滑動窗口的因子圖優化模型

通過前端基于IESKF完成IMU/Lidar緊耦合融合優化構建了IESKF-LIO因子，可以實現局部地圖模型下保持較高精度，進一步引入回環檢測因子、IMU預積分因子和GPS項約束，基于上述4種因子約束項，在后端融合建圖模塊。

首先將構建的IESKF-LIO作為初始位姿，當設備在動態運行不斷地獲取局部地圖觀測信息時，通過固定尺寸的滑動窗口計算并更新少數與當前觀測密切相關的點云數據，這樣可以減少多元約束融合模型的計算量。在動態滑動窗口下，設計基于增量式平滑建圖（ISAM）的因子圖融合方式，保證了在因子圖中每次加入新的觀測時不必重新計算整個因子圖的雅可比矩陣。最終的因子圖多源融合框架如圖2所示。融合因子包括激光慣性里程計因子、IMU預積分因子、GPS因子和回環檢測因子。

4 試驗與分析

針對運動位姿數據，本文通過使用EVO工具提供的兩種常見評價指標包括相對軌跡誤差（RPE）和絕對軌跡誤差（ATE）進行性能分析。通過使用不同的算法在數據集上獲得的地圖軌跡和真值軌跡進行誤差統計，可以非常直觀地反映各種算法之間的軌跡精度和誤差情況，從而更好地評價算法的全局一致性效果。

為了更加清晰直觀地體現本文構建算法的合理性，將針對KITTI數據集下07序列地圖，利用A-Loam、Lego-Loam、Lio-Sam和本文融合多因子圖模型算法方案進行對比分析。

在KITTI_07數據集下進行對比仿真，圖3所示是4種算法的軌跡運行情況的熱圖誤差對比分析。其中參考值是KITTI數據集提供的軌跡真值，彩色軌跡表示模型輸出軌跡，軌跡顏色越靠近紅色表示與真值誤差越大，越靠近藍色表示與真值軌跡越貼合，誤差越小（因本刊為黑白印刷，有疑問之處請向作者咨詢）。從圖3中可以看出，對比真實軌跡下的絕對誤差熱圖可以發現，本文所建的建圖模型與其他3種相比，具有更小的誤差最大值和誤差真值分布區間，表明了整體軌跡定位精度控制效果最優。

針對點云建圖的模型評估指標如圖4所示，主要包括軌跡誤差標準差（Std）、方均根誤差（RMSE）、最小軌跡偏移量（Min）、軌跡誤差中值（Median）、平均值（Mean）和最大值（Max），其中的標準差、方均根誤差和平均值皆取3次實驗的平均值，最大最小值取3次試驗的最值，各項指標的具體數值記錄在表1中。可以看出本文構建的模型在多個誤差指標中皆具有不同程度的優越性。本文所建模型的方均根誤差和誤差均值皆控制在0.5m左右，反映本文所建模型軌跡穩定性更高，防數據畸變效果更好。

5 建圖效果展示

本實驗的地圖環境為KITTI_07地圖模型，由于本文所建算法是基于多因子融合的方式進行數據融合，針對激光慣性里程計使用優化卡爾曼濾波方式進行構建，理論中可以明顯改善點云畸變問題，同時減少當前幀掃描點云下的偏移誤差累積，進而提高局部到全局地圖的融合精度。由于針對建圖效果沒有定量的指標分析評估手段，本小節僅通過地圖可視化分析建圖效果，如圖5所示。

從圖5中可以看出在本文所用算法構建的點云地圖與數據集提供的真實地圖基本重合。由于優化了點云掃描當前幀的去畸變效果，在動態建圖掃描中融合了多種因子進行補償糾正，在局部的彎道路徑中建圖效果表現較好，未出現點云重影或回環圖像不重合等現象，實現了較高精度的建圖效果。

6 結語

本文主要針對點云三維建圖進行相關研究。首先通過使用緊耦合的迭代誤差狀態卡爾曼濾波（IESKF）將雷達特征點云數據與IMU數據融合，構建激光慣性里程計獲得更精準的初始位姿信息。進一步使用優化因子圖方式實現多傳感器數據融合，所建融合因子包括：基于迭代擴展卡爾曼濾波融合的激光慣性里程計因子、IMU預積分因子、基于關鍵幀的回環因子與增加置信度評價的GPS因子。最后通過對上述整體模型在兩種KITTI數據集上進行對比仿真分析，結果表明本文所建融合模型的建圖效果較傳統方案有較高的精度和較為明顯的優化效果。

參考文獻：

［1］ SMITH R C，CHEESEMAN P. On the representation and estimation of spatial uncertainly［J］. International Journal of Robotics Research，1987，5（4）：56-68.

［2］ SMITH R，SELF M，CHEESEMAN P. Estimating uncertain spatial relationships in robotics［C］//Proceedings of 1987 IEEE International Conference on Robotics and Automation. Raleigh，NC，USA： IEEE，2003：850.

［3］ XU W，ZHANG F. FAST-LIO：a fast，robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter［J］. IEEE Robotics and Automation Letters，2021，6（2）：3317-3324.

［4］ CHANG L，NIU X J，LIU T Y. GNSS/IMU/ODO/LiDAR-SLAM integrated navigation system using IMU/ODO pre-integration［J］. Sensors，2020，20（17）：4702.

［5］ 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）. Madrid，Spain： IEEE，2018：4758-4765.

［6］ SHAN T X，ENGLOT B，MEYERS D，et al. LIO-SAM：tightly-coupled lidar inertial odometry via smoothing and mapping［C］//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS） ，Las Vegas，NV， USA： ［s.n.］，2020：5135-5142.

［7］ 付林，鄭佳楠，何洪磊，等. 面向大型室內場景的無人機三維激光雷達解耦SLAM方法［J］. 電子測量技術，2022，45（13）：96-103.

［8］ 劉振宇，惠澤宇，郭旭，等. 基于滑動窗口優化的激光雷達慣性測量單元緊耦合同時定位與建圖算法［J］. 科學技術與工程，2022，22（21）：9167-9175.

［9］ 袁艷，李峰，王東. 基于開關卡爾曼濾波的葉輪故障振動信號特征提取［J］. 機械制造與自動化，2024，53（2）：67-70.