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

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

中圖分類(lèi)號(hào)：TP391.9" 文獻(xiàn)標(biāo)志碼：A" 文章編號(hào)：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 引言

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

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

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

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

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

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

2 關(guān)鍵因子構(gòu)建

2.1 因子圖模型

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

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

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

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

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

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

將Scan-Context獲取的環(huán)向量作為描述符，每個(gè)環(huán)向量都被用來(lái)構(gòu)造KD-Tree，在KD-Tree中通過(guò)最近鄰搜索選取與當(dāng)前關(guān)鍵幀環(huán)鍵相似的環(huán)鍵對(duì)應(yīng)的關(guān)鍵幀作為回環(huán)候選關(guān)鍵幀。

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

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

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

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

2.3 IMU預(yù)積分因子、 GPS環(huán)境因子構(gòu)建

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

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

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

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

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

4 試驗(yàn)與分析

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

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

在KITTI_07數(shù)據(jù)集下進(jìn)行對(duì)比仿真，圖3所示是4種算法的軌跡運(yùn)行情況的熱圖誤差對(duì)比分析。其中參考值是KITTI數(shù)據(jù)集提供的軌跡真值，彩色軌跡表示模型輸出軌跡，軌跡顏色越靠近紅色表示與真值誤差越大，越靠近藍(lán)色表示與真值軌跡越貼合，誤差越?。ㄒ虮究癁楹诎子∷?，有疑問(wèn)之處請(qǐng)向作者咨詢(xún)）。從圖3中可以看出，對(duì)比真實(shí)軌跡下的絕對(duì)誤差熱圖可以發(fā)現(xiàn)，本文所建的建圖模型與其他3種相比，具有更小的誤差最大值和誤差真值分布區(qū)間，表明了整體軌跡定位精度控制效果最優(yōu)。

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

5 建圖效果展示

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

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

6 結(jié)語(yǔ)

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

參考文獻(xiàn)：

［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)場(chǎng)景的無(wú)人機(jī)三維激光雷達(dá)解耦SLAM方法［J］. 電子測(cè)量技術(shù)，2022，45（13）：96-103.

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

［9］ 袁艷，李峰，王東. 基于開(kāi)關(guān)卡爾曼濾波的葉輪故障振動(dòng)信號(hào)特征提?。跩］. 機(jī)械制造與自動(dòng)化，2024，53（2）：67-70.