面向煤礦巷道環(huán)境的LiDAR與IMU融合定位與建圖方法

馬艾強(qiáng)，姚頑強(qiáng)，藺小虎，張聯(lián)隊(duì)，鄭俊良，武謀達(dá)，楊鑫

（1.西安科技大學(xué) 測(cè)繪科學(xué)與技術(shù)學(xué)院，陜西 西安 710054；2.陜西彬長(zhǎng)礦業(yè)集團(tuán)有限公司，陜西 咸陽(yáng) 712000）

0 引言

煤礦事故發(fā)生后，井下存在不確定和危險(xiǎn)因素，依靠人工救援成功率低，煤礦井下移動(dòng)機(jī)器人可替代救援人員深入礦井探測(cè)災(zāi)害現(xiàn)場(chǎng)情況，檢查事故現(xiàn)場(chǎng)，并在第一時(shí)間將獲得的環(huán)境信息傳輸?shù)街笓]中心，制定救援策略[1]，以提高救援效率[2]。然而，傳統(tǒng)的導(dǎo)線測(cè)量[3]、貫通設(shè)計(jì)、三維激光掃描等技術(shù)方案測(cè)量速度慢、效率低，嚴(yán)重制約了煤礦井下環(huán)境建模，且實(shí)施過程中難以保證建模精度[4]。近年來，激光雷達(dá)（Light Detection and Ranging，LiDAR）、慣性測(cè)量單元（Inertial Measurement Unit，IMU）和全球?qū)Ш?衛(wèi) 星 系 統(tǒng)（Global Navigation Satellite System，GNSS）[5]的移動(dòng)激光雷達(dá)平臺(tái)被廣泛用于獲取三維地理信息數(shù)據(jù)[6]。煤礦井下環(huán)境復(fù)雜，無GNSS信號(hào)，工況惡劣，地面常規(guī)定位與建圖技術(shù)無法直接應(yīng)用，因此采用同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）[7]技術(shù)輔助煤礦井下移動(dòng)機(jī)器人執(zhí)行路徑規(guī)劃、自主探索、自主導(dǎo)航任務(wù)[8]?；赟LAM技術(shù)礦井機(jī)器人可以實(shí)時(shí)準(zhǔn)確地構(gòu)建煤礦井下三維地圖，該地圖可以靈活、可靠地輔助礦井機(jī)器人進(jìn)行智能導(dǎo)航和避障，并應(yīng)用于礦井下危險(xiǎn)區(qū)作業(yè)、自動(dòng)巡檢、遠(yuǎn)程調(diào)度等領(lǐng)域，對(duì)實(shí)現(xiàn)智能化定位導(dǎo)航起著至關(guān)重要作用[9-10]。然而，在煤礦井下噴漿表面、對(duì)稱巷道環(huán)境中，單一傳感器的SLAM技術(shù)已不能滿足智能感知精度及可靠性的需求。因此，多源傳感器融合的SLAM技術(shù)應(yīng)用在煤礦井下成為了當(dāng)前學(xué)術(shù)研究與工程應(yīng)用的聚焦點(diǎn)。

目前，LiDAR和IMU的融合方法主要分為松耦合和緊耦合2種方法。松耦合類方法[11]分別處理LiDAR和IMU的位姿估計(jì)，然后再進(jìn)行融合。文獻(xiàn)[12]提出了利用IMU信息來輔助估計(jì)激光里程計(jì)，IMU積分狀態(tài)為激光點(diǎn)云匹配提供位姿初值，該方法缺乏回環(huán)抑制漂移誤差。緊耦合類方法[13]對(duì)LiDAR和IMU的數(shù)據(jù)進(jìn)行互相估計(jì)與更新。文獻(xiàn)[14]將IMU預(yù)積分因子、激光里程計(jì)因子、GNSS因子和閉環(huán)因子進(jìn)行聯(lián)合優(yōu)化，取得了較好的建圖效果，但并未考慮煤礦環(huán)境對(duì)稱巷道退化對(duì)SLAM結(jié)果的影響。文獻(xiàn)[15]提出用誤差卡爾曼濾波對(duì)IMU進(jìn)行預(yù)測(cè)，并與LiDAR進(jìn)行緊融合的方法，該方法用激光雷達(dá)高程圖和先驗(yàn)數(shù)字高程圖進(jìn)行匹配，并用匹配值對(duì)狀態(tài)進(jìn)行更新，該方法展示了在長(zhǎng)距離無GNSS信號(hào)場(chǎng)景下的導(dǎo)航能力，但需要先驗(yàn)地圖，無法應(yīng)對(duì)無先驗(yàn)地圖的場(chǎng)景。文獻(xiàn)[16]提出了一種基于因子圖的LiDAR和IMU緊耦合算法，通過最小化IMU預(yù)積分和LiDAR點(diǎn)云匹配結(jié)果的殘差來優(yōu)化LiDAR位姿和IMU偏置，根據(jù)IMU的預(yù)積分值對(duì)LiDAR位姿進(jìn)行線性插值，去除點(diǎn)云畸變，實(shí)驗(yàn)表明該方法可應(yīng)對(duì)LiDAR退化的場(chǎng)景和快速運(yùn)動(dòng)的情況，但效率較低，實(shí)時(shí)性較差，而且基于勻速假設(shè)的去畸變策略無法適用于運(yùn)動(dòng)變化較快的場(chǎng)景。文獻(xiàn)[17-18]提出了一種緊耦合算法，在畸變?nèi)コ糠?，該算法使用高斯過程回歸對(duì)IMU預(yù)積分測(cè)量值進(jìn)行采樣，利用采樣值對(duì)LiDAR位姿進(jìn)行插值，完成畸變?nèi)コ?，取得了很好效果，但該算法?jì)算效率較低。文獻(xiàn)[19]提出了一種MC2SLAM的3D LiDAR和IMU緊耦合里程計(jì)算法，該算法使用因子圖融合3D LiDAR和IMU，在畸變?nèi)コ糠?，使用一種基于勻速假設(shè)的非剛性點(diǎn)云匹配算法，通過最小化點(diǎn)云匹配的殘差優(yōu)化LiDAR位姿，再對(duì)優(yōu)化得到的位姿進(jìn)行線性插值，但是該算法的特征點(diǎn)選取策略過于簡(jiǎn)單且存在隨機(jī)性，精度不夠穩(wěn)定。

為解決發(fā)生煤礦事故后救援機(jī)器人難以自主導(dǎo)航定位的問題，本文提出了一種面向煤礦巷道環(huán)境的LiDAR與IMU融合的實(shí)時(shí)定位與建圖方法，用于探測(cè)礦井災(zāi)害現(xiàn)場(chǎng)情況，提高煤礦事故救援效率。該方法通過數(shù)據(jù)預(yù)處理、點(diǎn)云掃描匹配和后端因子圖優(yōu)化等多層次的數(shù)據(jù)融合，降低軌跡漂移，增強(qiáng)救援機(jī)器人系統(tǒng)在煤礦巷道環(huán)境中快速移動(dòng)時(shí)的魯棒性，實(shí)現(xiàn)高精度定位與建圖。

1 SLAM基本框架

SLAM基本框架由預(yù)處理、激光里程計(jì)、地圖構(gòu)建3個(gè)部分組成，如圖1所示。首先對(duì)原始點(diǎn)云進(jìn)行分割，利用IMU預(yù)積分位姿去除原始點(diǎn)云非線性運(yùn)動(dòng)畸變，并對(duì)得到的點(diǎn)云進(jìn)行線、面特征提取。然后將相鄰幀的線、面特征進(jìn)行匹配，在分層位姿估計(jì)過程中融合IMU預(yù)積分所得到的位姿初值，減少計(jì)算迭代次數(shù)，提高特征點(diǎn)匹配的精度，解算出當(dāng)前幀的位姿。最后向因子圖中插入局部地圖因子、IMU因子、關(guān)鍵幀因子，對(duì)位姿進(jìn)行優(yōu)化約束，對(duì)關(guān)鍵幀與局部地圖進(jìn)行匹配，通過八叉樹結(jié)構(gòu)實(shí)現(xiàn)地圖構(gòu)建。

圖1 SLAM基本框架Fig.1 SLAM basic framework

2 基于特征點(diǎn)匹配的相對(duì)位姿估計(jì)

2.1 預(yù)處理

在實(shí)際作業(yè)環(huán)境中，搭載LiDAR的移動(dòng)機(jī)器人不可避免產(chǎn)生非線性運(yùn)動(dòng)，導(dǎo)致掃描的激光點(diǎn)云存在非線性運(yùn)動(dòng)畸變，因此，利用高頻率的IMU狀態(tài)對(duì)激光點(diǎn)進(jìn)行補(bǔ)償校正[16]。因?yàn)槊旱V巷道地面特征點(diǎn)和墻壁特征點(diǎn)對(duì)于垂直方向有一定的約束，且平面特征對(duì)水平方向的平移和旋轉(zhuǎn)有較好的約束，所以先用基于隨機(jī)樣本一致性（Random Sample Consensus，RANSAC）[20]的快速點(diǎn)云分割算法提取環(huán)境中的巷道地面點(diǎn)和墻壁的平面特征。特征提取部分利用深度圖計(jì)算某一點(diǎn)的曲率，將曲率較大的非地面點(diǎn)標(biāo)記為線特征點(diǎn)，將曲率較小的非地面點(diǎn)標(biāo)記為平面特征點(diǎn)[21]。

2.2 激光里程計(jì)

在提取特征的前后幀點(diǎn)云中，首先利用KDtree[22]搜索方法尋找最近鄰相似屬性的特征點(diǎn)，并將法向和主方向夾角均小于5°且距離小于1 cm的特征點(diǎn)作為同一類特征點(diǎn)。建立點(diǎn)到線和點(diǎn)到面距離最小的約束，進(jìn)行分層位姿估計(jì)，如圖2所示。由于煤礦巷道的面特征比線特征多，占據(jù)位姿權(quán)重較大，數(shù)據(jù)可信度高，因此，選擇源點(diǎn)云面特征和目標(biāo)點(diǎn)云面特征點(diǎn)去對(duì)應(yīng)匹配位姿，確定垂向平移量、橫滾角、俯仰角，得出一個(gè)較準(zhǔn)確的位姿。再根據(jù)源點(diǎn)云線特征和目標(biāo)點(diǎn)云線特征匹配確定側(cè)向平移量、前向平移量和航向角，從而達(dá)到整體優(yōu)化位姿精度。

圖2 分層位姿估計(jì)Fig.2 Hierarchical pose estimation

3 多因子聯(lián)合圖優(yōu)化及地圖構(gòu)建

3.1 關(guān)鍵幀因子提取

為確保本文算法在煤礦井下實(shí)時(shí)運(yùn)行，在前端里程計(jì)中部署了關(guān)鍵幀選取策略，T1—T6為連續(xù)幀，?T為連續(xù)幀T1—T6之間的位姿變化，選擇T1和T6為關(guān)鍵幀，如圖3所示。

圖3 關(guān)鍵幀選取Fig.3 Key frames selection

關(guān)鍵幀的選取可極大地提高計(jì)算效率，確保點(diǎn)云能夠快速匹配。選取關(guān)鍵幀的標(biāo)準(zhǔn)是給歐氏距離?S 、關(guān)鍵幀旋轉(zhuǎn)矩陣?R、時(shí)間間隔?t設(shè)定一個(gè)閾值，任意滿足其一，則當(dāng)前第k幀點(diǎn)云被選取為候選關(guān)鍵幀。選擇關(guān)鍵幀時(shí)，必須減少匹配錯(cuò)誤幀和冗余關(guān)鍵幀，以達(dá)到節(jié)省計(jì)算量的目的。在保證精度的前提下減少數(shù)據(jù)冗余，以提高運(yùn)算效率。因此，基于實(shí)時(shí)點(diǎn)云關(guān)鍵幀選擇標(biāo)準(zhǔn)，將第1幀點(diǎn)云作為判斷準(zhǔn)則開始選擇關(guān)鍵幀。連續(xù)關(guān)鍵幀的相對(duì)變換位姿為

式中：?Tk?1,k為第 k?1幀到第k幀關(guān)鍵幀的位姿變化；Tk?1為 第k?1幀 的關(guān)鍵幀；Tk為第 k 幀的關(guān)鍵幀；?q為第 k?1幀 到第 k 幀關(guān)鍵幀的旋轉(zhuǎn)矩陣；?d，?e，?f分別為在X軸、Y軸、Z軸的位移變化；tk為第k幀關(guān)鍵幀的時(shí)間戳。

在確定關(guān)鍵幀后，再以初始相對(duì)變換位姿優(yōu)化點(diǎn)云配準(zhǔn)，最終變換的位姿應(yīng)用于位姿和位姿圖中位置之間的約束。

3.2 多因子位姿優(yōu)化及地圖構(gòu)建

假設(shè)LiDAR第i（i=1,2，…，j，j為關(guān)鍵幀總次數(shù)）幀關(guān)鍵幀的掃描起始時(shí)刻為ti，掃描得到的全部點(diǎn)云為Pi；IMU在內(nèi)采集的數(shù)據(jù)為I(i,j)。系統(tǒng)在ti時(shí)刻的姿態(tài)、位置、速度和IMU零點(diǎn)偏置為式中：xi為關(guān)鍵幀在ti時(shí)刻的狀態(tài)；Ri為李群表示的旋轉(zhuǎn)矩陣；vi為速度；bi為零點(diǎn)偏差。

設(shè)在tk時(shí)刻的所有關(guān)鍵幀為Kk，其對(duì)應(yīng)的狀態(tài)量其對(duì)應(yīng)的所有觀測(cè)量

在給定觀測(cè)量Zk和先驗(yàn)信息 p(χ0)的條件下，預(yù)測(cè)關(guān)鍵幀Kk對(duì) 應(yīng)的初始狀態(tài)量χ0的后驗(yàn)概率問題為狀態(tài)估計(jì)問題。

變量因子最大后驗(yàn)概率為

式中：χt為t時(shí)刻的狀態(tài)量；r為觀測(cè)模型與實(shí)際觀測(cè)的殘差，是關(guān)于狀態(tài)量χk的函數(shù)；Kt為t時(shí)刻的關(guān)鍵幀；Γ為對(duì)應(yīng)的協(xié)方差矩陣。

根據(jù)IMU動(dòng)力學(xué)模型，采用離散化積分方法對(duì)加速度和角速度在IMU采樣間隔時(shí)間?t內(nèi)積分，將高頻輸出的加速度和角速度觀測(cè)量轉(zhuǎn)換為狀態(tài)量間的位姿變換，構(gòu)成關(guān)鍵幀狀態(tài)量之間的約束因子。

式中：?Rij，?vij，?sij分 別為在?t 內(nèi)第i ～j幀關(guān)鍵幀之間的旋轉(zhuǎn)矩陣、速度、位移；Rj為第j幀關(guān)鍵幀的旋轉(zhuǎn)矩陣；vi，vj分別為第i，j幀關(guān)鍵幀的速度；?Rik為?t 內(nèi)第i～k幀關(guān)鍵幀之間的旋轉(zhuǎn)矩陣；分別為第k幀關(guān)鍵幀的角速度、加速度；bak， bgk分別為IMU的零偏加速度與零偏角速度；si，sj分別為第i，j幀關(guān)鍵幀的位移；g為重力加速度；η為觀測(cè)噪聲。

假設(shè)關(guān)鍵幀在狀態(tài)xi與xj的IMU零點(diǎn)偏置保持不變，則可由式（9）?式（11）得預(yù)積分觀測(cè)模型。

由式（12）?式（14）可推導(dǎo)出殘差項(xiàng) rI(i,j)。

多因子圖優(yōu)化模塊是在系統(tǒng)中維護(hù)一個(gè)全局因子圖，因子圖結(jié)構(gòu)如圖4所示。設(shè)給定初始狀態(tài)為x0，采用數(shù)據(jù)預(yù)處理模塊計(jì)算相鄰關(guān)鍵幀之間的IMU預(yù)積分，并向因子圖插入IMU預(yù)積分因子rI(i,j)。

圖4 多因子圖優(yōu)化Fig.4 Multi-factor graph optimization

式中mj為第j幀的關(guān)鍵幀局部地圖。

在因子圖中插入關(guān)鍵幀因子rp(i,j)。

當(dāng)提取一個(gè)連續(xù)幀作為關(guān)鍵幀時(shí)，根據(jù)連續(xù)關(guān)鍵幀的位姿估計(jì)，將關(guān)鍵幀點(diǎn)云匹配進(jìn)行地圖的更新。首先利用線、面特征匹配求得LiDAR坐標(biāo)中連續(xù)關(guān)鍵幀之間的位姿，然后通過坐標(biāo)變換更新關(guān)鍵幀位姿，并將其點(diǎn)云轉(zhuǎn)換到全局坐標(biāo)系中，最后通過八叉樹結(jié)構(gòu)對(duì)點(diǎn)云圖進(jìn)行更新。

4 實(shí)驗(yàn)結(jié)果和分析

4.1 實(shí)驗(yàn)設(shè)備及數(shù)據(jù)介紹

為了測(cè)試本文所提方法性能，自主搭建Autolabor 、VLP-16 LiDAR和Ellipse-N IMU的實(shí)驗(yàn)平臺(tái)，如圖5所示。本次實(shí)驗(yàn)中LiDAR頻率為10 Hz，IMU頻率為200 Hz，2個(gè)傳感器采用硬同步方式進(jìn)行時(shí)間對(duì)齊。煤礦井下移動(dòng)機(jī)器人可進(jìn)行原地旋轉(zhuǎn)，最大旋轉(zhuǎn)角速度為0.523 5 rad/s，最大位移速度為1.5 m/s。移動(dòng)機(jī)器人系統(tǒng)采用C++實(shí)現(xiàn)，在機(jī)器人操作系統(tǒng)（Robot operation system，ROS）中運(yùn)行。利用全站儀記錄移動(dòng)機(jī)器人經(jīng)過參考點(diǎn)時(shí)的絕對(duì)位置，定量分析移動(dòng)機(jī)器人位姿精度。實(shí)驗(yàn)所采集的數(shù)據(jù)分別有煤礦巷道和樓道數(shù)據(jù)，具體信息見表1。

圖5 實(shí)驗(yàn)平臺(tái)Fig.5 Experimental platform

表1 實(shí)測(cè)數(shù)據(jù)Table1 Measured data

4.2 實(shí)驗(yàn)結(jié)果分析

為驗(yàn)證本文所提方法的定位性能與建圖效果，在煤礦巷道A和樓道走廊B中選取起點(diǎn)和終點(diǎn)相同的閉環(huán)環(huán)境，定量計(jì)算各方案的累計(jì)位姿誤差，定性評(píng)價(jià)各方案的建圖效果及起點(diǎn)和終點(diǎn)的激光里程計(jì)軌跡閉合情況。

為了評(píng)價(jià)不同方法的絕對(duì)定位精度，對(duì)煤礦巷道場(chǎng)景A所示的10個(gè)控制點(diǎn)（A0?A9）和樓道走廊場(chǎng)景B所示的6個(gè)控制點(diǎn)（B0?B5）的坐標(biāo)真值（圖6）進(jìn)行測(cè)量。分別與目前主流的LeGO?LOAM，LIO?SAM方法進(jìn)行對(duì)比。通過全站儀測(cè)量真值，并記錄移動(dòng)機(jī)器人在對(duì)應(yīng)控制點(diǎn)位置停止區(qū)間時(shí)刻的起點(diǎn)和終點(diǎn)，將其時(shí)間段內(nèi)的位置估計(jì)結(jié)果求取均值，作為當(dāng)前方法在該點(diǎn)的觀測(cè)值。全站儀實(shí)測(cè)數(shù)據(jù)作為判斷移動(dòng)機(jī)器人定位精度的依據(jù)。

圖6 場(chǎng)景A（參考點(diǎn)A0?A9）和場(chǎng)景B（參考點(diǎn)B0?B5）的坐標(biāo)真值Fig.6 Coordinate ture value of scenario A （reference points A0-A9）and scenario B（reference points B0-B5）

4.2.1 定位精度分析

為分析移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中的定位精度，給移動(dòng)機(jī)器人粘貼標(biāo)志點(diǎn)，分別在場(chǎng)景A（A0?A9）、場(chǎng)景B（B0?B5）處，使用全站儀測(cè)量其真值，并對(duì)3種方法絕對(duì)定位誤差分布效果進(jìn)行分析，如圖7所示。在場(chǎng)景A中可看出，LeGO?LOAM方法定位精度較差，在X方向上最大絕對(duì)定位誤差為300 cm；LIO?SAM方法定位精度雖高于LeGO?LOAM方法，但在三軸方向的絕對(duì)定位誤差在50 cm附近；本文方法在三軸方向的絕對(duì)定位誤差的均值和中值均小于32 cm。這是因?yàn)辄c(diǎn)云匹配之后進(jìn)行了分層位姿估計(jì)，多因子優(yōu)化可有效降低全局累計(jì)誤差，對(duì)軌跡精度和地圖的一致性的提升具有重要作用。在場(chǎng)景B中可看出，LeGO?LOAM方法在Z軸的誤差均值為50～55 cm，X、Y軸誤差均值為30～35 cm；LIO?SAM方法三軸誤差均值為15～25 cm；本文方法三軸的誤差均值為5～15 cm，誤差范圍小，精度高。

圖7 場(chǎng)景A、B的絕對(duì)定位誤差分布Fig.7 Absolute positioning error distribution of scenarios A and B

3種方法的累計(jì)誤差見表2。在場(chǎng)景A中可看出，LeGO?LOAM方法在狹小的煤礦巷道點(diǎn)云匹配發(fā)生退化，造成三軸的累計(jì)誤差很大，位置偏差達(dá)到49.16 m，使得建圖效果發(fā)生了漂移；本文方法和LIO?SAM方法的定位精度相對(duì)較高，而本文方法對(duì)X軸的位姿估計(jì)精度最高，其累計(jì)誤差為1.65 m，位置偏差為2.97 m，從三軸或累計(jì)誤差來看，本文方法建圖效果整體良好，建圖軌跡未發(fā)生漂移。在場(chǎng)景B中可看出，LeGO?LOAM方法在X、Y軸的累計(jì)誤差均小于1.5 m，Z軸誤差達(dá)到了2.33 m，位置累計(jì)偏差為2.95 m；LIO?SAM方法在X、Y軸的累計(jì)誤差分別為2.21，3.15 m，Z軸誤差僅為1.05 m，位置偏差為3.99 m；本文方法在X、Y、Z軸的誤差均小于1.01 m，累計(jì)位置偏差僅為1.67 m。綜合分析得出，在較少特征的樓道中，LeGO?LOAM方法與LIO?SAM方法點(diǎn)云匹配時(shí)發(fā)生退化，導(dǎo)致X、Y、Z軸的平移誤差較大，點(diǎn)云模型相差較大，建圖定位精度較差。本文方法應(yīng)用于樓道退化場(chǎng)景依舊有較強(qiáng)的魯棒性，定位精度高，有良好的建圖效果。

表2 3種方法的累計(jì)誤差Table 2 Cumulative error of there methods

4.2.2 建圖效果分析

為了進(jìn)一步說明3種方法在煤礦巷道和樓道定位的建圖效果，分別在煤礦巷道與樓道環(huán)境進(jìn)行驗(yàn)證，建圖結(jié)果與軌跡如圖8和圖9所示。

圖8 煤礦巷道場(chǎng)景A 中3種方法建圖結(jié)果與軌跡Fig.8 Mapping resultsand tracksof three methods in coal mine roadway scenarios A

圖9 樓道走廊場(chǎng)景B中3種方法建圖結(jié)果與軌跡Fig.9 Mapping results and tracks of three methods in corridor scenarios B

由圖8可看出，LeGO?LOAM方法由于點(diǎn)云誤匹配出現(xiàn)大的漂移現(xiàn)象，建圖失效；LIO?SAM方法可以構(gòu)建出大致輪廓的點(diǎn)云地圖，但由于Z軸上出現(xiàn)整體明顯漂移，導(dǎo)致移動(dòng)機(jī)器人所生成的點(diǎn)云地圖結(jié)構(gòu)變形且不完整；本文方法構(gòu)建的點(diǎn)云地圖，在完整性和幾何結(jié)構(gòu)真實(shí)性方面均有著優(yōu)秀的表現(xiàn)，可以直觀地反映巷道環(huán)境的實(shí)際情況，在煤礦井下環(huán)境具有良好的魯棒性。

由圖9可看出，采用LeGO?LOAM方法進(jìn)行建圖時(shí)，軌跡重復(fù)，整體發(fā)生偏移，移動(dòng)機(jī)器人在回到終點(diǎn)時(shí)，軌跡無法閉合，建圖失效；采用LIO?SAM方法進(jìn)行建圖時(shí)，隨著激光里程計(jì)里程的增加，由于部分場(chǎng)景相似，導(dǎo)致建立了錯(cuò)誤的回環(huán)約束，發(fā)生漂移，以致點(diǎn)云地圖構(gòu)建失敗；采用本文方法進(jìn)行建圖時(shí)，地圖完整性與環(huán)境匹配均有良好的性能，這是由于通過增加關(guān)鍵幀因子，插入因子圖對(duì)其新增節(jié)點(diǎn)相關(guān)變量進(jìn)行優(yōu)化，降低了位姿估計(jì)漂移，定位與建圖精度相對(duì)較高。

5 結(jié)論

（1）提出了一種LiDAR與IMU融合的實(shí)時(shí)定位與建圖方法。該方法首先采用面特征匹配估計(jì)位姿，然后進(jìn)行線特征匹配，實(shí)現(xiàn)激光序列幀的相對(duì)位姿估計(jì)，提高點(diǎn)云匹配精度，最后在因子圖優(yōu)化與地圖構(gòu)建過程中插入關(guān)鍵幀因子、IMU預(yù)積分因子、局部地圖配準(zhǔn)因子，對(duì)LiDAR和IMU數(shù)據(jù)進(jìn)行整體優(yōu)化，提高了實(shí)時(shí)定位與建圖的精度，降低了煤礦環(huán)境下SLAM的累計(jì)誤差。

（2）該方法在自采數(shù)據(jù)集上分別進(jìn)行了定量定性分析，并與現(xiàn)有的LeGO?LOAM，LIO?SAM方法進(jìn)行比較。結(jié)果表明：①在煤礦巷道環(huán)境中，本文方法三軸的絕對(duì)定位誤差的均值和中值均小于32 cm；X軸的位姿估計(jì)精度最高，其累計(jì)誤差為1.65 m，位置偏差為2.97 m，建圖效果整體良好，建圖軌跡未發(fā)生漂移；構(gòu)建的點(diǎn)云地圖，在完整性和幾何結(jié)構(gòu)真實(shí)性方面均有著優(yōu)秀的表現(xiàn)，可以直觀地反映巷道環(huán)境的實(shí)際情況，具有良好的魯棒性。②在樓道走廊環(huán)境中，本文方法三軸的誤差均小于1.01 m，誤差均值為5～15 cm，誤差范圍小，精度高；累計(jì)位置偏差僅為1.67 m；地圖完整性與環(huán)境匹配均有良好的性能。