FC-Planner方案實現細節與難點講解

1. 骨架提取

骨架提取是FC-Planner的核心模塊之一,其目的是從輸入的點云數據中提取出場景的骨架結構。這一步的關鍵是如何準確高效地計算每個點的ROSA點。

1.1 ROSA點計算

ROSA點的計算涉及到兩個優化問題:

1. ROSA點方向$v_p$的優化:

   $v_p$的優化目標是最小化其與鄰域點法線方向的方差。這可以通過求解一個二次規劃問題來實現:

   $$\begin{array}{r}{v}_p^{i+1}=\underset{v_p}{\mathrm{argmin}}{v}_p^T{\Sigma }_p^i{v}_p,\\ \text{s.t.}||v_p|{|}_2=1,v_p?v_p^i>0.\end{array}$$

   其中,${\Sigma }_p^i$是鄰域點法線的協方差矩陣。這個二次規劃問題可以用標準的優化庫高效求解。

2. ROSA點位置$x_p$的優化:

   $x_p$的優化目標是最小化其到鄰域點法線延長線的距離平方和:

   $$\underset{x_p\in {\mathbb{R}}^3}{\mathrm{argmin}}\sum _{p_k\in N_p}||(x_p?p_k)\times n(p_k)|{|}_2^2.$$

   這個問題有解析解,可以直接計算得到最優的$x_p$。

1.2 骨架生成

在得到所有點的ROSA點后,作者用一維移動最小二乘法將這些離散的ROSA點連接成骨架曲線。這一步的難點在于如何選擇合適的參數化方法和基函數,使得生成的骨架曲線既光滑又能很好地擬合ROSA點。在實現中采用了線性參數化和多項式基函數,取得了不錯的效果。

2. 視點生成

視點生成模塊的任務是在骨架的引導下,生成一組數量最少但能完全覆蓋場景的視點。其中的難點主要有兩個:如何避免視點之間的冗余,以及如何優化視點的姿態使其覆蓋更多的區域。

2.1 視點采樣

為了避免冗余的視點采樣,作者利用骨架提取出一組視點采樣射線。這些射線起始于骨架,穿過占用的體素,延伸到自由空間。只在這些射線上采樣視點,而不是在整個空間中隨機采樣。這種有針對性的采樣策略可以顯著減少冗余視點的數量。

2.2 視點姿態優化

提出了一種基于引力模型的視點姿態優化方法。其基本思想是,將視點看作帶電粒子,覆蓋區域多的視點帶正電,覆蓋區域少的視點帶負電。然后讓帶負電的視點在帶正電視點的吸引下移動,直到達到平衡狀態。這個過程可以用下面的公式描述:

$$p_q=p_q+\sum _{v{p}_a\in V{P}_a}\frac{c_a}{c_q}?(p_a?p_q),\text{s.t.}{c}_a<c_q.$$

其中,$p_q$是當前視點的位置,$V{P}_a$是其吸引視點集合,$c_a$和$c_q$分別是吸引視點和當前視點的覆蓋區域數量。通過這種優化,可以使每個視點的姿態都朝向覆蓋區域最多的方向調整,從而減少視點總數。

3. 分層覆蓋規劃

分層覆蓋規劃是FC-Planner的另一個核心模塊,其目的是在生成的視點集合上規劃一條最短的覆蓋路徑。采用分治的策略,將整個問題分解為全局規劃、局部規劃和軌跡優化三個層次。

3.1 全局序列規劃

在全局層,通過求解一個非對稱TSP問題,得到一個遍歷所有子空間的最優序列。這里的難點在于如何快速求解TSP問題。采用了啟發式的LKH算法,該算法能在可接受的時間內給出近似最優解。

3.2 局部路徑規劃

在局部層,為每個子空間并行地規劃覆蓋路徑。這里的關鍵是如何在保證覆蓋完整性的同時最小化路徑長度。將其表述為一個帶時間窗約束的TSP問題:

min ? ∑ i = 1 R ∑ j = 1 R c ( v p i , v p j ) ? x ( v p i , v p j ) s.t. ∑ j = 1 R x ( v p i , v p j ) = 1 , ? i ∈ { 1 , … , R } , ∑ i = 1 R x ( v p i , v p j ) = 1 , ? j ∈ { 1 , … , R } , x ( v p i , v p j ) ∈ { 0 , 1 } , v p 1 = v p s t a r t ( g i ) , v p R = v p e n d ( g i ) . \begin{aligned} \min &\sum_{i=1}^{R} \sum_{j=1}^{R} c_{(vp_i,vp_j)} \cdot x_{(vp_i,vp_j)} \\ \text{s.t.} & \sum_{j=1}^{R} x_{(vp_i,vp_j)} = 1, \forall i \in \{1,\dots,R\}, \\ & \sum_{i=1}^{R} x_{(vp_i,vp_j)} = 1, \forall j \in \{1,\dots,R\}, \\ & x_{(vp_i,vp_j)} \in \{0,1\}, \\ & vp_1 = vp^{(g_i)}_{start}, ~ vp_R = vp^{(g_i)}_{end}. \end{aligned} mins.t.?i=1∑R?j=1∑R?c(vpi?,vpj?)??x(vpi?,vpj?)?j=1∑R?x(vpi?,vpj?)?=1,?i∈{1,…,R},i=1∑R?x(vpi?,vpj?)?=1,?j∈{1,…,R},x(vpi?,vpj?)?∈{0,1},vp1?=vpstart(gi?)?,?vpR?=vpend(gi?)?.?

其中,$x_{(v{p}_i,v{p}_j)}$是決策變量,表示是否從視點$v{p}_i$飛到$v{p}_j$,而$c_{(v{p}_i,v{p}_j)}$是兩視點間的代價。最后兩個約束條件保證了路徑的起點和終點分別是該子空間的入口和出口視點。

3.3 軌跡優化

得到覆蓋路徑后,需要進一步將其優化為一條無碰撞、動態可行的軌跡。這里利用了凸優化的方法。首先,在覆蓋路徑上生成一系列的安全飛行走廊(SFC)。每個SFC都是一個凸多面體,可以保證無人機在其中飛行時不會與障礙物碰撞。然后,在SFC的約束下,求解一個最小化時間的軌跡優化問題:

$$\begin{array}{rl}\underset{T,tp}{\min }& \sum _{i=1}^M{T}_i\\ \text{s.t.}& t{p}_i(t)\in CP(i),?t\in [0,T_i],\\ & \Vert {\dot{tp}}_i(t){\Vert }_2^2\le v_{max}^2,\\ & \Vert {\ddot{tp}}_i(t){\Vert }_2^2\le a_{max}^2,\\ & {\Vert \frac{d^{3}t{p}_i(t)}{d{t}^3}\Vert }_2^2\le j_{max}^2,\\ & t{p}_i(0)=p_R^{(i?1)},t{p}_i(T_i)=p_R^i.\end{array}$$

這個問題可以用凸優化的方法高效求解,得到一條平滑、安全、滿足動力學約束的軌跡。

4. 總結

FC-Planner的實現涉及到多個領域的技術,包括計算幾何、組合優化、凸優化等。其中的難點主要集中在兩個方面:一是如何在保證覆蓋完整性的同時最小化視點數量和路徑長度;二是如何高效地求解由此產生的各種優化問題。針對第一個難點,提出了骨架引導的空間分解和視點生成策略,可以避免大量的冗余計算。針對第二個難點,巧妙地利用了問題的結構特點,將其分解為多個易于并行求解的子問題,同時采用了各種啟發式算法和凸優化技術,大大提高了求解效率。這些思想和技術不僅限于無人機覆蓋規劃,在其他路徑規劃問題中也有廣泛的應用前景。

具體細節可以閱讀下下面的論文原文（中文版）：

FC-Planner: 一個基于骨架引導的快速覆蓋復雜3D場景的規劃框架

摘要

UAV的3D覆蓋路徑規劃是多種實際應用中的一個關鍵問題。然而,現有方法在大型復雜場景中的系統簡潔性、計算效率和路徑質量方面表現不盡如人意。為了應對這些挑戰,我們提出了FC-Planner,一個基于骨架引導的規劃框架,可以在無需預處理的情況下快速實現對復雜3D場景的空中覆蓋。我們通過基于骨架的空間分解(SSD)將場景分解為幾個簡單的子空間。此外,骨架引導我們輕松確定自由空間。我們利用骨架高效生成一組最小的專門化和信息豐富的視點,以實現完全覆蓋。基于SSD,分層規劃器有效地將大規劃問題劃分為獨立的子問題,實現每個子空間的并行規劃。然后,我們精心設計的全局和局部規劃策略被整合,以保證路徑生成的高質量和高效率。我們進行了廣泛的基準測試和真實世界測試,其中FC-Planner的計算速度比最先進的方法快10倍以上,路徑更短,覆蓋更完整。源代碼將公開發布以造福社區。項目頁面: https://hkust-aerial-robotics.github.io/FC-Planner。

I. 引言

近來,由于無人機(UAV)的靈活性和機動性,它們在結構檢查[1, 2]和3D重建[3]等各種任務中收集目標場景信息方面變得越來越受歡迎。為了完成這些任務,UAV需要找到一條最短路徑來完全覆蓋3D場景,這通常被表述為3D覆蓋路徑規劃問題[4]。

現有的工作[1, 2, 5]-[10]通常通過將這個問題分為兩個子任務來解決:1)確定一組覆蓋目標場景的視點,2)確定具有完全覆蓋的最短路徑。然而,當前這些子任務的解決方案存在一些限制,阻礙了系統簡潔性、計算效率和路徑質量的滿意實現。對于第一個子任務,現有方法需要使用額外的工具(如Blender [11], CloudCompare [12])進行預處理以提取自由空間,確保視點的安全性,這增加了系統的復雜性。此外,缺乏一種能夠產生最小的信息豐富的視點集,同時全面覆蓋目標場景的高效策略。現有方法在視點數量和完整性之間存在權衡,即少量視點無法保證完全覆蓋,而大量視點則會帶來顯著的計算成本。另一方面,大多數框架中的路徑規劃器全局優化一條完全覆蓋場景的最短路徑,通常被表述為組合優化問題(COP) [13]。然而,由于規劃問題的大規模,現有方法在大型復雜場景中經常難以迅速找到高質量的解決方案。目前,在復雜3D場景覆蓋的背景下,迫切需要一種高效的規劃方法,該方法應能夠將艱巨的規劃問題分解為可并行化的子問題,同時確保解決方案的質量和計算效率。

為了解決上述限制,我們提出了FC-Planner,這是一個為快速覆蓋大型復雜3D場景量身定制的基于骨架引導的規劃框架。我們精心設計的全局和局部規劃程序,最終生成高質量的覆蓋路徑,同時保持高計算效率。

我們將提出的方法與最先進的工作進行了仿真比較。結果表明,我們的方法在計算效率方面取得了顯著提升(快10倍以上),同時實現了更完整的覆蓋和更短的路徑。為了進一步驗證FC-Planner,我們在一個具有挑戰性的場景中進行了真實世界的覆蓋測試。仿真和真實世界的實驗都證明了我們的系統相比于最先進方法在簡潔性和性能方面的優越性。本文的貢獻如下:

1. 一種高效提取復雜3D場景骨架并指導覆蓋規劃的SSD方法。
2. 一種高效且專門化的視點采樣方法,由骨架引導,以及一個引力模型,可有效篩選出最小的信息豐富視點集。
3. 一種分層覆蓋規劃器,將整個規劃問題分解為可并行化的子問題。結合精心設計的全局和局部規劃模塊,它確保了高路徑質量和計算效率。
4. 大量實驗驗證了所提出方法的有效性。我們的實現源代碼將公開發布。

II. 相關工作

3D場景中的覆蓋路徑規劃問題(CPP)一直是UAV研究的活躍話題,有廣泛的實際應用。現有方法通常采用基于采樣的框架,分兩步解決3D CPP問題,即視點生成和路徑規劃。

視點生成: 這指的是獲得一組視點以滿足目標場景覆蓋需求的過程。現有方法通常需要預處理來識別自由空間,從中采樣安全有效的視點。一些早期方法[1, 5]-[8]利用網格三角形來采樣視點。另外,其他工作[9, 14]采用了點云的形式。然而,這兩種方法都落入了后一類。通常,這些方法首先采樣大量視點候選,然后迭代選擇一個視點子集以實現完全覆蓋。這種策略導致了大量的計算時間用于選擇,并且很難確定所需的最少視點數量。

相比之下,我們的方法通過利用骨架來確定自由空間并采樣場景周圍的視點,避免了預處理。為了高效找到最小的視點集以實現完全覆蓋,我們開發了一種策略,通過重復更新一組視點的姿態而不依賴耗時的選擇過程。

路徑規劃: 此步驟的目標是在全面覆蓋場景的同時規劃最短路徑。以前的方法[1, 5]-[10]將這個問題表述為COP的具體形式,例如,旅行商問題(TSP)[15]、子模塊方向TSP(SDOP)[16]等。眾所周知,這些問題是NP難的,計算時間隨著場景復雜度的增加而急劇增加。為解決這個問題,HCPP[2]提出了一種將場景劃分為八叉樹的方法。每個葉節點在八叉樹中包含最多N個視點,定義一個子空間。然后,規劃問題被轉化為解決每個子空間的局部TSP。然而,將視點限制在單個子空間內并不能保證該子空間的完全覆蓋。此外,來自其他子空間的視點可能需要實現完全覆蓋。因此,每個子空間的覆蓋區域不能預先分配,下一個子空間的覆蓋目標只能在前一個子空間規劃完成后更新。這在為不同子空間實現并行規劃時帶來了挑戰。

相反,我們的SSD將場景分解為幾個由骨架引導的簡單子空間。這確保了每個子空間內的視點避免與該子空間不匹配。因此,可以為每個子空間并行生成路徑,從而顯著節省計算時間。

III. 系統概述

所提出的框架以目標場景的點云作為輸入,這在各種應用中被廣泛采用。如圖2所示,它由SSD(Sect.IV)、高效視點生成(Sect.V)和分層覆蓋規劃(Sect.VI)組成。SSD提取場景的骨架,并將點云分解為具有簡單幾何形狀的子空間。在骨架的引導下,我們生成采樣射線來采樣安全的初始視點,無需預處理。然后該方法最小化視點數量,并使用引力模型更新它們的姿態(Sect.V)。之后,分層規劃器找出子空間的全局訪問序列,并行優化每個子空間的局部路徑,優化局部路徑之間的連接部分,并生成一條實現完全覆蓋的軌跡(第IV節)。

注釋:本章概述了FC-Planner的三大模塊:基于骨架的空間分解(SSD)、高效視點生成和分層覆蓋規劃。SSD利用骨架將復雜場景分解為簡單子空間,視點生成模塊在骨架引導下高效采樣視點并優化姿態,最后分層規劃器實現了子問題的并行優化,從而顯著提高計算效率。這三個模塊協同工作,快速生成高質量的覆蓋路徑。

IV. 基于骨架的空間分解

在本節中,我們分三步介紹基于骨架的空間分解。1) 給定目標場景的點云,計算由廣義旋轉對稱軸(ROSA)點[17]組成的骨架(第IV-A節)。2) 將骨架分解為幾個簡單形狀,稱為分支(第IV-B節)。3) 將點云分配到每個分支中,形成相應的子空間(第IV-C節)。

A. 骨架提取

首先,我們通過法線估計[18]從輸入點云獲得定向點云$P_O$。受[17]啟發,我們使用一組ROSA點表示骨架,每個點由位置和方向表示為r=(x, v)。為加快計算過程,我們對$P_O$進行下采樣得到$P_D$,并計算$P_D$中每個點p的ROSA點$r_p=(x_p,v_p)$。對于每個$p$,其對應的ROSA點在由$v_p$和p構成的平面內對p的鄰域$N_p$進行操作。ROSA點在$N_p$周圍表現出高度旋轉對稱性。這一特性要求方向$v_p$最小化$v_p$與$N_p$中法線之間角度的方差,而位置$x_p$最小化到$N_p$中法線延長線的平方距離之和。首先,給定初始方向$v_p^0$,我們通過求解以下二次規劃問題迭代優化$v_p$:

$$\begin{array}{r}{v}_p^{i+1}=\underset{v_p}{\mathrm{argmin}}{v}_p^T{\Sigma }_p^i{v}_p,\\ \text{s.t.}||v_p|{|}_2=1,v_p?v_p^i>0,\end{array}$$

其中$v_p^i$表示第i次迭代的方向。${\Sigma }_p^i$是p鄰域$N_p^i$中所有點法線的協方差矩陣。找到ROSA點的方向后,我們使用以下程序計算$x_p$:

$$\underset{x_p\in {\mathbb{R}}^3}{\mathrm{argmin}}\sum _{p_k\in N_p}||(x_p?p_k)\times n(p_k)|{|}_2^2,$$

其中$n(p_k)$表示$p_k\in N_p$的點法線。最后,我們對所有ROSA點應用一維移動最小二乘法[19]生成骨架。骨架存儲在無向圖$G=(V_s,E_s)$中,其中頂點$V_s$包含所有ROSA點,邊$E_s$表示它們之間的連接。

注釋:骨架提取利用了ROSA點的旋轉對稱性,通過優化ROSA點的位置和方向來生成骨架。其核心是一個二次規劃問題,用于優化ROSA點的方向,使其與鄰域點的法線方向一致。這一步驟高效提取了場景的骨架結構。

B. 骨架分解

如算法1所述,我們將提取的骨架分解為幾個分支,每個分支包含G中的一組邊。這些分支都包含在分支集B中。在第2行,我們識別G上的關節J,定義為度數大于2的頂點。由于簡單幾何要求,一個分支通常從一個關節開始,以一個關節或葉頂點(度等于1)結束。為了獲得滿足上述需求的分支,我們在第3-14行開發了一個類似深度優先搜索(DFS)的程序[20]。此外,為確保每個分支盡可能簡單易覆蓋,我們要求一個分支內的邊應具有相似的方向。因此,在第16-26行,我們將方向變化較大的分支分解為更簡單的分支。

注釋:骨架分解利用深度優先搜索,將骨架分解為多個簡單分支。為了確保分支的簡單性,算法還會將方向變化較大的分支進一步分解。這一步驟簡化了骨架結構,為后續的空間分配和路徑規劃提供了基礎。

C. 空間分配

給定分解后的骨架,我們的目標是將$P_O$中的點分配到多個子空間中,每個子空間對應B中的一個分支。我們將這些分支中的邊離散化為幾個定向點,其方向是它所在邊的方向。由這些定向點確定的平面記為Γ。然后,對于每個平面,我們從$P_O$中查詢該平面內的點。如果一個點p∈$P_O$位于多個平面上,p將被分配到其定向點最接近它的平面。之后,由同一分支的平面查詢的點(稱為分配點A)被包含在一個子空間中。所有子空間包含在$S=s_1,...,s_N$中。

注釋:空間分配步驟將點云中的點按照它們與骨架分支的距離分配到不同的子空間。這種分配方式簡單高效,同時保證了子空間的獨立性,為并行路徑規劃創造了條件。

V. 基于骨架的視點生成

本節介紹高效生成完全覆蓋的視點。骨架允許識別內部空間,確保視點的安全性,無需預處理。此外,骨架產生一組視點采樣射線,指導視點的生成(第V-A節)。為高效找到最小視點集,我們提出了一個類似引力的模型,迭代調整視點的姿態并移除冗余視點,篩選出更具代表性的子集(第V-C節)。這個過程通過允許更快速可見性檢查的雙向光線投射(BiRC)進一步加速(第V-B節)。

A. 內部空間和視點采樣射線

內部空間用體素地圖表示,所有體素初始化為自由狀態。包含$P_O$的體素被更新為占用狀態。對于每個平面${\gamma }_i\in \Gamma$(第IV-C節),我們從其定向點$o_i$(位于骨架上)到每個分配點A_i進行光線投射。在穿過占用體素之前的射線上的體素被標記為內部。射線繼續延伸到自由空間,我們將這些射線從占用體素開始的部分定義為視點采樣射線。沿著它們采樣安全和信息豐富的視點,減少冗余采樣的視點數量。這些專門的視點還提供了更好的覆蓋,因為骨架引導它們都指向表面。此外,從$o_i$投射的視點采樣射線屬于與$o_i$對應的子空間。

B. 雙向光線投射

光線投射廣泛用于視點的可見性檢查。然而,這種方法從射線的一端開始單向遍歷體素。當占用體素位于射線的另一端附近時,它可能無法有效識別遮擋。為解決這個問題,我們實現了雙向光線投射(BiRC),其中體素從兩端向射線中點遍歷。這種方法在遇到上述情況時更早停止,消除了冗余檢查。

注釋:本小節介紹了兩個加速可見性檢查的技術。首先,利用骨架和光線投射快速識別內部空間和視點采樣射線,減少冗余采樣。其次,雙向光線投射通過從兩端遍歷體素的方式,進一步提高了可見性檢查效率。

C. 視點姿態的迭代更新

我們采用5自由度視點,表示為$vp=[p,\theta ,\phi ,id]$,其中$p$是傳感器的3D位置,$\theta$和$\phi$分別表示俯仰角和偏航角。參數id是視點所屬的子空間。視點采樣射線$r_{v}s$的起點和方向分別描述為$sr=[x_{sr},y_{sr},z_{sr}]$和$dr=[n{x}_{dr},n{y}_{dr},n{z}_{dr}]$。

我們沿每個r_vs在距離D處采樣一個視點,其id指的是r_vs所屬的子空間。采樣視點的位置、俯仰角和偏航角按如下方式確定:

$$\begin{array}{rl}p& =sr+D?dr,\\ \theta & =\arcsin (?n{z}_{dr}/||dr|{|}_2),\\ ?& =\arctan (?n{y}_{dr}/?n{x}_{dr}).\end{array}$$

這些采樣的視點稱為初始視點$V{P}_{i}ni$。第一步是使用提出的BiRC確定$V{P}_{i}ni$中每個視點覆蓋的體素。接下來,為減少視點數量,如果一個體素被兩個以上的視點觀察到,我們將其分配給覆蓋體素最多的視點。然后,我們從$V{P}_{i}ni$中移除未分配任何體素的視點。之后,我們為$V{P}_{i}ni$構建一個kd樹[21] $T_{i}ni$,并為每個視點定義二元狀態(活躍或休眠),初始將所有視點設置為活躍。

為確保每個視點覆蓋盡可能多的體素,我們使用引力模型將覆蓋體素較少的視點合并到覆蓋體素較多的視點中。具體來說,從覆蓋體素最多的視點開始,到覆蓋體素最少的視點,每個視點(表示為$v{p}_q$)使用半徑搜索從$T_{i}ni$中查詢其鄰域$V{P}_q$。半徑$r_q$由最大可見距離$d_v$和視場$(FoV)[f_h,f_w]$確定:

$$r_q=d_v?\tan (\min (f_h,f_w)/2).$$

我們使用$V{P}_q$中所有活躍視點$V{P}_a$通過引力模型更新查詢視點$v{p}_q$的姿態:

$$p_q=p_q+\sum _{v{p}_a\in V{P}_a}\frac{c_a}{c_q}?(p_a?p_q),\text{s.t.}{c}_a<c_q,$$

其中$c_q$和$c_a$分別表示$v{p}_q$和$V{P}_a$中每個視點覆蓋的體素數量。$p_q$表示$v{p}_q$的更新位置。類似地,我們獲得更新后的俯仰角${\theta }_q$和偏航角${\phi }_q$。它們進一步調整以使$v{p}_q$覆蓋的體素分布在FoV的中心周圍。$v{p}_q$所屬的子空間與$V{P}_{i}ni$中最接近$v{p}_q$的視點相同。接下來,我們將$V{P}_a$中用于更新$v{p}_q$的視點狀態設置為休眠。之后,我們通過對所有活躍視點執行第一步來識別未覆蓋的體素,然后采樣新的視點來覆蓋它們,表示為$V{P}_{u}nc$。最后,我們對$V{P}_{u}nc$重復上述步驟以實現完全覆蓋。經過這種視點姿態的迭代更新后,所有剩余的活躍視點根據它們的id分配到S中的每個子空間,每個視點子集表示為$V=V{P}_1,...,V{P}_N$。

注釋:本小節提出了一種基于引力模型的視點姿態優化方法。通過將視點向覆蓋體素更多的方向移動,并去除冗余視點,該方法可以在保證覆蓋完整性的同時最小化視點數量。優化后的視點集為路徑規劃提供了高質量的輸入。

VI. 分層覆蓋規劃

給定上述視點,這個階段致力于找到穿過它們的最短無碰撞路徑。在SSD的支持下,我們將大規模規劃問題分解為幾個獨立且可并行的子問題,分為五個步驟。

全局序列規劃。它試圖解決一個最優巡游問題,即從UAV當前位置出發訪問所有子空間,這被定義為非對稱旅行商問題(ATSP)[22]。我們首先計算每個子空間中視點的質心,然后構建這些質心和UAV當前位置之間的歐幾里得距離矩陣M_G。Lin-Kernighan-Helsgaun (LKH)求解器[23]以M_G作為輸入,獲得全局序列[g_1, …, g_N]。

局部邊界選擇。我們期望為每個子空間選擇合適的起始和結束視點,確保局部規劃的結果與全局序列兼容。根據全局序列,我們將所有質心排序為$Se{q}_C=[k_0,...,k_N]$,其中$k_0$是UAV的當前位置,其他是排序后的質心。基于$Se{q}_C$,第i個訪問子空間的起始$v{p}_{start}^{(g_i)}$和結束$v{p}_{end}^{(g_i)}$通過以下方式獲得:

$$\begin{array}{rl}v{p}_{start}^{(g_i)}& =\underset{v{p}_{g_i}\in V{P}_{g_i}}{\mathrm{argmin}}||p_{g_i}?k_{i?1}|{|}_2^2+||p_{g_i}?k_i|{|}_2^2,\\ v{p}_{end}^{(g_i)}& =\underset{v{p}_{g_i}\in V{P}_{g_i}}{\mathrm{argmin}}||p_{g_i}?k_i|{|}_2^2+||p_{g_i}?k_{i+1}|{|}_2^2.\end{array}$$

特別地,最后訪問的子空間將沒有結束視點,因為沒有后續子空間。

并行局部路徑規劃。一旦確定了邊界視點,我們就可以為每個子空間單獨規劃局部路徑。我們將這個問題表述為TSP的一個變體,增加了給定起點和終點的約束。假設一個子空間有R個視點,這個TSP的成本矩陣M_L如下:

M L ( i , j ) = { 0 , i = = j 或? j = 0 + ∞ , i = R ? 1 且? j ∈ { 1 , . . . , R ? 2 } c ( v p i , v p j ) , 其他 , M_L(i,j) = \begin{cases} 0, & i == j \text{ 或 } j = 0 \\ +\infty, & i = R-1 \text{ 且 } j \in \{1, ..., R-2\} \\ c_{(vp_i,vp_j)}, & \text{其他} \\ \end{cases}, ML?(i,j)=? ? ??0,+∞,c(vpi?,vpj?)?,?i==j?或?j=0i=R?1?且?j∈{1,...,R?2}其他?,

$$c_{(v{p}_i,v{p}_j)}=\max \left(\max \left(\frac{L(p_i,p_j)}{v_{max}},\frac{Ang({\theta }_i,{\theta }_j)}{{\omega }_{max}}\right),\frac{Ang({?}_i,{?}_j)}{{\omega }_{max}}\right),$$

$$Ang(a_1,a_2)=\min (|a_1?a_2|,2\pi ?|a_1?a_2|),$$

其中$L(?)$是通過A*算法搜索的$v_i$和$v_j$之間的無碰撞路徑長度,$v_{max}$和${\omega }_{max}$是速度和俯仰角、偏航角角速度的限制。對于最后訪問的子空間,$M_L$中所有$+\infty$條目都設置為$c_{(v{p}_i,v{p}_j)}$。對于每個$V{P}_i\in V$,我們使用多線程編程并行生成覆蓋其對應子空間的局部路徑。假設總共有$m\in {\mathbb{R}}^{+}$個視點,所有子問題中最大的視點數是$e\in {\mathbb{R}}^{+}$,$m?e$。由于使用LKH求解器解決TSP的時間復雜度為$O(n^{2.2})$[24],不進行問題分解直接解決所有視點的TSP需要$O(m^{2.2})$時間。得益于我們的并行規劃,解決所有子問題的時間復雜度降低到$O(e^{2.2})$。因此,我們的方法有效降低了時間復雜度并提高了計算效率。我們按全局序列的順序組合所有局部路徑,得到整個覆蓋路徑$P_C$。

注釋:分層覆蓋規劃的核心是將大規模TSP分解為多個子問題并行求解。全局序列規劃決定了子空間的訪問順序,局部邊界選擇確定了每個子空間的起始和結束視點,從而實現子問題的獨立性。并行局部路徑規劃利用多線程編程同時優化各個子空間內的路徑,大大減少了計算時間。

局部路徑優化。由于前一步中的覆蓋路徑是在每個子空間內優化的,它不能保證全局最優性,因此可能產生不必要的繞路。我們觀察到這些繞路通常發生在局部路徑的連接處附近。因此,我們提出了一個局部優化策略,以進一步改善連接處的整體覆蓋路徑,如算法2所示。在第1行,連接點被定義為兩個分支相交的頂點。第2-16行展示了使用局部搜索進行迭代優化的程序,共進行K次迭代。它首先選擇整個覆蓋路徑中靠近連接點的部分。然后,每次迭代從這些部分隨機選擇三個視點,如果交換它們可以縮短覆蓋路徑,就使用2-opt[25]進行交換。

注釋:局部路徑優化通過局部搜索和2-opt操作改進覆蓋路徑質量。它集中優化相鄰子空間連接處的路徑,消除不必要的繞路,同時保留子空間內部已優化的路徑,在提高路徑質量的同時將計算成本降到最低。

無碰撞軌跡優化。它的目標是將優化后的覆蓋路徑$P_R=v_R^0,...$ , 為一條平滑、動態可行、安全且時間最短的軌跡,通過所有視點。具體來說,我們將P_R分成M段軌跡,然后生成3D安全飛行走廊(SFCs),其中每個凸飛行走廊被分配一組連續的軌跡段。對于位置軌跡,SFCs用于確保安全性:

$$t{p}_i(t)\in CP(i),?t\in [0,T_i],?1\le i\le M,$$

其中$t{p}_i$是第$i$段軌跡,持續時間為$T_i$,$CP(i)$表示SFCs中分配給$t{p}_i$的凸飛行走廊。為了避免激進飛行導致的視覺模糊并確保動態可行性,還考慮了速度、加速度和加加速度限制:

KaTeX parse error: Undefined control sequence: \dddot at position 215: …i \leq M, \\ ||\?d?d?d?o?t?{tp}_i(t)||^2_2…

其中$v_{max}$、$a_{max}$和$j_{max}$是動態限制。我們還為俯仰角和偏航角生成軌跡,其中最大角速度的限制類似于式(9)。

我們利用MINCO[26]優化覆蓋軌跡的時間分配,同時滿足上述約束。

注釋:無碰撞軌跡優化基于覆蓋路徑生成平滑、安全且動態可行的飛行軌跡。它利用安全飛行走廊(SFC)確保無碰撞性,同時考慮無人機的動力學約束,如速度、加速度限制等。此外,它還優化了軌跡的時間分配,使執行時間最短。生成的軌跡可直接用于無人機控制。

VII. 實驗

A. 實現細節

在骨架提取步驟中,我們將輸入點云歸一化到一個單位球內,這增強了其對場景尺度變化的魯棒性。我們在算法1中設置$\delta$= 45°,局部路徑優化中的迭代次數K = 10000,因為每次迭代僅消耗約0.01ms。在所有實驗中,我們選擇一個2軸云臺相機作為裝備在UAV上的傳感器。使用幾何控制器[27]進行$(p,\theta ,\phi )$軌跡的跟蹤控制。所有模塊在Intel Core i9-13900K CPU上運行。

B. 基準比較和分析

為驗證我們提出的框架,我們在模擬中進行基準測試,包括三個大型復雜場景：管道(73×66×41 m3),基督救世主像(10×36×38 m3),和濱海灣金沙(100×25×49 m3)。我們比較了兩種最先進的方法：SIP[1]和HCPP[2]。由于HCPP[2]沒有開源代碼,所以我們使用了自己的實現。在模擬中,云臺的俯仰角限制在[-90°, 70°]范圍內,相機視場為[75°, 55°]。我們為所有方法設置動態限制為$v_{max}=2.0m/s$,${\omega }_{max}=1.0rad/s$, $a_{max}=1.0m/{\mathrm{s}}^2$, $j_{max}=0.5m/{\mathrm{s}}^3$。表I報告了平均比較結果,其中Exec. Time表示軌跡執行時間。

從評估結果來看,我們的方法在計算效率、覆蓋完整性和路徑質量方面都顯著優于其他方法。由于提出的基于骨架引導的視點生成,我們的框架以最少的視點實現了最大的覆蓋。得益于我們有效的SSD和并行規劃,我們的方法運行速度比現有方法快10倍以上。在路徑質量方面,我們生成了更短的覆蓋路徑和軌跡,實現了更少的執行時間。此外,隨著場景復雜度和規模的增加,這些優勢變得更加明顯。

我們進行了消融研究,以證明局部路徑優化和分層規劃的有效性。我們將不使用局部優化的框架表示為$NR$,而$GO$則用全局優化所有視點的路徑替代分層規劃。表II顯示了局部路徑優化和分層規劃對路徑質量和計算效率的貢獻。與$NR$相比,我們的方法在最小額外計算的情況下縮短了路徑。與$GO$相比,我們的方法顯著減少了計算時間,同時路徑質量損失最小。

此外,表III提供了每個模塊的運行時間和空間分解詳情。更多關于每個模塊的詳細性能信息可以在我們的項目頁面上找到。

注釋:本章通過大量實驗全面評估了FC-Planner的性能。基準測試表明,無論在計算效率、覆蓋完整性還是路徑質量方面,FC-Planner都大大優于現有方法。消融研究進一步證明了局部路徑優化和分層規劃這兩個關鍵模塊的有效性。這些實驗結果充分說明了我們提出方法的優越性。

C. 真實世界測試

為進一步驗證所提出的框架,我們在一個具有挑戰性的場景中進行了真實世界的覆蓋測試,其大小為10×5×2.5 m3。對于這次測試,我們設計了一個定制的四旋翼飛行器。云臺在[-90°, 20°]范圍內旋轉,相機視場為[60°, 80°]。動態限制設置為v_max = 0.7 m/s,ω_max = 1.0 rad/s,a_max = 0.5 m/s2,j_max = 0.5 m/s3。我們首先使用激光雷達收集場景的點云。然后,我們的框架生成覆蓋軌跡,其中我們的方法合理地將場景分解為五個子空間進行并行規劃。之后,四旋翼飛行器在執行軌跡時捕獲圖像。最后,我們使用這些圖像重建場景,結果表明我們的方法確實實現了完全覆蓋。更多關于這次測試的細節可以在我們的視頻中找到。

注釋:在真實場景中的測試進一步證實了FC-Planner的實用性和魯棒性。整個系統的工作流程,包括場景采集、覆蓋規劃、軌跡執行和場景重建,都按預期順利進行。最終重建的場景展示了我們方法出色的覆蓋效果。這一測試表明FC-Planner在實際應用中具有巨大潛力。

VIII. 結論

在本研究中,我們提出了一個專門為快速覆蓋大型復雜3D場景設計的基于骨架引導的規劃框架。提出的SSD有效地提取場景的骨架,實現了將場景合理分解為簡單子空間,并在無需預處理的情況下引導高效生成專門化和信息豐富的視點。基于SSD,分層覆蓋規劃器將整個規劃問題分解為可并行化的子問題,結合設計的全局規劃和局部優化方法,高效地生成高質量的覆蓋路徑。大量實驗證明了我們框架的效率和優越性。未來,我們計劃將我們的框架擴展到多UAV系統,以實現在更大和未知的3D場景中的協作覆蓋。

注釋:本文提出了FC-Planner,一個創新的基于骨架引導的三維覆蓋路徑規劃框架。它通過骨架提取、視點生成和分層規劃三個緊密協作的模塊,實現了對復雜大型場景的高效覆蓋。實驗結果充分證明了該方法在各個方面的卓越表現。未來,FC-Planner有望擴展到多無人機系統和未知環境,解決更加復雜和具有挑戰性的實際問題。總的來說,本研究為無人機三維覆蓋路徑規劃提供了一個新的思路和基準。

參考文獻

[1] A. Bircher, K. Alexis, M. Burri, P. Oettershagen, S. Omari, T. Mantel, and R. Siegwart, “Structural inspection path planning via iterative viewpoint resampling with application to aerial robotics,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 6423–6430.

[2] C. Cao, J. Zhang, M. Travers, and H. Choset, “Hierarchical coverage path planning in complex 3d environments,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3206–3212.

[3] C. Feng, H. Li, F. Gao, B. Zhou, and S. Shen, “Predrecon: A prediction-boosted planning framework for fast and high-quality autonomous aerial reconstruction,” in 2023 IEEE International Conference on Robotics and Automation (ICRA), 2023, pp. 1207–1213.

[4] E. Galceran and M. Carreras, “A survey on coverage path planning for robotics,” Robotics and Autonomous systems, vol. 61, no. 12, pp. 1258–1276, 2013.

[5] W. Jing, J. Polden, W. Lin, and K. Shimada, “Sampling-based view planning for 3d visual coverage task with unmanned aerial vehicle,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016, pp. 1808–1815.

[6] R. Almadhoun, T. Taha, D. Gan, J. Dias, Y. Zweiri, and L. Seneviratne, “Coverage path planning with adaptive viewpoint sampling to construct 3d models of complex structures for the purpose of inspection,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 7047–7054.

[7] W. Jing, D. Deng, Z. Xiao, Y. Liu, and K. Shimada, “Coverage path planning using path primitive sampling and primitive coverage graph for visual inspection,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 1472–1479.

[8] H. W. Tong, B. Li, H. Huang, and C.-y. Wen, “Uav path planning for complete structural inspection using mixed viewpoint generation,” in 2022 17th International Conference on Control, Automation, Robotics and Vision (ICARCV). IEEE, 2022, pp. 727–732.

[9] X. Huang, Y. Liu, L. Huang, S. Stikbakke, and E. Onstein, “Bim-supported drone path planning for building exterior surface inspection,” Computers in Industry, vol. 153, p. 104019, 2023.

[10] M. Roberts, D. Dey, A. Truong, S. Sinha, S. Shah, A. Kapoor, P. Hanrahan, and N. Joshi, “Submodular trajectory optimization for aerial 3d scanning,” in Proceedings of the IEEE International Conference on Computer Vision, 2017, pp. 5324–5333.

[13] C. H. Papadimitriou and K. Steiglitz, Combinatorial optimization: algorithms and complexity. Courier Corporation, 1998.

[14] R. M. Claro, M. I. Pereira, F. S. Neves, and A. M. Pinto, “Energy efficient path planning for 3d aerial inspections,” IEEE Access, vol. 11, pp. 32 152–32 166, 2023.

[15] G. Dantzig, R. Fulkerson, and S. Johnson, “Solution of a large-scale traveling-salesman problem,” Journal of the operations research society of America, vol. 2, no. 4, pp. 393–410, 1954.

[16] C. Chekuri and M. Pal, “A recursive greedy algorithm for walks in directed graphs,” in 46th annual IEEE symposium on foundations of computer science (FOCS’05). IEEE, 2005, pp. 245–253.

[17] A. Tagliasacchi, H. Zhang, and D. Cohen-Or, “Curve skeleton extraction from incomplete point cloud,” in ACM SIGGRAPH 2009 papers, 2009, pp. 1–9.

[18] R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),” in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 1–4.

[19] I.-K. Lee, “Curve reconstruction from unorganized points,” Computer aided geometric design, vol. 17, no. 2, pp. 161–177, 2000.

[20] R. Tarjan, “Depth-first search and linear graph algorithms,” SIAM journal on computing, vol. 1, no. 2, pp. 146–160, 1972.

[21] K. Zhou, Q. Hou, R. Wang, and B. Guo, “Real-time kd-tree construction on graphics hardware,” ACM Transactions on Graphics (TOG), vol. 27, no. 5, pp. 1–11,v^M_R}$轉換2008.

[22] Z. Meng, H. Qin, Z. Chen, X. Chen, H. Sun, F. Lin, and M. H. Ang, “A two-stage optimized next-view planning framework for 3-d unknown environment exploration, and structural reconstruction,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1680–1687, 2017.

[23] K. Helsgaun, “An effective implementation of the lin–kernighan traveling salesman heuristic,” European journal of operational research, vol. 126, no. 1, pp. 106–130, 2000.

[24] C. H. Papadimitriou, “The complexity of the lin–kernighan heuristic for the traveling salesman problem,” SIAM Journal on Computing, vol. 21, no. 3, pp. 450–465, 1992.

[25] S. M. McGovern and S. M. Gupta, “2-opt heuristic for the disassembly line balancing problem,” in Environmentally conscious manufacturing iii, vol. 5262. SPIE, 2004, pp. 71–84.

[26] Z. Wang, X. Zhou, C. Xu, and F. Gao, “Geometrically constrained trajectory optimization for multicopters,” IEEE Transactions on Robotics, vol. 38, no. 5, pp. 3259–3278, 2022.

[27] T. Lee, M. Leoky, and N. H. McClamroch, “Geometric tracking control of a quadrotor uav on se (3),” in Proc. of the IEEE Control and Decision Conf. (CDC), Atlanta, GA, Dec. 2010, pp. 5420–5425.