無人機(jī)蜂群靜態(tài)航線規(guī)劃方法研究

張帆 胡航

（中國人民解放軍66133部隊(duì) 北京市 100144）

無人機(jī)蜂群在進(jìn)入特定環(huán)境時，根據(jù)總體任務(wù)進(jìn)行了全局任務(wù)分配，每個無人機(jī)在確定自己需要承擔(dān)的任務(wù)后需進(jìn)行全局的飛行航線規(guī)劃。在航線規(guī)劃時每個無人機(jī)需考慮的要素包括：所有的任務(wù)點(diǎn)、任務(wù)區(qū)域、進(jìn)入點(diǎn)和退出點(diǎn)。

1 RRT算法研究

1.1 RRT算法原理簡介

假設(shè)任務(wù)空間為C，Cfree表示可行區(qū)域，Cobs表示障礙區(qū)域，Cfree和Cobs同為C的子集，且滿足C=Cfree∪Cobs和Cfree∩Cobs=φ。初始位置qinit∈Cfree和目標(biāo)位置qgoal∈Cfree，現(xiàn)需要為規(guī)劃出從初始位置到目標(biāo)位置的可行路徑。

使用 RRT 算法進(jìn)行路徑規(guī)劃分為任務(wù)空間內(nèi)的隨機(jī)樹生長和可行路徑的反向搜索兩個階段。其中隨機(jī)樹的生長，即如何構(gòu)造隨機(jī)樹是 RRT 算法的主要內(nèi)容。RRT 算法通過逐步迭代的增量方式進(jìn)行隨機(jī)樹的構(gòu)造，首先在任務(wù)區(qū)域內(nèi)選定起始節(jié)點(diǎn)qinit作為樹的根節(jié)點(diǎn)，通過從根節(jié)點(diǎn)不斷擴(kuò)展出葉節(jié)點(diǎn)的方式構(gòu)建隨機(jī)樹。首先以概率Pg選擇目標(biāo)位置qgoal作為隨機(jī)目標(biāo)點(diǎn)qrand，或者以概率1-Pg在任務(wù)空間內(nèi)隨機(jī)選擇一個隨機(jī)目標(biāo)點(diǎn)qrand；從隨機(jī)樹當(dāng)前所有的葉節(jié)點(diǎn)之中，選擇出一個離qrand最近的，稱之為臨近節(jié)點(diǎn)qnear；然后從qnear向qrand的方向延伸一個步長的距離ε，得到一個新的節(jié)點(diǎn)qnew。在延伸過程中，判斷是否與已知的威脅區(qū)域有沖突，若無沖突接受該新節(jié)點(diǎn)qnew，并將其添加為隨機(jī)樹的節(jié)點(diǎn)；若qnew與威脅區(qū)域有沖突，說明該次擴(kuò)展出的新節(jié)點(diǎn)不符合安全要求，則舍棄該新節(jié)點(diǎn)，并重新進(jìn)行隨機(jī)目標(biāo)點(diǎn)qrand的選取。通過這樣不斷的延伸擴(kuò)展，當(dāng)隨機(jī)樹中的葉節(jié)點(diǎn)與目標(biāo)位置足夠近的時候，則認(rèn)為隨機(jī)數(shù)的構(gòu)造工作完成，此時以距離目標(biāo)位置最近的葉節(jié)點(diǎn)為起始，依次向上搜索父節(jié)點(diǎn)，則可以獲得一條從起始位置到目標(biāo)位置的可行路徑。二維空間內(nèi)的RRT算法節(jié)點(diǎn)擴(kuò)展過程如圖1。

1.2 RRT算法的改進(jìn)

RRT*的原理是在RRT算法的基礎(chǔ)上，生成隨機(jī)點(diǎn)qrand，找到樹上最臨近節(jié)點(diǎn)qnearest，根據(jù)設(shè)置好的步長Segment，會預(yù)先生成一個qnew，在以qnew為中心，以radius為半徑的圓內(nèi)，找到所有點(diǎn)半徑小于radius的節(jié)點(diǎn)qnear，遍歷所有的qnear，計(jì)算每個qnear作為qnew父節(jié)點(diǎn)時的代價，將每條路徑的代價與原路徑代價相比較。如果比原路徑代價小且能夠通過碰撞檢測，則將正在遍歷的qnear作為新的父節(jié)點(diǎn)。

如前所述，由于無人機(jī)的平臺性能是受限的，因此在基于RRT算法進(jìn)行航線規(guī)劃時，應(yīng)當(dāng)考慮無人機(jī)上一節(jié)點(diǎn)與下一個需要擴(kuò)展的節(jié)點(diǎn)的夾角。根據(jù)前面的討論，新產(chǎn)生的節(jié)點(diǎn)應(yīng)當(dāng)滿足如圖2所示的約束。

另一方面，直接應(yīng)用RRT算法存在的缺點(diǎn)是算法向各個方向擴(kuò)展的概率是相同的。這將會導(dǎo)致生成大量的冗余節(jié)點(diǎn)，極大的降低了算法的計(jì)算效率。在航線規(guī)劃時擴(kuò)展樹中的節(jié)點(diǎn)均是隨機(jī)生成擴(kuò)展而來，因此隨機(jī)節(jié)點(diǎn)xrand的選取合適與否將直接影響最終生成航線的質(zhì)量。為了得到質(zhì)量更高的xrand節(jié)點(diǎn)，可以采用多重隨機(jī)采樣策略，即從規(guī)劃空間中產(chǎn)生一組隨機(jī)采樣點(diǎn)，然后對這一組點(diǎn)中選擇最優(yōu)的xrand進(jìn)行拓展。為了在一個采樣集合中得到最優(yōu)的xrand，需要根據(jù)相應(yīng)的評價函數(shù)對節(jié)點(diǎn)集進(jìn)行評價。由航線規(guī)劃的目的可知，優(yōu)化的航線應(yīng)是連接起始點(diǎn)與終點(diǎn)，滿足各種約束且能夠保證無人機(jī)不冗余飛行的航線。因此，RRT算法擴(kuò)展的與目標(biāo)方向相反的航點(diǎn)大概率將會生成低效的飛行航線。

無人機(jī)在空中飛行時，決定飛機(jī)狀態(tài)評價的參數(shù)包括兩個參數(shù)，即當(dāng)前飛行位置，飛行方向。因此在對采樣點(diǎn)的評價函數(shù)確定為：

如圖3所示，其中d表示采樣點(diǎn)到終點(diǎn)的歐氏距離，a表示起始點(diǎn)到目標(biāo)點(diǎn)與采樣點(diǎn)到目標(biāo)點(diǎn)的夾角，w1、w2分別是距離和角度的權(quán)重系數(shù)。

由于距離與方向量綱不統(tǒng)一，還需對該評價函數(shù)中的d和a進(jìn)行歸一化處理，即計(jì)算k 個采樣點(diǎn)的距離和角度值，然后求出其平均值d和a，即：

圖1：RRT搜索原理圖示

圖2：基于無人機(jī)的機(jī)動性能確定航線規(guī)劃的約束

圖3：節(jié)點(diǎn)評價函數(shù)構(gòu)造示意圖

圖4：貝塞爾曲線

求出均值后，再對每一個采樣點(diǎn)對應(yīng)的距離和角度進(jìn)行歸一化處理，因此評價函數(shù)變?yōu)椋?/p>

另一種改進(jìn)方法為基于最近鄰可達(dá)點(diǎn)的改進(jìn)RRT方法，即NAPRRT。由于基本RRT算法過程中包含了大量的浮點(diǎn)運(yùn)算，影響了算法效率，針對此問題，希望在擴(kuò)展子節(jié)點(diǎn)時優(yōu)先擴(kuò)展當(dāng)前節(jié)點(diǎn)附近的、無碰的節(jié)點(diǎn)，而不需要處理隨機(jī)點(diǎn)角度相關(guān)的復(fù)雜運(yùn)算，故提出該種改進(jìn)方法。

2 貝塞爾曲線路徑的平滑處理

貝塞爾對于路徑平滑非常有效，它有一個最重要的特點(diǎn)就是在不更改整條路徑形狀的前提下重新構(gòu)建局部路線。研究中運(yùn)用貝塞爾曲完成RRT*算法執(zhí)行后的后續(xù)工作，創(chuàng)建一條更加平滑的路徑。

貝塞爾曲線可以定義為下式：

在此重點(diǎn)考慮n-3的情況，即三次貝塞爾多項(xiàng)式Bp(t)。隨著伯恩斯坦多項(xiàng)式的擴(kuò)展，得到貝塞爾曲線的參數(shù)方程：

上式中，四個點(diǎn)P1、P2、P3和P4確定了一個三次貝塞爾曲線，其中，P1和P4為曲線的端點(diǎn)，P2和P3為曲線的控制點(diǎn)，曲線的屬性如下：

（1）Bp(0)=P1且Bp(1)=P4；

根據(jù)貝塞爾曲線的屬性（1），將路徑段Gp構(gòu)建為P1=qinit與P4=qgoal，也就是說，隨機(jī)數(shù)在擴(kuò)展的過程中，每一個擴(kuò)展的端點(diǎn)qinit可以定為P1，每一個擴(kuò)展的目標(biāo)點(diǎn)qgoal可以定為P4，那么隨機(jī)樹就能夠按照三次貝塞爾曲線進(jìn)行擴(kuò)展了。擴(kuò)展過程如圖4所示。

3 融合RRT*和貝塞爾曲線的無人機(jī)蜂群航線規(guī)劃方法

圖5：參照無人機(jī)調(diào)整航線示意圖

情況設(shè)想中，有多個無人機(jī)編隊(duì)共同執(zhí)行任務(wù)，各編隊(duì)執(zhí)行的任務(wù)各不相同。編隊(duì)中各架飛機(jī)執(zhí)行任務(wù)情況相同，無人機(jī)之間保持通信，同時到達(dá)任務(wù)點(diǎn)執(zhí)行任務(wù)。因此，無人機(jī)蜂群在完成任務(wù)過程中是作為一個集群在飛行，需要保證群隊(duì)飛行的效果。每架無人機(jī)在規(guī)劃航線時都必須考慮編隊(duì)飛行，而不僅僅考慮自身航線最優(yōu)。

為了達(dá)到此目的，可在編隊(duì)中選出一架參照無人機(jī)，參照無人機(jī)獨(dú)立運(yùn)用RRT*和貝塞爾曲線規(guī)劃出最優(yōu)航線，同時將最優(yōu)航線發(fā)布給編隊(duì)中其他無人機(jī)，其他無人機(jī)據(jù)此進(jìn)行自身航線規(guī)劃。參照無人機(jī)的選擇標(biāo)準(zhǔn)有：起始狀態(tài)下位于編隊(duì)中的位置（前方或中間）、飛行能力較強(qiáng)、油料較多。具體方法如下：

（1）運(yùn)用RRT*算法和貝塞爾曲線規(guī)劃出參照無人機(jī)執(zhí)行任務(wù)的最優(yōu)路線；

（2）參照無人機(jī)向編隊(duì)中其他無人機(jī)發(fā)布最優(yōu)航線，其他無人機(jī)以一定安全距離伴隨飛行；

（3）參照無人機(jī)適當(dāng)調(diào)整自己的航線，需要考慮其他無人機(jī)的可飛性，適當(dāng)增加參照無人機(jī)與障礙物間的距離，保證其他無人機(jī)伴隨飛行時的安全等，使整個編隊(duì)到達(dá)下一個任務(wù)點(diǎn)的時間最短或盡可能避障、遠(yuǎn)離威脅,如圖5所示。

（4）其他無人機(jī)規(guī)劃伴隨飛行航線應(yīng)以無人機(jī)間安全距離和避障等為約束，編隊(duì)規(guī)模較小時，其他無人機(jī)根據(jù)參照無人機(jī)方位飛行，使整個盡可能避免沖突、遠(yuǎn)離威脅；編隊(duì)規(guī)模較大，則編隊(duì)中部分無人機(jī)與參照無人機(jī)的距離遠(yuǎn)，無法很好地進(jìn)行通信和伴隨飛行，這時需要將編隊(duì)進(jìn)行分層，如果參照無人機(jī)在編隊(duì)前部，則中部無人機(jī)伴隨參照無人機(jī)飛行，后部無人機(jī)以中部無人機(jī)為參照伴隨飛行。

接著，假設(shè)每個無人機(jī)編隊(duì)中的無人機(jī)為等距橫向排列，設(shè)置編隊(duì)中無人機(jī)之間的間隔為λ米，機(jī)群中先到達(dá)執(zhí)行任務(wù)點(diǎn)的無人機(jī)在匯合過程任務(wù)范圍內(nèi)徘徊，匯合過程路徑由貝塞爾曲線產(chǎn)生；

最后，將規(guī)劃好的航路段分配給所屬的無人機(jī)編隊(duì)，并將無人機(jī)編隊(duì)的每個航路段連接起來，無人機(jī)編隊(duì)等間距λ生成五條航線，同時將第3個步驟中生成的匯合過程路徑與編隊(duì)航線連接起來，即為規(guī)劃的完整航線。