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

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

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

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

 

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

 

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

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

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

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

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

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

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

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

 

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

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

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

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

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

Heightmap梯度匹配 (Heightmap Gradient Matching):

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

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

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

模板匹配 (Template Matching): 使用非歸一化相關(guān)系數(shù)（non-normalized correlation coefficient）將局部二值Heightmap（$T$）與先驗(yàn)地圖（$I$）進(jìn)行模板匹配。該指標(biāo)在局部Heightmap不完整時(shí)表現(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$是先驗(yàn)地圖，$\bar{T}$是局部地圖的均值，$\bar{I}{x,y}$是匹配區(qū)域在$(x, y)$處的均值。

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

粒子濾波器 (Particle Filter):

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

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

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

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

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

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

實(shí)驗(yàn)結(jié)果 (Experiments)

 

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

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

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

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

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

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

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

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

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

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

結(jié)論 (Conclusions) 

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