煤礦井下移動機器人多傳感器自適應融合SLAM 方法

摘要：基于同時定位與建圖（SLAM）技術的移動機器人能夠快速、準確、自動化地采集空間數據，進行空間智能感知和環(huán)境地圖構建，是實現(xiàn)煤礦智能化和無人化的關鍵。針對目前煤礦井下多傳感器融合SLAM 方法存在機器人前端位姿估計退化失效和后端融合精度不足的問題，提出了一種煤礦井下移動機器人激光雷達（LiDAR）?視覺?慣性（IMU）自適應融合SLAM 方法。對LiDAR 點云數據進行聚類分割，提取線面特征，利用IMU 預積分狀態(tài)進行畸變校正，采用基于自適應Gamma 校正和對比度受限的自適應直方圖均衡化（CLAHE）的圖像增強算法處理低照度圖像，再提取視覺點線特征。用IMU 預積分狀態(tài)為LiDAR 特征匹配與視覺特征跟蹤提供位姿初始值。根據LiDAR 相鄰幀的線面特征匹配得到移動機器人位姿，之后進行視覺點線特征跟蹤，分別計算LiDAR、視覺、IMU 位姿變化值，通過設定動態(tài)閾值來檢測前端里程計的穩(wěn)定性，自適應選取最優(yōu)位姿。對不同傳感器構建殘差項，包括點云匹配殘差、IMU 預積分殘差、視覺點線殘差、邊緣化殘差。為了兼顧精度與實時性，基于滑動窗口實現(xiàn)激光點云特征、視覺特征、IMU 測量的多源數據聯(lián)合非線性優(yōu)化，實現(xiàn)煤礦井下連續(xù)可用、精確可靠的SLAM。對圖像增強前后效果進行試驗驗證，結果表明，基于自適應Gamma 校正和CLAHE 的圖像增強算法能顯著提升背光區(qū)和光照區(qū)的亮度和對比度，增加圖像中的特征信息，大幅提升特征點提取數量和匹配質量，匹配成功率達90.7%。為驗證所提方法的性能，在狹長走廊和煤礦巷道場景下進行試驗驗證，結果表明，所提方法在狹長走廊場景的定位均方根誤差為0.15 m，構建的點云地圖一致性較高；在煤礦巷道場景中的定位均方根誤差為0.19 m，構建的點云地圖可真實地反映煤礦井下環(huán)境。

關鍵詞：煤礦井下移動機器人；同時定位與建圖；激光雷達?視覺?慣性自適應融合；圖像增強；位姿估計；多傳感器數據融合；滑動窗口緊耦合優(yōu)化；SLAM

中圖分類號：TD67 文獻標志碼：A

0 引言

移動機器人是實現(xiàn)煤礦智能化和無人化的關鍵，對提升煤礦企業(yè)安全性、降低礦工勞動強度和優(yōu)化管理效率具有劃時代意義[1-2]?；谕瑫r定位與建圖（Simultaneous Localization and Mapping， SLAM）技術的移動機器人能夠快速、準確、自動化地采集空間數據，進行空間智能感知和環(huán)境地圖構建，以滿足煤礦井下自主作業(yè)的要求[3]。煤礦井下無全球導航衛(wèi)星系統(tǒng)（Global Navigation Satellite System， GNSS）信號且地形復雜，低光照和弱紋理場景易使單一傳感器的SLAM 精度不足，機器人的前端位姿估計退化，甚至失效。具體表現(xiàn)：相機在低光照、弱紋理和高粉塵的煤礦巷道中無法穩(wěn)健使用[4]； 激光雷達（Light Laser Detection and Ranging， LiDAR）可提供準確的深度測量，不受場景照明和紋理變化的影響，但在狹長巷道中易導致位姿估計退化[5]；慣性測量單元（Inertial Measurement Unit，IMU）響應速度快，基本不受外界環(huán)境影響，但其測量誤差隨時間累計[6]。

多傳感器數據融合的SLAM 較單一傳感器的視覺SLAM、激光雷達SLAM 穩(wěn)定性更好，且具有更高的定位精度和更豐富的環(huán)境信息[7-8]。目前，多傳感器數據融合的SLAM 系統(tǒng)主要分為視覺慣性系統(tǒng)（Visual Inertial System， VIS）、激光慣性系統(tǒng)（LiDARInertial System， LIS） 及激光視覺慣性系統(tǒng)（LiDARVisual Inertial System， LVIS）。VIS 方面，文獻[9]通過單獨處理圖像和IMU 的測量數據，再融合各自的位姿狀態(tài)估計，得到里程計的位姿，但整體精度較低。文獻[10]將視覺與IMU 緊耦合，聯(lián)合優(yōu)化估計狀態(tài)，即在視覺良好的環(huán)境下可實現(xiàn)高精度的位姿估計與建圖，但在低照度、弱紋理場景表現(xiàn)欠佳。文獻[11]提出的點線特征融合視覺慣導里程計（Pointand Line Features-Visual Inertial Odometry， PL?VIO）和文獻[12]提出的線面特征結合的SLAM 對點線特征進行聯(lián)合優(yōu)化，克服了弱紋理環(huán)境下無法進行特征跟蹤的問題，但未充分考慮低照度對點線特征跟蹤的影響。LIS 方面，文獻[13]提出一種松耦合的LiDAR 慣性里程計及建圖系統(tǒng)，根據曲率大小提取點線特征，實現(xiàn)準確的位姿估計，但沒有后端閉環(huán)優(yōu)化模塊，容易累計漂移。文獻[14]提出一種緊耦合的激光雷達慣性里程計（Lidar Inertial Odometry viaSmoothing and Mapping， LIO?SAM） 方法， 該方法將LiDAR 里程計因子、IMU 預積分因子、全球定位系統(tǒng)（Global Positioning System，GPS）因子及回環(huán)檢測因子引入因子圖中，實現(xiàn)高精度、實時的移動機器人軌跡估計和地圖構建。該系統(tǒng)通過ISAM（IncrementalSmoothing and Mapping）實現(xiàn)局部和全局的優(yōu)化，表現(xiàn)出較高的魯棒性和精度，但尚未充分考慮退化場景對建圖的影響。為了將LiDAR、視覺、IMU 有效融合用于定位與建圖，研究者提出了LVIS。文獻[15]提出了V?LOAM（Visual-Lidar Odometry andMapping）算法，它融合了視覺里程計與LiDAR 里程計，用視覺里程計進行位姿估計，但該算法執(zhí)行過程按逐幀順序進行，全局整體一致性不強。文獻[16]提出了VIL?SLAM（Visual-Inertial SLAM with Pointsand Lines） 算法， 該算法是基于立體視覺、IMU、LiDAR 融合的SLAM 系統(tǒng)，將視覺慣性里程計與激光里程計相結合，提高了定位和地圖構建的精度，但該算法計算量大、硬件成本高，在光照和遮擋等復雜環(huán)境下易出現(xiàn)定位和地圖構建錯誤。文獻[17]提出了一種LiDAR 與視覺融合的方法，該方法的LiDAR里程計位姿是由視覺慣性里程計給予先驗信息初始化，同時可優(yōu)化兩者的位姿，提高位姿估計的精度，但在低光照時易發(fā)生初始化錯誤，導致LiDAR 里程計位姿誤差較大。文獻[18]提出了緊耦合的激光視覺慣導SLAM（Lidar-Visual-Inertial Odometry viaSmoothing and Mapping， LVI?SAM）系統(tǒng)，該系統(tǒng)被分成LIS 子系統(tǒng)和VIS 子系統(tǒng)，視覺為LiDAR 提供初始位姿估計值，LiDAR 給視覺提供深度信息，在后端利用視覺信息進行閉環(huán)檢測，利用LiDAR 進行閉環(huán)優(yōu)化。由于視覺部分依賴于圖像特征，所以可能會受到光照和紋理的影響。

針對煤礦井下復雜環(huán)境導致多傳感器融合SLAM存在機器人前端位姿估計退化失效和后端融合精度不足的問題，本文提出一種LiDAR?視覺?慣性自適應融合SLAM 方法。首先，在視覺圖像數據預處理階段引入圖像增強算法，以減少低光照場景對視覺特征提取的影響。然后，動態(tài)設置檢測閾值來自適應選取當前最優(yōu)位姿，提高位姿估計精度。最后，基于滑動窗口實現(xiàn)多傳感器數據聯(lián)合非線性優(yōu)化，實現(xiàn)一種高精度、低漂移的多傳感器數據融合SLAM系統(tǒng)。

1 方法概述

LiDAR?視覺?慣性自適應融合SLAM 方法可分為多源傳感器數據預處理、LiDAR/視覺/IMU 里程計、后端優(yōu)化3 個部分，如圖1 所示。在多源傳感器數據預處理部分，首先對LiDAR 非地面點云數據進行聚類分割，提取線面特征，然后利用IMU 預積分狀態(tài)進行畸變校正，最后對低照度圖像進行增強，再提取視覺點線特征。用IMU 預積分狀態(tài)為LiDAR 特征匹配與視覺特征跟蹤提供位姿初值。根據LiDAR 相鄰幀的線面特征匹配得到機器人位姿，之后進行視覺點線特征跟蹤，分別計算LiDAR、視覺、IMU 位姿變化值，通過設定動態(tài)閾值來檢測前端里程計的穩(wěn)定性，自適應選取最優(yōu)位姿。對不同傳感器構建殘差項，包括點云匹配殘差、IMU 預積分殘差、視覺點線殘差、邊緣化殘差。為了兼顧精度與實時性，基于滑動窗口實現(xiàn)激光點云特征、視覺特征、IMU 測量的多源數據聯(lián)合非線性優(yōu)化，實現(xiàn)煤礦井下連續(xù)可用、精確可靠的SLAM。

2 方法關鍵技術

2.1 多源傳感器數據預處理

2.1.1 IMU 預積分

通過IMU 預積分可獲得機器人狀態(tài)。為避免每次IMU 測量過程中增加新的狀態(tài)量，通常在2 幀之間增加1 個重新參數化過程來實現(xiàn)運動約束，避免重復積分。

k 時刻機器人的狀態(tài)量為

Ik =[Rk， pk，"vk，bk]（1）

式 中 ： Rk為 k 時 刻 的 旋 轉 矩 陣 ； pk， vk，bk分 別 為IMU 預積分獲得的位置、速度、偏置。

基于文獻[16]中的IMU 預積分方法，將關鍵幀的狀態(tài)加入到滑動窗口中，執(zhí)行光束法平差（BundleAdjustment， BA）優(yōu)化，以實現(xiàn)IMU 坐標系到世界坐標系的變換?？傻玫竭B續(xù)IMU 關鍵幀mi和mi+1之間的相對運動測量值：

2.1.2 激光點云數據預處理

LiDAR 點云數據預處理主要有點云數據分割、線面特征提取和運動畸變校正。首先，基于隨機樣本一致性（Random Sample Consensus， RANSAC） [19]的快速點云分割算法對LiDAR 點云數據進行分割處理。然后，利用深度圖計算出分割后的LiDAR 點云數據的曲率，將曲率較大的非地面點標記為邊緣特征點，將曲率較小的標記為平面特征點，從而提取煤礦巷道線特征和平面特征。最后，為了消除因傳感器移動導致的點云數據畸變問題，使用IMU 的預積分數據對LiDAR 點云數據進行運動補償。

2.1.3 視覺圖像預處理

煤礦井下高粉塵、弱紋理、燈光區(qū)域與背光區(qū)域亮度反差大，圖像中物體表面容易出現(xiàn)反光、高光現(xiàn)象，使得圖像整體或局部光照不均，嚴重影響視覺特征提取與跟蹤，易使視覺SLAM 發(fā)生漂移甚至失效。因此，需在視覺特征提取前對圖像質量進行增強，以突出圖像中的特征信息。在HSV 空間下，本文將自適應Gamma 校正[20-21]和對比度受限的自適應直方圖均衡化（Contrast Limited Adaptive HistogramEqualization， CLAHE）算法[22]相融合，以對圖像進行增強，從而提高光照不均環(huán)境下視覺特征提取數量。圖像增強算法流程如圖2 所示。首先，將圖像從RGB 空間變換到HSV 空間，避免在RGB 顏色空間中處理造成圖像失真。其次，只對亮度分量進行自適應Gamma 校正處理。然后，對Gamma 校正結果進行CLAHE 算法處理，將圖像中分布不均勻的直方圖變換為均勻分布的直方圖，有效拉伸灰度值的動態(tài)范圍，以提高圖像對比度。最后，將未處理的色調分量、飽和度分量和處理后的亮度分量進行融合，并逆變換回RGB 顏色空間，得到最終增強圖像。

對增強圖像進行視覺點線特征提取。首先，使用Kanade-Lucas-Tomasi（KLT）特征跟蹤稀疏光流算法[23]提取和跟蹤每個新圖像幀中的初始特征點，保持每幀圖像中點特征數量為200～400 個。然后，使用RANSAC 和基本矩陣模型進行異常值抑制[19]，剔除離群值大的特征點。最后，采用線段檢測器（LineSegment Detector， LSD） [24]提取線段，并用線帶描述符（Line Band Descriptor， LBD）[25]描述線特征。

2.2 LiDAR、視覺、IMU 里程計

2.2.1 點云數據線面特征匹配

考慮到煤礦巷道環(huán)境中平面特征遠多于線特征，采用平面特征進行點到面距離最小約束，估計出Z 方向、翻滾角、俯仰角的變化量；在此基礎上進行點到線距離最小約束，估計出X 方向、Y 方向、偏航角的變化量，聯(lián)合地面點和角點優(yōu)化估計連續(xù)幀的相對位姿；采用麥夸特（Levenberg-Marquardt，LM）算法[13]迭代求解最優(yōu)位姿，迭代計算的初值為當前幀IMU 預積分的位姿，可有效減少迭代次數，且消除零初值或勻速運動假設初值帶來的誤匹配。

式中：elineA和eplaneB分別為第A 個點到線、第B 條線到面的距離；ΓA和∧B分別為特征對應的主方向和法線方向；QA和PA 為在線特征點集FlineA+1集合中的線特征點；R 為旋轉矩陣；t 為時間；QB 和PB 為在面特征點集FplaneB+1集合中的面特征點；∑P∈FAepoint→line，plane為點到線、點到面的殘差；∑P∈FA為特征點在線特征點集合FA上的點；∑（A，A+1）∈B||eimu||2和Epriorimuodom分別為IMU 預積分因子和IMU 里程計提供的預測位姿先驗值。

2.2.2 視覺點線特征跟蹤

首先選擇相似的線段進行跟蹤，并添加線段長度相近的圖像幀上的特征點集，確保在新圖像至少有2 個特征點的線段。然后，根據文獻[26]中的跟蹤線段策略，對2 個連續(xù)圖像幀線進行跟蹤。最后，為提高視覺位姿的精度，采用點特征投影殘差、線特征投影殘差和IMU 的測量結果構造局部殘差約束。當插入1 個新的關鍵幀時，對當前建圖執(zhí)行1 個光束法平差調整。在滑動窗口中，通過最小化所有測量殘差的先驗項和代價項，對所有狀態(tài)變量進行優(yōu)化。

對于點特征，重投影誤差可定義為觀測點與重投影點在同一像素平面上的距離。假設第j個視覺點特征在第h幀圖像幀ch中首先被觀察到，則第i幀圖像幀ci中的重投影誤差為

式中：Lcik為圖像幀ci中的第k個視覺線特征；K為三維直線的內參矩陣；Nc為圖像幀c的旋轉矩陣；fv，fu分別為二維像素坐標系下的像素值；cu，cv分別為二維像素坐標系下圖像幀c的像素轉換值；L1，L2，L3分別為三軸方向的線段分量；γF1為視覺線特征重投影誤差；d（Ik，Lcik）為圖像幀ci中的第k個視覺線特征與投影到二維像素平面的第k個視覺線特征的距離；sk和lk為二維像素平面觀測到的端點。

2.2.3 自適應位姿選取

首先，設定一個動態(tài)閾值，根據IMU 預積分對三軸方向的加速度積分，得到三軸方向的速度，計算出IMU 在世界坐標系下的位移，將一定周期內的位移設置為動態(tài)閾值。然后，將LiDAR?IMU 里程計位姿、視覺?IMU 里程計位姿、LiDAR?視覺?IMU 里程計位姿轉換到世界坐標系下，分別計算出一定周期內的位姿偏差。最后，將動態(tài)閾值與LiDAR?IMU里程計位姿偏差、視覺?IMU 里程計位姿偏差、LiDAR?視覺?IMU 里程計位姿偏差進行檢驗，以此判斷上述3 個里程計輸出的位姿是否準確，從而根據動態(tài)閾值自適應選取最優(yōu)位姿。設定動態(tài)閾值前，需先對三軸加速度積分，并計算三軸在世界坐標系下的位移。

式中： 為動態(tài)閾值； η為IMU 的頻率；K 為幀數；n 為總幀數；V 為IMU 預積分后的速度；tK 為第幀的時間；V0 為 IMU 預積分后的初速度；a（ti）為ti時刻的加速度。

S是通過IMU 預積分后的速度計算得來的，IMU 誤差會隨著時間增加而累計。為此，在IMU 預積分周期內，將上一周期內的平均速度作為IMU 初速度來校準當前時刻的IMU 預積分速度，以防止累計誤差。

式中：（xlio， ylio， zlio）為LiDAR?IMU 里程計位姿坐標；（xcio， ycio， clio）為視覺?IMU 里程計位姿坐標；（xlci，ylci， zlci）為LiDAR?視覺?IMU 里程計位姿坐標； 為平移矩陣。

在周期內，根據IMU 對加速度積分計算得到機器人位姿（ximu， yimu， zimu）， 根據LiDAR?視覺?IMU里程計位姿得到機器人位置差Hlci、LiDAR?IMU 里程計位姿得到機器人位置差值Hlio、視覺?IMU 里程計位姿得到機器人位置差值Hcio。

自適應位姿選取方法通過輸入動態(tài)閾值和位置誤差（Hlci，Hlio，Hcio），對LiDAR?視覺?IMU 里程計的位置誤差Hlci與動態(tài)閾值S進行判斷。在Δt周期內：當Hlci＜S時，選擇LiDAR?視覺?IMU 里程計的位姿為后端優(yōu)化的初始值。當Hlci＞S時， 需比較Hlio與Hcio的大小，若Hlio＜Hcio且Hlio＜Hlci，選擇LiDAR?IMU 里程計的位姿；若Hlio＜Hcio且Hlio＞Hlci，選擇LiDAR?視覺?IMU 里程計的位姿；若Hcio＜Hlio且Hcio＜Hlci，選擇視覺?IMU 里程計的位姿； 若Hcio＜Hlio且Hcio＞Hlci時， 選擇LiDAR?視覺?IMU 里程計位姿。經過動態(tài)閾值S檢驗與位姿自適應篩選，可選取最優(yōu)位姿作為后端優(yōu)化的初始值。

2.3 后端優(yōu)化

在位姿估計和IMU 狀態(tài)預測后，使用滑動窗口緊耦合優(yōu)化方法估計系統(tǒng)的最優(yōu)狀態(tài)。為了確保較高的精度與實時性，本文選擇滑動窗口大小為10，當窗口滿時，需對窗口內的狀態(tài)節(jié)點進行滑動，從而實現(xiàn)局部光束法平差。

采用非線性優(yōu)化方法對LiDAR、視覺、IMU傳感器的觀測值進行聯(lián)合優(yōu)化。根據非線性優(yōu)化理論[27]，先構建不同類型觀測值相對于待估狀態(tài)的量測方程，再構建殘差項，進一步將不同類型的殘差加起來，得到整個優(yōu)化問題的代價函數。

殘差項構建后，根據因子圖理論將上述涉及到的多源數據抽象為測量因子，采用基于最大后驗估計的滑動窗口因子圖優(yōu)化算法構建因子圖優(yōu)化模型， 包括LiDAR 里程計（LiDAR Odometry， LO） 因子、IMU 預積分因子、視覺里程計（Visual Odometry，VO）因子、激光視覺慣導里程計（LiDAR Visual IMUOdometry，LVIO）因子。狀態(tài)節(jié)點變量用橘黃色圓圈表示，包括移動機器人的姿態(tài)、位置、速度等信息，如圖3 所示。首先，通過關鍵幀選取策略對實時采集的LiDAR 掃描幀和序列圖像提取相應的關鍵幀，并將關鍵幀對應的LiDAR 和相機狀態(tài)節(jié)點加入因子圖，同時將對應的LO 因子和VO 因子加入因子圖。然后，對LiDAR 掃描幀和圖像間的IMU 數據進行預積分， 得到IMU 預積分因子， 添加IMU 預積分因子。最后，構建該時刻的LVIO 因子，并加入因子圖進行優(yōu)化。

3 實驗及結果分析

3.1 試驗介紹

為了驗證本文方法的性能，自主搭建了LiDAR、相機、IMU 多源傳感器的硬件采集平臺，在狹長走廊和煤礦井下環(huán)境進行試驗，如圖4 所示。本文系統(tǒng)基于ROS 進行時間同步和坐標變換，非線性優(yōu)化問題使用Ceres 庫實現(xiàn)。提前使用文獻[28]中方法標定LiDAR 和相機，用Kalibr[29]標定IMU 和相機。上述硬件設備型號和詳細參數見表1。

狹長走廊中布設控制點L1—L6，光照不均勻，結構對稱；煤礦井下布設控制點M1—M9，存在低光照巷道，需翻越軌道，存在大量的噴漿表面和對稱巷道，如圖5 所示。在移動機器人上粘貼測量標志，使用全站儀測量標志點坐標為參考值，以移動機器人在對應控制點位置停止時間段內的平均位置估值為測量值。

3.2 圖像增強試驗

為驗證本文圖像增強算法的有效性，分別與常用的Retinex 算法、自適應Gamma 校正算法、CLAHE算法和文獻[30]所提算法進行對比。選取煤礦巷道低照度區(qū)域的（M7，M9）數據進行圖像增強和特征點匹配，定性分析圖像增強效果和定量評估特征點匹配情況。

5 種圖像增強算法處理結果如圖6 所示。可看出Retinex 算法調節(jié)了圖像的整體亮度，但同時給圖像帶來了一定噪聲，使整張圖像出現(xiàn)泛白現(xiàn)象；自適應Gamma 校正算法對灰度級進行拉伸，使得灰度級更加平滑，對暗區(qū)域增強效果更顯著，會降低光照區(qū)域的增強效果；CLAHE 算法雖然提高了圖像的對比度，保留了豐富的細節(jié)信息，但對暗區(qū)域的亮度增強效果不明顯；文獻[30]引入熵指數來控制增強程度，再根據圖像局部特征進行自適應增強，但增強的亮度不如CLAHE 算法明顯； 本文算法利用自適應Gamma 校正算法和CLAHE 算法優(yōu)勢互補，在增強圖像亮度的同時提高了圖像對比度，圖像細節(jié)體現(xiàn)更豐富，說明本文算法對煤礦巷道圖像亮度有明顯改善，更符合人眼視覺感知，充分展現(xiàn)出環(huán)境中的特征信息。

通過對比圖像增強前后特征點提取和跟蹤情況，對圖像質量進行定量評估，結果見表2。可看出原始圖像提取的特征點很少，且匹配成功率不超過39.6%，無法滿足視覺SLAM 要求。經過Retinex 算法增強后提取的特征點數量有所增加，由于增強過程中引入了噪聲，導致后續(xù)RANSAC 去除誤匹配點的匹配成功率較低。自適應Gamma 校正算法、CLAHE 算法和文獻[30]所提算法雖能夠提高匹配成功率，增強視覺里程計的穩(wěn)定性，但視覺位姿估計結果不夠準確。本文算法提升了背光區(qū)和光照區(qū)的亮度和對比度，增加了圖像中的特征信息，在特征點提取數量和匹配質量方面都有大幅度提升，匹配成功率達90.7%，提高了視覺里程計的魯棒性和精度。

3.3 多源傳感器數據融合SLAM 試驗

為了驗證激光雷達?相機?慣性自適應融合SLAM 方法在低照度、弱紋理、退化場景中的表現(xiàn)。將本文方法與圖像增強LVI?SAM（Image EnhancementLVI?SAM， IE?LVI?SAM）、激光雷達慣性測深和測繪（Lidar Inertial Odometry and Mapping，LIO?Mapping）、面向快速和旋轉（Oriented Fast and Rotated Brief，ORB） 的SLAM2（ORB?SLAM2） 、圖像增強ORB?SLAM2（IE?ORB?SLAM2）等方法從定位軌跡和建圖效果2 個方面進行對比。不同方法的平面軌跡對比如圖7 所示。從圖7（a）可看出，在狹長走廊中，ORB?SLAM2 方法面對低照度的走廊白墻缺乏足夠的視覺特征用于約束位姿，尺度估計失效，定位軌跡明顯縮??；IE?ORB?SLAM2 方法提高了圖像亮度和對比度， 提取特征點和匹配成功率有所增加， 較ORB?SLAM2 方法的定位軌跡更接近控制點；LIO?Mapping 方法由于走廊縱向末端提取的點云幾何特征稀疏，其特征點云出現(xiàn)誤匹配，導致LiDAR在前進方向發(fā)生退化，定位軌跡在Y 方向縮短；IE?LVI?SAM 方法在圖像增強的基礎上，將LiDAR 里程計、視覺里程計、IMU 預積分約束和回環(huán)約束進行聯(lián)合優(yōu)化，全局定位軌跡相對較好，但在轉彎處視覺和IMU 位姿估計誤差較大，導致融合位姿有一定漂移。本文方法定位軌跡較IE?LVI?SAM 方法更平滑。從圖7（b）可看出，在煤礦巷道場景中， ORB?SLAM2 方法在低照度噴漿墻面上提取特征較少，加上翻越軌道時劇烈晃動， 其定位軌跡偏差最大。IE?ORB?SLAM2 方法雖引入圖像增強算法增強圖像視覺效果，但面對噴漿墻面只能提取一些不穩(wěn)定的特征點，在M9 控制點翻越軌道時，軌跡漂移較大。LIO?Mapping 方法在巷道弱幾何結構環(huán)境下，LiDAR 前端線面特征約束較少， 且在轉彎處， 以IMU 狀態(tài)為迭代初值的位姿估計誤差較大，定位軌跡發(fā)生傾斜。IE?LVI?SAM 方法在M7 控制點附近增強圖像的亮度和對比度，避免過暗圖像對視覺里程計的影響，但后續(xù)長直噴漿墻面對視覺和LiDAR里程計仍有較大影響，聯(lián)合優(yōu)化位姿有一定漂移。本文方法根據位姿差值，自適應選取最優(yōu)位姿，減少單一傳感器失效對聯(lián)合優(yōu)化的影響，估計軌跡更靠近控制點。

進一步對上述定位結果進行量化分析，絕對定位誤差如圖8 所示， 定位軌跡的標準差（StandardDeviation， STD） 和均方根誤差（Root Mean SquaredError，RMSE）見表3?？煽闯觯谌跫y理墻面和低照度環(huán)境中，采用ORB?SLAM2 方法時視覺里程計無法對狹長走廊與煤礦巷道進行精準的位姿估計，定位軌跡RMSE 較大；IE?ORB?SLAM2 方法提高了特征提取數量及匹配正確率，較好地約束視覺里程計，狹長走廊與煤礦巷道中RMSE 小于3 m，但缺乏尺度信息；LIO?Mapping方法雖不受低照度的影響，但在特征相似的環(huán)境中LiDAR 易發(fā)生退化，導致狹長走廊與煤礦巷道中X，Y 軸發(fā)生退化；IE?LVI?SAM 方法提高了視覺里程計的穩(wěn)定性，但是狹長走廊與煤礦巷道中LiDAR 發(fā)生退化時，不能較好地約束X，Y 軸的位移量，在狹長走廊與煤礦巷道中RMSE 范圍為0.28～0.39 m；本文方法的RMSE 小于0.19 m。

3.4 建圖效果分析

對本文方法與IE?LVI?SAM 方法在狹長走廊與煤礦巷道得到的全局地圖（圖9、圖10）定性分析建圖效果。從圖9 可看出，由于狹長走廊口字型場景中存在相似結構，視覺尺度估計誤差較大，LiDAR 在長走廊中發(fā)生退化，IE?LVI?SAM 方法估計的局部軌跡發(fā)生漂移，狹長走廊轉彎處點云地圖出現(xiàn)重影現(xiàn)象；在煤礦巷道場景下，弱紋理的噴漿巷道導致IE?LVI?SAM 方法定位軌跡向Y 軸漂移，運輸巷道點云也向Y 軸偏移，點云地圖有明顯錯位現(xiàn)象。

從圖10 可看出，在狹長走廊中，本文方法可較好地提取幾何特征，融合多源傳感器數據進行高精度定位，構建的點云地圖輪廓與真實場景相似度較高；在煤礦巷道中，移動機器人進入狹長巷道時，本文方法可有效抑制LiDAR 退化和提升視覺特征匹配，構建的采掘巷道、綜掘巷道、運輸巷道點云地圖與煤礦井下巷道幾何特征一致，說明本文方法在面對煤礦井下復雜環(huán)境時，可以實現(xiàn)較高精度的定位與建圖。

4 結論

1） 提出了一種激光雷達?視覺?慣性自適應融合SLAM 方法。首先，采用基于自適應Gamma 校正和CLAHE 的圖像增強算法提升低照度圖像的亮度與對比度，提高視覺里程計的魯棒性。其次，通過設定動態(tài)閾值來檢測前端里程計的穩(wěn)定性，在此基礎上自適應選取移動機器人在煤礦井下的最優(yōu)位姿，為后端優(yōu)化迭代提供位姿初始值。最后，在后端基于滑動窗口實現(xiàn)多源傳感器數據聯(lián)合非線性優(yōu)化，從而提高定位與建圖的精度和魯棒性，并有效抑制軌跡發(fā)散與地圖扭曲現(xiàn)象。

2） 對原始圖像增強前后的特征提取數量及匹配正確率進行試驗驗證，結果表明，自適應Gamma 校正和CLAHE 相融合的算法能顯著提升背光區(qū)和光照區(qū)的亮度和對比度，增加圖像中的特征信息，大幅提升特征點提取和匹配質量，匹配成功率達90.7%。

3） 為驗證本文方法性能，在狹長走廊和煤礦巷道場景下進行試驗驗證，結果表明，激光雷達?視覺?慣性自適應融合SLAM 方法在狹長走廊場景下可很好地提取走廊內幾何特征，較好地約束三軸方向的位姿，定位RMSE 為0.15 m，構建的點云地圖一致性較高；在煤礦巷道場景下，激光雷達?視覺?慣性自適應融合SLAM 方法動態(tài)篩選出最優(yōu)位姿，可較好地補償三軸位置偏差，定位RMSE 為0.19 m，構建的點云地圖可以真實地反映煤礦井下環(huán)境。

參考文獻（References）：

[ 1 ]肖琳芬. 王國法院士：煤礦智能化技術體系建設進展與煤炭產業(yè)數字化轉型[J]. 高科技與產業(yè)化，2024，30（2）：12-15.

XIAO Linfen. Academician Wang Guofa：progress in theconstruction of intelligent technology system in coalmines and the digital transformation of coal industry[J].High-Technology amp; Commercialization， 2024， 30（2） ：12-15.

[ 2 ]任滿翊. 無人化智能煤礦建設探索與實踐[J]. 工礦自動化，2022，48（增刊1）：27-29.

REN Manyi. Exploration and practice of unmannedintelligent coal mine construction[J]. Journal of MineAutomation，2022，48（S1）：27-29.

[ 3 ]KHATTAK S， NGUYEN H， MASCARICH F， et al.Complementary multi-modal sensor fusion for resilientrobot pose estimation in subterranean environments[C].International Conference on Unmanned AircraftSystems，Athens，2020：1024-1029.

[ 4 ]龔云，頡昕宇. 基于同態(tài)濾波方法的煤礦井下圖像增強技術研究[J]. 煤炭科學技術，2023，51（3）：241-250.

GONG Yun， JIE Xinyu. Research on coal mineunderground image recognition technology based onhomomorphic filtering method[J]. Coal Science andTechnology，2023，51（3）：241-250.

[ 5 ]YANG Xin， LIN Xiaohu， YANG Wanqiang， et al. Arobust LiDAR SLAM method for underground coal minerobot with degenerated scene compensation[J]. RemoteSensing，2022，15（1）. DOI：10.3390/RS15010186.

[ 6 ]WU Weitong， LI Jianping， CHEN Chi， et al. AFLICalib：robust LiDAR-IMU extrinsic self-calibrationbased on adaptive frame length LiDAR odometry[J].ISPRS Journal of Photogrammetry and Remote Sensing，2023，199：157-181.

[ 7 ]COLE D M，NEWMAN P M. Using laser range data for3D SLAM in outdoor environments[C]. IEEEInternational Conference on Robotics and Automation，Orlando，2006：1556-1563.

[ 8 ]LI Menggang， ZHU Hua， YOU Shaoze， et al. Efficientlaser-based 3D SLAM in real time for coal mine rescuerobots[C]. 8th Annual International Conference onTechnology in Automation， Control， and IntelligentSystems，Tianjin，2018：971-976.

[ 9 ]KNEIP L， WEISS S， SIEGWART R. Deterministicinitialization of metric state estimation filters for looselycoupledmonocular vision-inertial systems[C].IEEE/RSJ International Conference on Intelligent Robotsand Systems，San Francisco，2011：2235-2241.

[10]BLOESCH M， BURRI M， OMARI S， et al. Iteratedextended Kalman filter based visual-inertial odometryusing direct photometric feedback[J]. The InternationalJournal of Robotics Research， 2017， 36（10） ：1053-1072.

[11]GOMEZ-OJEDA R， MORENO F A， ZUNIGA-NO?LD，et al. PL-SLAM：a stereo SLAM system through thecombination of points and line segments[J]. IEEETransactions on Robotics，2019，35（3）：734-746.

[12]YANG Gaochao， WANG Qing， LIU Pengfei， et al. Animproved monocular PL-SlAM method with point-linefeature fusion under low-texture environment[C]. 4thInternational Conference on Control and ComputerVision，Macau，2021：119-125.

[13]ZHAO Shibo， FANG Zheng， LI Haolai， et al. A robustlaser-inertial odometry and mapping method for largescalehighway environments[C]. IEEE/RSJ InternationalConference on Intelligent Robots and Systems， Macau，2019：1285-1292.

[14]SHAN Tixiao， ENGLOT B， MEYERS D， et al. Liosam：tightly-coupled lidar inertial odometry via smoothing and mapping[C]. IEEE/RSJ InternationalConference on Intelligent Robots and Systems， LasVegas，2020：5135-5142.

[15]ZHANG Ji， SINGH S. Visual-lidar odometry andmapping： low-drift， robust， and fast[C]. IEEEInternational Conference on Robotics and Automation，Seattle，2015：2174-2181.

[16]LIU Yanqing， YANG Dongdong， LI Jiamao， et al.Stereo visual-inertial SLAM with points and lines[J].IEEE Access，2018，6：69381-69392.

[17]TIAN Chengjun， LIU Haobo， LIU Zhe， et al. Researchon multi-sensor fusion SLAM algorithm based onimproved gmapping[J]. IEEE Access， 2023， 11（9） ：13690-13703.

[18]SHAN Tixiao， ENGLOT B， RATTI C， et al. Lvi-sam：tightly-coupled lidar-visual-inertial odometry viasmoothing and mapping[C]. IEEE InternationalConference on Robotics and Automation， Xi'an， 2021：5692-5698.

[19]隋心， 王思語， 羅力， 等. 基于點云特征的改進RANSAC 地面分割算法[J]. 導航定位學報， 2024，12（1）：106-114.

SUI Xin， WANG Siyu， LUO Li， et al. ImprovedRANSAC ground segmentation algorithm based on pointcloud features[J]. Journal of Navigation andPositioning，2024，12（1）：106-114.

[20]HUANG S C， CHENG F C， CHIU Y S. Efficientcontrast enhancement using adaptive gamma correctionwith weighting distribution[J]. IEEE Transactions onImage Processing，2013，22（3）：1032-1041.

[21]WANG Jun， WANG Rui， WU Anwen. Improvedgamma correction for visual slam in low-lightscenes[C]. IEEE 3rd Advanced InformationManagement， Communicates， Electronic andAutomation Control Conference， Chongqing， 2019：1159-1163.

[22]HUANG Lidong， ZHAO Wei， WANG Jun， et al.Combination of contrast limited adaptive histogram equalisation and discrete wavelet transform for imageenhancement[J]. IET Image Processing， 2015， 9（10） ：908-915.

[23]MESSIKOMMER N， FANG C， GEHRIG M， et al.Data-driven feature tracking for event cameras[C].IEEE/CVF Conference on Computer Vision and PatternRecognition，Vancouver，2023：5642-5651.

[24]GIOI R G，JAKUBOWICZ J，MOREL J M，et al. LSD：a fast line segment detector with a 1 detectioncontrol[J]. IEEE Transactions on Pattern Analysis andMachine Intelligence，2010，32（4）：722-732.

[25]ZHANG Lilian， KOCH R. An efficient and robust linesegment matching approach based on LBD descriptorand pairwise geometric consistency[J]. Journal ofVisual Communication and Image Representation，2013，24（7）：794-805.

[26]LIU Zhe， SHI Dianxi， LI Ruihao， et al. PLC-VIO：visual-inertial odometry based on point-lineconstraints[J]. IEEE Transactions on AutomationScience and Engineering，2022，19（3）：1880-1897.

[27]TRIGGS B， MCLAUCHLAN P F， HARTLEY R I，et al. Bundle adjustment-a modern synthesis[C]. VisionAlgorithms： Theory and Practic， 1999. DOI： 10.1007/3-540-44480-7_21.

[28]LI Ang， ZOU Danping， YU Wenxian. Robustinitialization of multi-camera slam with limited viewoverlaps and inaccurate extrinsic calibration[C].IEEE/RSJ International Conference on Intelligent Robotsand Systems，Pargue，2021：3361-3367.

[29]FURGALE P， REHDER J， SIEGWART R. Unifiedtemporal and spatial calibration for multi-sensorsystems[C]. IEEE/RSJ International Conference onIntelligent Robots and Systems， Tokyo， 2013：1280-1286.

[30]MAJEED S H， ISA N A M. Adaptive entropy indexhistogram equalization for poor contrast images[J].IEEE Access，2020，9：6402-6437.

基金項目：國家自然科學基金項目（42001417）；國土資源部煤炭資源勘查與綜合利用重點實驗室項目（KF2021-4）。