何怡靜 楊維

文章編號(hào)：1671?251X（2024）04?0094?09 ?DOI：10.13272/j.issn.1671-251x.2023080124

摘要：無(wú)人機(jī)在災(zāi)后礦井的自主導(dǎo)航能力是其勝任搶險(xiǎn)救災(zāi)任務(wù)的前提，而在未知三維空間的自主位姿估計(jì)技術(shù)是無(wú)人機(jī)自主導(dǎo)航的關(guān)鍵技術(shù)之一。目前基于視覺(jué)的位姿估計(jì)算法由于單目相機(jī)無(wú)法直接獲取三維空間的深度信息且易受井下昏暗光線影響，導(dǎo)致位姿估計(jì)尺度模糊和定位性能較差，而基于激光的位姿估計(jì)算法由于激光雷達(dá)存在視角小、掃描圖案不均勻及受限于礦井場(chǎng)景結(jié)構(gòu)特征，導(dǎo)致位姿估計(jì)出現(xiàn)錯(cuò)誤。針對(duì)上述問(wèn)題，提出了一種基于視覺(jué)與激光融合的井下災(zāi)后救援無(wú)人機(jī)自主位姿估計(jì)算法。首先，通過(guò)井下無(wú)人機(jī)搭載的單目相機(jī)和激光雷達(dá)分別獲取井下的圖像數(shù)據(jù)和激光點(diǎn)云數(shù)據(jù)，對(duì)每幀礦井圖像數(shù)據(jù)均勻提取 ORB特征點(diǎn)，使用激光點(diǎn)云的深度信息對(duì) ORB特征點(diǎn)進(jìn)行深度恢復(fù)，通過(guò)特征點(diǎn)的幀間匹配實(shí)現(xiàn)基于視覺(jué)的無(wú)人機(jī)位姿估計(jì)。其次，對(duì)每幀井下激光點(diǎn)云數(shù)據(jù)分別提取特征角點(diǎn)和特征平面點(diǎn)，通過(guò)特征點(diǎn)的幀間匹配實(shí)現(xiàn)基于激光的無(wú)人機(jī)位姿估計(jì)。然后，將視覺(jué)匹配誤差函數(shù)和激光匹配誤差函數(shù)置于同一位姿優(yōu)化函數(shù)下，基于視覺(jué)與激光融合來(lái)估計(jì)井下無(wú)人機(jī)位姿。最后，通過(guò)視覺(jué)滑動(dòng)窗口和激光局部地圖引入歷史幀數(shù)據(jù)，構(gòu)建歷史幀數(shù)據(jù)和最新估計(jì)位姿之間的誤差函數(shù)，通過(guò)對(duì)誤差函數(shù)的非線性優(yōu)化完成在局部約束下的無(wú)人機(jī)位姿的優(yōu)化和修正，避免估計(jì)位姿的誤差累計(jì)導(dǎo)致無(wú)人機(jī)軌跡偏移。模擬礦井災(zāi)后復(fù)雜環(huán)境進(jìn)行仿真實(shí)驗(yàn)，結(jié)果表明：基于視覺(jué)與激光融合的位姿估計(jì)算法的平均相對(duì)平移誤差和相對(duì)旋轉(zhuǎn)誤差分別為0.0011 m和0.0008°， 1幀數(shù)據(jù)的平均處理時(shí)間低于100 ms，且算法在井下長(zhǎng)時(shí)間運(yùn)行時(shí)不會(huì)出現(xiàn)軌跡漂移問(wèn)題；相較于僅基于視覺(jué)或激光的位姿估計(jì)算法，該融合算法的準(zhǔn)確性、穩(wěn)定性均得到了提高，且實(shí)時(shí)性滿足要求。

關(guān)鍵詞：井下無(wú)人機(jī)；位姿估計(jì)；單目相機(jī)；激光雷達(dá)；視覺(jué)與激光融合；ORB 特征點(diǎn)中圖分類號(hào)：TD67 ?文獻(xiàn)標(biāo)志碼：A

Autonomous pose estimation of underground disaster rescue drones based on visual and laser fusion

HE Yijing， YANG Wei

（School of Electronic and Information Engineering， Beijing Jiaotong University， Beijing 100044， China）

Abstract： The autonomous navigation capability of drones in post disaster mines is a prerequisite for their capability to perform rescue and disaster relief tasks. The autonomous pose estimation technology in unknown three-dimensional space is one of the key technologies for autonomous navigation of drones. At present， vision based pose estimation algorithms are prone to blurred scale and poor positioning performance due to the inability of monocular cameras to directly obtain depth information in three-dimensional space and the susceptibility to underground dim light. However， laser based pose estimation algorithms are prone to errors due to the small viewing angle， uneven scanning patterns， and constraints on the structural characteristics of mining scenes causedby LiDAR. In order to solve the above problems， an autonomous pose estimation algorithm of underground disaster rescue drones based on visual and laser fusion is proposed. Firstly， themonocular camera and LiDAR carried by the underground drone are used to obtain the image data and laser point cloud data of the mine. The ORB feature points are uniformly extracted from each frame of the mine image data. The depth information of the laser point cloud is used to recover the ORB feature points. The visual based drone pose estimation is achieved through inter frame matching of the feature points. Secondly， feature corner points and feature plane points are extracted from each frame of underground laser point cloud data， and laser based drone pose estimation is achieved through inter frame matching of feature points. Thirdly， the visual matching error function and the laser matching error function are placed under the same pose optimization function， and the pose of the underground drone is estimated based on vision and laser fusion. Finally， historical frame data is introduced through visual sliding windows and laser local maps to construct an error function between the historical frame data and the latest estimated pose. The optimization and correction of the drone pose under local constraints are completed through nonlinear optimization of the error function， avoiding the accumulation of estimated pose errors that may lead to trajectory deviation of the drone. The simulation experiments that simulating the complex environment after a mine disaster are conducted. The results show that the average relative translation error and relative rotation error of the pose estimation algorithm based on visual and laser fusion are 0.0011 m and 0.0008°， respectively. The average processing time of one frame of data is less than 100 ms. The algorithm does not experience trajectory drift during long-term operation underground. Compared to pose estimation algorithms based solely on vision or laser， the accuracy and stability of this fusion algorithm have been improved， and the real-time performance meets the requirements.

Key words： underground drones; pose estimation; monocular camera; Lidar; visual and laser fusion; ORB feature points

0引言

由于礦井生產(chǎn)條件復(fù)雜，加之有些企業(yè)內(nèi)部安全生產(chǎn)監(jiān)管缺失，易導(dǎo)致井下發(fā)生瓦斯爆炸、水害等事故[1]，給人民群眾的生命和財(cái)產(chǎn)造成極大損失。災(zāi)后礦井具有場(chǎng)景未知、光線昏暗、環(huán)境危險(xiǎn)的特點(diǎn)，為災(zāi)后救援工作帶來(lái)極大困難。無(wú)人機(jī)具有體積小、靈敏度高、適于狹小危險(xiǎn)環(huán)境中飛行的優(yōu)點(diǎn)[2]，可利用無(wú)人機(jī)承擔(dān)災(zāi)后礦井救援的偵測(cè)、搜救和運(yùn)送小型救援物資等任務(wù)[3]。無(wú)人機(jī)在井下災(zāi)后環(huán)境下的自主導(dǎo)航能力是其完成搶險(xiǎn)救災(zāi)任務(wù)的前提[4]，而在未知三維空間中的自主位姿估計(jì)技術(shù)是無(wú)人機(jī)自主導(dǎo)航的關(guān)鍵技術(shù)之一[5]。

目前，在災(zāi)后礦井無(wú)人機(jī)位姿估計(jì)研究方面，包括基于單目相機(jī)的視覺(jué)位姿估計(jì)方法和基于激光雷達(dá)的激光位姿估計(jì)方法。井下無(wú)人機(jī)搭載的單目相機(jī)連續(xù)拍攝災(zāi)后礦井圖像，通過(guò)對(duì)圖像提取并匹配 ORB（Oriented FAST and Rotated BRIEF）特征點(diǎn)，構(gòu)建視覺(jué)匹配誤差，將視覺(jué)匹配誤差最小化以完成基于視覺(jué)的井下無(wú)人機(jī)位姿估計(jì)[6-7]。但由于單目相機(jī)無(wú)法直接獲取三維空間的深度信息，僅基于視覺(jué)估計(jì)出的井下無(wú)人機(jī)位姿和真實(shí)位姿之間相差一個(gè)尺度因子。井下無(wú)人機(jī)通過(guò)搭載的三維激光雷達(dá)掃描井下環(huán)境得到激光點(diǎn)，1個(gè)掃描周期內(nèi)獲取的所有激光點(diǎn)的集合記為1幀激光點(diǎn)云。通過(guò)對(duì)井下激光點(diǎn)云進(jìn)行激光特征點(diǎn)的提取和匹配，構(gòu)建激光匹配誤差，通過(guò)對(duì)激光匹配誤差最小化完成基于激光的井下無(wú)人機(jī)位姿估計(jì)。但激光雷達(dá)存在視角小、掃描圖案不均勻的問(wèn)題，容易出現(xiàn)激光特征點(diǎn)匹配退化現(xiàn)象，為解決該問(wèn)題，Lin Jiarong 等[8]提出了 LOAM livox 算法來(lái)優(yōu)化激光點(diǎn)云篩選和激光特征點(diǎn)提取。為提升位姿估計(jì)在劇烈運(yùn)動(dòng)場(chǎng)景下的穩(wěn)定性，P. Dellenbach 等[9]基于數(shù)據(jù)間的連續(xù)性提出了?CT? ICP 算法，增加了彈性閉環(huán)檢測(cè)，可適應(yīng)激光雷達(dá)快速劇烈運(yùn)動(dòng)的場(chǎng)景。另外，K. Chen 等[10]提出了一種基于稠密點(diǎn)云塊的激光位姿估計(jì)算法，利用字典形式存儲(chǔ)激光點(diǎn)云，進(jìn)一步降低了位姿估計(jì)和閉環(huán)檢測(cè)的計(jì)算量。但由于災(zāi)后礦井巷道中橫向激光特征點(diǎn)的數(shù)量遠(yuǎn)高于縱向激光特征點(diǎn)，上述算法在井下巷道中運(yùn)行時(shí)容易出現(xiàn)激光特征點(diǎn)匹配失誤的問(wèn)題，導(dǎo)致位姿估計(jì)出現(xiàn)錯(cuò)誤，定位和建圖性能迅速劣化。

為解決基于視覺(jué)的井下無(wú)人機(jī)位姿估計(jì)的尺度模糊問(wèn)題，可通過(guò)融合激光雷達(dá)數(shù)據(jù)來(lái)彌補(bǔ)，因?yàn)榧す饫走_(dá)可直接獲取目標(biāo)的精確深度信息，可對(duì)ORB 特征點(diǎn)進(jìn)行深度恢復(fù)[11]。此外，由于災(zāi)后礦井巷道光線昏暗[12]，單目相機(jī)容易出現(xiàn)拍攝不足或過(guò)曝的問(wèn)題，導(dǎo)致在狹窄巷道中的定位性能較差，而激光雷達(dá)受光照等環(huán)境因素的影響較小，所以可通過(guò)融合激光雷達(dá)數(shù)據(jù)對(duì)基于視覺(jué)估計(jì)得到的井下無(wú)人機(jī)位姿進(jìn)行補(bǔ)償[13-19]。為彌補(bǔ)僅基于視覺(jué)或激光在災(zāi)后礦井中進(jìn)行位姿估計(jì)的局限性，本文提出了一種基于視覺(jué)與激光融合的井下災(zāi)后救援無(wú)人機(jī)自主位姿估計(jì)算法。通過(guò)對(duì)井下圖像和井下激光點(diǎn)云分別提取 ORB特征點(diǎn)和激光特征點(diǎn)，并分別進(jìn)行幀間匹配，得到視覺(jué)匹配誤差和激光匹配誤差，然后將2個(gè)匹配誤差置于同一位姿優(yōu)化函數(shù)下求解井下無(wú)人機(jī)位姿。同時(shí)，利用視覺(jué)滑動(dòng)窗口和激光局部地圖引入歷史幀數(shù)據(jù)，對(duì)最新估計(jì)的無(wú)人機(jī)位姿進(jìn)行修正和約束，避免直接拼接井下無(wú)人機(jī)位姿造成的軌跡漂移問(wèn)題。

1井下無(wú)人機(jī)自主位姿估計(jì)流程

建立井下巷道無(wú)人機(jī)坐標(biāo)系，如圖1所示。假設(shè)巷道為封閉空間，兩側(cè)墻壁為豎直，表面有結(jié)構(gòu)化紋理或因受災(zāi)而凹凸不平。以無(wú)人機(jī)在井下巷道中的初始位置為原點(diǎn)?W，建立三維世界坐標(biāo)系W? xyz，?x軸指向無(wú)人機(jī)前進(jìn)方向并平行于巷道縱向，y軸平行于巷道橫向，z軸豎直向上，且?x軸、y軸、z軸遵循右手定則。無(wú)人機(jī)搭載單目相機(jī)和激光雷達(dá)在巷道中飛行，分別以單目相機(jī)光心C和激光雷達(dá)中心L為原點(diǎn)，建立動(dòng)態(tài)的視覺(jué)坐標(biāo)系C? xCyCzC 和激光坐標(biāo)系?L? xLyLzL，2個(gè)坐標(biāo)系的 xC 和 xL 軸、yC 和 yL 軸、 zC 和zL 軸分別代表無(wú)人機(jī)的正前方、正左方和正上方，遵循右手定則。由于相機(jī)和激光雷達(dá)在飛行過(guò)程中相對(duì)位置不變，利用相機(jī)和激光雷達(dá)之間的外參標(biāo)定可獲取2個(gè)坐標(biāo)系之間的轉(zhuǎn)換矩陣TC;L 。

無(wú)人機(jī)自主位姿估計(jì)流程如圖2所示。井下無(wú)人機(jī)通過(guò)搭載的單目相機(jī)和激光雷達(dá)分別獲取井下圖像和井下激光點(diǎn)云。對(duì)于井下圖像提取 ORB 特征點(diǎn)并進(jìn)行深度恢復(fù)，通過(guò) ORB 特征點(diǎn)的幀間匹配構(gòu)建視覺(jué)匹配誤差，通過(guò)對(duì)視覺(jué)匹配誤差最小化實(shí)現(xiàn)基于視覺(jué)的無(wú)人機(jī)位姿估計(jì)；對(duì)于井下激光點(diǎn)云提取激光特征點(diǎn)，通過(guò)激光特征點(diǎn)的幀間匹配構(gòu)建激光匹配誤差，通過(guò)對(duì)激光匹配誤差最小化實(shí)現(xiàn)基于激光的無(wú)人機(jī)位姿估計(jì)。將視覺(jué)匹配誤差和激光匹配誤差置于同一位姿優(yōu)化函數(shù)下，基于視覺(jué)與激光融合估計(jì)井下無(wú)人機(jī)位姿。利用視覺(jué)滑動(dòng)窗口和激光局部地圖引入歷史幀數(shù)據(jù)，構(gòu)建歷史幀數(shù)據(jù)和最新估計(jì)位姿之間的重投影誤差函數(shù)，并通過(guò)非線性優(yōu)化完成在局部約束下的無(wú)人機(jī)位姿優(yōu)化和修正，實(shí)時(shí)輸出井下無(wú)人機(jī)6DOF 位姿和軌跡圖。

2基于視覺(jué)和激光的災(zāi)后礦井環(huán)境特征點(diǎn)提取

2.1 ORB 特征點(diǎn)深度恢復(fù)

井下無(wú)人機(jī)首先對(duì)單目相機(jī)采集的礦井圖像進(jìn)行 ORB特征點(diǎn)提取，由于提取的 ORB特征點(diǎn)不具備深度信息，通過(guò)匹配該特征點(diǎn)得到的井下無(wú)人機(jī)位姿與真實(shí)位姿之間將相差一個(gè)尺度因子，所以在完成 ORB特征點(diǎn)提取后，需利用激光點(diǎn)云對(duì)提取的 ORB 特征點(diǎn)進(jìn)行深度恢復(fù)。

利用空間比例投影法恢復(fù) ORB特征點(diǎn)的深度，如圖3所示。相機(jī)像素平面 P上第 k 幀數(shù)據(jù)的ORB 特征點(diǎn) i 的坐標(biāo)記作Xk;（P）i，將該特征點(diǎn)和視覺(jué)坐標(biāo)系 xC 軸正向的激光點(diǎn)按比例投影到 xC 軸正向dK 處的平面 K上，得到視覺(jué)特征投影點(diǎn)和激光點(diǎn)云投影點(diǎn)，視覺(jué)特征投影點(diǎn)的坐標(biāo)記為Xk;（P）i′。對(duì)于平面K上的每個(gè)視覺(jué)特征投影點(diǎn)，通過(guò) KD 樹(shù)（K-dimensional Tree）搜索算法搜尋與其距離最近的3個(gè)激光點(diǎn)云投影點(diǎn)，坐標(biāo)記為Xk;（L）1（′），Xk;（L）2（′），Xk;（L）3（′）。這3個(gè)激光點(diǎn)云投影點(diǎn)對(duì)應(yīng)的原始激光點(diǎn)的坐標(biāo)為 Xk;（L）1，Xk;（L）2，Xk;（L）3，將這3個(gè)原始激光點(diǎn)擬合成一個(gè)平面 F，并將 ORB 特征點(diǎn)按相機(jī)透視原理投影到平面 F上，得到 ORB 特征點(diǎn)的三維估計(jì)點(diǎn)，該點(diǎn)坐標(biāo)記為 X（?）k;（C）i 。ORB 特征點(diǎn)的三維估計(jì)點(diǎn)與無(wú)人機(jī)當(dāng)前所在位置的水平距離 d（?）k;i 即該 ORB特征點(diǎn)的深度信息。

2.2激光雷達(dá)特征點(diǎn)提取

與 ORB特征點(diǎn)不同的是，井下激光點(diǎn)云中的激光特征點(diǎn)分為兩類：一類是處在尖銳邊緣上的激光點(diǎn)，一般位于災(zāi)后塌落的雜物、石塊等，此類激光點(diǎn)與鄰域點(diǎn)差異較大，記作特征角點(diǎn)；另一類是位于光滑平面上的激光點(diǎn)，例如巷道壁或管道等，此類激光點(diǎn)與鄰域點(diǎn)的差異較小，記作特征平面點(diǎn)。因此，通過(guò)計(jì)算每個(gè)激光點(diǎn)與其鄰域點(diǎn)之間的差異，即可提取井下激光點(diǎn)云中的激光特征點(diǎn)，這種差異用曲率表示：

c = j2∈*j1"Xk;（L）j2? Xk;（L）j1"（1）

式中：c為激光點(diǎn) j1的曲率；S為激光點(diǎn) j1在其所在激光線上的鄰域點(diǎn)集合；Xk;（L）j1和 Xk;（L）j2分別為激光點(diǎn) j1和j2的坐標(biāo)。

根據(jù)井下環(huán)境設(shè)定曲率的角點(diǎn)閾值和平面點(diǎn)閾值。當(dāng)激光點(diǎn)的曲率高于角點(diǎn)閾值時(shí)，說(shuō)明該點(diǎn)與周圍環(huán)境差異較大，將其加入特征角點(diǎn)集合ε;當(dāng)激光點(diǎn)的曲率低于平面點(diǎn)閾值，說(shuō)明該點(diǎn)位于差異不大的平面上，將其加入特征平面點(diǎn)集合H。若激光點(diǎn)的曲率處于平面點(diǎn)閾值和角點(diǎn)閾值之間，則該點(diǎn)特征性較弱，對(duì)后續(xù)幀間匹配屬于冗余信息，不加入特征點(diǎn)集，避免計(jì)算資源浪費(fèi)。

3基于視覺(jué)與激光融合的井下無(wú)人機(jī)位姿估計(jì)

3.1視覺(jué)幀間匹配與位姿估計(jì)

對(duì)于視覺(jué)坐標(biāo)系下的井下無(wú)人機(jī)位姿，利用第 k 幀數(shù)據(jù)中的 ORB特征點(diǎn) i的歸一化坐標(biāo)X（?）k;（C）i =（?（x）k;i ; y（?）k;i ; k;i ），可得

其中，Rk（C）+1;k =[R（0） R（1） R（2）]T 和 tk（C）+1;k =[t（0） t（1） t（2）]T 分別為無(wú)人機(jī)從第k幀到第k+1幀數(shù)據(jù)的視覺(jué)坐標(biāo)系下1;k 的旋轉(zhuǎn)矩陣和平移矩陣，即

將式（2）和式（3）作為匹配誤差進(jìn)行非線性優(yōu)化[20]，即可估計(jì)出視覺(jué)坐標(biāo)系下無(wú)人機(jī)從第k幀到1;k 。

3.2激光幀間匹配與位姿估計(jì)

對(duì)于激光坐標(biāo)系下的井下無(wú)人機(jī)位姿，利用激光點(diǎn)云中的匹配特征角點(diǎn)和匹配特征平面點(diǎn)之間的距離作為激光匹配誤差eε和eH，將其放在同一位姿優(yōu)化函數(shù)下，使得2幀激光點(diǎn)云之間所有匹配誤差的二范數(shù)平方和達(dá)到最低，從而求解出基于激光的井下無(wú)人機(jī)位姿Tk（L）+1;k 。

3.3視覺(jué)與激光融合位姿估計(jì)

井下無(wú)人機(jī)從第k幀到第k+1幀數(shù)據(jù)的位姿可記作 Tk+1;k = exp （ξk+1;k ），其中ξk+1;k 為 Tk+1;k 的李代數(shù)表示，是一個(gè)六維向量，代表井下無(wú)人機(jī)的6DOF 位姿。構(gòu)造位姿優(yōu)化函數(shù)，尋找最優(yōu)ξk+1;k，使得視覺(jué)和激光特征點(diǎn)在前后幀的匹配總誤差最小，即第k幀數(shù)據(jù)和第k+1幀數(shù)據(jù)之間的匹配誤差ek 的二范數(shù)平方總和最小。

式中：ek（C）和ek（L）分別為第k幀數(shù)據(jù)中的 ORB特征點(diǎn)和激光特征點(diǎn)的匹配誤差；Xk（C）和 Xk（L）分別為第k幀數(shù)據(jù)中的 ORB特征點(diǎn)和激光特征點(diǎn)的坐標(biāo)；X 11分別為第k+1幀數(shù)據(jù)中的 ORB特征點(diǎn)和激光特征點(diǎn)的坐標(biāo)。

對(duì)式（6）進(jìn)行非線性優(yōu)化，在相機(jī)和激光雷達(dá)觀測(cè)數(shù)據(jù)的共同約束下，迭代求解無(wú)人機(jī)在井下的初始融合位姿。但式（6）對(duì)誤匹配的特征點(diǎn)非常敏感，因?yàn)檎`匹配會(huì)使得對(duì)應(yīng)誤差項(xiàng)偏大，而二范數(shù)平方的增長(zhǎng)速率極快，將會(huì)耗費(fèi)大量時(shí)間用于調(diào)整一個(gè)錯(cuò)誤的值，因此將二范數(shù)度量替換為一個(gè)增長(zhǎng)速率較緩的魯棒核函數(shù) Huber 函數(shù)。替換后的匹配誤差可表示為

式中δ為閾值。

加入魯棒核函數(shù)后，當(dāng)匹配特征點(diǎn)的誤差較小時(shí)，保持原有的度量，當(dāng)誤差大于閾值δ后，函數(shù)增長(zhǎng)由二次形式變成一次形式，保證了在對(duì)多個(gè)誤差平方進(jìn)行聯(lián)合優(yōu)化時(shí)減少無(wú)效的觀測(cè)殘差，降低了誤匹配特征點(diǎn)等異常項(xiàng)在代價(jià)函數(shù)中的影響，從而提高位姿優(yōu)化函數(shù)的穩(wěn)定性。

4井下無(wú)人機(jī)位姿的局部約束優(yōu)化

將算法估計(jì)得到的每一幀的無(wú)人機(jī)位姿進(jìn)行拼接，即可得到無(wú)人機(jī)在井下的運(yùn)動(dòng)軌跡。但由于無(wú)人機(jī)每一幀的位姿誤差不斷累計(jì)，最終將造成無(wú)人機(jī)軌跡出現(xiàn)無(wú)法修復(fù)的偏移，使得無(wú)人機(jī)在井下的位姿估計(jì)精度迅速下降。因此，提出引入視覺(jué)和激光的歷史幀數(shù)據(jù)來(lái)對(duì)最新估計(jì)的位姿進(jìn)行約束，保證無(wú)人機(jī)位姿的局部連貫性，避免軌跡漂移。

4.1視覺(jué)關(guān)鍵幀選取策略

當(dāng)無(wú)人機(jī)在井下運(yùn)行時(shí)，利用視覺(jué)滑動(dòng)窗口可保留過(guò)去一段時(shí)間內(nèi)的歷史圖像數(shù)據(jù)，計(jì)算窗口內(nèi)的歷史幀數(shù)據(jù)與最新估計(jì)的位姿之間的誤差函數(shù)，通過(guò)對(duì)其進(jìn)行最小化，即可實(shí)現(xiàn)對(duì)最新估計(jì)位姿的修正和約束。在構(gòu)建滑動(dòng)窗口時(shí)需要適時(shí)添加和刪除關(guān)鍵幀，在選取關(guān)鍵幀時(shí)，既要保證加入的關(guān)鍵幀數(shù)據(jù)能有效修正軌跡偏差，也要避免過(guò)高的計(jì)算成本[21]。因此，提出基于場(chǎng)景的關(guān)鍵幀選擇策略，如圖4所示。無(wú)人機(jī)在井下巷道中拍攝的所有圖像可分為動(dòng)態(tài)幀、靜態(tài)幀和普通幀3類。動(dòng)態(tài)幀是指無(wú)人機(jī)在井下出現(xiàn)大幅運(yùn)動(dòng)或在巷道轉(zhuǎn)角處的場(chǎng)景，此時(shí) ORB 特征點(diǎn)匹配難度增加，應(yīng)提高關(guān)鍵幀密度。靜態(tài)幀是無(wú)人機(jī)靜止或運(yùn)動(dòng)較為緩慢時(shí)，視野內(nèi)出現(xiàn)了其他運(yùn)動(dòng)的干擾物體的場(chǎng)景，此時(shí)將會(huì)出現(xiàn)動(dòng)態(tài)的 ORB特征點(diǎn)來(lái)干擾位姿估計(jì)，因而當(dāng)視野內(nèi)動(dòng)態(tài)特征點(diǎn)的數(shù)量超過(guò)閾值后，應(yīng)舍棄該幀。普通幀是指剩余的普通情況下的幀。關(guān)鍵幀的選取應(yīng)遵循以下原則：①初始時(shí)刻的圖像幀標(biāo)記為關(guān)鍵幀。②動(dòng)態(tài)特征點(diǎn)數(shù)量超過(guò)閾值的靜態(tài)幀不應(yīng)標(biāo)記為關(guān)鍵幀。③求解相鄰關(guān)鍵幀的位姿差距ΔT = T 1 Tn（?Tn 為最新估計(jì)位姿，Tn?1為與當(dāng)前幀數(shù)據(jù)距離最近的滑動(dòng)窗口內(nèi)的關(guān)鍵幀的位姿），若丟棄幀超過(guò)10幀或ΔT 超過(guò)設(shè)定閾值時(shí)，則認(rèn)為2幀數(shù)據(jù)之間視差較大，應(yīng)將最新一幀標(biāo)記為關(guān)鍵幀，加入滑動(dòng)窗口。遵循上述原則添加關(guān)鍵幀，可保證在無(wú)人機(jī)運(yùn)動(dòng)快、ORB 特征點(diǎn)匹配量少時(shí)，關(guān)鍵幀提取密集，從而實(shí)現(xiàn)位姿精度和計(jì)算實(shí)時(shí)性之間的平衡。

4.2局部約束優(yōu)化

利用關(guān)鍵幀選取策略將一幀圖像數(shù)據(jù)標(biāo)記為關(guān)鍵幀后，需引入其對(duì)應(yīng)的激光歷史幀數(shù)據(jù)，即利用坐標(biāo)變換將激光數(shù)據(jù)投影到世界坐標(biāo)系下[22]。同視覺(jué)滑動(dòng)窗口一樣，激光歷史幀數(shù)據(jù)同樣可以對(duì)最新加入的估計(jì)位姿進(jìn)行約束和優(yōu)化，由于激光雷達(dá)的特征點(diǎn)分為2類，需要用2類激光特征點(diǎn)分別約束。具體做法：對(duì)于每一個(gè)新加入的特征角點(diǎn)和平面點(diǎn)，分別在角點(diǎn)映射和平面點(diǎn)映射中進(jìn)行最近鄰搜索，得到特征點(diǎn)鄰近的特征點(diǎn)云簇，計(jì)算點(diǎn)云簇的協(xié)方差矩陣。根據(jù)協(xié)方差矩陣中特征值大小和特征向量的對(duì)應(yīng)關(guān)系可知，特征角點(diǎn)對(duì)應(yīng)的點(diǎn)云簇呈線狀，特征角點(diǎn)和對(duì)應(yīng)點(diǎn)云簇之間的距離為

式中：n?（x）;ε為點(diǎn)云簇坐標(biāo)均值點(diǎn)到特征角點(diǎn)的向量；nε為點(diǎn)云簇的方向向量，即其協(xié)方差矩陣的最大特征值對(duì)應(yīng)的特征向量。

特征平面點(diǎn)對(duì)應(yīng)的點(diǎn)云簇呈面狀，特征平面點(diǎn)到對(duì)應(yīng)點(diǎn)云簇的距離為

式中：n?（x）;H 為點(diǎn)云簇坐標(biāo)均值點(diǎn)到特征平面點(diǎn)的向量；nH 為點(diǎn)云簇的平面法向量。

將視覺(jué)滑動(dòng)窗口中的數(shù)據(jù)轉(zhuǎn)換到世界坐標(biāo)系下，對(duì)視覺(jué)和激光的歷史幀數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)在二者共同局部約束下的無(wú)人機(jī)位姿優(yōu)化。構(gòu)建井下無(wú)人機(jī)的整體狀態(tài)向量，其中包括滑動(dòng)窗口內(nèi)無(wú)人機(jī)在世界坐標(biāo)系下的位姿、ORB 特征點(diǎn)集和激光特征點(diǎn)集。利用滑動(dòng)窗口內(nèi)的歷史幀數(shù)據(jù)對(duì)無(wú)人機(jī)的整體狀態(tài)向量構(gòu)建局部約束，即可完成對(duì)無(wú)人機(jī)位姿的優(yōu)化。對(duì)整體狀態(tài)向量構(gòu)建約束的數(shù)學(xué)表示為

式中：ewin 為關(guān)鍵幀的共視特征點(diǎn)的重投影誤差； eprior 為滑動(dòng)窗口邊緣化帶來(lái)的先驗(yàn)信息誤差。

利用非線性優(yōu)化求解式（10），即可得到在視覺(jué)和激光共同局部約束下優(yōu)化的井下無(wú)人機(jī)位姿。由于單目相機(jī)和激光雷達(dá)提取到的特征點(diǎn)相互獨(dú)立，增量方程中對(duì)應(yīng)的特征矩陣為零或呈現(xiàn)較強(qiáng)稀疏性，極大降低了優(yōu)化的計(jì)算成本，保證了實(shí)時(shí)性。

5 仿真與分析

為檢驗(yàn)基于視覺(jué)與激光融合的井下災(zāi)后救援無(wú)人機(jī)自主位姿估計(jì)算法的有效性，在 Ubuntu16.04中模擬災(zāi)后礦井巷道并在仿真平臺(tái)上進(jìn)行實(shí)驗(yàn)，同時(shí)與僅基于視覺(jué)或激光的位姿估計(jì)算法進(jìn)行比較。模擬的災(zāi)后礦井巷道寬度為4 m、高度約為3 m?？紤]到災(zāi)后礦井巷道光線昏暗且存在煙塵，對(duì)仿真所有獲得的井下圖像進(jìn)行亮度降低與模糊處理。為保證仿真環(huán)境復(fù)雜度，設(shè)置大量走動(dòng)或靜止的行人、木箱、石塊等障礙物，并對(duì)側(cè)壁進(jìn)行凹凸處理。將環(huán)境柵格化、二進(jìn)制化處理，障礙物點(diǎn)為1、其余為0，得到環(huán)境矩陣C（矩陣大小為h × w）。計(jì)算環(huán)境矩陣的漢明距離D（C），則環(huán)境復(fù)雜度可表示為

真實(shí)礦井正常工作的平均復(fù)雜度為0.227，而仿真環(huán)境平均復(fù)雜度達(dá)0.518，可以模擬災(zāi)后環(huán)境的復(fù)雜程度。

5.1算法準(zhǔn)確性

不同算法的估計(jì)軌跡與真實(shí)軌跡比較如圖5所示（t 為時(shí)間）?？煽闯鱿啾扔趦H基于視覺(jué)或激光的位姿估計(jì)算法，基于視覺(jué)與激光融合的位姿估計(jì)算法在各個(gè)平面上的軌跡都明顯接近于真實(shí)軌跡，且更加平滑。這是由于基于視覺(jué)與激光融合的位姿估計(jì)算法添加了歷史幀約束，保證了局部軌跡的一致性，在井下巷道狹窄環(huán)境中能夠避免較大的軌跡漂移，實(shí)現(xiàn)較為精確的位姿估計(jì)。

除軌跡圖的定性評(píng)估外，采用絕對(duì)位姿誤差和相對(duì)位姿誤差[23]對(duì)算法的精確性進(jìn)行定量評(píng)估。在仿真平臺(tái)上分別運(yùn)行基于視覺(jué)的位姿估計(jì)算法、基于激光的位姿估計(jì)算法和基于視覺(jué)與激光融合的位姿估計(jì)算法，計(jì)算估計(jì)軌跡與真實(shí)軌跡的絕對(duì)位姿誤差和相對(duì)位姿誤差，分別以均方根誤差、標(biāo)準(zhǔn)差和均值等誤差評(píng)價(jià)指標(biāo)呈現(xiàn)，如圖6所示?？煽闯龌谝曈X(jué)的位姿估計(jì)算法在各誤差評(píng)價(jià)指標(biāo)中表現(xiàn)最差，而基于視覺(jué)與激光融合的位姿估計(jì)算法則表現(xiàn)最優(yōu)；基于視覺(jué)與激光融合的位姿估計(jì)算法的絕對(duì)平移誤差、相對(duì)平移誤差和相對(duì)旋轉(zhuǎn)誤差均遠(yuǎn)低于基于視覺(jué)的位姿估計(jì)算法和基于激光的位姿估計(jì)算法，各項(xiàng)誤差均值分別降低到4.52 m、0.0011 m、和0.0008°。因此，與基于視覺(jué)的位姿估計(jì)算法和基于激光的位姿估計(jì)算法相比，基于視覺(jué)與激光融合的位姿估計(jì)算法的精確度較高，能夠更精準(zhǔn)地實(shí)現(xiàn)無(wú)人機(jī)位姿估計(jì)。

5.2算法穩(wěn)定性

算法穩(wěn)定性測(cè)試通過(guò)計(jì)算算法估計(jì)軌跡和真實(shí)軌跡每隔100 m 的平均平移誤差和平均旋轉(zhuǎn)誤差來(lái)實(shí)現(xiàn)，結(jié)果如圖7所示。從圖7（a）可看出，僅基于視覺(jué)或激光的位姿估計(jì)算法的平移誤差隨運(yùn)行路徑長(zhǎng)度的增加而迅速增加，這是因?yàn)閹g誤差隨時(shí)間累計(jì)，長(zhǎng)時(shí)間運(yùn)行將會(huì)導(dǎo)致較大的定位漂移；而基于視覺(jué)與激光融合的位姿估計(jì)算法加入了歷史幀約束，實(shí)時(shí)對(duì)最新估計(jì)位姿進(jìn)行優(yōu)化，因此平移誤差相對(duì)較低且變化平穩(wěn)。從圖7（b）可看出，基于視覺(jué)與激光融合的位姿估計(jì)算法的平均旋轉(zhuǎn)誤差遠(yuǎn)低于僅基于視覺(jué)或激光的位姿估計(jì)算法，且隨運(yùn)行路徑長(zhǎng)度增加而平穩(wěn)下降。因此，相較于僅基于視覺(jué)或激光的位姿估計(jì)算法，基于視覺(jué)與激光融合的位姿估計(jì)算法的平均誤差均較低，具有較高的穩(wěn)定性，能夠滿足井下位姿估計(jì)的穩(wěn)定性要求。

5.3算法實(shí)時(shí)性

通過(guò)分析算法運(yùn)行時(shí)間和資源占用率來(lái)驗(yàn)證算法的實(shí)時(shí)性。算法各模塊的平均運(yùn)行時(shí)間見(jiàn)表1。可看出位姿估計(jì)中特征點(diǎn)提取所占時(shí)間最長(zhǎng)，這是為了保證在災(zāi)后礦井的任何未知場(chǎng)景下都能有足夠豐富的特征點(diǎn)用于后續(xù)的位姿估計(jì)；而在位姿融合估計(jì)前去除了大量冗余特征點(diǎn)，因此視覺(jué)與激光融合估計(jì)位姿的時(shí)間相對(duì)較短。由于在位姿優(yōu)化中引入大量歷史幀數(shù)據(jù)進(jìn)行約束，所以相較于位姿估計(jì)時(shí)間，位姿優(yōu)化的迭代收斂時(shí)間更長(zhǎng)。但由于位姿估計(jì)和優(yōu)化是由2個(gè)獨(dú)立的線程分別實(shí)現(xiàn)，高精度優(yōu)化不會(huì)影響初次位姿估計(jì)的計(jì)算，所以1幀數(shù)據(jù)的平均處理時(shí)間低于100 ms，可以滿足無(wú)人機(jī)在井下環(huán)境中位姿估計(jì)的實(shí)時(shí)性要求。

對(duì)基于視覺(jué)的位姿估計(jì)算法、基于激光的位姿估計(jì)算法和基于視覺(jué)與激光融合的位姿估計(jì)算法在運(yùn)行過(guò)程中計(jì)算機(jī)的?CPU（i5?11300H，3.11 GHz ×4）和內(nèi)存（16 GiB）的平均占用率進(jìn)行對(duì)比，結(jié)果見(jiàn)表2。

從表2可看出，基于視覺(jué)與激光融合的位姿估計(jì)算法的 CPU 占用率和內(nèi)存占用率僅略高于其他2種算法?；谝曈X(jué)與激光融合的位姿估計(jì)算法結(jié)合了視覺(jué)和激光算法的原理，并進(jìn)行了冗余信息的刪除和關(guān)鍵幀的優(yōu)化，以較低的計(jì)算資源代價(jià)換取了更高精度的位姿估計(jì)，保證了無(wú)人機(jī)算力資源的高效利用，能夠在井下實(shí)現(xiàn)較長(zhǎng)時(shí)間的運(yùn)行和實(shí)時(shí)位姿估計(jì)。

6結(jié)論

1）提出了一種基于視覺(jué)與激光融合的井下災(zāi)后救援無(wú)人機(jī)自主位姿估計(jì)算法。將井下無(wú)人機(jī)獲取的視覺(jué)和激光數(shù)據(jù)進(jìn)行融合，利用滑動(dòng)窗口和局部地圖引入歷史幀數(shù)據(jù)進(jìn)行局部約束，減少累計(jì)誤差帶來(lái)的井下無(wú)人機(jī)軌跡漂移，提升了自主位姿估計(jì)的精確性和穩(wěn)定性。

2）利用井下激光點(diǎn)云數(shù)據(jù)對(duì)單目相機(jī)的 ORB 特征點(diǎn)恢復(fù)深度信息，按空間比例投影法有效、精準(zhǔn)地?cái)M合 ORB特征點(diǎn)所在的平面，精確恢復(fù) ORB 特征點(diǎn)三維信息。

3）利用視覺(jué)滑動(dòng)窗口和激光局部地圖引入歷史幀數(shù)據(jù)，實(shí)現(xiàn)對(duì)最新估計(jì)的無(wú)人機(jī)位姿進(jìn)行修正和約束，避免直接拼接井下無(wú)人機(jī)的位姿造成的軌跡偏移。

4）仿真實(shí)驗(yàn)結(jié)果表明：基于視覺(jué)與激光融合的位姿估計(jì)算法的平均相對(duì)平移誤差和相對(duì)旋轉(zhuǎn)誤差分別為0.0011 m 和0.0008°， 遠(yuǎn)低于僅基于視覺(jué)或激光的位姿估計(jì)算法，能夠?qū)崿F(xiàn)更為精確的井下無(wú)人機(jī)6DOF 位姿估計(jì)；該算法在面對(duì)不同礦井場(chǎng)景下的位姿估計(jì)誤差均趨于穩(wěn)定，有較好的穩(wěn)定性；算法采用雙線程工作，可滿足實(shí)時(shí)性要求，且 CPU 和內(nèi)存占用率僅略高于基于單一傳感器的位姿估計(jì)算法，以較低的計(jì)算資源換取了較高精度的位姿估計(jì)。

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

[1]王恩元，張國(guó)銳，張超林，等.我國(guó)煤與瓦斯突出防治理論技術(shù)研究進(jìn)展與展望[J].煤炭學(xué)報(bào)，2022，47（1）：297-322.

WANG Enyuan，ZHANG Guorui，ZHANG Chaolin，et al. Research progress and prospect on theory and technology for coal and gas outburst control and protection in China[J]. Journal of China Coal Society，2022，47（1）：297-322.

[2]畢林，王黎明，段長(zhǎng)銘.礦井環(huán)境高精定位技術(shù)研究現(xiàn)狀與發(fā)展[J].黃金科學(xué)技術(shù)，2021，29（1）：3-13.

BI Lin，WANG Liming，DUAN Changming. Research situation and development of high-precision positioning technology for underground mine environment[J]. Gold Science and Technology，2021，29（1）：3-13.

[3]江傳龍，黃宇昊，韓超，等.井下巡檢無(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 study of positioning and obstacle avoidance[J]. Machine Design & Research，2021，37（4）：38-42，48.

[4]范紅斌.礦井智能救援機(jī)器人的研究與應(yīng)用[J].礦業(yè)裝備，2023（1）：148-150.

FAN Hongbin. Research and application of mineintelligent ?rescue ?robot[J]. Mining ?Equipment，2023（1）：148-150.

[5]馬宏偉，王巖，楊林.煤礦井下移動(dòng)機(jī)器人深度視覺(jué)自主導(dǎo)航研究[J].煤炭學(xué)報(bào)，2020，45（6）：2193-2206.

MA Hongwei，WANG Yan，YANG Lin. Research on depth vision based mobile robot autonomous navigation in underground coal mine[J]. Journal of China Coal Society，2020，45（6）：2193-2206.

[6] MUR-ARTAL R，MONTIEL J，TARDOS J D. ORB- SLAM：a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics，2015，31（5）：1147-1163.

[7] CAMPOS C，ELVIRA R，RODRIGUEZ J J G，et al. ORB-SLAM3： an accurate open-source library for visual，visual-inertial，and multimap SLAM[J]. IEEE Transactions on Robotics，2021，37（6）：1874-1890.

[8] 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.

[9] DELLENBACH P，DESCHAUD J E，JACQUET B，et al. CT-ICP：real-time elastic LiDAR odometry with loop closure[C]. International Conference on Robotics and Automation，Philadelphia，2022：5580-5586.

[10] CHEN K，LOPEZ B T，AGHA-MOHAMMADI A，et al. Direct LiDAR odometry：fast localization with dense point clouds[J]. IEEE Robotics and Automation Letters，2022，7（2）：2000-2007.

[11]余祖俊，張晨光，郭保青.基于激光與視覺(jué)融合的車輛自主定位與建圖算法[J].交通運(yùn)輸系統(tǒng)工程與信息，2021，21（4）：72-81.

YU Zujun，ZHANG Chenguang，GUO Baoqing. Vehicle simultaneous localization and mapping algorithm with lidar-camera fusion[J]. Journal of Transportation Systems Engineering and Information Technology，2021，21（4）：72-81.

[12]洪炎，朱丹萍，龔平順.基于 TopHat加權(quán)引導(dǎo)濾波的 Retinex 礦井圖像增強(qiáng)算法[J].工礦自動(dòng)化，2022，48（8）：43-49.

HONG Yan，ZHU Danping，GONG Pingshun. Retinex mine image enhancement algorithm based on TopHat weighted guided filtering[J]. Journal of Mine Automation，2022，48（8）：43-49.

[13]李艷，唐達(dá)明，戴慶瑜.基于多傳感器信息融合的未知環(huán)境下移動(dòng)機(jī)器人的地圖創(chuàng)建[J].陜西科技大學(xué)學(xué)報(bào)，2021，39（3）：151-159.

LI Yan，TANG Daming，DAI Qingyu. Map-building of mobile robot in unknown environment based on multi sensor information fusion[J]. Journal of ShaanxiUniversity of Science & Technology，2021，39（3）：151-159.

[14]毛軍，付浩，褚超群，等.慣性/視覺(jué)/激光雷達(dá) SLAM 技術(shù)綜述[J].導(dǎo)航定位與授時(shí)，2022，9（4）：17-30.

MAO Jun，F(xiàn)U Hao，CHU Chaoqun，et al. A review of simultaneous localization and mapping based on inertial- visual-lidar fusion[J]. Navigation Positioning and Timing，2022，9（4）：17-30.

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

[16] QIN Chao，YE Haoyang，PRANATA C E，et，al. R- LINS：a robocentric lidar-inertial state estimator for robust and ?efficient navigation[J].2019. DOI：10.48550/arXiv.1907.02233.

[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] ZUO Xingxing，GENEVA P，LEE W，et al. LIC-fusion： LiDAR-inertial-camera ?odometry[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems，Macau，2019：5848-5854.

[20] ZHAO Shibo，ZHANG Hengrui，WANG Peng，et al. Super odometry： IMU-centric LiDAR-visual-inertial estimator for challenging environments[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems，Prague，2021：8729-8736.

[21] KIM B，JUNG C，SHIM D H，et al. Adaptive keyframe generation based LiDAR inertial odometry for complex underground environments[C]. IEEE International Conference on Robotics and Automation， London，2023：3332-3338.

[22] LIN Jiarong， ZHENG Chunran， XU Wei， et al. R2LIVE： a robust， real-time， LiDAR-inertial-visual tightly-coupled state estimator and mapping[J]. IEEE Robotics and Automation Letters，2021，6（4）：7469-7476.

[23] STURM J，ENGELHARD N，ENDRES F，et al. A benchmark for the evaluation of RGB-D SLAM systems[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems， Vilamoura-Algarve，2012. DOI：10.1109/IROS.2012.6385773.