鄒筱瑜，黃鑫淼，王忠賓，房東圣，潘杰，司壘

（1.中國礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州 221116；2.中國礦業(yè)大學(xué) 江蘇省礦山機(jī)電裝備高校重點(diǎn)實(shí)驗(yàn)室，江蘇 徐州 221116）

0 引言

煤礦井下同步定位與三維地圖構(gòu)建技術(shù)可為井下移動機(jī)器人提供定位先驗(yàn)信息、本體路徑規(guī)劃與自主避障全局導(dǎo)航地圖、執(zhí)行機(jī)構(gòu)作業(yè)目標(biāo)位置信息等[1]。精確的地圖構(gòu)建是實(shí)現(xiàn)煤礦巷道移動機(jī)器人自主行走、導(dǎo)航避障、作業(yè)施工的關(guān)鍵技術(shù)[2-3]。因此，研究煤礦巷道三維地圖構(gòu)建方法具有重要意義。

同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）是一種廣泛應(yīng)用于移動機(jī)器人、無人駕駛等領(lǐng)域的關(guān)鍵技術(shù)，可對處于未知環(huán)境的移動機(jī)器人進(jìn)行姿態(tài)估計和定位?；诩す饫走_(dá)的三維SLAM算法可以實(shí)現(xiàn)圖像、物理模型等信息的融合，從而提高機(jī)器人操作系統(tǒng)的定位精度[4]。但是，三維點(diǎn)云數(shù)據(jù)量巨大，導(dǎo)致計算開銷較大。Zhang Ji等[5]提出了實(shí)時激光雷達(dá)測距與建圖（Lidar Odometry and Mapping in Real-time,LOAM）算法，開創(chuàng)性地引入視覺SLAM 中常用的特征點(diǎn)提取思想，大大減少了前端點(diǎn)云配準(zhǔn)的時間和計算開銷?；趫D優(yōu)化框架的激光2D/3D?SLAM算法?Cartographer，W.Hess等[6]采用基于子圖構(gòu)建全局地圖的思想，利用非線性優(yōu)化庫實(shí)現(xiàn)了圖優(yōu)化框架在激光SLAM 領(lǐng)域的應(yīng)用嘗試。Shan Tixiao等[7]提出了輕量級及地面優(yōu)化激光雷達(dá)測距與建圖（Lightweight and Ground-Optimized LOAM,LeGO?LOAM）算法，該算法基于LOAM 思想，進(jìn)一步簡化了特征點(diǎn)提取，通過剔去地面點(diǎn)來減少計算量，并通過2步優(yōu)化分別計算6個自由度，達(dá)到了與LOAM相近的性能且實(shí)時性更強(qiáng)。Shan Tixiao等[8]在文獻(xiàn)[7]的基礎(chǔ)上提出了基于平滑建圖的雷達(dá)慣性里程計（Lidar Inertial Odometry via Smoothing and Mapping，LIO?SAM）算法，通過激光雷達(dá)?慣性測量單元（Inertial Measurement Unit，IMU）?里程計緊耦合的方式增強(qiáng)了算法的魯棒性。目前大部分的三維激光SLAM算法通常采用前端構(gòu)建和后端優(yōu)化的策略，且多數(shù)前端構(gòu)建都采取了迭代最近點(diǎn)（Iterative Closest Point,ICP）算法或其衍生算法[9]。ICP算法可滿足大多數(shù)三維點(diǎn)云配準(zhǔn)要求，其運(yùn)行速度和精度主要取決于給定的初始變換估計、點(diǎn)云大小和初始位置精度[10]。Chen Hui等[11]提出了2步ICP算法，先利用ICP算法對初始點(diǎn)云數(shù)據(jù)進(jìn)行粗配準(zhǔn)，將得到的變換矩陣作為初始變換再次應(yīng)用到ICP算法中，進(jìn)行精細(xì)配準(zhǔn)，提高了點(diǎn)云配準(zhǔn)效率。為了克服上述ICP算法存在的問題，He Shijun等[12]提出了一種基于主成分分析的ICP方法，通過變換數(shù)據(jù)的維度為ICP提供良好的初始值，然而，當(dāng)點(diǎn)云中存在許多噪聲點(diǎn)時，該方法很難獲得較好的初始值。事實(shí)上，ICP在處理大規(guī)模點(diǎn)云配準(zhǔn)和特征點(diǎn)較少的點(diǎn)云時會出現(xiàn)擬合時間過長、運(yùn)動估計不準(zhǔn)的問題，在井下長直巷道三維地圖構(gòu)建中往往會面臨激光里程計失效情況。近年來，正態(tài)分布變換 （Normal-Distributions Transform, NDT）算法逐漸引起了研究者們的關(guān)注[13]。NDT算法使用標(biāo)準(zhǔn)的優(yōu)化技術(shù)來確定點(diǎn)云的最佳配準(zhǔn)，基于概率分布，不使用對應(yīng)點(diǎn)的特征來計算點(diǎn)云的配準(zhǔn)，對點(diǎn)云數(shù)據(jù)的適用能力強(qiáng)，但是其精度通常低于ICP算法。K.Koide[14]等提出了基于便攜式三維激光雷達(dá)的長期和廣域人行為測量系統(tǒng)，采用了 NDT配準(zhǔn)的接口。

針對井下特殊環(huán)境的激光SLAM 算法，Li Menggang等[15]提出了融合NDT激光里程計約束、環(huán)境中的平面特征約束及回環(huán)檢測約束的NDT?graph?SLAM，解決了井下移動機(jī)器人高效定位與地圖構(gòu)建的問題。Xu Jiachang等[16]結(jié)合復(fù)雜的煤礦環(huán)境，融合無人直升機(jī)IMU、激光探測與測距（LiDAR）系統(tǒng)和深度相機(jī)等，提出了基于概率膜計算的煤礦巡邏無人機(jī)SLAM方法。

上述方法為巷道環(huán)境定位與建圖提供了可行的解決方案，但針對煤礦長直巷道移動機(jī)器人的三維地圖構(gòu)建還存在如下問題：①目前的研究主要聚焦于多傳感融合建圖方法，較少關(guān)注激光SLAM算法建圖精度的提升。②長直巷道環(huán)境退化嚴(yán)重，缺少特征點(diǎn)，傳統(tǒng)激光點(diǎn)云匹配算法和激光里程計易失效，導(dǎo)致煤礦巷道建圖精度受限。

由于ICP算法在處理大規(guī)模點(diǎn)云配準(zhǔn)和特征點(diǎn)較少的點(diǎn)云時會出現(xiàn)擬合時間過長、運(yùn)動估計不準(zhǔn)等問題，在井下長直巷道三維地圖構(gòu)建中常常會面臨機(jī)器人在運(yùn)動而激光里程計認(rèn)為沒有運(yùn)動的情況，導(dǎo)致激光里程計失效。而 NDT算法對點(diǎn)云數(shù)據(jù)的適用性強(qiáng)，對數(shù)據(jù)特征點(diǎn)不敏感，且計算速度快[17]，井下長直巷道幾何特征少但點(diǎn)云分布特征明顯，在三維地圖構(gòu)建中可利用 NDT 算法對ICP算法進(jìn)行補(bǔ)充，但NDT算法精度通常低于ICP算法。因此，可在構(gòu)建激光里程計約束因子時，集成ICP特征點(diǎn)配準(zhǔn)方法和 NDT相似度配準(zhǔn)方法，提升建圖效果。本文針對煤礦巷道移動機(jī)器人建圖中面臨的上述問題，提出了一種基于集成式因子圖優(yōu)化的煤礦巷道移動機(jī)器人三維地圖構(gòu)建（ICP and NDT Ensemble SLAM，INE?SLAM）方法，集成ICP和NDT算法進(jìn)行因子圖優(yōu)化，以提高煤礦巷道移動機(jī)器人激光SLAM建圖精度。設(shè)計了INE?SLAM的前端點(diǎn)云配準(zhǔn)模塊和基于濾波、圖優(yōu)化的后端優(yōu)化方法，構(gòu)建了INE?SLAM的因子圖優(yōu)化模型，分析了ICP相對位姿因子、NDT相對位姿因子的特性。利用公開數(shù)據(jù)集、模擬煤礦巷道點(diǎn)云數(shù)據(jù)集開展了實(shí)驗(yàn)，驗(yàn)證了INE?SLAM 方法的有效性和優(yōu)越性。

1 相關(guān)理論

1.1 ICP算法原理

ICP算法被廣泛應(yīng)用于點(diǎn)云匹配中，常用于解決2個點(diǎn)云集合之間的轉(zhuǎn)換關(guān)系[10-12,18]。

假設(shè)激光雷達(dá)在井下探測到2組點(diǎn)云，分別用{Pi}和{Qi} 表示點(diǎn)云集合，其中{Qi}為參考幀，{ Pi}為當(dāng)前幀的點(diǎn)云，共有 N 個點(diǎn)。需要計算從{Pi}到{Qi}的變化參數(shù)，即旋轉(zhuǎn)和位移。如果變換參數(shù)是準(zhǔn)確的，那么點(diǎn)云{Pi}中的每一個點(diǎn) pi經(jīng)過變換后應(yīng)該與點(diǎn)云{Qi} 中的點(diǎn)qi完全重合，即 qi=hpi+t（h為旋轉(zhuǎn)矩陣，t為位移矩陣）。但是，由于井下噪聲的影響，不能使各個點(diǎn)都完全一致。因此，定義目標(biāo)函數(shù)為

使目標(biāo)函數(shù)最小的h和t即為所求變換參數(shù)。

1.2 NDT算法原理

NDT算法與ICP算法一樣是點(diǎn)云配準(zhǔn)的經(jīng)典算法之一。該算法基本思路為構(gòu)造多維變量正態(tài)分布，若變換參數(shù)為2個點(diǎn)云最優(yōu)匹配時，變換點(diǎn)概率密度達(dá)到最大值。為此，通過優(yōu)化找到使概率密度總和達(dá)到最大值的變換參數(shù)，其核心思想：將點(diǎn)云離散化，如果是二維平面，則離散成方格，如果是三維空間，則離散成立方體，使取樣的點(diǎn)云被分割成若干個格子，從而便于描述其局部特征，如點(diǎn)云局部的形狀（直線、平面或球體）、方向（平面法向、直線方向）等[13-16,19]。

2 INE?SLAM方法

2.1 INE?SLAM方法框架

煤礦井巷高精度定位和地圖構(gòu)建是自主移動機(jī)器人亟需解決的關(guān)鍵技術(shù)問題。為解決井下長直巷道特征點(diǎn)缺失、激光里程計失效的難題，本文提出了一種INE?SLAM方法。構(gòu)建激光里程計約束因子時，集成ICP特征點(diǎn)配準(zhǔn)方法和 NDT相似度配準(zhǔn)方法，以降低激光里程計累計誤差，并通過因子圖優(yōu)化得到一致性較高的三維地圖。該方法可提升基于激光雷達(dá)的煤礦巷道三維地圖構(gòu)建的精度及魯棒性。

INE?SLAM方法框架如圖1所示，分為前端構(gòu)建和后端優(yōu)化2個部分，包括點(diǎn)云預(yù)處理模塊、特征點(diǎn)匹配模塊、相似度匹配模塊和位姿圖優(yōu)化模塊。其中，特征點(diǎn)匹配模塊和相似度匹配模塊分別通過2種不同的點(diǎn)云配準(zhǔn)方法，構(gòu)建2種不同的約束因子提供給后端優(yōu)化。

圖1 INE?SLAM方法框架Fig.1 Framwork of ICPand NDT ensemble SLAM method

2.2 點(diǎn)云預(yù)處理

由于三維激光雷達(dá)傳回的每一幀數(shù)據(jù)數(shù)量巨大，尤其是井下巷道空間狹小，由于物體反射會產(chǎn)生很多無效數(shù)據(jù)，如果直接對所有點(diǎn)云數(shù)據(jù)進(jìn)行計算，將耗費(fèi)大量的計算資源甚至出現(xiàn)數(shù)據(jù)報錯。點(diǎn)云預(yù)處理模塊首先通過濾波、下采樣2步操作濾除點(diǎn)云的噪聲點(diǎn)，在不改變點(diǎn)云性質(zhì)的情況下減少計算量。然后通過點(diǎn)云矯正算法對雷達(dá)運(yùn)動產(chǎn)生的運(yùn)動畸變進(jìn)行矯正。最后將預(yù)處理后的點(diǎn)云數(shù)據(jù)輸出給特征點(diǎn)匹配模塊和相似度匹配模塊。點(diǎn)云預(yù)處理相關(guān)操作可參考文獻(xiàn)[20-21]。綜合考慮井下巷道的環(huán)境特點(diǎn)和激光雷達(dá)測量噪聲，本文主要需要濾除與被觀測物體有顯著偏差的離群點(diǎn)，利用統(tǒng)計濾波器能很好地解決離群點(diǎn)問題。

激光雷達(dá)穿透回來的原始點(diǎn)云數(shù)據(jù)通常比較稠密，尤其在井下巷道，豎直墻壁為主要特征，過多重復(fù)的點(diǎn)云給后續(xù)配準(zhǔn)工作的計算帶來挑戰(zhàn)。針對該問題，提出了一種體素濾波和體素格化處理相結(jié)合的方法。

機(jī)械旋轉(zhuǎn)式掃描激光雷達(dá)運(yùn)動時不可避免地存在點(diǎn)云扭曲，為了避免這一現(xiàn)象的發(fā)生，必須對扭曲進(jìn)行補(bǔ)償，以得到完整的點(diǎn)云數(shù)據(jù)。在煤礦巷道這類作業(yè)低速低位移場景中，以激光里程計常速運(yùn)動假設(shè)為前提，采用各點(diǎn)相應(yīng)時間戳對激光點(diǎn)云進(jìn)行插值還原，該方法效率非常高，效果良好。

2.3 特征點(diǎn)匹配

特征點(diǎn)匹配模塊采用與LOAM 系列算法相似的思想，對點(diǎn)云的曲率進(jìn)行計算，提取出曲率大的角點(diǎn)和曲率較小的平面點(diǎn)，利用PL?ICP（Point-to-Line ICP）和PP?ICP（Point-to-Plane ICP）算法，估計移動機(jī)器人的運(yùn)動并計算出變換矩陣及需要構(gòu)建的約束因子參數(shù)。相似度匹配模塊采用基于概率的 NDT算法，通過劃分體素塊，計算各個體素塊的均值和協(xié)方差矩陣，利用點(diǎn)云的分布情況計算運(yùn)動變換，進(jìn)而計算約束因子的參數(shù)。特征點(diǎn)匹配模塊和相似度匹配模塊輸出的關(guān)鍵幀攜帶移動機(jī)器人的位姿信息，與約束因子信息一起輸入位姿圖優(yōu)化模塊，通過時間幀對齊，相互約束，相互補(bǔ)充。為提取出當(dāng)前點(diǎn)云中的線特征及面特征，利用類似LOAM[5]和LeGO?LOAM[7]算法的特征提取方法，對局部區(qū)域點(diǎn)云進(jìn)行曲率計算。激光雷達(dá)在某個掃描周期 K 內(nèi)獲得的點(diǎn)云記為 PK，這期間對應(yīng)的雷達(dá)坐標(biāo)系定義為LK，點(diǎn)云中第i個點(diǎn) 在LK下的位置坐標(biāo)可表示為XKL,i。S 為點(diǎn)云 PK中點(diǎn) pi所在行中點(diǎn)組成的連續(xù)點(diǎn)集，此處設(shè)置|S |為10，即點(diǎn) pi兩側(cè)各有5個點(diǎn)。可以設(shè)計以下公式來評價點(diǎn) pi所在局部表面的曲率：

采用上述曲率計算方法可對點(diǎn)云排序并找到曲率最小的m個點(diǎn)，作為平面點(diǎn)和曲率最大的n個點(diǎn)，即邊緣點(diǎn)。

激光雷達(dá)頻率為10 Hz，若利用每一個雷達(dá)掃描幀來匹配特征和在因子圖上加入相對雷達(dá)因子，存在多余幀帶來極少新變換信息、造成巨大計算量等問題，不利于實(shí)時工作。針對此問題，設(shè)定最小平移距離閾值和最小旋轉(zhuǎn)角度閾值，其中一個閾值滿足條件，則當(dāng)前雷達(dá)幀為新關(guān)鍵幀。在相鄰關(guān)鍵幀間去除雷達(dá)掃描幀，將有助于降低后續(xù)優(yōu)化時的內(nèi)存消耗及地圖構(gòu)建時的存儲消耗，進(jìn)而保持因子圖的稀疏性，便于進(jìn)行實(shí)時非線性優(yōu)化。

特征匹配主要依據(jù)上述特征點(diǎn)，利用ICP 算法對2次連續(xù)掃描間的相對移動進(jìn)行匹配估計。2次掃描間的變換通過點(diǎn)與邊、點(diǎn)與平面間掃描匹配完成，即要從上一幀特征點(diǎn)集中找出相應(yīng)特征關(guān)系?？赏ㄟ^文獻(xiàn)[9]、文獻(xiàn)[18]中的方法，迭代計算出2幀數(shù)據(jù)間的位姿變換矩陣為第k幀的位姿），得到各幀的相對位姿，并提取關(guān)鍵幀的位姿作為后續(xù)輸出到圖優(yōu)化模塊的約束因子。

2.4 相似度匹配

將點(diǎn)云預(yù)處理后的第1幀數(shù)據(jù)作為參考幀，輸入 NDT算法作為初值，并作為第1幀關(guān)鍵幀。對點(diǎn)云進(jìn)行體素分割，分成A個體素塊，并對每個體素塊中的點(diǎn)云計算均值 μ和協(xié)方差矩陣 Σ：

式中Xa為第a個體素塊中的點(diǎn)云數(shù)據(jù)。

將最近一幀的點(diǎn)云數(shù)據(jù)投影到參考幀上，并對這一幀點(diǎn)云數(shù)據(jù)計算均值μ和協(xié)方差矩陣Σ，根據(jù)式（1）得到2幀點(diǎn)云之間變換矩陣[h| t]。由此可得當(dāng)前幀f的位姿：

式中 ?Tk?1,f為當(dāng)前幀f和前一個關(guān)鍵幀k?1之間的位姿變換矩陣。

假定移動機(jī)器人運(yùn)動狀態(tài)改變?yōu)榈退賱蛩贍顟B(tài)，可將前次相對變換用作NDT配準(zhǔn)初值以達(dá)到快速計算收斂的目的。當(dāng)移動機(jī)器人當(dāng)前幀相對于前一個關(guān)鍵幀位姿變化超過設(shè)定閾值時，就會造成NDT 不收斂，前一個關(guān)鍵幀及相對變換就會用來計算當(dāng)前輸入點(diǎn)云的里程。當(dāng)前幀f和前一個關(guān)鍵幀k?1之間的位姿變換矩陣可表示如下：

式中：?h為 旋轉(zhuǎn)變化；?t為位移變化。

與特征點(diǎn)匹配的關(guān)鍵幀提取相似，設(shè)置3個閾值來減少冗余關(guān)鍵幀，包括最小平移距離閾值、最小旋轉(zhuǎn)角度閾值、最小運(yùn)行時間閾值，只要滿足任意一個閾值就將當(dāng)前幀輸出為關(guān)鍵幀加入之后的因子圖優(yōu)化。

2.5 因子圖模型構(gòu)建

因子圖是一種概率圖模型，是一種無向圖。首先由F.Kschischang[22]提出并用于解決SLAM 問題。因子圖有2種節(jié)點(diǎn)，分別為表示優(yōu)化變量的變量節(jié)點(diǎn)和表示因子的因子節(jié)點(diǎn)。

從概率圖理論來看，基于因子圖的SLAM可以利用最大后驗(yàn)估計（Maximum a Posterior，MAP）進(jìn)行求解[23]。因子圖的優(yōu)化與位姿圖優(yōu)化類似，都可以使用稀疏QR分解、Schur分解或Cholesky 分解進(jìn)行求解。不同的是，M.Kaess等[24-25]提出的iSAM（Incremental Smoothing and Mapping）中對因子圖的優(yōu)化進(jìn)行了全新解讀，認(rèn)為可以增量式地處理優(yōu)化因子圖。傳統(tǒng)圖優(yōu)化要面臨的問題：當(dāng)移動機(jī)器人運(yùn)動后，位姿圖中得到新的節(jié)點(diǎn)和邊時，就要計算所有的節(jié)點(diǎn)來更新變量，計算量較大。文獻(xiàn)[24-25]提出的方法能夠增量優(yōu)化因子圖，只需計算新節(jié)點(diǎn)影響到的節(jié)點(diǎn)，減少了不必要的計算量，加速了優(yōu)化流程。在iSAM 的基礎(chǔ)上，F(xiàn).Dellaert[26]建立了GTSAM （Georgia Tech Smoothing and Mapping）優(yōu)化庫，方便其他研究人員使用。基于上述因子圖優(yōu)化模型目標(biāo)函數(shù)的構(gòu)建方法，可以構(gòu)建多方法集成式因子圖。構(gòu)建方法如圖2所示。

圖2 集成式因子圖構(gòu)建Fig.2 Construction of integrated factor graph

煤礦巷道多方法集成激光里程計的無約束優(yōu)化目標(biāo)函數(shù)定義為

式中：χ為待優(yōu)化的位姿參數(shù)；?priorχ為先驗(yàn)因子的代價項；?ICP(χ)為 使用ICP配準(zhǔn)的代價項；?NDT(χ)為使用NDT配準(zhǔn)的代價項。

式中：rprior和Hprior分別為先驗(yàn)因子的殘差和海森矩陣；w 為先驗(yàn)幀的索引；u和v為關(guān)鍵幀；rICP為ICP配準(zhǔn)的殘差；為ICP觀測向量；和分別為ICP和NDT方法所得前后2幀的協(xié)方差矩陣；為NDT配準(zhǔn)的殘差； z′kk+1為NDT觀測向量。

通過ICP配準(zhǔn)方法得到2幀之間的位姿相對變換，可以構(gòu)建ICP相對位姿因子，提供給新的關(guān)鍵幀與已優(yōu)化的關(guān)鍵幀位姿之間的約束。構(gòu)建的ICP相對位姿之間的優(yōu)化目標(biāo)項為

式中： x 為優(yōu)化變量；xu為關(guān)鍵幀u的索引對應(yīng)的運(yùn)動狀態(tài)；xv為關(guān)鍵幀v 的索引對應(yīng)的運(yùn)動狀態(tài)；ΣT為相對位姿變換協(xié)方差矩陣，ΣT∈ R6×6，R為旋轉(zhuǎn)矩陣。

在多方法集成過程中，狀態(tài)變量是基于移動機(jī)器人本體坐標(biāo)系。因此，轉(zhuǎn)換到移動機(jī)器人本體坐標(biāo)系下的相對位姿觀測為

式中：TBL為 移動機(jī)器人本體坐標(biāo)系到雷達(dá)坐標(biāo)系O的外參變換；為關(guān)鍵幀u和v 對應(yīng)的激光慣性里程計坐標(biāo)系下的待估計位姿，∈S E(3)（三維歐氏空間的數(shù)學(xué)表示）。

與ICP相對位姿因子構(gòu)建方法類似，通過 NDT相似度配準(zhǔn)方法得到的2幀之間的位姿相對變換，可以構(gòu)建 NDT相對位姿因子，提供給新關(guān)鍵幀與已優(yōu)化關(guān)鍵幀位姿之間的約束。構(gòu)建的NDT相對位姿之間的優(yōu)化目標(biāo)項為

3 方法驗(yàn)證

為驗(yàn)證INE?SLAM 方法的有效性，將在公開數(shù)據(jù)集和模擬煤礦巷道點(diǎn)云數(shù)據(jù)集中開展實(shí)驗(yàn)。

3.1 KITTI數(shù)據(jù)集實(shí)驗(yàn)

采用目前廣泛使用的KITTI數(shù)據(jù)集[27]進(jìn)行實(shí)驗(yàn)，以便于方法對比與復(fù)現(xiàn)。將本文提出的INE?SLAM方法分別與基于特征點(diǎn)匹配的A?LOAM方法、基于平面分割及特征點(diǎn)提取的LeGO?LOAM 方法進(jìn)行對比?；贙ITTI 里程計數(shù)據(jù)集的原始數(shù)據(jù)序列00，01，05，09進(jìn)行了對比實(shí)驗(yàn)，并與數(shù)據(jù)集提供的真實(shí)位姿數(shù)據(jù)進(jìn)行對比。3種方法的軌跡對比如圖3所示。

圖3 KITTI 數(shù)據(jù)集上實(shí)驗(yàn)結(jié)果對比Fig.3 Comparison of experimental resultson KITTI dataset

從圖3可看出，在序列00，05，09數(shù)據(jù)集上，各種方法的軌跡和真值相差不大，而在序列01數(shù)據(jù)集上，3種方法產(chǎn)生了一定差異。在00，05，09數(shù)據(jù)集上，INE?SLAM方法位姿估計精度與傳統(tǒng)的A?LOAM、LeGO?LOAM 方法類似，證明了其可行性，在01數(shù)據(jù)集上的表現(xiàn)說明INE?SLAM方法在室外場景中的位姿估計精度可能存在不足。

進(jìn)一步使用軌跡評估工具EVO對3種方法的性能進(jìn)行定量分析評估。采用的評價指標(biāo)為絕對位姿誤差（Absolute Pose Error,APE）和相對位姿誤差（Relative Pose Error, RPE）。APE能反映建圖的全局一致性，RPE能反映建圖的局部精度和細(xì)節(jié)還原程度。

3種方法在4個數(shù)據(jù)集上的APE和RPE對比結(jié)果如圖4、圖5所示，見表1、表2。

表1 APE對比Table 1 Absolute pose error comparison

表2 RPE對比Table 2 Relative pose error comparison

圖4 APE結(jié)果對比Fig.4 Result comparison of absolute pose error

圖5 RPE結(jié)果對比Fig.5 Result comparison of relativepose error

從圖4、圖5、表1、表2可看出：INE?SLAM方法在 RPE 指標(biāo)上有良好的發(fā)揮, 在控制RPE的最大值上表現(xiàn)最好；從建圖的全局一致性上，通過 APE指標(biāo)可看出，INE?SLAM方法還有一定不足，能與A?LOAM 和 LeGO?LOAM 方法達(dá)到相近的水平。這是因?yàn)樵诙喾椒系囊蜃訄D中，NDT 相對位姿因子是基于概率模型，相較于 ICP 相對位姿因子存在更大的誤差，在進(jìn)行基于信息矩陣的融合時，由于誤差的存在，會使整體優(yōu)化在全局一致性上產(chǎn)生持續(xù)的誤差累計。

由實(shí)驗(yàn)結(jié)果可見， INE?SLAM方法在全局一致性上與LeGO?LOAM方法相似，在建圖局部精度上優(yōu)于A?LOAM和LeGO?LOAM方法。

3 .2 模擬巷道實(shí)驗(yàn)

為了驗(yàn)證所提出的 INE?SLAM 方法在煤礦井下巷道應(yīng)用場景的性能，在模擬巷道進(jìn)行了實(shí)驗(yàn)，如圖6所示。

圖 6 模擬巷道實(shí)驗(yàn)現(xiàn)場Fig. 6 Experiment site in simulated roadway

采用履帶式移動機(jī)器人平臺進(jìn)行實(shí)驗(yàn)，搭載Ouster OS?1 128 線三維激光雷達(dá)采集數(shù)據(jù)，平臺移動速度約為1 m/s，探測器為移動掃描。通過離線建圖方式將INE?SLAM 方法運(yùn)行在 Intel Core i7?8750H CPU（2.20 GHz，6核）、16 GB內(nèi)存便攜式計算機(jī)上。INE?SLAM方法在模擬巷道的建圖效果如圖7所示，同時利用LeGO?LOAM方法對模擬巷道場景進(jìn)行建圖，移動機(jī)器人在巷道轉(zhuǎn)彎處行走約50 m時的建圖結(jié)果如圖8所示。從圖7可看出，INE?SLAM方法解決了LeGO?LOAM 方法因只采用特征點(diǎn)匹配的前端配準(zhǔn)方法，導(dǎo)致在如井下長直巷道這類特征點(diǎn)退化的特殊環(huán)境中激光里程計失效，無法有效構(gòu)建三維點(diǎn)云地圖的問題。由于采用了多方法集成的點(diǎn)云配準(zhǔn)，使用基于概率分布的 NDT算法構(gòu)建的相對位姿因子能夠更加有效地判斷出移動機(jī)器人在相似環(huán)境中的運(yùn)動，而基于特征匹配的ICP算法能夠使得三維建圖在細(xì)節(jié)上足夠準(zhǔn)確。從圖8可看出，LeGO?LOAM方法建圖的 z 軸數(shù)據(jù)即可定量分析出移動機(jī)器人在運(yùn)動方向的位移不超過6 m，遠(yuǎn)小于移動機(jī)器人實(shí)際行走距離。由于LeGO?LOAM方法只提取極少的特征點(diǎn)作匹配，在模擬巷道重復(fù)特征多、特征退化的情況下，激光里程計認(rèn)為移動機(jī)器人在原地不動，多幀點(diǎn)云匹配失敗，最終導(dǎo)致建圖失敗。

圖7 INE?SLAM方法在模擬巷道的建圖效果Fig.7 Mapping effect of ICPand NDTensemble SLAM in simulated roadway

圖8 LeGO?LOAM方法在模擬巷道的建圖結(jié)果Fig.8 Mapping result of lightweight and ground-optimized LOAM in simulated roadway

INE?SLAM方法的建圖細(xì)節(jié)如圖9所示，其中圖9（a）的點(diǎn)云尺寸放大如圖9（b）所示，可以更好地顯現(xiàn)巷道內(nèi)的門型軌道架。巷道進(jìn)入弧形彎道前收窄部分的細(xì)節(jié)如圖10所示，可看出收窄部分的墻體明顯，薄厚均勻，沒有因激光里程計失效導(dǎo)致重影。

圖9 INE?SLAM 方法的建圖細(xì)節(jié)Fig.9 Mapping details of ICP and NDT ensemble SLAM

圖10 巷道收窄部分INE?SLAM方法的建圖細(xì)節(jié)Fig.10 Mapping details of ICPand NDT ensemble SLAM in narrowing roadway

4 結(jié)論

（1）面向煤礦長直巷道特征點(diǎn)稀少導(dǎo)致激光里程計易失效的問題，提出了基于集成式因子圖優(yōu)化的煤礦巷道移動機(jī)器人三維地圖構(gòu)建方法。針對激光里程計在煤礦井下建圖時由于時間產(chǎn)生的漂移誤差，導(dǎo)致全局一致性差的問題，構(gòu)建了ICP 相對位姿因子和 NDT相對位姿因子，共同作為約束輸入因子圖優(yōu)化，能夠使地圖的全局一致性相對較高。在公開數(shù)據(jù)集和模擬巷道點(diǎn)云數(shù)據(jù)集中對該方法的性能進(jìn)行了實(shí)驗(yàn)驗(yàn)證，結(jié)果表明：該方法可解決幾何特征少，但點(diǎn)云分布特征明顯的長直巷道的激光SLAM建圖問題，與傳統(tǒng)A?LOAM、LeGO?LOAM 方法相比，在室外場景中性能指標(biāo)類似，但實(shí)時性強(qiáng)；在巷道場景下具有顯著優(yōu)勢，通過因子圖優(yōu)化，可得到一致性較高的三維地圖，提升了煤礦巷道三維地圖構(gòu)建的精度及魯棒性。

（2）該方法目前主要聚焦于通過提升局部位姿估計精度來實(shí)現(xiàn)煤礦巷道的地圖構(gòu)建，后續(xù)工作將研究如何提升煤礦巷道移動機(jī)器人的定位精度。