唐嘉寧 劉志聰 李孟霜 彭志祥 謝翠娟 陳云浩

摘?要：針對現(xiàn)有小型無人飛行器在復(fù)雜未知環(huán)境中執(zhí)行自主探索任務(wù)時，存在機(jī)載端GPU計算資源不足，在線建圖速度慢、探索效率低的問題.本文在傳統(tǒng)歐幾里德符號距離場（ESDF）方法的基礎(chǔ)上，融合光線跟蹤原理，加速構(gòu)建ESDF地圖，以提高飛行器在復(fù)雜未知環(huán)境中的探索效率.首先使用整數(shù)運算提高光線跟蹤遍歷體素的速度，從而加速體素占用概率的更新;然后通過調(diào)整哈希數(shù)據(jù)結(jié)構(gòu)，減少地圖占用內(nèi)存;最后使用廣度優(yōu)先搜索算法（BFS）實現(xiàn)地圖更新與融合.為驗證本文方法的有效性，分別在公開數(shù)據(jù)集、仿真環(huán)境和真實環(huán)境下，與當(dāng)前前沿的建圖方法進(jìn)行對比，實驗結(jié)果表明，本文所提出的方法在三種不同情況下的地圖平均更新時間分別減少了72.37%、60.80%和56.79%，顯著提高了建圖速度，為小型無人飛行器在線探索奠定基礎(chǔ).

關(guān)鍵詞：地圖構(gòu)建；光線跟蹤；符號距離場；梯度優(yōu)化

中圖分類號：V249

文獻(xiàn)標(biāo)志碼： A

文章編號：2096-398X（2023）04-0158-08

Abstract：For the existing small unmanned aerial vehicles （UAVs） performing autonomous exploration tasks in complex and unknown environments，there are problems such as insufficient computing resources of the airborne GPU，slow online mapping and low exploration efficiency.Based on the traditional Euclidean signed distance field （ESDF） method，this paper integrates the ray tracing principle and accelerates the construction of ESDF maps to improve the exploration efficiency of aircraft in complex and unknown environments.Firstly，integer operation is used to improve the speed of ray tracing voxel traversal，thus speeding up the update of voxel occupancy probability; Then adjust the hash data structure to reduce the memory occupied by the map; Finally，the breadth first search algorithm （BFS） is used to update and fuse the map.In order to verify the effectiveness of the method proposed in this paper，it is compared with the current cutting-edge mapping methods in the open dataset，simulation environment and real environment respectively.The experimental results show that the average map update time of the method proposed in this paper in three different situations is reduced by 72.37%，60.80% and 56.79% respectively，which significantly improves the mapping speed and lays a foundation for online exploration of small unmanned aerial vehicles.

Key words：mapping; ray tracing; signed distance fields; gradient optimization

0?引言

小型無人飛行器因其體積小和靈活性，已經(jīng)成為最廣泛的無人自主系統(tǒng)研究平臺之一，并且多應(yīng)用于未知環(huán)境的自主探索及自主導(dǎo)航等方面［1-3］.由于小型飛行器自身的低載荷及有限的機(jī)載計算能力，使小型飛行器在進(jìn)行自主探索的過程中需要快速、輕量級的算法進(jìn)行環(huán)境感知、地圖構(gòu)建和航跡規(guī)劃.

地圖是無人機(jī)進(jìn)行導(dǎo)航的基礎(chǔ)，無人機(jī)航跡規(guī)劃過程中有很多不同類型的地圖框架可以使用.其中，占用柵格地圖是規(guī)劃算法中常用的地圖形式之一，它可以用于快速碰撞檢測以及占用概率的評估，Octomap基于八叉樹的數(shù)據(jù)結(jié)構(gòu)，用來表示緊湊的內(nèi)存，進(jìn)行地圖查詢和存儲占用概率［4］.Chen等［5］利用八叉樹的數(shù)據(jù)結(jié)構(gòu)，將空閑空間進(jìn)行劃分，生成安全飛行走廊，用以生成無碰撞的安全軌跡.Ji等［6］提出了一個mapless-planner系統(tǒng)，它利用原始的點云地圖進(jìn)行規(guī)劃，通過最近鄰查詢算法，查找空間中最近的障礙物，以此生成球形飛行走廊而保證高質(zhì)量的軌跡.Florence等［7］提出一種新的地圖結(jié)構(gòu)，它能夠在規(guī)劃時有效查詢地圖構(gòu)建時位姿不確定性的局部3D幾何信息，從而實現(xiàn)提高規(guī)劃的效率以及魯棒性，支持無人機(jī)安全快速飛行的目的.但是對于無人機(jī)自主導(dǎo)航來說，軌跡規(guī)劃需要的是空閑空間的信息，而不是障礙物的信息，用于軌跡規(guī)劃［8］的理想地圖要求能夠快速查詢地圖中每個體素的空閑、占用狀態(tài)，或者能夠方便的獲取當(dāng)前體素到障礙物的距離，例如歐幾里德符號距離場（ESDF，Euclidean Signed Distance Field）地圖.ESDF地圖中的每個體素都儲存著當(dāng)前體素距離其最近障礙物的直線距離和梯度信息，因此廣泛的應(yīng)用于基于梯度的無人自主導(dǎo)航規(guī)劃［9-14］.基于梯度信息的軌跡規(guī)劃方法能夠?qū)⑸傻能壽E推離障礙物，以保證軌跡的安全性.Zhou等[15]中利用局部ESDF地圖中的梯度信息和動態(tài)約束，通過B樣條來優(yōu)化初始路徑，保證路徑的安全性和動態(tài)可行性，以此用于三維復(fù)雜環(huán)境中的快速飛行.

最近，眾多機(jī)構(gòu)在增量構(gòu)建三維規(guī)劃的ESDF地圖方面做了大量工作.Oleynikova等［16］提出的Voxblox框架，專注于從截斷符號距離場（TSDF，Truncated Signed Distance Fields）增量構(gòu)建ESDF地圖.該框架首先將傳感器數(shù)據(jù)集成到TSDF地圖，它包含曲面截斷半徑內(nèi)地距離信息，然后使用廣度優(yōu)先搜索算法從TSDF增量計算ESDF.與之相似的是Han等［17］提出的Fiesta框架，該方法以增量方式從占用柵格地圖構(gòu)建全局ESDF地圖，通過引入兩個獨立更新隊列分別插入和刪除障礙，并使用索引數(shù)據(jù)結(jié)構(gòu)和雙鏈表對地圖進(jìn)行維護(hù)更新.Chen等［18］提出了一種基于GPU的地圖構(gòu)建系統(tǒng)，該系統(tǒng)在GPU上并行構(gòu)建占用柵格地圖和ESDF，利用并行的線程同時處理刪除和插入障礙物操作.

滿足小型無人飛行器在未知環(huán)境進(jìn)行在線探索的主要挑戰(zhàn)是以高效和準(zhǔn)確的方式維護(hù)ESDF地圖更新，雖然有不同的方法能動態(tài)構(gòu)建ESDF地圖，但是只有少數(shù)方法可以在僅有CPU平臺式運行.針對機(jī)載端計算資源不足，導(dǎo)致在線建圖速度慢、探索效率低的問題.本文在傳統(tǒng)歐幾里德符號距離場（ESDF）方法的基礎(chǔ)上，融合光線跟蹤原理，加速構(gòu)建ESDF地圖，在僅有CPU的計算資源下保證小型無人飛行器在復(fù)雜未知環(huán)境中的探索效率.首先使用整數(shù)運算提高光線跟蹤遍歷體素的速度，從而加速體素占用概率的更新；然后通過調(diào)整哈希數(shù)據(jù)結(jié)構(gòu)，減少地圖占用內(nèi)存；最后使用廣度優(yōu)先搜索算法（BFS）實現(xiàn)地圖更新與融合.通過數(shù)據(jù)集、仿真環(huán)境以及真實環(huán)境的測試實驗，驗證了該算法的可行性及適用性.本文的主要貢獻(xiàn)如下：

（1） 提出了一種利用光線跟蹤快速體素遍歷的方法.

（2） 將快速體素遍歷算法集成到小型無人機(jī)平臺，提出了一種能夠在CPU平臺上快速構(gòu)建ESDF地圖的框架.

1?系統(tǒng)框架

構(gòu)建ESDF地圖的系統(tǒng)框架如圖1所示，虛線部分為本文的工作范圍.在地圖構(gòu)建過程中，系統(tǒng)首先從傳感器中獲取環(huán)境數(shù)據(jù)，其中深度圖像信息可以通過單目深度估計或者機(jī)載傳感器（如R-GBD深度相機(jī)、激光雷達(dá)）獲得.位姿信息可以從外部設(shè)備GPS或者Vicon動作捕捉系統(tǒng)獲得，也可以從內(nèi)部位姿估計如VIO（視覺慣性里程計）獲得.然后使用改進(jìn)的光線跟蹤方法將這些數(shù)據(jù)加速集成到占用柵格地圖中，將占用柵格地圖作為儲存占用信息的數(shù)據(jù)結(jié)構(gòu).通過使用體素哈希方法來處理持續(xù)增長的地圖［17］，最后用BFS算法將新的地圖數(shù)據(jù)融合到現(xiàn)有地圖，實現(xiàn)地圖快速更新.

2?基于光線跟蹤快速體素遍歷方法

當(dāng)前有兩種主流的方法構(gòu)建ESDF地圖，一種是從TSDF表示的基礎(chǔ)上逐步構(gòu)建ESDF地圖如voxblox；另一種是從占用柵格地圖中增量構(gòu)建ESDF地圖，對于占用柵格地圖，需將環(huán)境信息映射到一組由空閑或者占用狀態(tài)表示的體素中，如Fiesta.其中voxblox基于TSDF采用的是投影距離，即沿傳感器射線到測量表面的距離，容易高估到最近表面的實際歐幾里德距離，而Fiesta使用占據(jù)柵格圖，計算的是傳感器到體素中心的真實歐幾里德距離，地圖構(gòu)建精度更高.因此本文專注直接從占用柵格地圖構(gòu)建ESDF地圖，其構(gòu)建的步驟如下：

（1） 通過傳感器觀測獲取深度點云信息.

（2） 傳感器原點和點云中的每個點構(gòu)成一條具有固定長度的單獨光線.

（3） 對于所有光線，使用光線跟蹤遍歷體素，用以發(fā)現(xiàn)空閑和占用的體素.

（4） 將觀測到的體素合并到占用柵格圖中，并根據(jù)體素概率進(jìn)行地圖更新.

其中，在每次光線跟蹤處理中，上述步驟2、3占其處理時間90%以上.針對該問題本文將利用加速體素遍歷來縮短其光線跟蹤的處理時間.

占用柵格地圖通常將三維空間均勻分割為大小相等的正方體遍歷三維空間中的體素，其中每個正方體即為一個體素.在三維空間中，一個體素與相鄰的六個體素都有一個公共面，這種連接方式稱為6連接.與之相似的是26連接，因為每個體素包含26個鄰居（6個面連接，12個邊連接和8個頂點連接）.光線跟蹤的目的是為了遍歷這些體素并且確定這些體素哪些是占用哪些是空閑.地圖構(gòu)建過程示意圖如圖2 所示，當(dāng)相機(jī)發(fā)射一條光線，能穿過的中間體素標(biāo)記為空閑（圖中米黃色格），穿不過的即為占用（圖中黑色格）.當(dāng)進(jìn)行體素遍歷時，找到每條光線所在直線經(jīng)過的體素，后續(xù)只需要對這些體素所包含的對象與直線進(jìn)行求交點操作從而進(jìn)行體素遍歷.

對于體素遍歷算法，首先考慮二維情況，為了正確遍歷網(wǎng)格，算法必須按照如圖3所示的順序（a，b，c，d，e，f，j）訪問體素，光線方程為：

式（1）中：v光線方向，p0和p1分別為光線的起點和終點，t為遍歷一個體素的時間間隔.

遍歷算法包括兩個階段：初始化和增量遍歷.初始化階段從光線原點開始，如果光線原點在網(wǎng)格之外，則找到光線進(jìn)入網(wǎng)格的交點，并獲取相鄰的體素.二維體素遍歷如圖4所示.

在循環(huán)過程中，加入距離限制，dist為當(dāng)前體素到光線原點的歐式距離，當(dāng)這個距離大于maxDist（起點與終點的距離）時，跳出循環(huán).從二維算法擴(kuò)展到三維只需要適當(dāng)添加Z分量即tIntZ，ΔtZ的值即可.

3?ESDF地圖構(gòu)建方法

3.1?占用柵格地圖

本文通過占用柵格地圖構(gòu)建ESDF地圖，占用柵格地圖來存儲體素的占用概率，當(dāng)從傳感器獲取到新的深度信息時，占用柵格地圖不斷更新從光線跟蹤獲得的新的占用信息數(shù)據(jù)，對于每個體素，用p（s=1）表示為其空閑狀態(tài)的概率，p（s=0）表示為其占用狀態(tài)的概率.則兩者的比值可以表示為該體素的狀態(tài).

3.2?地圖數(shù)據(jù)結(jié)構(gòu)

對于構(gòu)建三維柵格地圖如果內(nèi)存夠大且已知地圖尺寸和分辨率大小，通常利用數(shù)組直接進(jìn)行索引工作.但是在自主探索過程中，無人機(jī)要探索的區(qū)域往往是未知的，通常采用哈希表來進(jìn)行體素索引工作，以減小內(nèi)存消耗.但是單純使用哈希表進(jìn)行體素索引，由于其所需查找索引的數(shù)量巨大，導(dǎo)致其性能比使用數(shù)組的方式差.參考文獻(xiàn)［19］利用哈希表和數(shù)組結(jié)合的方式索引一個體素坐標(biāo)，通過生成一個8×8×8個體素組成的體素塊，采用哈希表用于塊管理.從體坐標(biāo)計算得到塊坐標(biāo)后，哈希表用于查找相應(yīng)的塊，該塊中所有體素的索引儲存在相對于塊的數(shù)組中.由于塊哈希表的數(shù)量遠(yuǎn)少于體素哈希表的數(shù)量，因此哈希表用于塊管理可以節(jié)省更多的內(nèi)存，用于提高地圖構(gòu)建的性能.

3.3?地圖更新

ESDF地圖中每個地圖都存儲著距離最近障礙物的歐幾里德距離，即對于空閑空間中的點坐標(biāo)p（x，y，z），確定到所有障礙物b（i，j，k）的最近距離為：

ESDF地圖更新示意圖如圖5所示，ESDF地圖更新算法基于廣度優(yōu)先搜索算法.在圖5（a）中，當(dāng)obs1未插入時，體素V1的最近障礙物為obs0，此時插入一個新的障礙物obs1，則從該障礙物點的鄰居開始搜索，當(dāng)這個鄰居V1與obs1的距離小于之前存儲的最近距離（體素V1與其最近障礙物obs0的距離），則將存儲的距離信息更新為新的最近距離，并且把該鄰居作為生長點，搜索其下一個鄰居并進(jìn)行比較.如圖5（b）中，當(dāng)刪除一個障礙物時，即障礙物obs1刪除，把該點的距離設(shè)為無窮大，查找所有以obs1為最近障礙物的體素.若刪除前V2的最近障礙物為obs1，則查找V2鄰居的最近障礙物，用該鄰居的最近障礙物作為體素V2的最近障礙物.例如，圖5（b）中V2鄰居中nbr的最近障礙物為obs0，即把obs0作為V2的最近障礙物，以此來更新最近歐幾里德距離minDist（x，y，z）.

4?實驗結(jié)果與分析

4.1?實驗設(shè)置

為了驗證所提算法的可行性與適用性，本文在機(jī)器人操作系統(tǒng)（ROS，Robot Operating System）中進(jìn)行實驗，該系統(tǒng)包含了視覺圖像模塊與地圖構(gòu)建模塊之間的通信，并采用Rviz作為實時地圖構(gòu)建的顯示平臺.在同等測試條件下本文分別在公開RGB-D數(shù)據(jù)集、仿真環(huán)境和真實場景中進(jìn)行實驗，并將提出的方法與先進(jìn)的ESDF地圖構(gòu)建方法Fiesta框架進(jìn)行比較［17］.該系統(tǒng)運行在配備Ubuntu 18.04 64位操作系統(tǒng)，Intel Core i7-8750 （8核@2.20 GHz）和32 GB RAM的筆記本電腦上運行.實驗所涉及到的基本仿真參數(shù)如表1所示.

4.2?實驗結(jié)果

4.2.1?RGD-B數(shù)據(jù)集分析

本次實驗采用的是蘇黎世聯(lián)邦理工學(xué)院自主系統(tǒng)實驗室（ASL）的公開數(shù)據(jù)集cow_and_lady，該數(shù)據(jù)集是一個具有地面實況的小型數(shù)據(jù)集，拍攝的是一個室內(nèi)辦公室環(huán)境，包含一頭奶牛，一個人體模型和一些其他典型的辦公配件.數(shù)據(jù)集包含彩色RGB-D點云數(shù)據(jù)和位姿信息.cow_and_lady數(shù)據(jù)集實驗的原始場景及兩種方法實時構(gòu)建的ESDF地圖如圖6所示，地圖更新時間對比如圖9所示，在同等條件測試下Fiesta的方法平均地圖更新時間為87.63 ms，而本文的方法單次平均地圖更新時間僅為23.89 ms，本文地圖構(gòu)建速度比Fiesta的方法快了3倍左右.

4.2.2?仿真環(huán)境實驗

本文采用Rotors仿真環(huán)境，其中包含一個搭載雙目深度相機(jī)的六旋翼無人機(jī).仿真環(huán)境由Gazebo構(gòu)建，提供對外接口，能夠通過ROS獲取深度圖像數(shù)據(jù)和無人機(jī)位姿信息.本次實驗利用該仿真環(huán)境，在Gazebo構(gòu)建尺寸為8 m×8 m×3 m的室內(nèi)地圖，通過鍵盤控制無人機(jī)飛行.Rotors仿真環(huán)境試實驗環(huán)境及ESDF地圖對比圖如圖7所示，其中圖7（a）為仿真環(huán)境圖，圖7（b）、（c）分別為本文算法實時構(gòu)建的ESDF地圖和Fiesta構(gòu)建的地圖，地圖更新時間對比如圖9所示.在相同條件測試下Fiesta的方法單次平均地圖更新時間為35.44 ms，而本文的方法平均地圖更新時間僅為13.89 ms，本文地圖構(gòu)建速度比Fiesta的方法快2倍左右.

4.2.3?真實環(huán)境場景實驗

對于真實場景實驗，實驗采用英特爾RealSense d435i立體視覺深度相機(jī)，用以采集地圖構(gòu)建所需的深度圖像，相機(jī)采集頻率為30 Hz，深度測量范圍5 m，所獲深度圖像分辨率為640×480，并且通過vins_fusion［20］框架進(jìn)行視覺定位，用來獲取相機(jī)的位姿.本次實驗的場地為一條尺寸為40 m×5 m×3 m的走廊.真實場景實驗及構(gòu)建的ESDF地圖對比圖如圖8所示，其中圖8（a）為真實環(huán)境如，圖8（b）、（c）分別為本文實時構(gòu)建的ESDF地圖和Fiesta構(gòu)建的地圖.地圖更新時間對比如圖9所示，在同等條件測試下Fiesta的方法平均地圖更新時間為12.61 ms，而本文的方法單次平均地圖更新時間僅為5.42 ms，本文地圖構(gòu)建速度比Fiesta的方法快2倍左右.

圖9為對三次不同場景、兩種算法下ESDF地圖平均更新時間的性能對比結(jié)果，圖中簇狀圖為地圖平均更新時間，折線圖為本文所提算法與Fiesta方法在相同環(huán)境下地圖平均更新時間的降低比率.從圖9可以看出，在數(shù)據(jù)集、仿真環(huán)境和真實場景中，本文所提出算法的平均地圖更新時間分別比Fiesta方法減少了72.37%、60.80%和56.79%.多個場景的實驗結(jié)果表明，本文所提出的算法可以顯著降低實時構(gòu)建ESDF地圖的時間，具有一定的可行性和高效性.

為了驗證真實環(huán)境場景下不同相機(jī)采樣頻率對建圖精度的影響，進(jìn)行了不同相機(jī)頻率下的真實環(huán)境建圖實驗.實驗中，深度相機(jī)以2 m/s的速度勻速移動，采樣頻率分別設(shè)置為20 Hz、30 Hz、40 Hz、50 Hz和60 Hz，平均地圖更新時間控制在6 ms內(nèi).通過在建圖結(jié)果的長邊和寬邊分別隨機(jī)選取9個測量點，計算平均值后與實際尺寸對比以評估建圖精度.不同相機(jī)頻率下，論文所提方法的地圖構(gòu)建精度對比結(jié)果如圖10所示，其中，柱狀圖為不同相機(jī)采樣頻率下建圖的長邊長度，折線圖為寬邊的建圖長度.實驗結(jié)果與實際場景尺寸對比可知，相機(jī)的采樣頻率與建圖誤差之間沒有明顯的對應(yīng)關(guān)系，論文所提出建圖方法在實際場景下的建圖誤差較小，40 m真實環(huán)境的建圖誤差小于0.2 m，5 m真實環(huán)境的建圖誤差小于0.1 m.

對于本文方法構(gòu)建的ESDF地圖，本文還測量了在仿真環(huán)境及真實場景環(huán)境下地圖的長寬距離來計算其建圖精度.仿真與真實場景分別進(jìn)行了7次實驗，每次實驗測量了地圖的長寬距離來計算其建圖精度，其中長寬的計算為每條邊各取9個測量點所測算的平均距離.仿真環(huán)境與真實環(huán)境下地圖精度誤差對比圖如圖11所示.從圖中比對結(jié)果可以看出，不論在仿真環(huán)境或者真實場景下本文方法與傳統(tǒng)Fiesta方法在精度偏差分別能夠控制在0.06 m及0.12 m以內(nèi)，兩個建圖方法在精度上基本能保持一致.

從實驗可以看出在保持地圖分辨率相同時，本文方法在提高建圖速度后對于建圖的精度并沒有明顯降低.為了滿足無人機(jī)自主探索實時性的要求，對于當(dāng)前構(gòu)建ESDF地圖的精度誤差是可以接受的.因為對于大部分自主規(guī)劃算法來說，為了保證飛行過程中的安全性，都會適當(dāng)?shù)陌颜系K物進(jìn)行膨脹處理.而且ESDF地圖又適用于基于梯度優(yōu)化的方法，該方法能夠利用障礙物的距離信息，把飛行軌跡推離障礙物，從而保證了飛行安全性.

5?結(jié)論

針對機(jī)載端計算資源不足，導(dǎo)致在線建圖速度慢、探索效率低的問題.本文在傳統(tǒng)歐幾里德符號距離場方法的基礎(chǔ)上，融合光線跟蹤原理，加速構(gòu)建ESDF地圖，在僅有CPU的計算資源下保證小型無人飛行器在復(fù)雜未知環(huán)境中的探索效率.首先使用整數(shù)運算提高光線跟蹤遍歷體素的速度，從而加速體素占用概率的更新；然后通過調(diào)整哈希數(shù)據(jù)結(jié)構(gòu)，減少地圖占用內(nèi)存；最后使用廣度優(yōu)先搜索算法實現(xiàn)地圖更新與融合.通過數(shù)據(jù)集、仿真環(huán)境以及真實環(huán)境的測試實驗，證實本文所提出算法的平均地圖更新時間分別比Fiesta方法減少了72.37%、60.80%和56.79%，通過精度對比實驗也可以看出本文在減少地圖構(gòu)建時間后相較于傳統(tǒng)Fiesta方法還能不降低的地圖構(gòu)建精度，充分驗證了算法的可行性與高效性.

參考文獻(xiàn)

［1］ Xu Z，Deng D，Shimada K.Autonomous UAV exploration of dynamic environments via incremental sampling and probabilistic roadmap［J］.IEEE Robotics and Automation Letters，2021，6（2）：2 729-2 736.

［2］ Krátky＇ V，Petrácˇek P，Bácˇa T，et al.An autonomous unmanned aerial vehicle system for fast exploration of large complex indoor environments［J］.Journal of Field Robotics，2021，38（8）：1 036-1 058.

［3］ Zhao Y，Yan L，Chen Y，et al.Robust and efficient trajectory replanning based on guiding path for quadrotor fast autonomous flight［J］.Remote Sensing，2021，13（5）：972.

［4］ Hornung A，Wurm K M，Bennewitz M，et al.OctoMap：An efficient probabilistic 3D mapping framework based on octrees［J］.Autonomous Robots，2013，34（3）：189-206.

［5］ Chen J，Liu T，Shen S.Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments［C］//2016 IEEE International Conference on Robotics and Automation （ICRA）.Stockholm，Sweden：IEEE，2016：1 476-1 483.

［6］ Ji J，Wang Z，Wang Y，et al.Mapless-planner：A robust and fast planning framework for aggressive autonomous flight without map fusion［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）.Xi′an，China：IEEE，2021：6 315-6 321.

［7］ Florence P R，Carter J，Ware J，et al.Nanomap：Fast，uncertainty-aware proximity queries with lazy search over local 3d data［C］//2018 IEEE International Conference on Robotics and Automation （ICRA）.Brisbane，QLD，Australia：IEEE，2018：7 631-7 638.

［8］ Ratliff N，Zucker M，Bagnell J A，et al.CHOMP：Gradient optimization techniques for efficient motion planning［C］//2009 IEEE International Conference on Robotics and Automation.Kobe，Japan：IEEE，2009：489-494.

［9］ Wu Z，Meng Z.Safe polynomial trajectory generation for quadrotor flight in dense environments［M］.Singapore：Springer Singapore，2022.

［10］ Woods A C，La H M.A novel potential field controller for use on aerial robots［J］.IEEE Transactions on Systems，Man，and Cybernetics：Systems，2017，49（4）：665-676.

［11］ Klancˇar G，Seder M，Blazˇicˇ S，et al.Drivable path planning using hybrid search algorithm based on E* and bernstein-bézier motion primitives［J］.IEEE Transactions on Systems，Man，and Cybernetics：Systems，2019，51（8）：4 868-4 882.

［12］ Quan L，Zhang Z，Zhong X，et al.EVA-planner：Environmental adaptive quadrotor planning［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）.Xi′an，China：IEEE，2021：398-404.

［13］ Zhou B，Gao F，Pan J，et al.Robust real-time uav replanning using guided gradient-based optimization and topological paths［C］//2020 IEEE International Conference on Robotics and Automation （ICRA）.Paris，F(xiàn)rance：IEEE，2020：1 208-1 214.

［14］ Tang L，Wang H，Li P，et al.Real-time trajectory generation for quadrotors using b-spline based non-uniform kinodynamic search［C］//2019 IEEE International Conference on Robotics and Biomimetics （ROBIO）.Dali，China：IEEE，2019：1 133-1 138.

［15］ Zhou B，Gao F，Wang L，et al.Robust and efficient quadrotor trajectory generation for fast autonomous flight［J］.IEEE Robotics and Automation Letters，2019，4（4）：3 529-3 536.

［16］ Oleynikova H，Taylor Z，F(xiàn)ehr M，et al.Voxblox：Incremental 3d euclidean signed distance fields for on-board mav planning［C］//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Vancouver，BC，Canada：IEEE，2017：1 366-1 373.

［17］ Han L，Gao F，Zhou B，et al.Fiesta：Fast incremental euclidean distance fields for online motion planning of aerial robots［C］//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Macau，China：IEEE，2019：4 423-4 430.

［18］ Chen Y，Lai S，Wang B，et al.A GPU mapping system for real-time robot motion planning［C］//2021 IEEE International Conference on Real-time Computing and Robotics （RCAR）.Xining，China：IEEE，2021：762-768.

［19］ Niener M，Zollhfer M，Izadi S，et al.Real-time 3D reconstruction at scale using voxel hashing［J］.ACM Transactions on Graphics （ToG），2013，32（6）：1-11.

［20］ Qin T，Li P，Shen S.Vins-mono：A robust and versatile monocular visual-inertial state estimator［J］.IEEE Transactions on Robotics，2018，34（4）：1 004-1 020.

【責(zé)任編輯：蔣亞儒】

基金項目：國家自然科學(xué)基金項目（61963038）

作者簡介：唐嘉寧（1984—），女，云南昆明人，教授，博士，研究方向：多無人機(jī)協(xié)同、SLAM定位建圖

通訊作者：陳云浩（1991—），男，山東濟(jì)南人，講師，博士，研究方向：視覺SLAM、路徑規(guī)劃，cyh_619@163.com