基于因子圖的激光SLAM模型優(yōu)化算法

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

關鍵詞：SLAM；因子圖；IESKF；回環(huán)檢測

中圖分類號：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 引言

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

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

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

1 基于優(yōu)化卡爾曼濾波的LIO設計

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

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

2 關鍵因子構建

2.1 因子圖模型

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

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

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

2.2 改進最優(yōu)描述子的快速回環(huán)因子設計

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

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

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

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

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

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

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

2.3 IMU預積分因子、 GPS環(huán)境因子構建

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

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

3 基于滑動窗口的因子圖優(yōu)化模型

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

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

4 試驗與分析

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

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

在KITTI_07數(shù)據(jù)集下進行對比仿真，圖3所示是4種算法的軌跡運行情況的熱圖誤差對比分析。其中參考值是KITTI數(shù)據(jù)集提供的軌跡真值，彩色軌跡表示模型輸出軌跡，軌跡顏色越靠近紅色表示與真值誤差越大，越靠近藍色表示與真值軌跡越貼合，誤差越?。ㄒ虮究癁楹诎子∷ⅲ幸蓡栔幷埾蜃髡咦稍儯?。從圖3中可以看出，對比真實軌跡下的絕對誤差熱圖可以發(fā)現(xiàn)，本文所建的建圖模型與其他3種相比，具有更小的誤差最大值和誤差真值分布區(qū)間，表明了整體軌跡定位精度控制效果最優(yōu)。

針對點云建圖的模型評估指標如圖4所示，主要包括軌跡誤差標準差（Std）、方均根誤差（RMSE）、最小軌跡偏移量（Min）、軌跡誤差中值（Median）、平均值（Mean）和最大值（Max），其中的標準差、方均根誤差和平均值皆取3次實驗的平均值，最大最小值取3次試驗的最值，各項指標的具體數(shù)值記錄在表1中?？梢钥闯霰疚臉嫿ǖ哪Ｐ驮诙鄠€誤差指標中皆具有不同程度的優(yōu)越性。本文所建模型的方均根誤差和誤差均值皆控制在0.5m左右，反映本文所建模型軌跡穩(wěn)定性更高，防數(shù)據(jù)畸變效果更好。

5 建圖效果展示

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

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

6 結語

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

參考文獻：

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

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

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