馬寶良 ，崔麗珍 ，李敏超 ，張清宇

（內(nèi)蒙古科技大學(xué) 信息工程學(xué)院, 內(nèi)蒙古 包頭 014010）

0 引 言

“雙碳”目標(biāo)下[1]，煤炭依舊為我國(guó)主體能源，為響應(yīng)國(guó)家生態(tài)文明建設(shè)和實(shí)現(xiàn)高質(zhì)量發(fā)展，加快推進(jìn)綠色智能煤礦建設(shè)尤為重要[2-3]。煤炭開采環(huán)境復(fù)雜、條件惡劣，大量挖掘運(yùn)輸設(shè)備相互交叉運(yùn)行在礦場(chǎng)中，操作人員工作強(qiáng)度大、危險(xiǎn)系數(shù)高，安全問題難以保障[4]。因此，實(shí)現(xiàn)開采機(jī)械自動(dòng)化、無人駕駛礦車智能化在煤礦開采中的應(yīng)用是建設(shè)智能礦山的首要舉措[5-6]。同時(shí)定位與地圖構(gòu)建是實(shí)現(xiàn)采礦設(shè)備自動(dòng)化和采礦環(huán)境數(shù)字化的一種關(guān)鍵技術(shù)，該技術(shù)能夠讓機(jī)器人在未知的環(huán)境中，完成定位、建圖和路徑規(guī)劃[7]。

早期的SLAM 使用單一的激光雷達(dá)進(jìn)行定位，在非結(jié)構(gòu)化或復(fù)雜惡劣的環(huán)境下運(yùn)行時(shí)會(huì)造成點(diǎn)云地圖稀疏的問題。如REN[8]等在連續(xù)幀之間、連續(xù)關(guān)鍵幀之間和回環(huán)檢測(cè)幀之間使用GICP[9]配準(zhǔn)方法并使用了隨機(jī)抽樣一致性（Random Sample Consensus,RANSAC）對(duì)煤礦巷道點(diǎn)云進(jìn)行了分割，并將分割得到的巷道平面作為了觀測(cè)約束，實(shí)現(xiàn)了在結(jié)構(gòu)化的煤礦巷道環(huán)境低漂移定位與建圖。慣性測(cè)量單元能夠不受環(huán)境影響，實(shí)現(xiàn)高頻高精度位姿估計(jì)，但其在長(zhǎng)時(shí)間工作狀態(tài)下會(huì)導(dǎo)致自身誤差累積無法精確估計(jì)，因此將LiDAR 和IMU 進(jìn)行融合能夠?qū)崿F(xiàn)在復(fù)雜惡劣環(huán)境下長(zhǎng)時(shí)間高精度定位[10]。目前，國(guó)內(nèi)外學(xué)者在LiDAR/IMU 融合方面已經(jīng)進(jìn)行了大量研究，主要分為松耦合和緊耦合2 大類[11]。對(duì)LiDAR 和IMU 的測(cè)量數(shù)據(jù)分別處理后進(jìn)行加權(quán)融合確定運(yùn)動(dòng)狀態(tài)的方法被稱為松耦合，如ZHANG等[12]提出了激光里程計(jì)與建圖（LiDAR Odometry and Mapping，LOAM），該算法使用IMU 計(jì)算的姿態(tài)作為L(zhǎng)iDAR 掃描配準(zhǔn)的初始值，而未將其作為全局優(yōu)化的約束條件。后續(xù)對(duì)松耦合系統(tǒng)的研究大多延續(xù)了LOAM 算法框架，如SHAN 等[13]在LOAM 算法的基礎(chǔ)上增加了地面優(yōu)化，對(duì)具有相同類別的特征點(diǎn)進(jìn)行匹配，提高了融合計(jì)算效率，使得特征匹配更穩(wěn)定。XUE 等[14]在LeGO-LOAM 基礎(chǔ)上通過整合掃描上下文回環(huán)算法降低了點(diǎn)云維數(shù)，提高了在煤礦井下模擬場(chǎng)景下的實(shí)時(shí)性和魯棒性。

松耦合算法僅停留在數(shù)據(jù)結(jié)果層面分析，無法解決退化場(chǎng)景問題，為此，緊耦合算法使用圖優(yōu)化或者濾波的方式將LiDAR 與IMU 觀測(cè)數(shù)據(jù)融合，充分考慮了兩者間的內(nèi)在約束，系統(tǒng)的魯棒性和準(zhǔn)確性得到提升。YE 等提出的LIO-Mapping[15]使用VINS-Mono[16]中的優(yōu)化過程來最小化IMU 殘差和LiDAR 測(cè)量誤差，并以一種旋轉(zhuǎn)約束的方法來細(xì)化最終的地圖，但由于該約束的計(jì)算復(fù)雜性，系統(tǒng)難以實(shí)時(shí)運(yùn)行。SHAN 等[17]提出了基于圖優(yōu)化的LIOSAM 使用劃窗的方式來完成幀到局部子圖的掃描配準(zhǔn)，關(guān)鍵幀的選擇以及對(duì)舊幀的邊緣化有效降低了計(jì)算復(fù)雜性，提高了系統(tǒng)運(yùn)行速度。QIN 等基于濾波的LINS[18]提出了一種以機(jī)器人為中心，以誤差狀態(tài)進(jìn)行迭代卡爾曼濾波的融合算法。YANG 等[19]利用擾動(dòng)模型對(duì)煤礦長(zhǎng)巷道退化環(huán)境進(jìn)行了檢測(cè)和補(bǔ)償，IMU 預(yù)積分用于補(bǔ)償旋轉(zhuǎn)狀態(tài)退化，LiDAR 與IMU 融合產(chǎn)生的新姿態(tài)用于補(bǔ)償平移狀態(tài)退化，提高了煤礦巷道退化場(chǎng)景的魯棒性。XU 等在FAST-LIO[20]中提出了一個(gè)新的計(jì)算卡爾曼增益公式，顯著提高了計(jì)算效率。在此基礎(chǔ)之上的FASTLIO2[21]提出了一種新的數(shù)據(jù)結(jié)構(gòu)ikd-tree[22]，能夠高效動(dòng)態(tài)地對(duì)數(shù)據(jù)結(jié)構(gòu)進(jìn)行劃分，實(shí)現(xiàn)增量更新地圖，極大地提高了地圖管理效率。

盡管當(dāng)前SLAM 解決方案眾多，但是在露天煤礦環(huán)境下使用時(shí)的表現(xiàn)卻不盡如人意。該環(huán)境存在大量斜坡、碎石以及開采后產(chǎn)生的不規(guī)則單側(cè)石壁，算法會(huì)隨運(yùn)行距離增長(zhǎng)產(chǎn)生累計(jì)漂移問題，而且大多數(shù)SLAM 算法需要提取邊緣和平面等幾何特征進(jìn)行配準(zhǔn)，均采用LOAM 框架對(duì)海量點(diǎn)云進(jìn)行多次降采樣，導(dǎo)致配準(zhǔn)結(jié)果不準(zhǔn)確，影響定位精度以及建圖效果。另外，在這樣的環(huán)境下會(huì)出現(xiàn)分段式的相似環(huán)境，當(dāng)無法檢測(cè)到新的特征時(shí)LiDAR 會(huì)出現(xiàn)退化現(xiàn)象，一旦配準(zhǔn)失敗且經(jīng)過IMU 積分發(fā)散，SLAM的定位性能將迅速下降，導(dǎo)致建圖失敗。綜上所述，SLAM 如何在露天煤礦環(huán)境下保持系統(tǒng)的魯棒性和準(zhǔn)確性仍是一大難題。為此，提出了一種基于濾波與圖優(yōu)化相結(jié)合的緊耦合SLAM 算法，前端使用迭代擴(kuò)展卡爾曼濾波（Iterater Extended Kalman Filter,IEKF）來實(shí)現(xiàn)激光雷達(dá)與IMU 數(shù)據(jù)的融合，后端分別設(shè)計(jì)了雷達(dá)相對(duì)位姿因子和回環(huán)檢測(cè)因子來完成全局優(yōu)化，增強(qiáng)了在露天煤礦環(huán)境下對(duì)顛簸不平路況的魯棒性，實(shí)現(xiàn)了復(fù)雜環(huán)境下精確的定位建圖，為煤礦設(shè)備智能化提供了一種關(guān)鍵技術(shù)支撐。

1 系統(tǒng)框架

所提出的算法主要由前端迭代擴(kuò)展卡爾曼濾波和后端因子圖優(yōu)化兩部分組成。該算法使用LiDAR 和IMU 兩種傳感器作為數(shù)據(jù)輸入信息源。前端使用緊耦合的迭代擴(kuò)展卡爾曼濾波器將LiDAR 特征點(diǎn)與IMU 數(shù)據(jù)相融合，使用后向傳播來補(bǔ)償LiDAR 運(yùn)動(dòng)失真，經(jīng)過運(yùn)動(dòng)補(bǔ)償后與該時(shí)刻的特征點(diǎn)來構(gòu)造殘差，完成狀態(tài)更新。后端分別設(shè)計(jì)了雷達(dá)相對(duì)位姿因子和回環(huán)檢測(cè)因子，相對(duì)位姿因子作用于全局提供新的關(guān)位姿與局部子圖內(nèi)關(guān)鍵幀位姿之間的約束，并對(duì)當(dāng)前幀與局部子圖進(jìn)行匹配；回環(huán)檢測(cè)因子同樣作用于全局，檢測(cè)是否與歷史位姿重合并對(duì)相鄰關(guān)鍵幀進(jìn)行調(diào)整來，完成全局優(yōu)化。最后輸出優(yōu)化后的機(jī)器人軌跡，位姿以及全局點(diǎn)云地圖，系統(tǒng)結(jié)構(gòu)如圖1 所示。

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 System framework

2 系統(tǒng)模型描述

使用了文獻(xiàn)[23]中定義封裝了兩個(gè)算子?和?，?：S×Rn→S 及其逆?：S×S →Rn，其中 S為維數(shù)為n的流形（S=S O(3) ）。IMU 坐標(biāo)系記為I，一般假設(shè)IMU 與LiDAR 以外參ITL=(IRL,I pL)剛性連接在一起，則IMU 運(yùn)動(dòng)學(xué)模型如下：

其中，Δt為IMU 采樣時(shí)間間隔；f為函數(shù)；x為狀態(tài)；u為輸入；w為噪聲；定義如下：

3 激光慣性里程計(jì)模型構(gòu)建

使用IEKF 來估計(jì)式(2) 中的狀態(tài)。假設(shè)LiDAR 在時(shí)刻的掃描最優(yōu)狀態(tài)估計(jì)值是，則系統(tǒng)的隨機(jī)誤差狀態(tài)向量為

3.1 狀態(tài)傳播

IMU 測(cè)量時(shí)，按照式(2)將過程噪聲設(shè)置為零進(jìn)行傳播得到下式：

傳播協(xié)方差以迭代誤差動(dòng)態(tài)模型來處理，定義如下[19]：

傳播一直進(jìn)行到tk這一幀結(jié)束，傳播狀態(tài)以及協(xié)方差分別記為和，將白噪聲w的協(xié)方差定義為Q，則傳播協(xié)方差可以通過下式計(jì)算得出：

當(dāng)點(diǎn)云累積時(shí)間間隔達(dá)到tk時(shí)，將新產(chǎn)生的特征點(diǎn)集與式(9)中的傳播狀態(tài)和協(xié)方差進(jìn)行融合來產(chǎn)生最優(yōu)的狀態(tài)。對(duì)于采集一幀點(diǎn)云時(shí)間內(nèi)產(chǎn)生的時(shí)間偏差所造成的運(yùn)動(dòng)畸變，可以用高于IMU 測(cè)量值的后向傳播進(jìn)行補(bǔ)償。后向傳播可以在采樣時(shí)間ρj以及掃描結(jié)束時(shí)間tk之間，產(chǎn)生一個(gè)相對(duì)位姿。該相對(duì)位姿可以將局部測(cè)量投影到掃描結(jié)束時(shí)的測(cè)量位置：

3.2 狀態(tài)更新

xk先驗(yàn)分布由前向傳播得到：

其中，K為卡爾曼增益，隨著狀態(tài)更新，每個(gè)特征點(diǎn)（）將會(huì)投影到全局地圖中，實(shí)現(xiàn)地圖更新。

4 全局因子圖優(yōu)化

理論上前端的狀態(tài)更新后已經(jīng)可以獲得完整的激光雷達(dá)慣性里程計(jì)，但在實(shí)際試驗(yàn)過程中，上述得到的定位與建圖結(jié)果隨著系統(tǒng)運(yùn)行會(huì)產(chǎn)生偏移，并且這種偏移是不可逆的。這個(gè)問題在露天煤礦環(huán)境下顯得尤為突出，圖優(yōu)化的方式可以非常簡(jiǎn)潔方便的描述各個(gè)誤差項(xiàng)之間的關(guān)聯(lián)，因此本文后端使用因子圖來進(jìn)行全局位姿圖優(yōu)化，通過對(duì)殘差模型進(jìn)行分析，最終目的就是最小化一個(gè)殘差項(xiàng)。構(gòu)建了雷達(dá)相對(duì)位姿和回環(huán)檢測(cè)兩個(gè)因子來實(shí)現(xiàn)全局位姿優(yōu)化。

1）雷達(dá)相對(duì)位姿因子。在前端激光雷達(dá)慣性里程計(jì)中采用了幀到子圖的掃描配準(zhǔn)方式，并且通過滑動(dòng)窗口的方式來創(chuàng)建固定關(guān)鍵幀數(shù)量的子圖。機(jī)械式雷達(dá)的運(yùn)行頻率一般為10 Hz，相鄰幀之間變化量極小但卻需要消耗海量的計(jì)算資源，將每一幀都添加到因子圖中進(jìn)行優(yōu)化顯然是低效的，因此本文采用了選取關(guān)鍵幀的策略。關(guān)鍵幀的選取主要遵循兩個(gè)閾值指標(biāo)進(jìn)行確立：①最小平移閾值d當(dāng)系統(tǒng)接收到的當(dāng)前幀與上一幀的最小平移距離超過d時(shí)選取當(dāng)前幀為關(guān)鍵幀，這樣可以避免設(shè)備移動(dòng)緩慢甚至?xí)和r(shí)產(chǎn)生大量冗余激光幀。②最小旋轉(zhuǎn)角 α，當(dāng)接受到的當(dāng)前幀與上一幀的姿態(tài)旋轉(zhuǎn)角度變化值大于 α?xí)r選取該幀為關(guān)鍵幀，由于機(jī)械雷達(dá)掃描覆蓋范圍較大，該閾值的設(shè)定可避免設(shè)備在顛簸路面的微量轉(zhuǎn)動(dòng)提取到太多的激光點(diǎn)云數(shù)據(jù)。滿足上述任一指標(biāo)即可確立當(dāng)前幀為關(guān)鍵幀，利用關(guān)鍵幀來維護(hù)一個(gè)固定滑窗，即局部子圖。接著對(duì)新到來的關(guān)鍵幀進(jìn)行特征提取，利用該幀以及局部子圖來構(gòu)建以下代價(jià)函數(shù)求解位姿變換關(guān)系：

de和dp分別為新一幀提取到的邊緣特征和平面特征與其在局部子圖中相鄰幀提取到對(duì)應(yīng)特征之間的距離，以此構(gòu)建相鄰位姿間的優(yōu)化目標(biāo)：

其中，x為待優(yōu)化量；和分別為第幀和相鄰第j幀對(duì)應(yīng)運(yùn)因子動(dòng)狀態(tài)。

2）回環(huán)檢測(cè)。SLAM 系統(tǒng)運(yùn)行時(shí)，各個(gè)傳感器誤差會(huì)不斷積累，這一誤差難以消除并且會(huì)逐漸增大，導(dǎo)致定位結(jié)果漂移。回環(huán)檢測(cè)因子的使用對(duì)于糾正漂移誤差以及構(gòu)建全局一致的地圖具有重要作用[25]。此外，在露天煤礦環(huán)境下往來礦用卡車基本會(huì)走固定路線進(jìn)行運(yùn)輸，重復(fù)運(yùn)輸?shù)穆肪€使得檢測(cè)到回環(huán)的幾率大幅增加，因此本文將回環(huán)檢測(cè)因子加入因子圖，與雷達(dá)相對(duì)位姿因子共同進(jìn)行對(duì)全局位姿的優(yōu)化。

當(dāng)新的關(guān)鍵幀確立后，首先設(shè)定最小距離閾值，對(duì)因子圖內(nèi)的全部歷史關(guān)鍵幀進(jìn)行搜索，找到與新關(guān)鍵幀在歐氏距離上滿足最小閾值的關(guān)鍵幀，設(shè)其為待匹配幀。使用GICP 對(duì)新幀與對(duì)應(yīng)待匹配幀進(jìn)行配準(zhǔn)，通過式(19)計(jì)算出最近點(diǎn)均方根的最小距離獲得置信分?jǐn)?shù)，同時(shí)與設(shè)定的最小距離閾值進(jìn)行比較，確認(rèn)該幀是否為回環(huán)關(guān)鍵幀，確定后可利用式(20)計(jì)算出該位姿的相對(duì)變換并添加到因子圖中。該步驟需要嚴(yán)格設(shè)置最小距離閾值，一旦回環(huán)檢測(cè)匹配到錯(cuò)誤的關(guān)鍵幀就會(huì)導(dǎo)致地圖崩潰。

5 試驗(yàn)和討論

為驗(yàn)證文中算法的有效性以及魯棒性，我們分別在公共開源數(shù)據(jù)集以及露天煤礦實(shí)測(cè)數(shù)據(jù)集上進(jìn)行了試驗(yàn)。開源數(shù)據(jù)集選擇了M2DGR[26]：一個(gè)帶有全套傳感器的地面機(jī)器人對(duì)上海交通大學(xué)校園內(nèi)不同場(chǎng)景采集的大規(guī)模數(shù)據(jù)集，所有傳感器都經(jīng)過良好的外部校準(zhǔn)和時(shí)間同步。露天煤礦實(shí)測(cè)數(shù)據(jù)集使用的數(shù)據(jù)采集設(shè)備如圖2 所示，該設(shè)備主要搭載傳感器為速騰聚創(chuàng)RS-LiDAR-16 型激光雷達(dá)，10 Hz工作模式下水平分辨率為0.2°，垂直分辨率為2°；超核電子CH110 型9 軸IMU，設(shè)置采樣頻率為400 Hz；北斗星通C200 衛(wèi)星接收機(jī)；機(jī)載計(jì)算機(jī)為NVIDIA Jetson AGX Xavier，8 核ARM 處理器，主頻2.26 GHz，內(nèi)存32 G。提出的SLAM 算法均使用C++實(shí)現(xiàn)，運(yùn)行于Ubuntu20.04，ROS 版本為noetic。

圖2 數(shù)據(jù)采集設(shè)備Fig.2 Data collection equipment

5.1 開源數(shù)據(jù)集試驗(yàn)

使用開源數(shù)據(jù)集對(duì)目前主流激光SLAM 算法：A-LOAM、LeGO-LOAM、FAST-LIO2、LIO-SAM 與本文所提算法進(jìn)行了測(cè)試，通過計(jì)算最終絕對(duì)位姿誤差(Absolute Position Error，APE)來計(jì)算各個(gè)算法的定位精度。所有算法均在相同配置環(huán)境下使用，運(yùn)行設(shè)備為Intel(R) i9-10850K 3.60 GHz。分別選取了3 個(gè)不同場(chǎng)景下采集的數(shù)據(jù)street_08、door_02和gate_02 序列進(jìn)行試驗(yàn)，使用EVO[27]作為最終結(jié)果評(píng)估工具。其中street08 序列為戶外環(huán)境，采集過程中存在大量連續(xù)轉(zhuǎn)彎，圖3 所示為各個(gè)算法在該序列上的定位軌跡對(duì)比結(jié)果。

圖3 street_08 序列各算法軌跡對(duì)比Fig.3 Trajectory comparison of each algorithm for the street_08 sequence

從圖3 中可以看出A-LOAM、LeGO-LOAM 表現(xiàn)較差，運(yùn)行一段時(shí)間后開始出現(xiàn)明顯漂移，LIOSAM 同樣存在該問題，但繞行一周后成功檢測(cè)到了回環(huán)，糾正了累計(jì)漂移，仍取得較好的結(jié)果，F(xiàn)ASTLIO2 對(duì)新的數(shù)據(jù)結(jié)構(gòu)的應(yīng)用可以在不提取特征的情況下可以快速直接地將原始點(diǎn)配準(zhǔn)到地圖上，在短距離結(jié)構(gòu)化的環(huán)境下，精度較高。算法所設(shè)計(jì)的相對(duì)位姿因子有效抑制了IMU 累計(jì)誤差，同時(shí)后期檢測(cè)到了回環(huán)，表現(xiàn)良好。各算法在所選序列試驗(yàn)結(jié)果的APE 見表1，其中加粗項(xiàng)表示該項(xiàng)測(cè)試中誤差項(xiàng)最小的結(jié)果，對(duì)應(yīng)箱型圖如圖4 所示。

表1 開源數(shù)據(jù)集APE 誤差Table 1 Open source dataset APE error

圖4 開源數(shù)據(jù)集APE 誤差數(shù)據(jù)集箱型圖Fig.4 Open source dataset APE box plot

5.2 露天礦山實(shí)測(cè)數(shù)據(jù)集試驗(yàn)

為進(jìn)一步驗(yàn)證算法的有效性以及魯棒性，對(duì)內(nèi)蒙古哈爾烏素露天煤礦進(jìn)行了數(shù)據(jù)采集，實(shí)地環(huán)境如圖5 所示。該環(huán)境道路崎嶇不平，沿途布滿山石碎塊，呈現(xiàn)分段相似性，且結(jié)構(gòu)復(fù)雜無明顯的幾何特征。在采集的數(shù)據(jù)集中選取了兩個(gè)序列進(jìn)行試驗(yàn)分析，各算法配置均與開源數(shù)據(jù)集試驗(yàn)配置相同。

圖5 露天礦山環(huán)境Fig.5 Open pit mine environmental

序列1 全長(zhǎng)2 024 m，為實(shí)時(shí)觀測(cè)到礦山數(shù)據(jù)誤差累積情況，對(duì)序列1 進(jìn)行了分段切片處理，分別截取為500、1 000 和2 000 m。圖6 分別展示了不同算法在兩處不同地點(diǎn)的建圖效果。

圖6 序列1 建圖效果Fig.6 Sequence 1 mapping results

從圖6 建圖效果中能夠看出LeGO-LOAM 和FAST-LIO2 存在明顯的漂移現(xiàn)象，雖完成了最終建圖，但是誤差較大。A-LOAM 與LIO-SAM 無直觀地漂移現(xiàn)象，但累計(jì)定位誤差較大。本文所提出的算法完整地構(gòu)建出了高精度點(diǎn)云地圖，對(duì)碎石巖壁結(jié)構(gòu)建圖清晰，掃描到的礦卡輪廓直觀無重影，具有較高的一致性。針對(duì)露天煤礦環(huán)境體現(xiàn)出了較高的魯棒性。

序列2 全長(zhǎng)2 120 m，各算法運(yùn)行出的定位軌跡如圖7 所示。A-LOAM 和LeGO-LOAM 屬于單一激光雷達(dá)建圖算法，缺少IMU 的輔助出現(xiàn)了較大的累計(jì)誤差，尤其是輕量化LiDAR 里程計(jì)LeGOLOAM 嚴(yán)重依賴地面特征提取，在露天煤礦這樣的環(huán)境下，大量斜坡的存在導(dǎo)致該算法嚴(yán)重失效，狀態(tài)估計(jì)出現(xiàn)了較大誤差。FAST-LIO2 默認(rèn)不使用特征提取的方法，降采樣之后直接將原始點(diǎn)云配準(zhǔn)到地圖上來體現(xiàn)全局一致性，但是在碎石巖壁與顛簸道路環(huán)境中，原始點(diǎn)云經(jīng)過降采樣后呈現(xiàn)高度一致性，導(dǎo)致幾處相似場(chǎng)景出現(xiàn)了較大的漂移現(xiàn)象。

圖7 序列2 軌跡對(duì)比Fig.7 Sequence 2 trajectory comparison result

為避免這些問題，提出的算法將激光雷達(dá)幀間配準(zhǔn)結(jié)果作為約束因子在后端與回環(huán)因子進(jìn)行聯(lián)合優(yōu)化，進(jìn)一步提升了SLAM 系統(tǒng)的魯棒性，同時(shí)提高了定位建圖精度。各算法在所選序列試驗(yàn)結(jié)果的APE 見表2，其中加粗項(xiàng)表示該項(xiàng)測(cè)試中誤差項(xiàng)最小的結(jié)果，其對(duì)應(yīng)箱型圖如圖8 所示。

表2 露天煤礦數(shù)據(jù)集APE 誤差Table 2 open pit coal mine dataset APE error

圖8 露天煤礦數(shù)據(jù)集APE 誤差箱線圖Fig.8 Open pit coal mine dataset APE box plot

6 結(jié) 論

1）提出了一種針對(duì)露天煤礦復(fù)雜環(huán)境下具有高精度高魯棒性的LiDAR/IMU 緊耦合SLAM 算法，前端使用緊耦合的迭代擴(kuò)展卡爾曼將LiDAR 特征點(diǎn)與IMU 數(shù)據(jù)融合，后端使用因子圖來接收關(guān)鍵幀位姿狀態(tài)，將激光雷達(dá)幀間配準(zhǔn)結(jié)果作為約束因子并與回環(huán)檢測(cè)因子共同完成全局優(yōu)化。

2）利用該算法在開源數(shù)據(jù)集M2DGR 的3 個(gè)不同場(chǎng)景和露天煤礦實(shí)地環(huán)境進(jìn)行了試驗(yàn)測(cè)試，結(jié)果表明，算法在開源數(shù)據(jù)集的城市化環(huán)境下精度表現(xiàn)與當(dāng)前的SOAT 激光SLAM 算法保持一致，在長(zhǎng)達(dá)兩千多米的露天煤礦實(shí)地環(huán)境下所提算法較FASTLIO2、LIO-SAM 緊耦合算法在定位精度上分別提高了46.00%和23.15%，提高了復(fù)雜環(huán)境下的定位精度與魯棒性。