基于視覺與激光融合的井下災后救援無人機自主位姿估計

何怡靜 楊維

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

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

關鍵詞：井下無人機；位姿估計；單目相機；激光雷達；視覺與激光融合；ORB 特征點中圖分類號：TD67 ?文獻標志碼：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引言

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

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

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

1井下無人機自主位姿估計流程

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

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

2基于視覺和激光的災后礦井環境特征點提取

2.1 ORB 特征點深度恢復

井下無人機首先對單目相機采集的礦井圖像進行 ORB特征點提取，由于提取的 ORB特征點不具備深度信息，通過匹配該特征點得到的井下無人機位姿與真實位姿之間將相差一個尺度因子，所以在完成 ORB特征點提取后，需利用激光點云對提取的 ORB 特征點進行深度恢復。

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

2.2激光雷達特征點提取

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

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

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

根據井下環境設定曲率的角點閾值和平面點閾值。當激光點的曲率高于角點閾值時，說明該點與周圍環境差異較大，將其加入特征角點集合ε;當激光點的曲率低于平面點閾值，說明該點位于差異不大的平面上，將其加入特征平面點集合H。若激光點的曲率處于平面點閾值和角點閾值之間，則該點特征性較弱，對后續幀間匹配屬于冗余信息，不加入特征點集，避免計算資源浪費。

3基于視覺與激光融合的井下無人機位姿估計

3.1視覺幀間匹配與位姿估計

對于視覺坐標系下的井下無人機位姿，利用第 k 幀數據中的 ORB特征點 i的歸一化坐標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 分別為無人機從第k幀到第k+1幀數據的視覺坐標系下1;k 的旋轉矩陣和平移矩陣，即

將式（2）和式（3）作為匹配誤差進行非線性優化[20]，即可估計出視覺坐標系下無人機從第k幀到1;k 。

3.2激光幀間匹配與位姿估計

對于激光坐標系下的井下無人機位姿，利用激光點云中的匹配特征角點和匹配特征平面點之間的距離作為激光匹配誤差eε和eH，將其放在同一位姿優化函數下，使得2幀激光點云之間所有匹配誤差的二范數平方和達到最低，從而求解出基于激光的井下無人機位姿Tk（L）+1;k 。

3.3視覺與激光融合位姿估計

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

式中：ek（C）和ek（L）分別為第k幀數據中的 ORB特征點和激光特征點的匹配誤差；Xk（C）和 Xk（L）分別為第k幀數據中的 ORB特征點和激光特征點的坐標；X 11分別為第k+1幀數據中的 ORB特征點和激光特征點的坐標。

對式（6）進行非線性優化，在相機和激光雷達觀測數據的共同約束下，迭代求解無人機在井下的初始融合位姿。但式（6）對誤匹配的特征點非常敏感，因為誤匹配會使得對應誤差項偏大，而二范數平方的增長速率極快，將會耗費大量時間用于調整一個錯誤的值，因此將二范數度量替換為一個增長速率較緩的魯棒核函數 Huber 函數。替換后的匹配誤差可表示為

式中δ為閾值。

加入魯棒核函數后，當匹配特征點的誤差較小時，保持原有的度量，當誤差大于閾值δ后，函數增長由二次形式變成一次形式，保證了在對多個誤差平方進行聯合優化時減少無效的觀測殘差，降低了誤匹配特征點等異常項在代價函數中的影響，從而提高位姿優化函數的穩定性。

4井下無人機位姿的局部約束優化

將算法估計得到的每一幀的無人機位姿進行拼接，即可得到無人機在井下的運動軌跡。但由于無人機每一幀的位姿誤差不斷累計，最終將造成無人機軌跡出現無法修復的偏移，使得無人機在井下的位姿估計精度迅速下降。因此，提出引入視覺和激光的歷史幀數據來對最新估計的位姿進行約束，保證無人機位姿的局部連貫性，避免軌跡漂移。

4.1視覺關鍵幀選取策略

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

4.2局部約束優化

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

式中：n?（x）;ε為點云簇坐標均值點到特征角點的向量；nε為點云簇的方向向量，即其協方差矩陣的最大特征值對應的特征向量。

特征平面點對應的點云簇呈面狀，特征平面點到對應點云簇的距離為

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

將視覺滑動窗口中的數據轉換到世界坐標系下，對視覺和激光的歷史幀數據進行融合，實現在二者共同局部約束下的無人機位姿優化。構建井下無人機的整體狀態向量，其中包括滑動窗口內無人機在世界坐標系下的位姿、ORB 特征點集和激光特征點集。利用滑動窗口內的歷史幀數據對無人機的整體狀態向量構建局部約束，即可完成對無人機位姿的優化。對整體狀態向量構建約束的數學表示為

式中：ewin 為關鍵幀的共視特征點的重投影誤差； eprior 為滑動窗口邊緣化帶來的先驗信息誤差。

利用非線性優化求解式（10），即可得到在視覺和激光共同局部約束下優化的井下無人機位姿。由于單目相機和激光雷達提取到的特征點相互獨立，增量方程中對應的特征矩陣為零或呈現較強稀疏性，極大降低了優化的計算成本，保證了實時性。

5 仿真與分析

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

真實礦井正常工作的平均復雜度為0.227，而仿真環境平均復雜度達0.518，可以模擬災后環境的復雜程度。

5.1算法準確性

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

除軌跡圖的定性評估外，采用絕對位姿誤差和相對位姿誤差[23]對算法的精確性進行定量評估。在仿真平臺上分別運行基于視覺的位姿估計算法、基于激光的位姿估計算法和基于視覺與激光融合的位姿估計算法，計算估計軌跡與真實軌跡的絕對位姿誤差和相對位姿誤差，分別以均方根誤差、標準差和均值等誤差評價指標呈現，如圖6所示?？煽闯龌谝曈X的位姿估計算法在各誤差評價指標中表現最差，而基于視覺與激光融合的位姿估計算法則表現最優；基于視覺與激光融合的位姿估計算法的絕對平移誤差、相對平移誤差和相對旋轉誤差均遠低于基于視覺的位姿估計算法和基于激光的位姿估計算法，各項誤差均值分別降低到4.52 m、0.0011 m、和0.0008°。因此，與基于視覺的位姿估計算法和基于激光的位姿估計算法相比，基于視覺與激光融合的位姿估計算法的精確度較高，能夠更精準地實現無人機位姿估計。

5.2算法穩定性

算法穩定性測試通過計算算法估計軌跡和真實軌跡每隔100 m 的平均平移誤差和平均旋轉誤差來實現，結果如圖7所示。從圖7（a）可看出，僅基于視覺或激光的位姿估計算法的平移誤差隨運行路徑長度的增加而迅速增加，這是因為幀間誤差隨時間累計，長時間運行將會導致較大的定位漂移；而基于視覺與激光融合的位姿估計算法加入了歷史幀約束，實時對最新估計位姿進行優化，因此平移誤差相對較低且變化平穩。從圖7（b）可看出，基于視覺與激光融合的位姿估計算法的平均旋轉誤差遠低于僅基于視覺或激光的位姿估計算法，且隨運行路徑長度增加而平穩下降。因此，相較于僅基于視覺或激光的位姿估計算法，基于視覺與激光融合的位姿估計算法的平均誤差均較低，具有較高的穩定性，能夠滿足井下位姿估計的穩定性要求。

5.3算法實時性

通過分析算法運行時間和資源占用率來驗證算法的實時性。算法各模塊的平均運行時間見表1。可看出位姿估計中特征點提取所占時間最長，這是為了保證在災后礦井的任何未知場景下都能有足夠豐富的特征點用于后續的位姿估計；而在位姿融合估計前去除了大量冗余特征點，因此視覺與激光融合估計位姿的時間相對較短。由于在位姿優化中引入大量歷史幀數據進行約束，所以相較于位姿估計時間，位姿優化的迭代收斂時間更長。但由于位姿估計和優化是由2個獨立的線程分別實現，高精度優化不會影響初次位姿估計的計算，所以1幀數據的平均處理時間低于100 ms，可以滿足無人機在井下環境中位姿估計的實時性要求。

對基于視覺的位姿估計算法、基于激光的位姿估計算法和基于視覺與激光融合的位姿估計算法在運行過程中計算機的?CPU（i5?11300H，3.11 GHz ×4）和內存（16 GiB）的平均占用率進行對比，結果見表2。

從表2可看出，基于視覺與激光融合的位姿估計算法的 CPU 占用率和內存占用率僅略高于其他2種算法。基于視覺與激光融合的位姿估計算法結合了視覺和激光算法的原理，并進行了冗余信息的刪除和關鍵幀的優化，以較低的計算資源代價換取了更高精度的位姿估計，保證了無人機算力資源的高效利用，能夠在井下實現較長時間的運行和實時位姿估計。

6結論

1）提出了一種基于視覺與激光融合的井下災后救援無人機自主位姿估計算法。將井下無人機獲取的視覺和激光數據進行融合，利用滑動窗口和局部地圖引入歷史幀數據進行局部約束，減少累計誤差帶來的井下無人機軌跡漂移，提升了自主位姿估計的精確性和穩定性。

2）利用井下激光點云數據對單目相機的 ORB 特征點恢復深度信息，按空間比例投影法有效、精準地擬合 ORB特征點所在的平面，精確恢復 ORB 特征點三維信息。

3）利用視覺滑動窗口和激光局部地圖引入歷史幀數據，實現對最新估計的無人機位姿進行修正和約束，避免直接拼接井下無人機的位姿造成的軌跡偏移。

4）仿真實驗結果表明：基于視覺與激光融合的位姿估計算法的平均相對平移誤差和相對旋轉誤差分別為0.0011 m 和0.0008°， 遠低于僅基于視覺或激光的位姿估計算法，能夠實現更為精確的井下無人機6DOF 位姿估計；該算法在面對不同礦井場景下的位姿估計誤差均趨于穩定，有較好的穩定性；算法采用雙線程工作，可滿足實時性要求，且 CPU 和內存占用率僅略高于基于單一傳感器的位姿估計算法，以較低的計算資源換取了較高精度的位姿估計。

參考文獻（References）：

[1]王恩元，張國銳，張超林，等.我國煤與瓦斯突出防治理論技術研究進展與展望[J].煤炭學報，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]畢林，王黎明，段長銘.礦井環境高精定位技術研究現狀與發展[J].黃金科學技術，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]江傳龍，黃宇昊，韓超，等.井下巡檢無人機系統設計及定位與避障技術[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].礦業裝備，2023（1）：148-150.

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

[5]馬宏偉，王巖，楊林.煤礦井下移動機器人深度視覺自主導航研究[J].煤炭學報，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]余祖俊，張晨光，郭保青.基于激光與視覺融合的車輛自主定位與建圖算法[J].交通運輸系統工程與信息，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加權引導濾波的 Retinex 礦井圖像增強算法[J].工礦自動化，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]李艷，唐達明，戴慶瑜.基于多傳感器信息融合的未知環境下移動機器人的地圖創建[J].陜西科技大學學報，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]毛軍，付浩，褚超群，等.慣性/視覺/激光雷達 SLAM 技術綜述[J].導航定位與授時，2022，9（4）：17-30.

MAO Jun，FU 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.