原文作者：Michal Werner, David ?apek 等4名

原文鏈接：https://www.themoonlight.io

雷峰網(wǎng)(公眾號：雷峰網(wǎng))注：該系統(tǒng)由捷克理工大學(xué)（CTU in Prague）著名的多機器人系統(tǒng)小組（MRS）開發(fā)。它是針對德國 SPRIN-D Funke 挑戰(zhàn)賽（完全自主飛行挑戰(zhàn)賽） 專門研發(fā)的奪冠方案。比賽要求無人機在沒有 GNSS 信號、沒有預(yù)先構(gòu)建的稠密地圖的情況下，在 25 米以下（AGL）的低空自主飛行 9 公里進行航點導(dǎo)航。

本文介紹了一個為SPRIN-D Funke全自主飛行挑戰(zhàn)賽開發(fā)的機載無人機（UAV）系統(tǒng)，該系統(tǒng)旨在解決GNSS拒絕環(huán)境下長距離導(dǎo)航的難題。在挑戰(zhàn)賽中，無人機需要在無GNSS或預(yù)先密集建圖的情況下，完成9公里長的低空（低于25米AGL）航點導(dǎo)航。該系統(tǒng)通過將LiDAR生成的局部Heightmap與先驗地理數(shù)據(jù)Heightmap進行基于梯度Template matching的匹配，并通過一個聚類粒子濾波器（clustered particle filter）融合Odometry和匹配結(jié)果，從而實現(xiàn)了輕量級的漂移校正。該系統(tǒng)在僅CPU的硬件上實時運行，成功完成了跨越城市、森林和開闊地帶的公里級飛行，并顯著減少了相對于原始Odometry的漂移。

 

系統(tǒng)架構(gòu) (System for GNSS-Denied Autonomous Flight)

 

該系統(tǒng)集成了感知、定位、規(guī)劃和控制功能，具體包括：

硬件平臺 (Hardware): 基于[19]的UAV平臺，配備Livox Mid-360 LiDAR（用于障礙物檢測、局部建圖和Heightmap生成）、Intel RealSense D435深度相機（用于近距離障礙物感知）、Bluefox RGB相機與慣性測量單元（IMU）（用于OpenVINS VIO），以及一個Intel NUC i7機載計算機（僅CPU）。IMU和VIO相機通過3D打印的靜音塊機械解耦，以衰減高頻振動。車載磁力計提供絕對航向測量。

視覺慣性里程計 (Visual Inertial Odometry - VIO): 使用OpenVINS [20]提供單目VIO，其魯棒性通過IMU和相機與電池包的機械解耦來提高，以隔離電機和螺旋槳產(chǎn)生的高頻振動。

建圖、規(guī)劃和反饋控制 (Mapping, Planning and Feedback Control): Livox Mid-360 LiDAR點云被增量集成到局部占有率地圖（OctoMap [21], [22]）中，該地圖以UAV機身框架為中心，大小為40x40米，以10 Hz更新。碰撞自由路徑通過A*算法在歐幾里得符號距離場上找到，然后通過多項式軌跡生成模塊[23]轉(zhuǎn)換為動態(tài)可行軌跡，并由底層模型預(yù)測參考跟蹤和控制管道[24]進行跟蹤。

任務(wù)控制 (Mission Control): 采用有限狀態(tài)機管理任務(wù)執(zhí)行，包括無人機準(zhǔn)備、起飛、航點導(dǎo)航、航點檢測（進入15米半徑后激活）、超飛（檢測到旗幟后）、方形搜索模式（未檢測到旗幟時），以及返航和降落。

數(shù)字孿生驅(qū)動開發(fā) (Digital twin driven development): 在FlightForge模擬器[3]中創(chuàng)建環(huán)境數(shù)字孿生，基于公開地理數(shù)據(jù)（DEM、衛(wèi)星圖像等）進行地形、植被和建筑物建模。該模擬環(huán)境用于系統(tǒng)開發(fā)、定位模塊迭代測試以及航點檢測器訓(xùn)練數(shù)據(jù)的生成。

航點檢測器 (Waypoint Detector): 基于YOLOv8 [26]架構(gòu)。首先使用在FlightForge模擬器中生成的合成數(shù)據(jù)集訓(xùn)練YOLOv8m模型，然后利用該預(yù)訓(xùn)練模型輔助標(biāo)注真實世界數(shù)據(jù)，最終在合成和真實數(shù)據(jù)上訓(xùn)練輕量級的YOLOv8n模型，以滿足機載CPU計算約束，實現(xiàn)約100毫秒的實時檢測。

機載長距離GNSS拒絕定位 (Onboard Long-Range GNSS-Denied Localization)

 

這是該系統(tǒng)的核心創(chuàng)新點，其流程如圖8所示： 

Heightmap預(yù)處理 (Heightmap Pre-Processing):

先驗數(shù)據(jù) (Prior Data): 利用公開點云數(shù)據(jù)（LAStools [27]）或航空RGB圖像深度估計模型[28], [29]生成地理參考、北向?qū)R的大尺度環(huán)境Heightmap（數(shù)字高程模型DEM）。

局部Heightmap (Local Heightmap): 從機載LiDAR數(shù)據(jù)生成的在線占有率地圖中構(gòu)建，假設(shè)Odometry漂移在局部地圖范圍內(nèi)受限。地圖分辨率為1米寬的bin，通過計算給定點云（來自DEM或局部地圖占用單元格）的最大高度來構(gòu)建。

對齊 (Alignment): 利用機載指南針測量將Heightmap與北方方向?qū)R，確保與先驗DEM的一致性。

Heightmap梯度匹配 (Heightmap Gradient Matching):

目的 (Purpose): 解決UAV無法可靠獲取絕對高度（如氣壓計噪聲大、地面傾斜）的問題，通過匹配梯度而非絕對高度來消除垂直偏移。

梯度濾波 (Gradient Filtering): 僅考慮絕對值大于5米的梯度，以強調(diào)高大、穩(wěn)定的結(jié)構(gòu)（如建筑物和樹木），忽略小型或瞬態(tài)物體。

二值邊緣圖 (Binary Edge Maps): 將過濾后的強梯度區(qū)域標(biāo)記為1，生成二值邊緣圖。

模板匹配 (Template Matching): 使用非歸一化相關(guān)系數(shù)（non-normalized correlation coefficient）將局部二值Heightmap（$T$）與先驗地圖（$I$）進行模板匹配。該指標(biāo)在局部Heightmap不完整時表現(xiàn)更優(yōu)。 $$R(x, y) = \sum_{x',y'} \left( T(x', y') - \bar{T} \right) \cdot \left( I(x + x', y + y') - \bar{I}{x,y} \right)$$ 其中，$T$是局部二值Heightmap，$I$是先驗地圖，$\bar{T}$是局部地圖的均值，$\bar{I}{x,y}$是匹配區(qū)域在$(x, y)$處的均值。

高斯模糊 (Gaussian Blur): 對生成的相似性地圖應(yīng)用高斯模糊，以減少離散化偽影。

粒子濾波器 (Particle Filter):

融合 (Fusion): Odometry和相似性地圖在一個粒子濾波器中融合，以提供UAV位置的統(tǒng)一概率估計。

狀態(tài)維護 (State Maintenance): 粒子濾波器維護平移狀態(tài)的多個假設(shè)，而方向直接從指南針獲取。

傳播 (Propagation): 在重采樣步驟之間，粒子根據(jù)Odometry估計進行平移，并與指南針航向?qū)R。

加權(quán) (Weighting): 每個粒子根據(jù)其投影位置在相似性地圖上的歸一化值分配權(quán)重。

重采樣 (Resampling): 當(dāng)UAV根據(jù)Odometry移動10米后觸發(fā)重采樣，新粒子的位置由經(jīng)驗估計的Odometry協(xié)方差高斯噪聲擾動。

聚類 (Clustering): 使用K-means算法對粒子集進行聚類，選擇最大簇的質(zhì)心作為最終位置估計，以解決感知混疊和Odometry噪聲引起的多簇問題。

實驗結(jié)果 (Experiments)

 

該系統(tǒng)在SPRIN-D挑戰(zhàn)賽中進行了評估，比賽區(qū)域包含城市、森林和開闊地帶。

 評估方法 (Evaluation Methodology): 由于禁止GNSS，UAV的真實軌跡通過VIO數(shù)據(jù)結(jié)合相機/LiDAR footage的手動估計獲得（0-5米精度）。部分測試飛行則有GNSS地面真值。

自主GNSS拒絕飛行 (Autonomous GNSS-denied flights):

在有GNSS地面真值的測試飛行中，即使初始定位誤差達(dá)32米，該方法也能通過觀測環(huán)境特征（如樹木）進行校正，最終將誤差減少到約4米。

在比賽期間，系統(tǒng)成功完成了多次公里級自主飛行，總定位RMSE低于11米，而僅靠指南針對齊的Odometry的RMSE高達(dá)53米。

飛行任務(wù)的終止通常是由于電池限制或硬件問題，而非定位漂移。

在城市環(huán)境中，由于特征點和可區(qū)分物體較多，該方法表現(xiàn)更好。在開闊地帶，系統(tǒng)主要依賴Odometry。

系統(tǒng)能夠校正磁力計在某些區(qū)域產(chǎn)生的高達(dá)30度的緩慢變化的偏差。

比賽結(jié)果 (Results of the competition): 在九支參賽隊伍中，本系統(tǒng)是唯一一支能夠成功完成公里級飛行并訪問多個航點的隊伍，最終獲得第一名。

經(jīng)驗教訓(xùn) (Lessons learned): IMU和VIO相機的機械解耦對VIO魯棒性至關(guān)重要；磁力計在建筑物和鋼筋混凝土附近不可靠，傳感器融合是實現(xiàn)魯棒自主性的關(guān)鍵；系統(tǒng)整體性能受限于最弱的組件，如局部地圖大小限制了飛行速度；快速診斷和部署能力在時間緊迫的場景中至關(guān)重要。

結(jié)論 (Conclusions) 

本文成功展示了一個用于GNSS拒絕環(huán)境下可靠長距離UAV導(dǎo)航的機載系統(tǒng)。該方法通過在聚類粒子濾波器中利用梯度匹配技術(shù)將局部LiDAR Heightmap與先驗地理數(shù)據(jù)對齊，有效校正了Odometry漂移，并在城市、森林和開闊地帶等多種地形中表現(xiàn)出魯棒性。在SPRIN-D挑戰(zhàn)賽中，系統(tǒng)在僅CPU的硬件上實現(xiàn)了公里級飛行，RMSE低于11米，驗證了其在實際場景中的自主運行能力。研究表明，在長距離任務(wù)中，從高不確定性時期恢復(fù)并重新定位的能力比維持持續(xù)低瞬時RMSE更為關(guān)鍵。盡管在續(xù)航能力和低特征環(huán)境操作方面仍存在挑戰(zhàn)，但這項工作為開發(fā)實際部署所需的彈性GNSS拒絕自主系統(tǒng)提供了基礎(chǔ)藍(lán)圖。