多無人機航跡規劃的自適應B樣條算法

彭皓月，秦小林，侯 嶼，張力戈

1.中國科學院 成都計算機應用研究所，成都610041

2.中國科學院大學，北京100049

1 引言

由于無人機快速增長的市場和廣闊的應用領域，吸引了越來越多的研究人員和研究機構。與單個無人機相比，無人機集群能夠以更高的效率完成更復雜和更具挑戰性的任務。多無人機的研究方向主要包括無人機集群的路徑規劃，無人機編隊以及路徑平滑等。

無人機路徑規劃是無人機集群問題最主要的研究方向。其研究內容主要集中在環境建模、無人機航跡代價的定義、目標函數最優化求解方法等方向。在環境建模方面，王文彬等[1]將無人機飛行環境用人工勢場和Voronoi 圖進行建模，并結合滾動時域控制策略與改進的粒子群算法應用于單個無人機的航跡規劃問題中，生成無人機在復雜環境下安全飛行的路徑。

由于啟發式算法求解問題可行解的時間優勢十分突出，因此很多文獻將啟發式算法應用到無人機路徑規劃問題的求解中。Chen 等[2]采用改進的中心力優化方法來解決旋翼垂直起降飛機的復雜路徑優化問題。在路徑規劃過程中，應用粒子群優化算法（PSO）和遺傳算法（GA）變異算子的思想來改進原始中心力優化方法。該方法產生的軌跡優于大部分傳統的啟發式算法。周子為等通過分析雁群長途遷徙編隊隊形的內在機理，設計了多無人機編隊飛行的隊形和變換方法[3]。

無人機編隊控制的主要方法有三種，即領航-跟隨法、虛擬結構法和基于行為法[4-6]。在領航-跟隨法中，編隊中的一架無人機被指定為領隊，其余的無人機被視為追隨者。其基本思想是跟隨者跟蹤領隊的位置和方向。Wang[7]開發了從理論到實際飛行的編隊飛行試驗系統，使用主動干擾抑制控制（ADRC）方法和PID控制方法設計了三維無人機編隊方法，通過數值模擬設計并驗證了基于領航跟隨法的三維編隊控制算法。領航-跟隨法存在的弱點是，領航者是編隊的單一故障點，如果領航者出現故障，整個無人機群會出現編隊異常的現象。在虛擬結構法中，整個編隊被視為一個單一的結構。每個無人機接收相同的虛擬領隊的軌跡信息。虛擬結構法的優勢在于易于規定編隊行為。邵壯等提出了一種基于虛擬結構編隊策略的、可以在三維動態環境下實時避障以及機間避碰的分布式編隊保持算法[8]。虛擬結構法的缺點是對隊形沒有明確的反饋信息。基于行為法在飛機編隊飛行中的控制策略受鳥類、魚類、昆蟲的本能行為的啟發，為每架飛機規定幾種行為，并使每架飛機的控制動作成為每種行為控制的加權平均值。這些行為可能是障礙回避、碰撞回避、目標尋找和保持編隊等。比如無人機避障是通過移動到目標、保持相對距離、保持相對角度和停止移動到目標目的地等行為組合完成。Dougherty[9]指出，基于行為法所需的信息量比較大，一般不能保證隊形的穩定性。

路徑平滑是得到無人機集群實際可飛路徑的有效方法，能提高無人機飛行的安全性，也能使無人機飛行更加高效。很多學者在各個領域對路徑平滑技術進行了深入的研究。Huang[10]將K 度平滑方法與改進的蟻群算法結合，使多無人機在可接受的時間誤差內到達目的地，生成的光滑的無人機路徑滿足實際飛行需求。Askari[11]對機器人和無人機的連續曲率路徑進行了研究，將上限曲率和上限曲率導數包含在路徑中，使用五次Bezier曲線替換Dubins曲線，通過考慮路徑的運動學約束來計算形狀參數。Fang[12]提出了一種基于二階平滑的巡航導彈路徑跟蹤方法，在對稱多項式曲線方面產生具有連續曲率的可飛路徑，滿足了導彈轉彎半徑和精度跟蹤的要求。Heiden[13]提出了一種梯度相關平滑算法，通過局部優化頂點的位置，同時滿足系統的動力學約束，使給定軌跡變形。比現有的后平滑算法產生的軌跡更短并且擁有更高的平滑度。Wang[14]提出了一種軌跡平滑的實時動態Dubins-Helix方法，該方法由Dubins-Helix 軌跡生成和俯仰角平滑兩部分組成，Dubins 路徑規劃方法生成三維軌跡在水平面上的投影，使得軌跡滿足曲率半徑約束。螺旋曲線是其滿足俯仰角約束。該方法能夠在不同的初始條件下生成有效的軌跡，能夠實現實時計算。Ravankar[15]較為全面地總結了機器人導航中的各種路徑平滑技術，將其分為了基于插值的路徑平滑、使用特殊曲線的路徑平滑、基于優化的路徑平滑，并且詳細地比較了每種方法的優點和缺點以及適用場景。

本文將采用領航-跟隨者方法來進行無人機編隊控制。在實現過程中，采用滾動時域控制和快速粒子群優化算法為領航者生成平滑且安全的參考航跡；然后根據跟隨者與領航者之間的幾何關系，得到跟隨者的參考航跡，采用自適應的B 樣條方法對跟隨者的參考航跡進行平滑和避障處理。由于B 樣條方法應用于航跡平滑時得到的曲線滿足C2連續性，所以采用自適應B樣條方法進行航跡平滑。

2 相關工作

2.1 滾動時域控制

滾動時域控制（Receding Horizon Control，RHC）[16]將控制問題描述為有限時域內的含約束的最優化問題，它具有能夠在線處理有約束地控制輸入與輸出的優勢。在一個采樣時間內，通過在確定的時間區間內搜索來優化目標函數，最終得出最優的控制輸入序列，之后RHC會保留上述序列的第一個值；然后，系統狀態在得出控制輸入后發生變化；接下來系統會根據當前及未來的狀態估計值重復上述過程。圖1是RHC的基本框架。

2.2 B樣條曲線

B 樣條曲線[15]是貝賽爾曲線的一般化形式，其公式化的定義如下：

圖1 RHC的基本框架

其中，m 是節點xi的個數，且xi的取值范圍是{x0,x1,… ,xm-1}并 且x0≤x1≤ …≤xm-1。Pi是 控 制 節點，此處共有m-n-1個控制節點。Bi,n表示n階的B 樣條基函數，可以根據De Boor-Cox的遞推定義得到：

B 樣條曲線可以分為：（1）均勻B 樣條曲線，其特點是基函數呈周期性；（2）準均勻B 樣條曲線，其特點是兩端的控制節點具有重復度k；（3）分段Bezier 曲線，其特點是兩端的控制節點有重復度k，內部的控制節點有重復度k-1；（4）非均勻B 樣條曲線，它的特點是基函數定義域的區間長度不相同。

其中，準均勻B 樣條曲線和分段Bezier 曲線會與控制折線的首尾控制點接觸并與首、尾邊接觸、相切，如圖2（a）中實線所示，這樣產生的曲線稱為閉曲線；而另外的兩類曲線則不會與首尾控制點接觸，如圖2（b）中實線所示，這樣產生的曲線稱為開曲線。

圖2 B樣條曲線實例

B 樣條曲線主要的優點有：（1）計算曲線的成本低；（2）易于保證路徑的C2連續性；（3）通過改變節點可以控制樣條的形狀。類似的航跡平滑方法還有Dubins 曲線和貝塞爾曲線，但是前者不具有曲率連續性，而后者所選取的任何一個控制點發生改變都會影響全局的結果。相比于這兩種方法，B 樣條曲線容易實現路徑的C2連續性，并且在改變部分控制點時只會對局部曲線產生影響。因此，相比于這兩者，B 樣條曲線在航跡平滑這一任務上具有明顯的優勢。

3 基于自適應B樣條的動態航跡規劃

當多無人機編隊在執行任務時，需要在飛行過程中保持預先設定的隊形。為了實現這個目標，常用的編隊保持控制方法有：領航-跟隨法、虛擬結構法、基于行為法、圖論法、基于一致性方法以及多種方法的混合。由于領航-跟隨法在實際中的應用很多，同時該方法具有理論上容易分析、實現較為容易等優點，所以本文采用了領航-跟隨法來進行編隊控制，圖3 是領航-跟隨法的示例。

圖3 領航-跟隨法示例

領航-跟隨法的基本思想是將無人機集群系統中的無人機分為領航者和跟隨者，其中領航者跟蹤一條預先給定的軌跡，跟隨者與領航者之間保持指定的相對距離并跟隨領航者運動。因此，要實現領航-跟隨法，就要解決以下兩個問題：（1）為領航者指定參考航跡；（2）基于領航者給出跟隨者的參考航跡。

3.1 結合滾動時域的無人機編隊路徑規劃

針對第（1）個問題，本文使用了文獻[1]中王文彬等提出的基于滾動時域控制和快速粒子群優化（Receding Horizon Control and Fast Particle Swarm Optimization，RHC-FPSO）算法。該算法將復雜情況下單無人機的航跡規劃問題分為代價評估階段、在線航跡規劃階段，并分別給出了解決辦法，如圖4 所示，第一階段，根據當前環境生成基于Voronoi 圖的代價圖，同時結合人工勢場計算航跡端點到目標點的代價估計。第二階段，采用快速粒子群優化算法在滾動時域的框架下進行在線航跡規劃。

圖4 RHC-FPSO方法結構示意圖

RHC-FPSO 方法具有如下優點：（1）在滿足障礙物約束和無人機自身約束的條件下，該方法能使無人機順利地穿過障礙物區域，到達目標區域，由于為無人機設定了速度和加速度等動力學約束，所以規劃出的航跡可行且平滑；（2）該方法在耗時方面具有優勢，它滿足在線計算的要求；（3）對新檢測到的障礙物，該方法可以做出快速反應，以達到躲避障礙物的目的；（4）當環境發生變化時，可不計算代價圖，只需更新勢場，就減少了計算量。因此，在分布有動靜態障礙物的環境中，采用RHCFPSO方法可以為領航者無人機生成安全的參考航跡。

針對第（2）個問題，首先要考慮無人機編隊的隊形問題。對于不同數量的無人機有不同的隊形，本文考慮三架無人機構成編隊的情況，并將期望隊形選擇為常見的等邊三角形。編隊中的各個無人機的期望位置分別是等邊三角形的三個頂點，并且領航者位于隊伍的最前方。該隊形的幾何表示圖如圖5。

圖5 多無人機三角形編隊

在圖5中，ld表示兩無人機之間的期望距離，(xi,yi)是無人機i在二維坐標系下的位置，vi是無人機i的速度，θi是無人機i的偏航角。1表示領航者，跟隨者2、3在隊形中的期望位置分別是2?、3?，并且可以分別通過公式（4）、（5）來計算：

于是，根據領航者1 的參考航跡就可以得到跟隨者2 和跟隨者3 的參考航跡。但是通過上述步驟生成的航跡，會存在兩個不足：（1）在生成跟隨者航跡的過程中并未考慮到環境中的動、靜態障礙物，所以生成的航跡可能和障礙物相交；（2）得到的參考航跡是由一組航跡點組成的，可能會造成航跡不平滑。針對上述不足，本文提出了基于自適應參數B 樣條的分段樣條插值方法來進行避障處理和航跡平滑。

3.2 路徑的動態重規劃與平滑

針對3.1 節提出的兩點不足，本文從避障的角度出發，使用B 樣條樣條插值方法來實現跟隨者航跡的避障平滑處理。

首先，對環境中的所有動靜態障礙物進行膨脹化處理，即把實際障礙物的邊界往外擴展一定距離，得到膨脹后的障礙物范圍，由此得出新的障礙物威脅圖。這里擴展的距離為無人機的最小安全半徑r，如圖6所示。

圖6 障礙物膨脹化處理

然后，結合3.1節中得到的跟隨者航跡點，進行障礙物的規避并生成平滑的航跡。由于環境中可能會出現動態障礙物，所以為了保證無人機能實時地檢測出動態障礙物并及時做出避障的措施，本文采用分段處理的方式。當無人機處于當前位置時，只考慮接下來的少量航跡點，根據航跡點是否在障礙物外部，做出平滑或航跡重規劃處理。由于無人機從當前階段的航跡點移動至下一階段的航跡點時，都會進行動靜態障礙物的檢測與避障處理，而每一階段只包含少量航跡點，確保無人機在每個階段的運行時間很短，因此分段處理的方式能夠實現無人機的動態避障。

分段處理的具體實現思路如下：對于每個跟隨者的參考航跡，先按照順序選取N 個航跡點，判斷這些航跡點是否與環境中的障礙物發生碰撞，如果沒發生碰撞，就用B 樣條樣條插值方法進行平滑處理，然后再繼續往后選取N 個航跡點，并進行判斷；如果會發生碰撞，則進行航跡的重規劃過程。

航跡重規劃方法的思想是：首先統計這N 個航跡點與障礙物相交的個數N0，如果N0＜N，則選取離相交區域最近的障礙物頂點的外擴點作為新的航跡點，同時把第一個交點前的第二、三個點與最后一個交點后的第二、三個點作為新的航跡點，使用B 樣條方法來進行平滑處理；如果N0≥N，則再往后選取N 個航跡點，直到與障礙物沒有相交為止，針對這種情況，選取離相交區域最近的障礙物頂點的外擴點作為新的航跡點，同時把第一個交點前的第二、三個點與最后一個交點后的第二、三個點作為新的航跡點，使用B 樣條方法進行平滑處理，如圖7 所示。在選取外擴點時，還需考慮相交部分的長度，如果超過了矩形障礙物的某一邊長，則適當增加控制點的數量，如圖8所示。

圖7 障礙物的頂點外擴點作為新的控制點

圖8 新增控制點

在上述處理過程中，會存在一種情況：如果連續兩個分段都與障礙物沒有交點，那么在分別進行平滑處理之后，會在兩段的連接處出現不光滑的情況，如圖9 中的點3 處。為了避免這種情況，文中提出了一種自適應B樣條方法，如圖10所示。

圖9 分段平滑之后出現航跡不光滑的情況

圖10 自適應B樣條方法以避免航跡不平滑

下面將對自適應選點的思想進行說明。假設圖10中的點1、2、3、4、5、3′、3″、5′、5″的坐標分別是P1、P2、P3、P4、P5、P3′、P3″、P5′、P5″，其中點1、2、3、4、5 是跟隨者航跡上連續的五個控制點，如果每三個控制點進行一次判斷，那么這里首先要考慮的是點1、2、3，接下來要考慮的是點3、4、5。假設這兩段航跡跟障礙物是沒有發生碰撞的，則這兩段將直接進行B 樣條的平滑處理，但是處理之后會出現如圖9 中的情況，即平滑后的航跡在點3處不連續。所以本文在考慮點1、2、3 時，將點3 用點3′來替換，即對點1、2、3′構成的航跡進行平滑并得到圖10中左起的第一段曲線。這里點3′的選取方式為：

其中，k 是大于0 的常數。接下來處理點3、4、5，為了避免連接處出現不平滑的情況，用點3″來替換點3，用點5′來替換點5，于是對點3″、3、4、5′進行B 樣條平滑處理并得到圖10 中間的曲線。其中，點3″和點5′的選取方式分別是：

l 是大于0的常數，且l ＞k。由于B樣條方法具有C2連續性，所以在點3′、3″處與線段3′3″是相切的，在點5′、5″處也與線段5′5″是相切的。所以，經過上面的處理就得到了平滑的航跡。

3.3 算法流程

步驟1 根據RHC-FPSO 算法規劃出領航者無人機的參考航跡。

步驟2 根據公式（4）和公式（5）計算出跟隨者無人機的參考航跡。

步驟3 對環境中的障礙物進行膨脹化處理，獲取新的障礙物分布圖。

步驟4 分別對兩個跟隨者無人機的參考航跡點進行分段避障和平滑處理。若某一段與障礙物沒有交點，則轉步驟5；若某一段與障礙物有交點，則轉步驟6。直至遍歷所有航跡點。

步驟5 使用基于自適應B樣條方法進行平滑。

步驟6 根據交點的具體情況重新選擇航跡點，使用自適應B樣條方法進行平滑、避障處理。

4 實驗設計與結果分析

文中的實驗針對的是三架無人機組成的編隊，隊形是等邊三角形，編隊保持策略是領航跟隨法。這三架無人機要完成的任務是從指定區域出發，穿過分布了多個障礙物的區域，到達指定的區域。

4.1 領航者航跡生成

在采用領航跟隨策略的無人機編隊中，首先要為領航者指定參考航跡，本文使用了RHC-FPSO 方法生成了領航者無人機的參考航跡，其結果如圖11 和圖12所示。

圖11 RHC-FPSO生成領航者航跡

圖12 領航者參考航跡

圖11 、圖12 展示了領航者無人機在分布了多個障礙物的環境下的航跡，可以看出該航跡與所有障礙物都未發生碰撞，并能從起點到達指定的終點。

4.2 跟隨者航跡生成

在生成領航者無人機的參考航跡之后，需要進一步考慮跟隨著無人機的參考航跡，本文根據公式（4）、（5）的幾何關系生成兩跟隨者的航跡，結果如圖13所示。

圖13 跟隨者的參考航跡

從圖13 可以看出這兩條航跡存在很多不平滑的區域，并且圖中的參考航跡與障礙物區域相交。與此同時，考慮到在生成跟隨者參考航跡時并未考慮航跡的光滑性與安全性，所以必須要進一步考慮對航跡的平滑及避障處理。

4.3 跟隨者航跡平滑及避障

在對跟隨者的參考航跡進行平滑及避障處理之前，首先對可見范圍內的障礙物進行膨脹化處理以得到絕對安全的區域。如果只考慮航跡的分段平滑問題，則可以通過自適應選點的B 樣條平滑策略來處理，其結果如圖14所示。

圖14 跟隨者的平滑航跡

從圖14 中可以看出，此時的航跡與膨脹化之后的障礙物仍然相交，下面同時考慮參考航跡的平滑和避障問題。在平滑航跡的過程中，對碰撞的部分進行航跡的重規劃過程，其結果如圖15所示。

圖15 展示的是平滑且避障之后的航跡，圖中的兩個小窗口顯示了避障的重規劃情況。在小窗口里，點狀部分是參考航跡與膨脹后的障礙物發生碰撞的部位，在重規劃過程之后得到了圍繞在障礙物外圍的光滑曲線。至此，就得到了兩跟隨者無人機的光滑且安全的航跡。

圖15 航跡重規劃示意圖

4.4 平滑效果對比

預先生成的無人機航跡越平滑，那么無人機在實際飛行過程中的波動就會相對較少，所以參考航跡的平滑性是評價航跡優劣的一個重要標準。文中與圓弧插補技術進行了比較。圓弧插補技術[17]的思想是用圓弧GH去替代轉折角ABC，以此來實現路徑的平滑，其原理如圖16所示。

圖16 圓弧插補算法原理

在相同的環境設置下，用圓弧插補算法進行航跡平滑的結果如圖17 所示。而使用本文提出的自適應B 樣條方法的結果則如圖18所示。

圖17 圓弧插補算法生成跟隨者航跡

從以上實驗結果可以看出，圖17 中標注部分的航跡存在不光滑的轉折點，而總體而言用圓弧插補得到的結果存在不光滑處；圖18 中標出了對應于圖17 標注過的區域，相比較而言，圖18 中展示的航跡更光滑，而總體而言用自適應B 樣條方法來進行平滑得到的航跡不存在不光滑處。綜上可以看出，相比于圓弧插補技術，自適應B樣條方法能使航跡更光滑。

圖18 自適應B樣條生成跟隨者航跡

5 結束語

本文對無人機編隊控制問題進行了研究，采用了領航-跟隨法的策略進行了編隊控制。在生成領航者無人機的航跡時，使用了RHC-FPSO 算法；然后基于三架無人機組成的編隊，根據三者的幾何關系，結合領航者無人機的參考航跡，生成了兩跟隨者無人機的參考航跡。在進行航跡平滑時，針對傳統B 樣條曲線應用于分段的航跡點時會造成的不平滑現象，提出了自適應的B 樣條曲線方法，并在此基礎上考慮到航跡會與障礙物發生碰撞，提出了基于上述方法的航跡重規劃算法。通過使用自適應的B 樣條曲線方法來進行航跡平滑，其結果相較于使用圓弧插補算法得到的結果更好；使用航跡重規劃算法可以使無人機安全避開環境中的障礙物。隨著無人機編隊的應用越來越多，將面對的環境會更多元化，因此在未來的工作中要更多的考慮到各種不同的動靜態障礙物；同時，用于快速尋找光滑且安全的航跡規劃算法仍然是研究的重點。