低空自主無人機數(shù)字孿生系統(tǒng)設(shè)計

摘" 要： 為了提高無人機在城市低空空域執(zhí)行巡檢、配送等任務(wù)的安全性和自主性，文中設(shè)計了一種基于數(shù)字孿生技術(shù)的無人機自主飛行系統(tǒng)。該系統(tǒng)綜合物理空間和虛擬空間的數(shù)據(jù)，精準(zhǔn)控制無人機的飛行。采用實景三維建模技術(shù)創(chuàng)建了高度逼真的虛擬空間，并通過通用橫軸墨卡托（UTM）投影方法實現(xiàn)了虛擬空間與物理空間的高度對應(yīng)。利用多傳感器融合的同步定位與建圖（SLAM）技術(shù)實現(xiàn)無人機的精確定位，并在虛擬空間中進行全局的三維路徑規(guī)劃用于控制無人機的飛行。實驗結(jié)果表明，虛實空間的平均映射誤差為0.38 m，這表明虛擬空間數(shù)據(jù)在保障無人機安全飛行方面具有可行性。通過在實際環(huán)境中的飛行測試，進一步驗證了所設(shè)計的無人機數(shù)字孿生系統(tǒng)的有效性，可為小型自主無人機的設(shè)計與應(yīng)用提供參考。

關(guān)鍵詞： 無人機； 數(shù)字孿生； 虛實映射； 自主飛行； 精確定位； 路徑規(guī)劃

中圖分類號： TN965?34；V279" " " " " " " " " " "文獻標(biāo)識碼： A" " " " " " " " " " " 文章編號： 1004?373X（2024）13?0123?06

Design of digital twin system for low altitude autonomous UAV

ZOU Zehai1， ZHENG Enhui1， DING Kai1， QUE Jianfei2， WANG Jiqiang1， YAO Yuwen1

（1. School of Mechanical and Electrical Engineering， China Jiliang University， Hangzhou 310000， China;

2. Sichuan Highway Planning， Survey， Design and Research Institute Ltd.， Chengdu 610000， China）

Abstract： An autonomous unmanned aerial vehicle （UAV） flight system based on digital twin technology is designed to improve the safety and autonomy performance of UAV in performing inspection and distribution tasks in urban low?altitude airspace. In the system， data from physical space and virtual space are synthesized to control the flight of the UAV precisely. A highly realistic virtual space is created with real 3D modeling technology. A high degree of correspondence between the virtual space and the physical space is achieved by the universal transverse Mercator （UTM） projection method. The simultaneous localization and mapping （SLAM） technique with multi?sensor fusion is used to achieve precise positioning of the UAV， and global 3D path planning in the virtual space is used to control the UAV flight. The experimental results show that the average mapping error of the virtual space is 0.38 meters， which indicates the feasibility of virtual space data in guaranteeing the safe flight of UAV. The effectiveness of the designed UAV digital twin system is further verified by flight tests in real environments， which can provide a reference for the design and application of small autonomous UAV.

Keywords： UAV; digital twin; virtual?real mapping; autonomous flight; precise positioning; path planning

0" 引" 言

隨著技術(shù)的創(chuàng)新和低空空域的有序開放，無人機在軍用和民用領(lǐng)域?qū)l(fā)揮更重要的作用，為各個領(lǐng)域提供更大的便利和效益[1?2]。然而，無人機存在著一些潛在的風(fēng)險和問題，如無人機失控和意外碰撞[3]。為增強無人機的安全自主運行能力，研究者們投入了大量精力研究基于同步定位與建圖（Simultaneous Localization and Mapping， SLAM）技術(shù)的定位與導(dǎo)航算法[4?7]。然而，依靠定位與導(dǎo)航算法來提高無人機的自主運行能力會受到無人機硬件資源的限制。

近年來，數(shù)字孿生技術(shù)作為一種有效的智能化提升手段，受到了廣泛關(guān)注。這項新興技術(shù)無疑為無人機系統(tǒng)的開發(fā)提供了新思路。文獻[8]中采用數(shù)字孿生技術(shù)對四旋翼無人機單兵作戰(zhàn)系統(tǒng)進行建模，在實現(xiàn)物理實體與虛擬實體之間交互映射的基礎(chǔ)上探索并行智能的應(yīng)用實踐。文獻[9]中對四旋翼飛行器的數(shù)字孿生系統(tǒng)做出設(shè)計，從工程應(yīng)用上體現(xiàn)了數(shù)字孿生技術(shù)對無人機飛行仿真的重要作用。文獻[10]中將數(shù)字孿生引入無人機單目視覺導(dǎo)航中，降低了應(yīng)用、算法開發(fā)和部署成本，提高了導(dǎo)航策略的遷移能力。文獻[11]中構(gòu)建了云平臺、5G以及虛擬現(xiàn)實技術(shù)融合的無人機數(shù)字孿生系統(tǒng)，用于無人機系統(tǒng)的人機交互、實時監(jiān)管。

在相關(guān)的研究中，虛擬空間和物理空間的關(guān)聯(lián)度不高，虛擬空間的信息不能很好地指導(dǎo)無人機執(zhí)行任務(wù)。本文構(gòu)建了一個高效實用的無人機數(shù)字孿生系統(tǒng)，結(jié)合數(shù)字孿生技術(shù)實現(xiàn)了自主無人機的關(guān)鍵技術(shù)，為提高無人機的安全性和自主性提供了一種技術(shù)實現(xiàn)途徑。

1" 無人機數(shù)字孿生系統(tǒng)框架

本文將數(shù)字孿生五維模型[12]與自主無人機應(yīng)用需求結(jié)合，構(gòu)建了如圖1所示的無人機數(shù)字孿生系統(tǒng)。該系統(tǒng)建立了無人機飛行區(qū)域的實景三維模型，通過軌跡映射模塊將虛擬空間和物理空間關(guān)聯(lián)起來，并通過數(shù)據(jù)傳輸通道實現(xiàn)虛擬空間的交互，為虛實融合分析提供基礎(chǔ)。

物理無人機在執(zhí)行任務(wù)時，定位模塊進行實時定位，并將結(jié)果同步到虛擬空間，驅(qū)動虛擬無人機同步運動。控制和避障模塊根據(jù)無人機的實時位置和目標(biāo)位置控制飛行。在線監(jiān)測模塊通過數(shù)據(jù)庫存儲的歷史數(shù)據(jù)和傳感器的實時數(shù)據(jù)融合，分析出無人機運行過程中可能出現(xiàn)的傳感器故障、碰撞等問題。由于虛擬空間的飛行環(huán)境是基于現(xiàn)實環(huán)境三維重建的結(jié)果，通過路徑規(guī)劃模塊得到的飛行路徑可以直接用來控制物理無人機的飛行。用戶決策模塊可以在無人機執(zhí)行飛行任務(wù)前進行任務(wù)設(shè)置，并在無人機飛行過程中根據(jù)在線檢測模塊的分析結(jié)果進行必要的人工介入。

2" 虛擬空間建模

搭建一個數(shù)字孿生系統(tǒng)的前提是要建立一個虛擬空間模型，本文的研究考慮了無人機模型和飛行區(qū)域的場景環(huán)境模型。對于虛擬空間的可視化展示與集成開發(fā)，本文使用了Unity這個強大的游戲引擎和開發(fā)工具，使用Unity的物理引擎和特定的規(guī)則算法可以模擬無人機的運動、碰撞等。

2.1" 飛行區(qū)域?qū)嵕叭S建模

為了快速準(zhǔn)確地重建飛行區(qū)域場景環(huán)境，本文采用無人機傾斜攝影技術(shù)進行實景三維建模。為了提高影像重疊率，進而獲取結(jié)構(gòu)特征質(zhì)量更好的三維模型，采用錄制視頻采集原始影像數(shù)據(jù)的方式。從數(shù)據(jù)采集到實景三維模型建立的流程如下：

1） 使用支持傾斜攝影航線規(guī)劃功能的無人機進行數(shù)據(jù)采集。無人機起飛前主要根據(jù)重建范圍規(guī)劃無人機航線，并設(shè)置無人機拍攝時記錄包含時間同步數(shù)據(jù)的SRT文件。

2） 數(shù)據(jù)預(yù)處理。整理錄制的原始視頻以及對應(yīng)的同步SRT文件，將視頻抽幀導(dǎo)出為圖像，將SRT文件中對應(yīng)的航點數(shù)據(jù)寫入對應(yīng)圖像元數(shù)據(jù)中，并標(biāo)定無人機相機獲取內(nèi)參數(shù)據(jù)。

3） 空中三角測量。將處理后的圖像數(shù)據(jù)和相機標(biāo)定數(shù)據(jù)導(dǎo)入Metashape軟件處理得到每張圖像優(yōu)化后的相機位姿。

4） 模型三維重建。將處理后的圖像數(shù)據(jù)、相機標(biāo)定數(shù)據(jù)和空中三角測量結(jié)果導(dǎo)入ContextCapture軟件處理得到逼真的實景三維模型。

2.2" 虛擬無人機仿真模型

虛擬無人機的仿真模型包含基本的幾何形狀、物理屬性、傳感器模型。將制作好的無人機3D模型導(dǎo)入Unity，添加物理碰撞模型、虛擬相機、虛擬激光雷達。碰撞模型如圖2a）所示，物理碰撞模型的形狀為一個能夠包含無人機的長方體包圍盒。虛擬相機如圖2b）所示，可設(shè)置安裝角度、視場角等參數(shù)。虛擬激光雷達模型如圖2c）所示。

3" 系統(tǒng)關(guān)鍵技術(shù)

3.1" 虛實空間精準(zhǔn)映射

物理空間的絕對位置信息為地理坐標(biāo)系下的經(jīng)緯度坐標(biāo)，而虛擬空間中使用笛卡爾坐標(biāo)系，兩者需要進行坐標(biāo)的投影變換。本文系統(tǒng)的軌跡映射模塊在投影變換過程中采用基于WGS84橢球體的通用橫軸墨卡托（Universal Transverse Mercator， UTM）投影方法[13]。UTM投影將該區(qū)域內(nèi)的失真降至最低，中央子午線附近的變形很小，適合局部小區(qū)域投影。本文相關(guān)符號定義為：

[k0=0.999 6t=tan?c=cos?l=λ-λ0ν2=e2cos2?N=a2b1+ν2FE=500 000FN=0或10 000 000] （1）

式中：[k0]為比例因子；[?]為緯度；[t]為緯度正切值；[c]為緯度余弦值；[λ]為經(jīng)度；[λ0]為中央子午線經(jīng)度；[l]為經(jīng)度差；[e]為第二偏心率；[ν]為輔助量；[N]為卯酉圈曲率半徑；[a]為橢球體長半軸；[b]為橢球體短半軸；FE和FN為坐標(biāo)偏移，F(xiàn)N在北半球為0，在南半球為10 000 000。所有的長度都以米為單位，所有的地理坐標(biāo)都以弧度為單位。

將經(jīng)緯度坐標(biāo)通過UTM投影正解公式轉(zhuǎn)換為笛卡爾坐標(biāo)系中的平面坐標(biāo)[（x，y）]，即：

[x=FE+k0Ncl+N6c3l3l3+N120c5l5l5+" " " " " " " " " " " " "N5 040c7l7l7y=FN+k0M+Nt2c2l2+Nt24c4l4l4+" " " " " " " " " " " " " Nt720c6l6l6+Nt40 320c8l8l8] （2）

式中：[M]為赤道到緯度的子午線弧長；[l3]、[l4]、[l5]、[l6]、[l7]、[l8]為相應(yīng)項的系數(shù)，對應(yīng)的計算公式為：

[l3=1-t2+ν2l4=5-t2+9ν2+4ν4l5=5-18t2+t4+14ν2-58t2ν2l6=61-58t2+t4+270ν2-330t2ν2l7=61-479t2+179t4-t6l8=1 385-3 111t2+543t4-t6] （3）

將笛卡爾坐標(biāo)系中的平面坐標(biāo)通過UTM投影反解公式轉(zhuǎn)換為經(jīng)緯度坐標(biāo)，即：

[?=?f+tf2N2fx2Δx2+tf24N4fx4Δx4+tf720N6fx6Δx6+tf40 320N8fx8Δx8λ=λ0+1NfcfΔx+16N3fcfx3Δx3+1120N5fcfx5Δx5+15 040N7fcfx7Δx7] （4）

式中帶下標(biāo)[f]的參數(shù)需要通過底點緯度[?f]計算。[Δx]、[x2]、[x3]、[x4]、[x5]、[x6]、[x7]、[x8]的計算公式為：

[Δx=x-FEk0x2=-1-ν2fx3=-1-2t2f-ν2fx4=5+3t2f+6ν2f-6t2fν2f-3ν4f-9t2fν4fx5=5+28t2f+24t4f+6ν2f+8t2fν2fx6=-61-90t2f-45t4f-107ν2f+162t2fν2f+45t4fν2fx7=-61-662t2f-1 320t4f-720t6fx8=1 385+3 633t2f+4 095t4f+1 575t6f] （5）

3.2" 基于多源融合SLAM的無人機自主定位

系統(tǒng)需要計算全局經(jīng)緯度信息以將無人機定位結(jié)果映射到虛擬空間，同時要計算連續(xù)且無漂移的位置信息以確保映射的準(zhǔn)確性。本文設(shè)計了一種多源信息融合的SLAM位姿估計方法，其核心在于不同信息源的融合，考慮到無人機的重量和成本，將全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System， GNSS）和基于相機與慣性測量單元（Inertial Measurement Unit， IMU）的視覺慣性里程計（Visual?inertial Odometry， VIO）融合起來。

以開源的GVINS[14]為基礎(chǔ)，采用緊耦合方式融合GNSS、IMU和相機數(shù)據(jù)。為了改善GVINS的初始化過程，系統(tǒng)將通過虛擬空間進行輔助優(yōu)化，具體算法框圖如圖3所示。

本文中，對于GNSS數(shù)據(jù)，首先過濾低俯仰角和不良衛(wèi)星信號，確保進入系統(tǒng)的衛(wèi)星能夠連續(xù)鎖定一定時間，以消除不穩(wěn)定的信號。IMU測量值被預(yù)積分，圖像則被抽象為稀疏的特征點序列。在初始化階段，通過將IMU預(yù)積分的結(jié)果與純視覺運動結(jié)構(gòu)（Structure from Motion， SFM）的結(jié)果進行對齊，實現(xiàn)視覺慣性初始化。如果視覺慣性系統(tǒng)成功對齊，則進行從粗到精的優(yōu)化過程，以初始化GNSS狀態(tài)。在GNSS初始化過程中，利用虛擬空間加快初始化并優(yōu)化結(jié)果。具體而言，當(dāng)觀察到無人機起飛點位置時，在虛擬空間提取無人機坐標(biāo)作為先驗信息發(fā)送給無人機，以加快初始化，一旦GNSS初始化成功，系統(tǒng)將監(jiān)測和處理GNSS退化情況，即使無法進行GNSS初始化，系統(tǒng)仍可在視覺慣性模式下進行狀態(tài)估計。

3.3" 虛擬空間三維路徑規(guī)劃

由于機載計算機資源有限，不易維護全局地圖進行路徑規(guī)劃，本文通過數(shù)字孿生技術(shù)在虛擬空間實現(xiàn)三維路徑規(guī)劃，為無人機實時提供全局路徑規(guī)劃，減輕機載計算機壓力。但無人機飛行區(qū)域?qū)嵕叭S模型數(shù)據(jù)量大，計算機加載實景三維模型在線構(gòu)建路徑規(guī)劃算法的搜索圖計算消耗大。為了能夠從虛擬空間的實景三維模型進行實時路徑規(guī)劃，本文實現(xiàn)了一種將離線保存的八叉樹地圖與Lazy Theta*算法[15]相結(jié)合的路徑規(guī)劃方法。

八叉樹地圖構(gòu)建過程是將無人機飛行區(qū)域?qū)嵕叭S模型劃分為大立方體網(wǎng)格，并不斷細分至最大深度。采用八叉樹數(shù)據(jù)結(jié)構(gòu)構(gòu)建路徑節(jié)點網(wǎng)格地圖可顯著減少路徑節(jié)點數(shù)量，縮短搜索時間。八叉樹地圖構(gòu)建流程如圖4所示。

首先，通過Unity獲取實景三維模型所有的三角形網(wǎng)格信息，對每個三角形的3三個頂點分別沿著頂點法線方向加上一個安全距離擴展障礙物表面，使規(guī)劃出來的路徑點與障礙物保持足夠的安全距離；然后，根據(jù)每個三角形的頂點判斷三角形網(wǎng)格是否與八叉樹當(dāng)前節(jié)點表示的立方體網(wǎng)格相交，若相交，則標(biāo)記當(dāng)前節(jié)點為包含障礙物，并創(chuàng)建8個子節(jié)點，對每個子節(jié)點重復(fù)操作，直到八叉樹擴展到設(shè)置的最大深度；最后，將八叉樹所有節(jié)點的信息保存到文件。系統(tǒng)在運行時提前從文件讀取八叉樹地圖數(shù)據(jù)，結(jié)合具體的路徑搜索算法實現(xiàn)實時路徑規(guī)劃。

4" 系統(tǒng)實驗驗證

4.1" 數(shù)據(jù)傳輸

有效的數(shù)據(jù)傳輸是整個無人機數(shù)字孿生系統(tǒng)運行的關(guān)鍵，本文實現(xiàn)了兩種數(shù)據(jù)傳輸方式：第一種是在云服務(wù)器部署MySQL數(shù)據(jù)庫實現(xiàn)數(shù)據(jù)遠程傳輸和存儲；第二種是通過Unity?ROS通信框架，其需要兩個工具包來實現(xiàn)，作為ROS節(jié)點運行的TCP端點和Unity端的TCP連接器，通過異地組網(wǎng)建立虛擬局域網(wǎng)也可以實現(xiàn)遠程傳輸。圖5所示為在4G網(wǎng)絡(luò)環(huán)境下連續(xù)傳輸100次無人機位置、姿態(tài)和速度信息來測試兩種不同數(shù)據(jù)傳輸方式的傳輸時間。從測試結(jié)果來看，在4G網(wǎng)絡(luò)下通過云服務(wù)器進行數(shù)據(jù)傳輸?shù)臅r間在40 ms左右，滿足實時通信需求。對于圖像回傳顯示等非實時處理數(shù)據(jù)，可通過構(gòu)建的虛擬局域網(wǎng)進行傳輸。

4.2" 虛實映射精度分析

為了驗證虛擬空間和物理空間中對應(yīng)位置點的映射精度，在虛擬空間均勻地選取9個位置，然后在物理空間對應(yīng)位置用RTK設(shè)備測量其經(jīng)緯度坐標(biāo)，把這些坐標(biāo)通過UTM投影轉(zhuǎn)換后的三維坐標(biāo)與虛擬空間選取位置的三維坐標(biāo)進行對比評估其映射精度。如圖6所示，誤差最大值為0.51 m，誤差最小值為0.19 m，誤差平均值為0.38 m。

4.3" 無人機定位精度分析

為了測試系統(tǒng)定位精度，手持無人機在起點處行走一圈后返回，通過比較兩次坐標(biāo)值計算單點定位誤差。在實驗中對比了VIO和GNSS觀測信息與VIO融合兩種定位算法，兩種算法的定位軌跡如圖7a）所示。從圖中可見，基于相機和慣性傳感器數(shù)據(jù)融合的VIO可在短時間內(nèi)提供精確定位信息，但長時間運行后存在累積誤差，導(dǎo)致定位軌跡漂移，系統(tǒng)運行一圈后在起點的定位誤差為7.43 m。而本文將VIO和GNSS通過緊耦合的方式進行融合的定位方法能夠充分利用兩者的優(yōu)勢，定位誤差僅為0.46 m。通過VIO和GNSS融合定位的軌跡映射到虛擬空間如圖7b）所示，可直觀看到圍繞水池行走一圈的軌跡。

4.4" 無人機自主飛行測試

為了驗證本文設(shè)計的自主無人機數(shù)字孿生系統(tǒng)的有效性，選用大疆M210無人機作為實驗平臺，并為其配備了由相機、IMU、GNSS接收機和機載計算機組成的定位模塊，無人機實物圖如圖8a）所示。測試時，首先根據(jù)4.2節(jié)和4.3節(jié)分析的虛實空間映射誤差、無人機定位誤差以及無人機機身尺寸，確定在虛擬空間規(guī)劃路徑的安全緩沖距離為2 m，以確保無人機在實際飛行時不會撞到障礙物。然后，在無人機數(shù)字孿生系統(tǒng)軟件界面，通過鼠標(biāo)選取無人機飛行的起點和終點，規(guī)劃出一條飛行路徑，如圖8b）所示。

最后，虛擬空間規(guī)劃的航點數(shù)據(jù)會上傳至云服務(wù)器的數(shù)據(jù)庫，無人機通過機載計算機從數(shù)據(jù)庫獲取航點數(shù)據(jù)，并控制無人機沿著飛行路徑自主飛行。圖8c）所示為無人機實際飛行路徑的定位軌跡映射到虛擬空間。

從可視化結(jié)果來看，無人機能夠很好地跟蹤在虛擬空間規(guī)劃的飛行路徑，成功穿越了兩棟建筑物間的間隙，驗證了本文系統(tǒng)的有效性。

5" 結(jié)" 語

本文從自主無人機應(yīng)用需求出發(fā)，構(gòu)建了無人機數(shù)字孿生系統(tǒng)，旨在提高無人機飛行的安全性和自主性。結(jié)合數(shù)字孿生技術(shù)，實現(xiàn)了虛實映射、自主定位和路徑規(guī)劃等關(guān)鍵技術(shù)。通過實驗證明了系統(tǒng)的有效性和實用性，為未來研究和發(fā)展提供了參考。接下來，將結(jié)合大數(shù)據(jù)和深度學(xué)習(xí)技術(shù)，為基于數(shù)字孿生的無人機提供實時故障監(jiān)測和處理能力，進一步提高飛行的安全性和自主性。

注：本文通訊作者為鄭恩輝。

參考文獻

[1] MOHSAN S A H， OTHMAN N Q H， KHAN M A， et al. A comprehensive review of micro UAV charging techniques [J]. Micromachines， 2022， 13（6）： 977.

[2] MOHSAN S A H， KHAN M A， NOOR F， et al. Towards the unmanned aerial vehicles （UAVs）： A comprehensive review [J]. Drones， 2022， 6（6）： 147.

[3] SHAKHATREH H， SAWALMEH A， Al?FUQAHA A I， et al. Unmanned aerial vehicles （UAVs）： A survey on civil applications and key research challenges [J]. IEEE access， 2019， 7： 48572?48634.

[4] 陳超，徐軍，張偉偉.多傳感器融合的移動機器人同步定位與建圖[J].現(xiàn)代電子技術(shù)，2020，43（14）：164?169.

[5] QIN T， LI P L， SHEN S J. VINS?Mono： A robust and versatile monocular visual?inertial state estimator [J]. IEEE transactions on robotics， 2018， 34（4）： 1004?1020.

[6] CAMPOS C， ELVIRA R， RODRíGUEZ 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.

[7] SHAN T X， ENGLOT B J， MEYERS D， et al. LIO?SAM： Tightly?coupled lidar inertial odometry via smoothing and mapping [C]// 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. New York： IEEE， 2020： 5135?5142.

[8] JI G， HAO J G， GAO J L， et al. Digital twin modeling method for individual combat quadrotor UAV [C]// 2021 IEEE 1st International Conference on Digital Twins and Parallel Intelligence （DTPI）. New York： IEEE， 2021： 1?4.

[9] 吳東陽，竇建平，李俊.四旋翼飛行器的數(shù)字孿生系統(tǒng)設(shè)計[J].計算機工程與應(yīng)用，2021，57（16）：237?244.

[10] MIAO J S， ZHANG P. UAV visual navigation system based on digital twin [C]// 2022 18th International Conference on Mobi?lity， Sensing and Networking （MSN）. New York： IEEE， 2022： 865?870.

[11] MENG W， YANG Y L， ZANG J Y， et al. DTUAV： A novel cloud?based digital twin system for unmanned aerial vehicles [J]. Simulation， 2023， 99（1）： 69?87.

[12] 陶飛，劉蔚然，張萌，等.數(shù)字孿生五維模型及十大領(lǐng)域應(yīng)用[J].計算機集成制造系統(tǒng)，2019，25（1）：1?18.

[13] LAND T. The use of Google maps and universal transverse Mercator （UTM） coordinate in land measurement of region in different zone [J]. Journal of theoretical and applied information technology， 2018， 96（23）： 8071?8080.

[14] CAO S Z， LU X Y， SHEN S J. GVINS： Tightly coupled GNSS?visual?inertial fusion for smooth and consistent state estimation [J]. IEEE transactions on robotics， 2022， 38（4）： 2004?2021.

[15] NASH A， KOENIG S， TOVEY C A. Lazy theta*： Any?angle path planning and path length analysis in 3D [C]// Proceedings of the AAAI Conference on Artificial Intelligence. [S.l.]： AAAI， 2010： 147?154.