文章名：EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments

【單位】浙大fastlab

【期刊】ICRA2021

摘要：本文提出了一種基于機載資源的多機器人自主導(dǎo)航的分布式異步系統(tǒng)解決方案,。在基于梯度的局部規(guī)劃框架下建立規(guī)劃系統(tǒng),，通過將碰撞風(fēng)險確定為非線性優(yōu)化問題的懲罰項來實現(xiàn)碰撞避免。為了提高魯棒性和避開局部極小值,，引入了一種輕量級拓?fù)滠壽E生成方法,。然后，agent使用不可靠的軌跡共享網(wǎng)絡(luò)在幾毫秒內(nèi)生成安全,、平滑和動態(tài)可行的軌跡,。利用深度圖像中的agent檢測校正agent之間的相對定位漂移。通過仿真和實際實驗驗證了該方法的有效性,。源代碼已發(fā)布以供社區(qū)參考,。

1.引言：

        四旋翼的敏捷性使該機器能夠在未知環(huán)境中進行單智能體自主導(dǎo)航,，在開放或已知領(lǐng)域進行多智能體精確編隊控制。 然而,，很少有工作將它們兩者結(jié)合起來展示任何能夠?qū)Ш焦蚕硐嗤粗臻g的四旋翼集群的真實世界系統(tǒng),，尤其是僅在機載處理的情況下。 在未知環(huán)境中部署多個四旋翼飛行器的困難包括但不限于障礙物參數(shù)化的重要性,、感知范圍有限,、通信不可靠和帶寬受限以及定位不一致導(dǎo)致的定位漂移。 一些相關(guān)的工作,，例如 [1, 2],，將能夠避障的現(xiàn)實世界四旋翼群推動得更遠(yuǎn)。 然而,，對于大多數(shù)先前的工作,，在運動捕捉系統(tǒng)或純仿真中，上述困難總是被忽略,，限制了其算法在實踐中的應(yīng)用,。

        本文提出了一種系統(tǒng)化的解決方案，可以在存在上述困難的情況下,，為四旋翼機群在雜亂的環(huán)境中實現(xiàn)高性能飛行,。 此外，它不需要外部定位和計算或預(yù)先構(gòu)建的地圖,。所提出的EGO-Swarm系統(tǒng)是我們先前工作的一個擴展,，它是基于無ESDF梯度的LOcal規(guī)劃器（EGO-Planner）的擴展，它為未知環(huán)境下單個四旋翼的機載局部規(guī)劃奠定了堅實的基礎(chǔ),。擴展包括拓?fù)湟?guī)劃和機間避碰兩部分,。如圖3所示,，非凸配置空間可能導(dǎo)致不期望的行為,，例如動力學(xué)不可行或擁擠的導(dǎo)航。因此,，像拓?fù)湟?guī)劃這樣的策略來避開局部極小值是有益的,。基于EGO Planner中的碰撞代價公式,，前端拓?fù)渎窂剿阉魇请[式進行的,，因此幾乎不需要計算。通過在目標(biāo)函數(shù)中加入群碰撞的加權(quán)懲罰,，實現(xiàn)了分布式機間避碰,。通過比較未來某一時刻的agent分布和正在優(yōu)化的軌跡來評估這個懲罰函數(shù)。為了盡量減少數(shù)據(jù)傳輸并允許不可靠的通信,，使用廣播網(wǎng)絡(luò)來共享軌跡,。為了糾正相對定位漂移,，可以將其增加精度到半米，我們比較了目標(biāo)agent的觀測結(jié)果和軌跡評估的預(yù)測結(jié)果,。

        提出了真實世界的實驗來驗證我們提出的群系統(tǒng),。 據(jù)我們所知，這是在未知雜亂環(huán)境中完全自主分散的四旋翼集群的第一個系統(tǒng)解決方案,，這意味著感知,、規(guī)劃和控制集成到機載系統(tǒng)中。 與幾種 SOTA 方法的比較顯示了計算效率和魯棒性,。 本文的貢獻總結(jié)如下：

（1）我們擴展了以前的工作EGO - Planner,，提出了一種新的、魯棒的拓?fù)湟?guī)劃方法,，幾乎不需要額外的計算,。

（2）我們提出了分布式和異步的無人機集群框架，它對不可靠的通信和定位漂移不敏感,。

（3）將該方法集成到一個全自動的四旋翼系統(tǒng)中,，并發(fā)布了硬件和軟件，供社區(qū)參考

2.相關(guān)工作

A.單個無人機的局部規(guī)劃

        基于梯度的運動規(guī)劃是四旋翼局部規(guī)劃的主流,。在將局部規(guī)劃問題描述為無約束非線性優(yōu)化的開創(chuàng)性工作[4,5]的基礎(chǔ)上,，提出了一系列工作[6]–[10]。他們使用各種參數(shù)化方法,，包括多項式和B樣條,，考慮軌跡的平滑性、可行性和安全性,。最近,，我們提出了一個名為EGO_Planner[3]的單四旋翼導(dǎo)航系統(tǒng)，它使用更緊湊的環(huán)境表示法進一步減少了計算時間,。這就是本文所基于的工作,。

B.拓?fù)湟?guī)劃

        拓?fù)湟?guī)劃用于避開局部最小值?；谠醋詮?fù)分析 [11] 的二維表面中的homology等價關(guān)系,，Rosmann 等人 [12] 提出了一種使用 Voronoi 圖和基于采樣的前端以及 TEB 本地規(guī)劃器 [13] 作為后端的獨特拓?fù)滠壽E規(guī)劃方法。然而,，3-D 中的homology等價關(guān)系要簡單得多,。為了捕捉獨特的有用路徑，Jaillet 等人 [14] 構(gòu)建可見性變形路圖,，其編碼比homotogy類的代表性路徑更豐富,、更相關(guān)的信息。基于 [14],Zhou 等人 [15]通過提出有效的拓?fù)涞刃z查來實現(xiàn)實時拓?fù)湟?guī)劃,。我們擴展了 EGO-Planner 以進一步加速拓?fù)湟?guī)劃的前端,。

C.分布式無人機集群

        例如，[16]-[20] 中提出了去中心化方法,。 利用速度障礙來保證質(zhì)點機器人 [16],、完整agent [17] 和非完整agent [18] 的無碰撞軌跡。 劉等人[21] 提出了一種分散和異步的無人機規(guī)劃策略,，以避免靜態(tài)/動態(tài)障礙物和機間碰撞,。 雖然這些算法是通過仿真驗證的，但沒有集成傳感,、測繪,、規(guī)劃能力。實驗結(jié)果已在 [19, 20] 中顯示,。  [19] 可以在沒有外部障礙的情況下實現(xiàn)多機點對點過渡,，并且 [20] 依賴于臨時規(guī)劃優(yōu)先級。 然而,，它們都沒有在外場環(huán)境中實現(xiàn)完全自主,。

3.基于梯度的局部規(guī)劃隱式拓?fù)滠壽E生成

        在這一部分中，我們首先介紹了我們之前在EGOPlanner[3]上的工作,，提出的swarm系統(tǒng)就是基于這一工作,。然后對提出的拓?fù)湟?guī)劃策略進行了說明。

A.無ESDF圖的梯度局部規(guī)劃器

        作為基于梯度的局部規(guī)劃器,，EGO-Planner 將軌跡生成公式化為一個非線性優(yōu)化問題,，該問題在平滑度 Js、碰撞 Jc,、動態(tài)可行性 Jd 和終端進度 Jt 之間進行權(quán)衡,。優(yōu)化問題，決策變量來自用于參數(shù)化軌跡的均勻 B 樣條 Φ 的控制點 Q ,，由下式給出

其中 ,下標(biāo) λ 表示相應(yīng)的權(quán)重,。J項可分為兩類：最小誤差和軟單邊約束。最小誤差項Js和Jt,將決策變量L  (Q) 的線性變換與期望值D之間的總誤差最小化,，由下式給出：

軟單邊約束項Jc和Jd, 懲罰超過特定閾值的決策變量,，表示為如下,。

其中參數(shù)S,、n和影響 [13] 中所述的單側(cè)約束近似精度。根據(jù)懲罰類型選擇變換和參數(shù),。

        在 EGO-Planner 中,，我們根據(jù)每個 Q 獨立擁有的環(huán)境信息提出了一種新的障礙物距離估計方法。 由幾個 {p, v} 對參數(shù)化的信息是從周圍的障礙物中高度抽象出來的，其中 p 表示障礙物表面的錨點,，v 表示從障礙物內(nèi)部到外部的安全方向,，如圖 4a 所示。 則第 i 個控制點 Qi 到第 j 個障礙物的障礙物距離 dij 定義為

        {p, v} 對生成和軌跡優(yōu)化過程如圖 4a 和 4b 所示,。 首先,，給出一個簡單的初始軌跡Φ，不管碰撞,。 然后搜索連接Φ的碰撞段兩端的安全路徑Γ,。之后，從到產(chǎn)生向量,以及p定義在障礙物表面,。利用產(chǎn)生的對,，規(guī)劃器最大化并且返回優(yōu)化后的軌跡。由于文章篇幅有限,，這里我們只對EGO-Planner的基本思想做一個簡單的描述,。 詳細(xì)解釋見[3]。 

  圖4:關(guān)于EGO-Planner和拓?fù)滠壽E生成的闡述,。(a)安全path已經(jīng)搜索到,，然后對已經(jīng)生成。(b)優(yōu)化器已找到一條安全軌跡,，滿足大于一個常值,。(c)產(chǎn)生新的 對，其中新的v是原來v的負(fù)方向,。(d)另一條不同的軌跡生成,。

 （B）隱式拓?fù)滠壽E生成

        [14, 15] 中的分析表明，廣泛使用的homotopy概念不足以捕獲 3D空間中的候選軌跡,，如圖 5 所示,。 因此，Jaillet 等人[14] 在 3D 空間中提出了一種更有用的關(guān)系,，稱為可見性變形 (visibility deformation),，以及 Zhou 等人[15] 進一步提取了稱為統(tǒng)一可見性變形 (UVD) 的 VD 子集，它可以實現(xiàn)實時操作,。 然而,，我們在本文中仍然使用術(shù)語拓?fù)湟?guī)劃，因為之前的工作沒有歧義,。 滿足 UVD 的軌跡被認(rèn)為是同胚的homeomorphic,。  [15]中定義的UVD是：

定義1：兩條軌跡τ1(s)，τ2(s),，

         傳統(tǒng)的拓?fù)湟?guī)劃方法[12]-[15],]由拓?fù)洳煌穆窂剿阉骱秃蠖藘?yōu)化組成,，主要關(guān)注于在不同的homotopy中找到多個初始路徑,。與這些方法不同，該方法通過將v反轉(zhuǎn)為來構(gòu)造不同方向的距離場,。然后是搜索過程

4.飛機集群導(dǎo)航

A.機間避碰

        agent-k考慮其他agents的存在,，如圖6所示。不同于文獻[22],，在這里我們忽略了障礙物和動力學(xué)限制（這部分已經(jīng)在第三節(jié)A部分討論過）,。類似于避障和動力學(xué)可行性的懲罰函數(shù)，我們將第k個agent的集群機間避碰的懲罰函數(shù)定義為軟單邊約束,，如下式：        

圖6：自身無人機通過接收到的周圍無人機在同一時刻的軌跡,，進行對比來產(chǎn)生自身的軌跡。 

把權(quán)重項加到方程1中去,，形成1個總優(yōu)化問題：

         任何軌跡參數(shù)化方法包含一個從決策變量映射到軌跡上點的過程,。本文用階均勻B樣條參數(shù)化軌跡，

B. 定位漂移補償

        由于在未知環(huán)境中的個體定位（沒有可靠的高頻閉環(huán)）,，漂移在飛行期間累積,。Xu等人 [23] 提出了一種具有額外UWB距離測量的無人機群狀態(tài)估計方法,，實現(xiàn)了精確的協(xié)同定位,。然而，我們更關(guān)注于穿越障礙環(huán)境,，并且必須為其他應(yīng)用程序預(yù)留計算和通信資源,。因此,，在文獻 [23] 的啟發(fā)下，提出了一種簡化且輕量的相對漂移估計方法,，該方法通過比較從接收其他agent的軌跡評估的預(yù)測位置和從觀測者的深度圖像測量的位置來實現(xiàn),。當(dāng)軌跡跟蹤誤差可忽略不計，并且任何兩個可能發(fā)生碰撞的agent中至少有一個agent看到另一個agent時,，此策略有效,。因此，我們使用來自 [24] 的控制器進行精確跟蹤,，并使用廣角相機來減少丟失agent的可能性,。

        漂移消除程序如下。 在評估agent-i的當(dāng)前位置后,，確定以 為中心,、半徑為 R 的球形信任區(qū)域，其中 R 是一個經(jīng)驗參數(shù),，指示從實驗估計的典型漂移的上限,。        

        然后我們將 內(nèi)的每個點投影到世界框架中，并收集屬于 S 的點,，從而得到一個點簇 P ? S,。然后將智能體觀察 P 的位置視為 P 的中心（第一個原始矩）,， 即

         如果 P 僅包含相應(yīng)agent的觀察而沒有任何不相關(guān)的對象,，則等式 9 成立,，這是不能保證的。然而,，由于每個智能體都規(guī)劃了與附近物體有間隙的軌跡,，因此 Equ.9 大部分時間都成立。添加了其他標(biāo)準(zhǔn)以提高agent檢測的魯棒性,，例如像素數(shù),、P 的第二個中心矩、當(dāng)前測量值與先前測量值的偏差等,。更嚴(yán)格的標(biāo)準(zhǔn)會增加假陰性率,，但由于定位是無害的漂移變化緩慢。最后,，將和 P 之間的誤差饋送到濾波器,，然后從中獲取估計的漂移。

C.agent從深度圖中移動

        我們使用占用網(wǎng)格圖來存儲靜態(tài)障礙物,，并使用深度圖像進行地圖融合,。移動agent在第四節(jié)-A 中得到處理。因此,，記錄移動的agents并把它們在建圖過程中視為障礙物是不必要的,，甚至是有害的。為了消除移動物體的影響,，我們從深度圖像中屏蔽并去除在第四節(jié)-B中檢測到的agent的像素,，如圖 7 所示。除此之外,，覆蓋大部分視野的移動物體會對 VIO 造成干擾,。因此，灰度圖像上的agent也被移除,，對相應(yīng)的深度圖像使用相同的掩碼,。這里使用的agent檢測標(biāo)準(zhǔn)不那么嚴(yán)格，因為錯誤的正值比錯誤的負(fù)值更有害,。

圖7：左：VIO漂移被估計在從接收軌跡評估的預(yù)測位置和信任區(qū)域內(nèi)的深度圖像中的觀察位置之間,。右：將觀察到的agent掩蔽以避免影響建圖。

5.系統(tǒng)架構(gòu)

        系統(tǒng)架構(gòu)如圖 8 所示,，其中包含單個agent和多agent通信系統(tǒng)的詳細(xì)架構(gòu),。

A.單agent導(dǎo)航系統(tǒng)

        單個agent系統(tǒng)，包括硬件和軟件設(shè)置,，基于我們之前的工作 EGO-Planner [3],，帶有一個額外的模塊,，可以補償 VIO 漂移并移除圖像上的被看到的agent。 所有組件都集成到一個自組裝的 250 毫米軸距四旋翼飛機中,。 對于未知環(huán)境中的軌跡生成,，使用局部地規(guī)劃器。 當(dāng)當(dāng)前軌跡與新發(fā)現(xiàn)的障礙物發(fā)生碰撞,，或者agent接近當(dāng)前軌跡的末端時,，就會激活規(guī)劃。 

B.通信框架

        兩個網(wǎng)絡(luò)用于連接系統(tǒng),，一個是廣播網(wǎng)絡(luò)用于共享軌跡,，另一個鏈網(wǎng)絡(luò)，用于同步時間戳和管理啟動序列,。

1）廣播網(wǎng)絡(luò)： 一旦一個agent生成一個新的無碰撞軌跡,，它就會立即廣播給所有agent。然后其他agent接收并存儲此軌跡,，用于在必要時為自己生成安全軌跡,。 這種閉環(huán)策略在連接穩(wěn)定且延遲可忽略不計的理想情況下正常工作。 但是,，這在實踐中并不能保證,。 因此，我們提出了兩種方法來減少碰撞的可能性,。

       首先,，在網(wǎng)絡(luò)負(fù)載下以給定頻率廣播一個軌跡。這不會造成計算負(fù)擔(dān),，因為包含三維航路點和其他參數(shù)的典型軌跡的大小小于0.5kb,。相比之下，Bluetooth等現(xiàn)代無線網(wǎng)絡(luò)可以達(dá)到1 Mbps以上的速度,。其次,，每個agent在從廣播網(wǎng)絡(luò)接收到軌跡時立即檢查碰撞，并且如果檢測到潛在碰撞,，則生成新的無沖突軌跡,。該策略可以解決，當(dāng)多個agent在非常接近的時刻生成軌跡時由于延遲或數(shù)據(jù)包丟失沒接收到其他agent的軌跡,。此外,，還考慮了計算復(fù)雜度隨agent數(shù)量的增加而增加的問題。在規(guī)劃之前,，每個agent將其當(dāng)前位置與接收到的周圍agent的軌跡進行比較,，其中任何超出規(guī)劃范圍的軌跡都將被忽略。

2)鏈網(wǎng)絡(luò)：連接穩(wěn)定的鏈?zhǔn)骄W(wǎng)絡(luò)用于時間戳同步和系統(tǒng)啟動管理,。在系統(tǒng)啟動時,agents以一個預(yù)定義的順序生成軌跡,。每個agent生成自身最初的軌跡在收到通過更高的優(yōu)先級的鏈?zhǔn)骄W(wǎng)絡(luò)發(fā)來的其他agents的軌跡之后,。這種策略避免了在系統(tǒng)啟動時由于同步軌跡生成造成的混亂，因為此時agent沒有來自其他agent的軌跡信息,。

6.benchmark

我們在英特爾酷睿 i7-9700KF CPU 上進行了關(guān)于拓?fù)湟?guī)劃,、在空闊和雜亂空間中的集群規(guī)劃的廣泛基準(zhǔn)比較。

A.拓?fù)湟?guī)劃

        我們在候選軌跡數(shù)和前端拓?fù)渎窂剿阉鞯挠嬎銜r間兩方面比較了所提出的 EGO-Swarm 與 Fast-Planner [15] 的拓?fù)湟?guī)劃性能,。 如圖 9 所示,，EGOSwarm 找到的候選軌跡較少,，這意味著找到全局最優(yōu)的概率較低,，但比 [15] 快兩個數(shù)量級。 由于 Fast-Planner 通過 PRM [25] 圖搜索,、路徑縮短和路徑修剪找到拓?fù)洳煌穆窂?，與所提出的隱式拓?fù)渎窂剿阉鞣椒ㄏ啾龋@些方法耗時但具有更高的自由度,。

圖9： 拓?fù)渎窂剿阉鞯谋容^,。與 Fast-Planner 相比，本文提出的規(guī)劃器發(fā)現(xiàn)的候選拓?fù)洳煌能壽E更少,，但消耗的計算更少,。請注意，顯示的時間僅用于前端局部最小值發(fā)現(xiàn),，而顯示的軌跡經(jīng)過優(yōu)化以獲得更好的可視化,。

B.集群規(guī)劃

1)在空曠空間中：我們將所提出的方法與DMPC  [19] 、ORCA  [16] 和RBP  [20] 進行了比較,，從飛行距離,、飛行時間、每個agent的碰撞次數(shù)和計算時間幾個方面進行了比較,。除最大速度和加速度外,，每種比較方法都使用默認(rèn)參數(shù)。如圖10所示,，八個agent在一個圓上執(zhí)行交換變換,。表I中的結(jié)果是所有agent的平均值。tcal用“*”標(biāo)記,，因為我們記錄的計算時間是規(guī)劃所有agent的整個軌跡的總時間(對DMPC和RBP而言),，而對于ORCA和EGO -Swarm，它是每個agent的局部重新規(guī)劃時間,。

         表I 和圖10 表明 RBP 傾向于生成安全但保守的軌跡,，因為構(gòu)建凸的相對安全飛行走廊 [20] 顯著壓縮了解空間。  DMPC 專為分布式部署而設(shè)計,。 然而,，它需要準(zhǔn)確和高頻的姿態(tài)通信,，這在現(xiàn)實應(yīng)用中無法保證。 高效的規(guī)則使 ORCA 更新更快,。 但是,，使用速度作為控制命令使其與四旋翼等三階系統(tǒng)不兼容。 碰撞的風(fēng)險也限制了它的應(yīng)用,。 相比之下,，所提出的方法生成最短的無碰撞、非保守軌跡,，計算速度快,。 因此，它可以實現(xiàn)四旋翼的實時應(yīng)用,。

2）在充滿障礙物的空間中：我們仿真了從地圖一側(cè)到另一側(cè)的10架無人機,，速度限值為2米/秒，四旋翼半徑為0.2米,。圖2是0.42個障礙物每平方米的仿真環(huán)境,。每個Agent獨立感知環(huán)境，構(gòu)建的局部地圖以不同的顏色顯示,。結(jié)果總結(jié)在表II中,，其中是平均飛行距離，是飛行試驗期間最接近障礙物的距離,。設(shè)計了一種逆點對點轉(zhuǎn)換,，使圍繞地圖中心的機間碰撞避免項不可避免。在這種情況下,，屬于群的每個agent都可以規(guī)劃平滑和安全的軌跡,。

3）可測性分析：我們在以直線排列的agents飛到 50 米外的隨機目標(biāo)點的情況下評估計算性能。如圖 11 所示,，由于第五節(jié)-B1中的按需碰撞檢查策略,，時間復(fù)雜度隨著agent數(shù)量的增加而逐漸趨于平穩(wěn)。

7.真機實驗

A.室內(nèi)

        我們在 1.5m/s 的速度限制下展示了幾個室內(nèi)實驗,，如圖 12 所示,。 路徑的顏色表示每個四旋翼的大致飛行時間。 頂部顯示了三個四旋翼在空闊空間中執(zhí)行循環(huán)交換,，因此需要相互避免碰撞,。 在中間，四旋翼飛機在飛行過程中避開障礙物,，一個接一個地通過狹窄的門,。 在下圖中，我們設(shè)置了一個由一些垂直和水平障礙物組成的更雜亂的環(huán)境。 三個四旋翼飛機設(shè)法在這個環(huán)境中導(dǎo)航,。

 B.室外

        如圖1所示,，在樹木相隔約2米的森林中，三個四旋翼開始在森林中,，并設(shè)法到達(dá)外部森林的目標(biāo)位置,。速度限制設(shè)置為 1.5m/s。 為了進一步強調(diào)相互避碰,，我們顛倒了目標(biāo)位置相對于起始位置的順序,，使得相互回避不可避免，就像我們在第六節(jié)-B2中所做的那樣,。 有關(guān)實驗的更多信息,，請參閱代碼并在 github上觀看我們附加的視頻。

8.總結(jié)和未來工作

        在本文中,，提出了一種僅使用機載資源在未知雜亂環(huán)境中進行多機器人導(dǎo)航的系統(tǒng)解決方案,。 基準(zhǔn)比較表明其計算時間短,，軌跡質(zhì)量高,。 真實世界的實驗驗證了它的穩(wěn)健性和效率。 未來,，我們將致力于實現(xiàn)精準(zhǔn)共定位和現(xiàn)實世界動態(tài)避障,。 主要挑戰(zhàn)在于對其他無人機的不精確觀察以及不可靠的運動物體檢測和預(yù)測。 最后,，我們希望為機器人社區(qū)提供一個完整的四旋翼群在復(fù)雜環(huán)境中的自主導(dǎo)航解決方案,。