基于全局點云地圖的煤礦井下無人機定位方法

高海躍 王凱 王保兵 王丹丹

摘要：即時定位與建圖（SLAM）技術應用于煤礦井下無人機自主定位時，由于采用特征點構建地圖，易出現退化問題，導致定位不準確，且因其以機體作為參考坐標系，無法實現全局定位。針對該問題，提出了一種基于全局點云地圖的煤礦井下無人機定位方法。以 Fast?LIO2算法作為激光 SLAM算法，獲得無人機位姿估計；采用迭代最近鄰算法，對獲取的激光雷達實時點云和全局點云地圖進行兩步匹配，實現無人機位姿校正；針對因點云數量過多導致點云匹配速度無法保證定位實時性的問題，設計了基于時間的位姿輸出策略，提高了無人機位姿數據輸出頻率。在1000 m煤礦井下巷道中測試無人機定位方法的 SLAM精度和位姿校正效果，結果表明：在長距離巷道環境中，Fast?LIO2算法的定位累計誤差小于1 m，在600 m 以上范圍內小于0.3 m，明顯小于 LOAM?Livox算法和 LIO?Livox算法；Fast?LIO2算法輸出的位姿估計經校正算法校正后，飛行路徑全部位于全局點云地圖中，驗證了位姿校正算法有效；單次 SLAM算法運行耗時14.83 ms，單次位姿校正耗時883 ms，位姿數據輸出頻率為10 Hz，滿足無人機定位實時性要求。

關鍵詞：無人機定位；即時定位與建圖；激光雷達；慣性測量單元；全局點云地圖；位姿校正中圖分類號： TD67??? 文獻標志碼： A

Positioning method for underground unmanned aerial vehicles in coal mines based on global point cloud map

GAO Haiyue， WANG Kai， WANG Baobing， WANG Dandan

（CCTEG Beijing Tianma Intelligent Control Technology Co.， Ltd.， Beijing 101399， China）

Abstract： When simultaneous localization and mapping （SLAM） technology is applied to autonomous positioning of unmanned aerial vehicles in coal mines， the use of feature points to construct maps can easily lead to degradation issues， resulting in inaccurate positioning. Moreover， due to its use of the body as a reference coordinate system， global positioning cannot be achieved. In order to solve the problems， a positioning method for underground unmanned aerial vehicles （UAV） in coal mines based on global point cloud map is proposed. The method uses Fast-LIO2 algorithm as the lidar SLAM algorithm to obtain UAV position and attitude estimation. An iterative nearest-neighbor algorithm is used for two-step matching of the acquired real-time lidar point cloud and the global point cloud map to achieve UAV position and attitude correction. To address the issue of point cloud matching speed not ensuring real-time positioning due to the excessive number of point clouds， a time- based position and attitude output strategy is designed to increase the frequency of outputting UAV position and attitude data. The SLAM precision and position and attitude correction effect of the UAV positioning method are tested in a 1000 m underground coal mine roadway. The results show that in long-distance roadway environments， the cumulative positioning error of the Fast-LIO2 algorithm is less than 1 m， and is less than 0.3 min the range of 600 m or more， which is significantly smaller than the cumulative positioning errors of LOAM- Livox algorithm and LIO-Livox algorithm. The position and attitude estimation output by the Fast-LIO2 algorithm has been corrected by the correction algorithm， and all flight paths are located in the global point cloud map， verifying the effectiveness of the position and attitude correction algorithm. The time consumption of single SLAM algorithm operation is 14.83 ms， the one of single position and attitude correction is 883 ms， and the output frequency of position and attitude data is 10 Hz， meeting the real-time requirements of UAV positioning.

Key words： UAV positioning; simultaneous localization and mapping; lidar; inertial measurement unit; global point cloud map; position and attitude correction

0 引言

近年來，無人機在煤礦災后救援[1-2]、井下探測[3]、設備巡檢[4]等領域得到了研究和應用。精確定位是無人機開展各項應用的重要前提。目前民用無人機定位和導航依賴全球導航衛星系統（Global Navigation Satellite System，GNSS）實現。而煤礦井下無法接收到 GNSS 信號，這對無人機在井下的應用造成了很大困難。近年來，隨著即時定位與建圖（Simultaneous Localization and Mapping，SLAM）技術的發展，無人機已經能在限定條件的無 GNSS 環境中實現自動巡航飛行，如文獻[5-7]將 SLAM 技術應用于煤礦井下環境中的無人機定位，其中文獻[7]采用激光雷達實時建圖（Lidar Odometry and Mapping in Real-time，LOAM）算法[8-9]，在精確定位基礎上實現了無人機自主避障。為提高 SLAM 算法性能，文獻[10]采用回環檢測方法優化了 SLAM建圖效果；文獻[11]引入基于關鍵幀的滑動窗口及因子圖，提高了 SLAM定位精度；文獻[12]采用卡爾曼濾波器實現慣性測量單元（Inertial Measurement Unit， IMU）數據和激光點云數據的緊耦合，進一步提高了 SLAM 定位精度。上述研究可在一定程度上提升基于 SLAM 算法的煤礦井下移動平臺自主定位效果[13-16]。但井下巷道是一類典型的缺乏結構約束的環境，基于特征點云匹配的方法易出現退化問題，導致定位不準確。另外，SLAM 算法以機體作為參考坐標系，無法得到全局坐標信息，但實際應用中需要全局坐標信息，用于提供不同架次飛行中獲取的具有一致坐標信息的任務數據。鑒此，本文提出一種煤礦井下無人機定位方法，結合激光 SLAM 和基于全局點云地圖的位姿校正，實現了無人機在井下巷道較長范圍內的穩定定位。

1 井下無人機定位方法架構

煤礦井下無人機定位方法主要包括前端激光 SLAM 算法和后端基于點云地圖的位姿校正算法，如圖1所示。 SLAM 處理激光雷達點云數據和 IMU 輸出的角速度、加速度數據，通過激光 SLAM 算法得到里程計信息，即無人機當前位姿估計。基于點云地圖的位姿校正算法使用點云地圖確定井下全局坐標系，對無人機的位姿估計誤差進行校正，并基于當前點云幀和點云地圖計算無人機的全局位姿矩陣。

2 激光 SLAM 算法

以 LOAM 為代表的傳統激光 SLAM 算法常在原始點云中提取邊緣特征點和平面特征點，且使用特征點構建地圖，提高了運算效率，但在非結構化場景中定位精度較差。 Fast?LIO2算法[17-18]采用 IMU預積分方法在點云幀采集時間內進行狀態傳播，使用傳播后的狀態校正點云失真，進而使用點云構造殘差，通過迭代擴展卡爾曼濾波器（Iterated Extended Kalman Filter，IEKF）將激光雷達特征點云和 IMU 預積分狀態進行融合。該算法使用原始點云直接計算局部地圖，因此在非結構化或復雜場景中依然保持較高的定位精度；采用新的卡爾曼增益計算公式進行濾波，使計算量依賴于狀態量的維度而不是觀測量的維度，在保證計算精度的同時，降低了 IEKF 的運算復雜度；在近鄰點搜索及地圖構建過程中使用ikd?tree 算法，進一步降低了算法運行時間。因此，采用 Fast?LIO2算法作為激光 SLAM 算法，計算井下無人機位姿估計，流程如圖2所示。需要說明的是，本節提到的點云地圖是激光 SLAM 算法維護的點云地圖，與本文使用的全局點云地圖不同。

3 基于點云地圖的位姿校正算法

3.1 兩步匹配算法

為了解決 SLAM 應用于長廊形巷道易導致無人機位姿漂移的問題，提出了兩步匹配算法，依靠激光掃描點云和全局點云地圖進行匹配，進而實現無人機位姿校正，具體流程如圖3所示。

1）采用離線方式掃描并建立巷道全局點云地圖Pmap，其包含無人機的全部作業場景。設 SLAM 坐標系為 G，全局點云地圖坐標系為M，激光雷達坐標系為 L，當前時刻tk掃描的激光點云為Pk（L）。將激光點云Pk（L）和全局點云地圖Pmap進行第1次體素降采樣，以降低點云數量和密度，提高后續匹配速度。具體過程：將原始點云空間按邊長d1分割為小正方體（即體素），對每個體素中的點做平均，得到均值點，最終只保留均值點。令第1次體素降采樣后的激光點云和全局點云地圖分別為Pk（′）L，Pm（′）ap。

2）通過 Fast –LIO2算法獲取位姿變換矩陣 Tk，將 Pk（′）L按 Tk 進行變換，得到點云 Pk（′）M 。對 Pk（′）M 和 Pm（′）ap進行點云粗匹配，得到位姿變換矩陣 Tk（′）。點云匹配采用迭代最近點（Iterative Closest Point，ICP）算法，具體過程在3.2節給出。

3）對激光點云Pk（L）和全局點云地圖Pmap進行第2次體素降采樣，邊長為d2，d2＜d1，得到點云Pk（′′）L 和全局點云地圖 Pm（′′）ap。將 Pk（′′）L按TkTk（′）進行變換，得到點云Pk（′′）M。對 Pk（′′）M 和 Pm（′′）ap進行點云精匹配，得到位姿變換矩陣 Tk（′′）。最終得到全局位姿變換矩陣 Tk =

3.2 基于 ICP 的點云匹配方法

設待匹配的 2組點云分別為{s1（L），s2（L），··· ， sn（L）}，{s1（M），s2（M），··· ， sn（M）}，si（L）為從激光雷達獲取的第i個點云，si（M）為全局點云地圖中si（L）的最近鄰點云，i=1，2，…，n，n 為待匹配點云個數。則 ICP 目標公式為

式中：E（R′; x′）為代價函數；R′為旋轉矩陣；x′為平移向量。

設sM，sL分別為全局點云地圖和激光雷達點云的質心，令A = sM ? R′sL，則E（R′; x′）可進一步表示為"si（M）? R′si（L）? x′+ A? A"=

由此將求解2組點云的位姿變換問題轉換為求使tr（R′ Z）最大的R′。

對 Z 進行奇異值分解（Singular Value Decomposition，SVD），得

式中：Σ為由 SVD得到的奇異值構成的對角矩陣； U，V為Σ對應的正交矩陣。

取R′= VUT，將R′代入式（5）得到平移向量x′，進而得到全局點云地圖和當前時刻激光點云的全局位姿變換矩陣Tk。

3.3 基于時間的位姿輸出策略

由于點云匹配耗費的時間和點的數量呈正相關，當點數量過多時，點云匹配算法的運行速度無法保證定位數據的實時性。對此，設計了基于時間的位姿輸出策略，在滿足位姿精度要求的前提下，保證位姿的輸出頻率，具體流程如下。

1）設定點云匹配算法的運行頻率f，單次匹配算法運行時間 t應滿足t＞1/f 。

2）獲取激光 SLAM 算法得到的位姿變換矩陣 Tk 。

3）通過點云匹配得到當前時刻全局位姿變換矩陣Tk，則 SLAM 坐標系到點云地圖坐標系的位姿變換矩陣Tk（G）?M = Tk Tk。

4）設下一次運行點云匹配算法的時刻為 tm，將tk到 tm 之間所有的位姿變換矩陣{Tk+1; Tk+2;···; Tm }全部按 Tk（G）?M進行位姿變換，得到全局位姿變換矩陣{T（?）k+1; T（?）k+2;···; T（?）m }。之后轉步驟3）。

需要注意的是，定位系統初始運行時全局位姿變換矩陣的初始經驗值 T（?）1需人為給出。

位姿變換算法偽代碼如下。

4 試驗驗證

4.1 硬件架構

試驗用無人機硬件包括無人機平臺、機載計算機、三維激光雷達，如圖4所示。無人機平臺包括飛行控制器及動力系統，采用 X 型四旋翼布局，如圖5所示。無人機平臺對稱軸距為600 mm，最大起飛質量為4 kg，搭載Livox–AVIA雷達采集環境點云數據和 IMU 數據，通過網口將點云數據和 IMU 數據發送至khadas–vim3機載計算機。機載計算機對接收數據進行定位程序處理，計算無人機當前位姿，并實時規劃無人機飛行軌跡，將當前位姿和飛行路徑發送至飛行控制器，實現無人機精準定位及位置控制。

4.2 SLAM 精度試驗對比

受井下環境條件的限制，難以布置測量設備得到無人機飛行位置的坐標真值，因此直接評估定位精度成本和代價非常高，也不是本文重點。巷道是典型的長廊形結構，由于其在長度方向上缺乏明顯的結構特征，所以激光 SLAM 容易出現定位漂移。為了分析和驗證本文算法在長廊形巷道中的定位精度，設計以下試驗過程。

以某礦輔助運輸大巷為試驗環境，試驗巷道長度為1000 m，巷道較平直，無明顯轉彎，無照明，無大型設備。標記無人機起飛位置 a 和降落位置 b，用100 m 鋼卷尺從位置 a 開始，沿巷道中線每100 m測量標記1個位置點，記為{a1，a2，··· ， a9，b}。無人機從位置 a 起飛至距地面1 m 高度時記錄時間戳 t0，控制無人機沿巷道平飛。飛行過程中記錄激光雷達點云掃描數據、IMU 原始數據，以及到達標記位置和終點的時間戳{t0，t1，··· ， t9，tb }。采用不同的 SLAM 算法處理飛行過程中記錄的數據，主要步驟如下：

1）記錄 SLAM 算法在{t0，t1，··· ， t9，tb }時刻得到的位姿數據{T0，T1，··· ， T9，Tb }。

2）依次計算{t1，t2，··· ， t9，tb }時刻的里程計數據與t0時刻里程計數據差值d ={d1，d2，··· ， d9，db }。

3）計算 d 和各標記點真實里程的誤差。

為驗證本文算法在巷道環境中定位的穩定性，同時消除試驗設置的特殊性，分別從試驗巷道200，400，600 m 處作為起點，以 b 點為終點進行試驗，記錄途經標記點的真實里程誤差。試驗場景如圖6 所示。

采用 LOAM?Livox算法[19]、LIO?Livox算法[20]和本文算法進行里程精度對比。 LOAM?Livox算法僅使用激光點云作為輸入，是 LOAM 算法針對Livox激光雷達的改進版本；LIO?Livox是一種激光點云和 IMU 數據緊耦合的算法。

3種 SLAM 算法的建圖效果如圖7所示，在4組試驗中的定位誤差見表1，誤差曲線如圖8所示。可看出3種算法的定位誤差總體上隨飛行距離不斷累計，在標記 a 點起飛試驗100 m標記處、標記200 m 處起飛試驗400 m 和500 m標記處，LIO?Livox算法的誤差較小，但在600 m 以上范圍內本文算法的累計誤差明顯小于 LOAM?Livox算法和 LIO?Livox算法，且在長距離定位場景中誤差絕對值始終小于1 m，驗證了本文算法在長距離巷道環境中具有較高的定位精度。特別指出的是，在大于600 m距離的試驗巷道場景中，本文算法的定位誤差始終小于0.3 m，滿足實際應用要求。

4.3 位姿校正試驗

由于 SLAM 算法定位以起飛時刻的位姿作為定位坐標系，導致每次飛行中使用的坐標系不同。為驗證提出的位姿校正算法的有效性，設計了以下試驗：使用 Fast?LIO2算法建立巷道的全局點云地圖并保存，使用該地圖確定井下全局坐標系；將無人機起飛位置偏轉一定角度，重新沿巷道飛行，記錄 Fast?LIO2算法輸出的位置數據和本文位姿校正算法輸出的位置數據。試驗結果如圖9所示，藍色為校正前的路徑，紅色為校正后的路徑。可看出校正前的路徑偏離至全局點云地圖之外，校正后的路徑全部位于全局點云地圖中，驗證了算法的有效性。10個標記點坐標見表2。

4.4 算法復雜度分析和實時性試驗

Fast?LIO2算法復雜度分析詳見文獻[17-18]，算法中主要包括 IEKF 算法和ikd?tree 算法。傳統的 IEKF 算法的時間復雜度和觀測量相關，設觀測量維度為 r，時間復雜度為O（r2），而 Fast?LIO2算法采用新的卡爾曼增益計算公式，時間復雜度和狀態量維度直接相關，在保證運算精度的同時，降低了時間復雜度。ikd?tree 算法主要實現地圖的增量操作、地圖重建和 K 近鄰搜索，設ikd?tree 的尺寸為Ntree，則增量操作的時間復雜度為O（log2 Ntree ），地圖重建的時間復雜度為 O（Ntree ），K 近鄰搜索的時間復雜度為 O（Ntree log2 Ntree ）。

ICP 算法的時間復雜度依賴于源點云和目標點云的大小，設Nlidar為激光雷達某一點云幀中的三維點數，Nmap 為全局點云地圖中三維點數，則 ICP 算法單次迭代的時間復雜度為O（NlidarNmap ），詳細分析見文獻[21]。

機載計算機 CPU 為 Amlogic?A311D（ARM 架構、4核2.2 GHz+雙核1.8 GHz），激光雷達數據幀為10 Hz，根據實測，位姿校正算法每20 s運行1次即可滿足要求。4.2節和4.3節的試驗過程中，記錄各步驟運行的平均時間，結果見表3。單次 SLAM 算法運行耗時14.83 ms，單次位姿校正耗時883 ms。位姿校正算法和激光 SLAM 算法分別在2個線程中運行，最終整體計算輸出的位姿數據頻率為10 Hz，而低速無人機位置控制頻率不低于5 Hz 即可滿足實時性，因此本文方法能夠滿足實時性要求。

5 結論

1）提出的井下無人機定位方法采用基于 IEKF 的激光雷達和 IMU 融合 SLAM 獲得無人機位姿估計，基于點云地圖和 ICP點云匹配對無人機位姿進行校正，從而得到無人機井下全局位姿數據。

2）在1000 m井下巷道環境中開展無人機定位試驗，比較了 LOAM?Livox算法、LIO?Livox算法和本文算法在相同數據集上的結果差異，驗證了本文方法具有更高的定位精度和穩定性。

參考文獻（References）：

[1] 鄭學召，童鑫，張鐸，等.礦井危險區域多旋翼偵測無人機關鍵技術探討[J].工礦自動化，2020，46（12）：48-56.

ZHENG Xuezhao，TONG Xin，ZHANG Duo，et al. Discussion on key technologies of multi-rotor detection UAVs in mine dangerous area [J]. Industry and Mine Automation，2020，46（12）：48-56.

[2] 呂文紅，夏雙雙，魏博文，等.基于改進A*算法的災后井下無人機航跡規劃[J].工礦自動化，2018，44（5）：85-90.

LYU Wenhong，XIA Shuangshuang，WEI Bowen，et al. Route planning of unmanned aerial vehicle in post- disaster? underground? based? on? improved? A* algorithm[J]. Industry and Mine Automation，2018，44（5）：85-90.

[3] 張鐸，吳佩利，鄭學召，等.礦井偵測無人機研究現狀與發展趨勢[J].工礦自動化，2020，46（7）：76-81.

ZHANG Duo，WU Peili，ZHENG Xuezhao，et al. Research status and development trend of mine detection unmanned aerial vehicle[J]. Industry and Mine Automation，2020，46（7）：76-81.

[4] 李標.基于無人機技術的煤礦帶式輸送機巡檢方案[J].煤礦安全，2020，51（7）：128-131.

LI Biao. Inspection scheme of coal mine belt conveyor based on UAV technology[J]. Safety in Coal Mines，2020，51（7）：128-131.

[5] 王巖，馬宏偉，王星，等.基于迭代最近點的井下無人機實時位姿估計[J].工礦自動化，2019，45（9）：25-29.

WANG Yan，MA Hongwei，WANG Xing，et al. Real- time pose estimation of underground unmanned aerial vehicle based on ICP method[J]. Industry and Mine Automation，2019，45（9）：25-29.

[6] 夏雙雙，殷立杰.煤礦井下無人機SLAM定位算法研究[J].電子質量，2017（12）：56-61，66.

XIA Shuangshuang，YIN Lijie. Research on SLAM location algorithm of downhole UAV[J]. Electronics Quality，2017（12）：56-61，66.

[7] 江傳龍，黃宇昊，韓超，等.井下巡檢無人機系統設計及定位與避障技術[J].機械設計與研究，2021，37（4）：38-42，48.

JIANG Chuanlong，HUANG Yuhao，HAN Chao，et al. Design of underground inspection UAV system and studyof positioning and obstacle avoidance[J]. Machine Design & Research，2021，37（4）：38-42，48.

[8] ZHANG Ji， SINGH S. Visual-lidar odometry and mapping： low-drift， robust， and fast[C]. IEEE International Conference on Robotics and Automation， Piscataway，2015：2174-2181.

[9] ZHANG Ji，SINGH S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots，2017，41（2）：401-416.

[10] SHAN Tixiao，ENGLOT B. LeGO-LOAM：lightweightand ground-optimized lidar odometry and mapping on variable terrain[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems，Piscataway，2018：4758-4765.

[11] SHAN Tixiao，ENGLOT B，MEYERS D，et al. LIO- SAM： tightly-coupled lidar inertial odometry via smoothing and mapping[C]. IEEE International Conference on Intelligent Robots and Systems，Las Vegas，2020：5135-5142.

[12] CHAO Qin，YE Haoyang，PRANATA C E，et al. LINS：a lidar-inertial state estimator for robust and efficient navigation[C]. IEEE International Conference on Robotics? and? Automation， Piscataway， 2020：8899-8906.

[13] 楊林，馬宏偉，王巖.基于激光慣性融合的煤礦井下移動機器人 SLAM算法[J].煤炭學報，2022，47（9）：3523-3534.

YANG Lin，MA Hongwei，WANG Yan. LiDAR-inertial SLAM for mobile robot in underground coal mine[J]. Journal of China Coal Society，2022，47（9）：3523-3534.

[14] 鄒筱瑜，黃鑫淼，王忠賓，等.基于集成式因子圖優化的煤礦巷道移動機器人三維地圖構建[J].工礦自動化，2022，48（12）：57-67，92.

ZOU Xiaoyu，HUANG Xinmiao，WANG Zhongbin， et al.3D map construction of coal mine roadway mobile robot based on integrated factor graph optimization[J]. Journal of Mine Automation，2022，48（12）：57-67，92.

[15] 李猛鋼，胡而已，朱華.煤礦移動機器人LiDAR/IMU緊耦合SLAM方法[J].工礦自動化，2022，48（12）：68-78.

LI Menggang，HU Eryi，ZHU Hua. LiDAR/IMU tightly- coupled SLAM method for coal mine mobile robot[J]. Journal of Mine Automation，2022，48（12）：68-78.

[16] 馬艾強，姚頑強，藺小虎，等.面向煤礦巷道環境的 LiDAR與IMU融合定位與建圖方法[J].工礦自動化，2022，48（12）：49-56.

MA Aiqiang，YAO Wanqiang，LIN Xiaohu，et al. Coal mine roadway environment-oriented LiDAR and IMU fusion positioning and mapping method[J]. Journal ofMine Automation，2022，48（12）：49-56.

[17] XU Wei， ZHANG Fu. 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.

[18] XU Wei，CAI Yixi，HE Dongjiao，et al. FAST-LIO2：fast? direct? LiDAR-Inertial? odometry[J]. IEEE Transactions on Robotics，2022，38（4）：2053-2073.

[19] LIN Jiarong，ZHANG Fu. LOAM Livox：a fast，robust， high-precision LiDAR odometry and mapping package for LiDARs of small FoV[C]. IEEE International Conference on Robotics and Automation，Paris，2020：3126-3131.

[20] GitHub-Livox-SDK/LIO-Livox：a robust LiDAR-inertial odometry for LivoxLiDAR[EB/OL]. [2022-12-22]. https：//github.com/Livox-SDK/LIO-Livox.

[21] BESL P，MCKAY N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，1992，14（2）：239-256.