高海躍 王凱 王保兵 王丹丹

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

關(guān)鍵詞：無(wú)人機(jī)定位；即時(shí)定位與建圖；激光雷達(dá)；慣性測(cè)量單元；全局點(diǎn)云地圖；位姿校正中圖分類號(hào)： TD67??? 文獻(xiàn)標(biāo)志碼： 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 引言

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

1 井下無(wú)人機(jī)定位方法架構(gòu)

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

2 激光 SLAM 算法

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

3 基于點(diǎn)云地圖的位姿校正算法

3.1 兩步匹配算法

為了解決 SLAM 應(yīng)用于長(zhǎng)廊形巷道易導(dǎo)致無(wú)人機(jī)位姿漂移的問題，提出了兩步匹配算法，依靠激光掃描點(diǎn)云和全局點(diǎn)云地圖進(jìn)行匹配，進(jìn)而實(shí)現(xiàn)無(wú)人機(jī)位姿校正，具體流程如圖3所示。

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

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

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

3.2 基于 ICP 的點(diǎn)云匹配方法

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

式中：E（R′; x′）為代價(jià)函數(shù)；R′為旋轉(zhuǎn)矩陣；x′為平移向量。

設(shè)sM，sL分別為全局點(diǎn)云地圖和激光雷達(dá)點(diǎn)云的質(zhì)心，令A(yù) = sM ? R′sL，則E（R′; x′）可進(jìn)一步表示為"si（M）? R′si（L）? x′+ A? A"=

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

對(duì) Z 進(jìn)行奇異值分解（Singular Value Decomposition，SVD），得

式中：Σ為由 SVD得到的奇異值構(gòu)成的對(duì)角矩陣； U，V為Σ對(duì)應(yīng)的正交矩陣。

取R′= VUT，將R′代入式（5）得到平移向量x′，進(jìn)而得到全局點(diǎn)云地圖和當(dāng)前時(shí)刻激光點(diǎn)云的全局位姿變換矩陣Tk。

3.3 基于時(shí)間的位姿輸出策略

由于點(diǎn)云匹配耗費(fèi)的時(shí)間和點(diǎn)的數(shù)量呈正相關(guān)，當(dāng)點(diǎn)數(shù)量過多時(shí)，點(diǎn)云匹配算法的運(yùn)行速度無(wú)法保證定位數(shù)據(jù)的實(shí)時(shí)性。對(duì)此，設(shè)計(jì)了基于時(shí)間的位姿輸出策略，在滿足位姿精度要求的前提下，保證位姿的輸出頻率，具體流程如下。

1）設(shè)定點(diǎn)云匹配算法的運(yùn)行頻率f，單次匹配算法運(yùn)行時(shí)間 t應(yīng)滿足t＞1/f 。

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

3）通過點(diǎn)云匹配得到當(dāng)前時(shí)刻全局位姿變換矩陣Tk，則 SLAM 坐標(biāo)系到點(diǎn)云地圖坐標(biāo)系的位姿變換矩陣Tk（G）?M = Tk Tk。

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

需要注意的是，定位系統(tǒng)初始運(yùn)行時(shí)全局位姿變換矩陣的初始經(jīng)驗(yàn)值 T（?）1需人為給出。

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

4 試驗(yàn)驗(yàn)證

4.1 硬件架構(gòu)

試驗(yàn)用無(wú)人機(jī)硬件包括無(wú)人機(jī)平臺(tái)、機(jī)載計(jì)算機(jī)、三維激光雷達(dá)，如圖4所示。無(wú)人機(jī)平臺(tái)包括飛行控制器及動(dòng)力系統(tǒng)，采用 X 型四旋翼布局，如圖5所示。無(wú)人機(jī)平臺(tái)對(duì)稱軸距為600 mm，最大起飛質(zhì)量為4 kg，搭載Livox–AVIA雷達(dá)采集環(huán)境點(diǎn)云數(shù)據(jù)和 IMU 數(shù)據(jù)，通過網(wǎng)口將點(diǎn)云數(shù)據(jù)和 IMU 數(shù)據(jù)發(fā)送至khadas–vim3機(jī)載計(jì)算機(jī)。機(jī)載計(jì)算機(jī)對(duì)接收數(shù)據(jù)進(jìn)行定位程序處理，計(jì)算無(wú)人機(jī)當(dāng)前位姿，并實(shí)時(shí)規(guī)劃無(wú)人機(jī)飛行軌跡，將當(dāng)前位姿和飛行路徑發(fā)送至飛行控制器，實(shí)現(xiàn)無(wú)人機(jī)精準(zhǔn)定位及位置控制。

4.2 SLAM 精度試驗(yàn)對(duì)比

受井下環(huán)境條件的限制，難以布置測(cè)量設(shè)備得到無(wú)人機(jī)飛行位置的坐標(biāo)真值，因此直接評(píng)估定位精度成本和代價(jià)非常高，也不是本文重點(diǎn)。巷道是典型的長(zhǎng)廊形結(jié)構(gòu)，由于其在長(zhǎng)度方向上缺乏明顯的結(jié)構(gòu)特征，所以激光 SLAM 容易出現(xiàn)定位漂移。為了分析和驗(yàn)證本文算法在長(zhǎng)廊形巷道中的定位精度，設(shè)計(jì)以下試驗(yàn)過程。

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

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

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

3）計(jì)算 d 和各標(biāo)記點(diǎn)真實(shí)里程的誤差。

為驗(yàn)證本文算法在巷道環(huán)境中定位的穩(wěn)定性，同時(shí)消除試驗(yàn)設(shè)置的特殊性，分別從試驗(yàn)巷道200，400，600 m 處作為起點(diǎn)，以 b 點(diǎn)為終點(diǎn)進(jìn)行試驗(yàn)，記錄途經(jīng)標(biāo)記點(diǎn)的真實(shí)里程誤差。試驗(yàn)場(chǎng)景如圖6 所示。

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

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

4.3 位姿校正試驗(yàn)

由于 SLAM 算法定位以起飛時(shí)刻的位姿作為定位坐標(biāo)系，導(dǎo)致每次飛行中使用的坐標(biāo)系不同。為驗(yàn)證提出的位姿校正算法的有效性，設(shè)計(jì)了以下試驗(yàn)：使用 Fast?LIO2算法建立巷道的全局點(diǎn)云地圖并保存，使用該地圖確定井下全局坐標(biāo)系；將無(wú)人機(jī)起飛位置偏轉(zhuǎn)一定角度，重新沿巷道飛行，記錄 Fast?LIO2算法輸出的位置數(shù)據(jù)和本文位姿校正算法輸出的位置數(shù)據(jù)。試驗(yàn)結(jié)果如圖9所示，藍(lán)色為校正前的路徑，紅色為校正后的路徑?？煽闯鲂Ｕ暗穆窂狡x至全局點(diǎn)云地圖之外，校正后的路徑全部位于全局點(diǎn)云地圖中，驗(yàn)證了算法的有效性。10個(gè)標(biāo)記點(diǎn)坐標(biāo)見表2。

4.4 算法復(fù)雜度分析和實(shí)時(shí)性試驗(yàn)

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

ICP 算法的時(shí)間復(fù)雜度依賴于源點(diǎn)云和目標(biāo)點(diǎn)云的大小，設(shè)Nlidar為激光雷達(dá)某一點(diǎn)云幀中的三維點(diǎn)數(shù)，Nmap 為全局點(diǎn)云地圖中三維點(diǎn)數(shù)，則 ICP 算法單次迭代的時(shí)間復(fù)雜度為O（NlidarNmap ），詳細(xì)分析見文獻(xiàn)[21]。

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

5 結(jié)論

1）提出的井下無(wú)人機(jī)定位方法采用基于 IEKF 的激光雷達(dá)和 IMU 融合 SLAM 獲得無(wú)人機(jī)位姿估計(jì)，基于點(diǎn)云地圖和 ICP點(diǎn)云匹配對(duì)無(wú)人機(jī)位姿進(jìn)行校正，從而得到無(wú)人機(jī)井下全局位姿數(shù)據(jù)。

2）在1000 m井下巷道環(huán)境中開展無(wú)人機(jī)定位試驗(yàn)，比較了 LOAM?Livox算法、LIO?Livox算法和本文算法在相同數(shù)據(jù)集上的結(jié)果差異，驗(yàn)證了本文方法具有更高的定位精度和穩(wěn)定性。

參考文獻(xiàn)（References）：

[1] 鄭學(xué)召，童鑫，張鐸，等.礦井危險(xiǎn)區(qū)域多旋翼偵測(cè)無(wú)人機(jī)關(guān)鍵技術(shù)探討[J].工礦自動(dòng)化，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] 呂文紅，夏雙雙，魏博文，等.基于改進(jìn)A*算法的災(zāi)后井下無(wú)人機(jī)航跡規(guī)劃[J].工礦自動(dòng)化，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] 張鐸，吳佩利，鄭學(xué)召，等.礦井偵測(cè)無(wú)人機(jī)研究現(xiàn)狀與發(fā)展趨勢(shì)[J].工礦自動(dòng)化，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] 李標(biāo).基于無(wú)人機(jī)技術(shù)的煤礦帶式輸送機(jī)巡檢方案[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] 王巖，馬宏偉，王星，等.基于迭代最近點(diǎn)的井下無(wú)人機(jī)實(shí)時(shí)位姿估計(jì)[J].工礦自動(dòng)化，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] 夏雙雙，殷立杰.煤礦井下無(wú)人機(jī)SLAM定位算法研究[J].電子質(zhì)量，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] 江傳龍，黃宇昊，韓超，等.井下巡檢無(wú)人機(jī)系統(tǒng)設(shè)計(jì)及定位與避障技術(shù)[J].機(jī)械設(shè)計(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] 楊林，馬宏偉，王巖.基于激光慣性融合的煤礦井下移動(dòng)機(jī)器人 SLAM算法[J].煤炭學(xué)報(bào)，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] 鄒筱瑜，黃鑫淼，王忠賓，等.基于集成式因子圖優(yōu)化的煤礦巷道移動(dòng)機(jī)器人三維地圖構(gòu)建[J].工礦自動(dòng)化，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] 李猛鋼，胡而已，朱華.煤礦移動(dòng)機(jī)器人LiDAR/IMU緊耦合SLAM方法[J].工礦自動(dòng)化，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] 馬艾強(qiáng)，姚頑強(qiáng)，藺小虎，等.面向煤礦巷道環(huán)境的 LiDAR與IMU融合定位與建圖方法[J].工礦自動(dòng)化，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.