多無人機(jī)航跡規(guī)劃的自適應(yīng)B樣條算法

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

1.中國科學(xué)院 成都計算機(jī)應(yīng)用研究所，成都610041

2.中國科學(xué)院大學(xué)，北京100049

1 引言

由于無人機(jī)快速增長的市場和廣闊的應(yīng)用領(lǐng)域，吸引了越來越多的研究人員和研究機(jī)構(gòu)。與單個無人機(jī)相比，無人機(jī)集群能夠以更高的效率完成更復(fù)雜和更具挑戰(zhàn)性的任務(wù)。多無人機(jī)的研究方向主要包括無人機(jī)集群的路徑規(guī)劃，無人機(jī)編隊(duì)以及路徑平滑等。

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

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

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

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

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

2 相關(guān)工作

2.1 滾動時域控制

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

2.2 B樣條曲線

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

圖1 RHC的基本框架

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

B 樣條曲線可以分為：（1）均勻B 樣條曲線，其特點(diǎn)是基函數(shù)呈周期性；（2）準(zhǔn)均勻B 樣條曲線，其特點(diǎn)是兩端的控制節(jié)點(diǎn)具有重復(fù)度k；（3）分段Bezier 曲線，其特點(diǎn)是兩端的控制節(jié)點(diǎn)有重復(fù)度k，內(nèi)部的控制節(jié)點(diǎn)有重復(fù)度k-1；（4）非均勻B 樣條曲線，它的特點(diǎn)是基函數(shù)定義域的區(qū)間長度不相同。

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

圖2 B樣條曲線實(shí)例

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

3 基于自適應(yīng)B樣條的動態(tài)航跡規(guī)劃

當(dāng)多無人機(jī)編隊(duì)在執(zhí)行任務(wù)時，需要在飛行過程中保持預(yù)先設(shè)定的隊(duì)形。為了實(shí)現(xiàn)這個目標(biāo)，常用的編隊(duì)保持控制方法有：領(lǐng)航-跟隨法、虛擬結(jié)構(gòu)法、基于行為法、圖論法、基于一致性方法以及多種方法的混合。由于領(lǐng)航-跟隨法在實(shí)際中的應(yīng)用很多，同時該方法具有理論上容易分析、實(shí)現(xiàn)較為容易等優(yōu)點(diǎn)，所以本文采用了領(lǐng)航-跟隨法來進(jìn)行編隊(duì)控制，圖3 是領(lǐng)航-跟隨法的示例。

圖3 領(lǐng)航-跟隨法示例

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

3.1 結(jié)合滾動時域的無人機(jī)編隊(duì)路徑規(guī)劃

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

圖4 RHC-FPSO方法結(jié)構(gòu)示意圖

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

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

圖5 多無人機(jī)三角形編隊(duì)

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

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

3.2 路徑的動態(tài)重規(guī)劃與平滑

針對3.1 節(jié)提出的兩點(diǎn)不足，本文從避障的角度出發(fā)，使用B 樣條樣條插值方法來實(shí)現(xiàn)跟隨者航跡的避障平滑處理。

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

圖6 障礙物膨脹化處理

然后，結(jié)合3.1節(jié)中得到的跟隨者航跡點(diǎn)，進(jìn)行障礙物的規(guī)避并生成平滑的航跡。由于環(huán)境中可能會出現(xiàn)動態(tài)障礙物，所以為了保證無人機(jī)能實(shí)時地檢測出動態(tài)障礙物并及時做出避障的措施，本文采用分段處理的方式。當(dāng)無人機(jī)處于當(dāng)前位置時，只考慮接下來的少量航跡點(diǎn)，根據(jù)航跡點(diǎn)是否在障礙物外部，做出平滑或航跡重規(guī)劃處理。由于無人機(jī)從當(dāng)前階段的航跡點(diǎn)移動至下一階段的航跡點(diǎn)時，都會進(jìn)行動靜態(tài)障礙物的檢測與避障處理，而每一階段只包含少量航跡點(diǎn)，確保無人機(jī)在每個階段的運(yùn)行時間很短，因此分段處理的方式能夠?qū)崿F(xiàn)無人機(jī)的動態(tài)避障。

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

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

圖7 障礙物的頂點(diǎn)外擴(kuò)點(diǎn)作為新的控制點(diǎn)

圖8 新增控制點(diǎn)

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

圖9 分段平滑之后出現(xiàn)航跡不光滑的情況

圖10 自適應(yīng)B樣條方法以避免航跡不平滑

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

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

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

3.3 算法流程

步驟1 根據(jù)RHC-FPSO 算法規(guī)劃出領(lǐng)航者無人機(jī)的參考航跡。

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

步驟3 對環(huán)境中的障礙物進(jìn)行膨脹化處理，獲取新的障礙物分布圖。

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

步驟5 使用基于自適應(yīng)B樣條方法進(jìn)行平滑。

步驟6 根據(jù)交點(diǎn)的具體情況重新選擇航跡點(diǎn)，使用自適應(yīng)B樣條方法進(jìn)行平滑、避障處理。

4 實(shí)驗(yàn)設(shè)計與結(jié)果分析

文中的實(shí)驗(yàn)針對的是三架無人機(jī)組成的編隊(duì)，隊(duì)形是等邊三角形，編隊(duì)保持策略是領(lǐng)航跟隨法。這三架無人機(jī)要完成的任務(wù)是從指定區(qū)域出發(fā)，穿過分布了多個障礙物的區(qū)域，到達(dá)指定的區(qū)域。

4.1 領(lǐng)航者航跡生成

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

圖11 RHC-FPSO生成領(lǐng)航者航跡

圖12 領(lǐng)航者參考航跡

圖11 、圖12 展示了領(lǐng)航者無人機(jī)在分布了多個障礙物的環(huán)境下的航跡，可以看出該航跡與所有障礙物都未發(fā)生碰撞，并能從起點(diǎn)到達(dá)指定的終點(diǎn)。

4.2 跟隨者航跡生成

在生成領(lǐng)航者無人機(jī)的參考航跡之后，需要進(jìn)一步考慮跟隨著無人機(jī)的參考航跡，本文根據(jù)公式（4）、（5）的幾何關(guān)系生成兩跟隨者的航跡，結(jié)果如圖13所示。

圖13 跟隨者的參考航跡

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

4.3 跟隨者航跡平滑及避障

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

圖14 跟隨者的平滑航跡

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

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

圖15 航跡重規(guī)劃示意圖

4.4 平滑效果對比

預(yù)先生成的無人機(jī)航跡越平滑，那么無人機(jī)在實(shí)際飛行過程中的波動就會相對較少，所以參考航跡的平滑性是評價航跡優(yōu)劣的一個重要標(biāo)準(zhǔn)。文中與圓弧插補(bǔ)技術(shù)進(jìn)行了比較。圓弧插補(bǔ)技術(shù)[17]的思想是用圓弧GH去替代轉(zhuǎn)折角ABC，以此來實(shí)現(xiàn)路徑的平滑，其原理如圖16所示。

圖16 圓弧插補(bǔ)算法原理

在相同的環(huán)境設(shè)置下，用圓弧插補(bǔ)算法進(jìn)行航跡平滑的結(jié)果如圖17 所示。而使用本文提出的自適應(yīng)B 樣條方法的結(jié)果則如圖18所示。

圖17 圓弧插補(bǔ)算法生成跟隨者航跡

從以上實(shí)驗(yàn)結(jié)果可以看出，圖17 中標(biāo)注部分的航跡存在不光滑的轉(zhuǎn)折點(diǎn)，而總體而言用圓弧插補(bǔ)得到的結(jié)果存在不光滑處；圖18 中標(biāo)出了對應(yīng)于圖17 標(biāo)注過的區(qū)域，相比較而言，圖18 中展示的航跡更光滑，而總體而言用自適應(yīng)B 樣條方法來進(jìn)行平滑得到的航跡不存在不光滑處。綜上可以看出，相比于圓弧插補(bǔ)技術(shù)，自適應(yīng)B樣條方法能使航跡更光滑。

圖18 自適應(yīng)B樣條生成跟隨者航跡

5 結(jié)束語

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