一種自主水下航行器編隊航路規(guī)劃算法*

余耀宇 蔡 超

（華中科技大學人工智能與自動化學院多譜信息處理技術(shù)國家級重點實驗室 武漢 430074）

1 引言

自主水下航行器（Autonomous Underwater Vehicle，AUV）已經(jīng)成為人類探索和開發(fā)海洋的重要工具［1］。隨著任務復雜性的增加，單個AUV在任務執(zhí)行過程中的局限性越來越明顯，此時需要通過多個AUV編隊協(xié)作來完成任務，從而突破單航行器在感知，決策及執(zhí)行能力等方面的限制。目前實現(xiàn)AUV編隊航行常用的控制方法有領(lǐng)航跟隨法［2］、虛擬結(jié)構(gòu)法［3］、基于行為法［4］和人工勢場法［5］等。但是水下環(huán)境復雜且通信困難，實現(xiàn)編隊控制是一件非常困難的事情。提前規(guī)劃好編隊航路，可以降低AUV編隊控制的復雜度，減少AUV編隊間通信的信息量。編隊航路規(guī)劃通常要求航路滿足編隊隊形要求，或者確保航行器同時到達指定區(qū)域從而形成編隊。國內(nèi)外許多學者針對形成編隊的多航路規(guī)劃做了大量研究［6～9］，大多沒有考慮編隊保持階段的航路規(guī)劃。對于編隊保持階段的航路規(guī)劃，文獻［10］基于A*算法將無人機編隊看作一個整體，以編隊中心作為規(guī)劃結(jié)點，得到一組具有編隊約束的三維航跡；文獻［11］通過對編隊航行任務的分解，采用粒子群算法用來解決編隊航路規(guī)劃問題，生成了具有隊形約束的協(xié)同航路，但是沒有考慮編隊轉(zhuǎn)彎的情況，導致編隊在轉(zhuǎn)彎前后的狀態(tài)不一致。

粒子群優(yōu)化算法（Particle Swarm Optimization）［12］是一種全局優(yōu)化進化算法，可以很好地用于求解多航路規(guī)劃問題。粒子群算法解的好壞和收斂速度受初始化粒子質(zhì)量的影響，本文引入通視性分析和混沌映射的思想來改善初始化粒子的質(zhì)量，既保證了初始化粒子的有效性又保證了隨機性，將該方法應用于編隊航路規(guī)劃，通過分析編隊航行的過程，針對不同的航行階段規(guī)劃出不同的協(xié)同航路，解決粒子群算法在進行編隊航路規(guī)劃時存在的編隊轉(zhuǎn)彎問題，實現(xiàn)編隊保持階段的隊形與狀態(tài)一致性。

2 編隊航行問題及約束條件

2.1 編隊航行問題

編隊航行任務通常由編隊形成、編隊保持、編隊執(zhí)行任務這三個主要的階段組成。在進行編隊規(guī)劃之前，需要確定AUV編隊的任務，AUV數(shù)目以及編隊的隊形。編隊形成階段是指AUV由完全無序的狀態(tài)按照某種規(guī)則在規(guī)定區(qū)域形成編隊的過程，在編隊形成階段，往往需要為AUV規(guī)劃出一組從起始點到達隊形約束形成點的協(xié)同航路。在編隊保持階段，需要AUV編隊在航行過程中保持穩(wěn)定的隊形，通常是采用非線性控制器來實現(xiàn)，但是由于水下環(huán)境復雜，水聲通信微弱，動力學的非線性和強耦合性，這對于控制系統(tǒng)的實時性和魯棒性提出了較高的要求。因此可以考慮在編隊保持過程中規(guī)劃出具有隊形約束的航路，從而減輕控制系統(tǒng)和通訊系統(tǒng)的負擔。編隊執(zhí)行的任務是由任務規(guī)劃系統(tǒng)決定，本文的編隊任務為同時打擊多個目標，因此同樣也需要為AUV規(guī)劃出一組滿足時間與空間約束的協(xié)同航路。

圖1 編隊航行示意圖

通過分析編隊航行的過程，按照是否需要維持編隊隊形，可以將編隊航行過程劃分為編隊保持階段和編隊非保持階段。在編隊保持階段，編隊中的AUV之間要保證相應的隊形關(guān)系，同時確保狀態(tài)的一致性；在編隊非保持階段（編隊形成階段，編隊執(zhí)行任務階段），需要滿足時間上的同時到達和空間上的避免碰撞。

在編隊保持階段規(guī)劃航路時，需要考慮編隊轉(zhuǎn)彎問題，確保編隊轉(zhuǎn)彎前后的隊形和狀態(tài)的一致性。狀態(tài)一致性指的是AUV編隊的隊形應與AUV航行方向始終維持一個固定的角度關(guān)系；隊形一致性指的是編隊中AUV應始終維持相應的位置關(guān)系。如圖2（a）航路，編隊在轉(zhuǎn)彎前后保證了隊形是一字型，但沒有保證狀態(tài)一致性；圖2（b）航路，通過設(shè)置編隊轉(zhuǎn)彎航路，保證隊形和狀態(tài)的一致性。

圖2 編隊轉(zhuǎn)彎前后的隊形和狀態(tài)一致性

2.2 編隊航行問題

實驗中所涉及的環(huán)境區(qū)域主要有限制區(qū)、風浪區(qū)、威脅區(qū)、陸地和島嶼。不同類型的區(qū)域有不同的約束限制。限制區(qū)是限制航行的區(qū)域，也即規(guī)劃的航路不能通過限制區(qū)。風浪區(qū)則是指海平面上的波浪區(qū)域，禁止AUV在風浪區(qū)進行上浮，因此上浮點不能位于風浪區(qū)內(nèi)。威脅區(qū)一般為敵方軍艦，這些區(qū)域?qū)τ诤叫衅鱽碚f具有一定威脅性，有一定概率被摧毀，可將威脅代價定義為

其中l(wèi)表示AUV穿過威脅區(qū)的路徑長度，ε為威脅系數(shù)。

時間協(xié)同約束是指在發(fā)射多個AUV后，AUV能夠按照預定的時間到達各自的目標位置；空間協(xié)同約束是指在任意時間節(jié)點上，AUV之間的距離應大于最小安全距離。時間協(xié)同約束和空間協(xié)同約束可如式（2）所示：

其中，上標T表示目標點，i和j為AUV的編號，k為AUV的數(shù)目。為第i個AUV的到達目標點的時間，Dt(xi,yi)表示第i個AUV在t時刻的空間位置坐標。Tmax為允許航路時間代價差值的最長時間間歇，Dmin為最小安全距離。

編隊保持階段需要編隊間保持相應的隊形關(guān)系，本文以領(lǐng)航跟隨法為隊形約束基準，根據(jù)領(lǐng)航者的位置，確定出其余跟隨者的位置。隊形約束可表示為

其中Pl(t)表示領(lǐng)航者AUV在t時刻的空間位置，Pi(t)表示第i個跟隨者AUV在t時刻的空間位置。D(x,y)和A(x,y)分別為計算x與y之間距離和夾角的函數(shù)，Li和θi分別表示第i個跟隨者AUV與領(lǐng)航者AUV的期望距離與夾角。

在編隊保持過程中，不可避免遇到轉(zhuǎn)彎，編隊轉(zhuǎn)彎會導致AUV編隊各自的航路長度不一致，這時候需要控制算法控制AUV的航行速度，外側(cè)的速度要大于內(nèi)側(cè)速度，才能在轉(zhuǎn)彎過程中保持編隊隊形。以一字型編隊為例，假設(shè)AUV內(nèi)側(cè)的拐彎半徑為R，編隊轉(zhuǎn)彎前的航行方向與正北方向的夾角為θi，轉(zhuǎn)彎后的航行方向與正北方向的夾角為θi+1，AUV間的距離為d。轉(zhuǎn)彎過程中兩段航路的距離差為：。以內(nèi)側(cè)AUV為基準，編隊轉(zhuǎn)彎的時間代價為

3 基于粒子群算法的編隊航路分段規(guī)劃

結(jié)合編隊航行的特性，可以將編隊航路規(guī)劃分為編隊保持階段的規(guī)劃和編隊非保持階段的規(guī)劃兩個步驟。

步驟一：確定隊形約束形成點和隊形約束結(jié)束點的位置，并得到中間為了避障的編隊轉(zhuǎn)彎開始點和結(jié)束點，以及編隊轉(zhuǎn)彎圓弧。步驟二：得到起始點到隊形約束形成點，以及隊形約束結(jié)束點到目標點的滿足時間協(xié)同約束和空間協(xié)同約束的航路。

步驟一得到的導航點稱為一級導航點，步驟二得到的導航點稱為二級導航點。

3.1 編隊保持階段的航路規(guī)劃

采用粒子群算法解決單航路規(guī)劃問題時，往往將起始點和目標點以及之間的導航點作為一個粒子。受虛擬結(jié)構(gòu)法的啟發(fā)，將編隊中所有的航路看作一個粒子，假設(shè)一共有K個AUV，N個一級導航點，粒子的形式如式（5）所示：

上式中一級導航點均需滿足式（3）對應的隊形約束條件。但是由于未考慮編隊轉(zhuǎn)彎的情況，此時編隊保持階段的航路雖然滿足了隊形一致性，但并未滿足編隊的狀態(tài)一致性，這樣采用粒子群算法得到的編隊航路示意圖如圖2（a）所示。對于每一個需要轉(zhuǎn)彎的導航點，以內(nèi)側(cè)航路為基準，按照最小編隊轉(zhuǎn)彎半徑，得到內(nèi)側(cè)航路的編隊轉(zhuǎn)彎開始點和編隊轉(zhuǎn)彎結(jié)束點和轉(zhuǎn)彎圓弧。并根據(jù)AUV編隊的位置關(guān)系，得到其他航路的編隊轉(zhuǎn)彎開始點和編隊轉(zhuǎn)彎結(jié)束點以及轉(zhuǎn)彎圓弧。

1）通視-混沌粒子初始化

粒子群算法的尋優(yōu)能力和收斂速度受初始化粒子影響，通常希望初始化粒子的分布具有很好的均勻性，增強粒子的多樣性。對于編隊航路規(guī)劃來說，復雜的海洋環(huán)境以及編隊航路約束等條件使得算法較難找到最優(yōu)解，增加粒子種群的規(guī)模，搜索的范圍也越大，越容易找到最優(yōu)解，但是會極大增加算法的復雜度。如果初始化的粒子已經(jīng)是較優(yōu)粒子，則會加快算法的收斂速度，但是因為缺乏粒子的多樣性，粒子群算法很可能會陷入局部最優(yōu)解。

如何平衡粒子群算法的尋優(yōu)能力和收斂速度是一大難題。對于實際問題來說，在對航路進行離線預規(guī)劃時往往側(cè)重于尋優(yōu)能力；但是對于在線航路規(guī)劃問題，更側(cè)重于收斂速度，不必苛求所求解是否最優(yōu)。假設(shè)初始化時粒子種群大小為L，較優(yōu)的粒子個數(shù)為Lbetter，分布均勻的粒子個數(shù)為Lrandom，滿足：

μ取值0＜μ＜1，μ較大時粒子的收斂速度越快，μ較小時全局搜索能力強。通視分析［13］本質(zhì)上是判斷觀測點與目標點之間的視線是否通達，如果遇到障礙物則繞過，到達下一個觀測點，最終到達目標點，非常適用于約束區(qū)域較為稀松的廣闊海洋規(guī)劃環(huán)境的快速規(guī)劃。本文采用通視分析來構(gòu)建較優(yōu)的初始粒子，如圖3所示，E、T分別為起始點和目標點，首先判斷兩點之間是否通達，發(fā)現(xiàn)其穿過限制區(qū)1，得到繞過限制區(qū)的路徑EAT和EBT，選擇較短路徑EBT，將當前任務分解為E到B和B到T的兩個子任務，判斷兩個子任務是否通達，對于不通達的路徑進一步分解，以此類推，直到所有連線不穿過任何禁行區(qū)域為止，選出其中最短的路徑，圖中ECBT即為所求。

圖3 通視分析原理示意圖

以某一航行器為例，采用通視分析得到該AUV從起始點到達目標點的航路，將航路按照一定間隔均勻分為N個一級導航點，包括一個隊形約束開始點、一個隊形約束結(jié)束點以及N-2個中間導航點。按照隊形約束關(guān)系，得到其余AUV的航路，這樣便得到了一個較優(yōu)的初始化粒子（本文稱為通視粒子）。然后分別以其余AUV構(gòu)建通視粒子，最終得到K個通視粒子。

Logistic混沌具有較好的均勻分布性［14］，其基本公式為

Xi表示第i個混沌向量，θ為預先設(shè)定常數(shù)。當θ=4時，系統(tǒng)在［0，1］內(nèi)處于混沌狀態(tài)。假設(shè)粒子群算法中粒子的維數(shù)為d，隨機生成一個d維向量X0，向量的每個維度取值為0～1之間。根據(jù)式（7）可以映射得到l個混沌向量，將每個混沌向量的每個維度分量映射到粒子對應維度的取值區(qū)間上，即可得到l個分布均勻粒子（本文稱為混沌粒子）。

在編隊航路規(guī)劃的初始化上，以其中一個AUV為基準，采用混沌映射方式得到l個混沌向量，按照隊形約束條件，得到其他的AUV航路，這樣便得到了l個混沌粒子。然后分別以其余AUV為基準進行相應的混沌初始化，從而可以得到l×K個混沌粒子。

2）粒子適應度

如圖4中導航點的定義，假設(shè)有K個AUV，N個一級導航點，N-2個轉(zhuǎn)彎點，則對應會有N-2個轉(zhuǎn)彎開始點和轉(zhuǎn)彎結(jié)束點，需要優(yōu)化的目標為

圖4 編隊規(guī)劃航路示意圖

式中，f(x,y)表示從x點到達y點的時間代價。xy可能是直線或圓弧。如果未經(jīng)過任何環(huán)境區(qū)域直線的時間代價為兩點距離除以AUV速度；圓弧的時間代價如式（4）所示。如果xy經(jīng)過了禁行區(qū)域（陸地、島嶼或限制區(qū)），則時間代價為無窮大；如果經(jīng)過了威脅區(qū)域則除了xy本身的時間代價還需要加上式（1）的威脅代價。FA為編隊起始點到隊形約束形成點和隊形約束結(jié)束點到目標點的時間代價，采用通視分析進行預估。在K個AUV到達隊形約束形成點的預估時間代價中，取最大值作為該階段的基準協(xié)同時間RefTime；對于隊形約束結(jié)束點到達目標點的預估時間代價也采用同樣的方式得到基準協(xié)同時間。FB+FC為編隊保持階段的時間代價，F(xiàn)B為編隊保持階段直線環(huán)節(jié)的時間代價，F(xiàn)C為編隊轉(zhuǎn)彎部分的時間代價，對于編隊轉(zhuǎn)彎部分，在計算粒子適應度時需要得到編隊轉(zhuǎn)彎圓弧的代價，但是在粒子進行迭代尋優(yōu)的過程中，保持轉(zhuǎn)彎點的不變。此外，為使編隊保持階段的航路盡可能長，F(xiàn)A需乘以一個大于1的加權(quán)系數(shù)ε。

3）粒子位置與速度更新策略

粒子群算法是通過跟蹤個體極值和群體極值，從而更新粒子的速度和位置，其更新策略為

式中，wmax和 wmin，c1max和 c1min，c2max和 c2min分別代表慣性權(quán)重w，學習因子c1和c2的最大最小值，t為當前迭代次數(shù)，Tmax為最大迭代次數(shù)。

最后，將粒子中的需要轉(zhuǎn)彎的導航點按照最小轉(zhuǎn)彎半徑為基準，轉(zhuǎn)換為轉(zhuǎn)彎開始點和轉(zhuǎn)彎結(jié)束點以及編隊轉(zhuǎn)彎圓弧，從而得到編隊保持階段的航路。

3.2 編隊非保持階段的航路規(guī)劃

在矢量規(guī)劃空間上，不同起始點和目標點規(guī)劃出來的航路產(chǎn)生空間碰撞的概率很小，無需在進行航路規(guī)劃的過程中實時進行空間碰撞檢測，只需在得到多組協(xié)同航路后進行空間碰撞檢測從而篩選出最優(yōu)航路即可。粒子群算法可以一次性產(chǎn)生多條航路，可以將這多條航路按照空間位置進行分類，也即將粒子種群劃分為多個粒子子群，從這些子群中各自篩出其中的最優(yōu)航路，這樣即可得到多條空間散布均勻的航路。本文采用K均值聚類算法對粒子群算法產(chǎn)生的多條航路進行分類。

1）混沌粒子初始化

采用粒子群算法進行協(xié)同航路規(guī)劃時，需要建立多個粒子種群來實現(xiàn)，其中每一個粒子種群對應一個AUV。相較于編隊保持階段的航路規(guī)劃，這一環(huán)節(jié)算法復雜度相對較小，無需設(shè)置通視粒子，同樣采用混沌映射的方式，對種群進行初始化。

2）粒子適應度

在協(xié)同航路規(guī)劃中，時間協(xié)同約束條件與單航路規(guī)劃時的時間代價是沖突的，最優(yōu)的單航路規(guī)劃的集合是很難滿足協(xié)同任務需求的，而且編隊非保持階段的航路要確保AUV幾乎同時到達目標點。因此編隊非保持階段的規(guī)劃代價除了威脅代價以外還需要考慮時間協(xié)同代價，用航路的時間代價與RefTime的差值來描述。對于某個AUV的航路，其第i個粒子的優(yōu)化目標為

其中，yi表示第i個粒子對應的航路，ftime(y)表示航路時間代價，fthreat(y)表示威脅代價。α和β為權(quán)重因子，根據(jù)實際情況來設(shè)定其大小。

4 編隊航路規(guī)劃實驗分析

為驗證編隊航路規(guī)劃算法的有效性和可行性，針對不同的環(huán)境約束條件以及編隊任務設(shè)計多組實驗。實驗所用的規(guī)劃地圖為S57矢量電子海圖，除島嶼外，還添加了限制區(qū)，威脅區(qū)，風浪區(qū)。AUV的速度為5nmi/h（約9.26km/h），編隊最小轉(zhuǎn)彎半徑R為2.2km。粒子群算法的參數(shù)最大最小值為c1max=2.3，c1min=0.1，c2max=1.8，c2min=0.1，wmax=1.2，wmin=0.6?，F(xiàn)分別針對三角形編隊，菱形編隊，一字型編隊設(shè)計三組實驗。三角形編隊的隊形為由三個AUV構(gòu)成的底邊為1.1km的等腰直角三角形；菱形編隊由四個AUV構(gòu)成，邊長為0.777km；一字型編隊由六個AUV構(gòu)成，每兩個臨近的AUV間的距離為0.55km。實驗結(jié)果中，紅色旗子是起始點，藍色三角是目標點，黑點是隊形約束形成與結(jié)束點，黃點、白點分別為編隊轉(zhuǎn)彎開始、結(jié)束點，棕色點為上浮開始和結(jié)束點，綠色點為二級導航點。

針對復雜環(huán)境下三角形編隊實驗，其起始點的經(jīng) 緯 度 為（126.494°，26.907°），（126.388°，26.897°），（126.307°，26.871°）；目標點的經(jīng)緯度為（127.016°，26.251°），（127.000°，26.203°），（126.935°，26.153°）。規(guī)劃出的航路如圖5所示，AUV編隊航行的關(guān)鍵結(jié)點時間代價如表1所示。

圖5 復雜環(huán)境下三角形編隊規(guī)劃航路

表1 復雜環(huán)境下三角形編隊時間代價

針對復雜環(huán)境下菱形編隊的實驗，其起始點的經(jīng)緯度為（126.544°，26.907°），（126.438°，26.897°），（126.357°，26.871°），（126.283°，26.849°）；目標點的經(jīng)緯度為（126.656°，26.084°），（126.732°，26.110°），（126.788°，26.139°），（126.842°，26.171°）。規(guī)劃出的航路如圖6所示，AUV編隊航行的關(guān)鍵結(jié)點時間代價如表2所示。

圖6 復雜環(huán)境下菱形編隊規(guī)劃航路

表2 復雜環(huán)境下菱形編隊時間代價

針對一字型編隊的規(guī)劃實驗，其起始點的經(jīng)緯度為（122.250°，29.310°），（122.245°，29.290°），（122.239°，29.270°），（122.233°，29.250°），（122.239°，29.230°），（122.244°，29.210°）；結(jié)束點的 經(jīng) 緯 度 為（123.114°，29.430°），（123.120°，29.405°），（123.126°，29.380°），（123.132°，29.355°） ，（123.137°，29.335°），（123.132°，29.300°）。規(guī)劃出的航路如圖7所示，AUV編隊航行的關(guān)鍵結(jié)點時間代價如表3所示。

表3 一字型編隊時間代價

圖7 一字型編隊規(guī)劃航路

從以上三組不同編隊在不同環(huán)境下的編隊規(guī)劃的實驗結(jié)果可知，所提算法能夠規(guī)劃出一組滿意的編隊航路。在編隊保持階段可以很好地保持隊形結(jié)構(gòu)，針對不同的復雜環(huán)境也能夠以較優(yōu)的方式轉(zhuǎn)彎繞行，保證了編隊隊形和狀態(tài)的一致性，航路間的時間代價基本一致，且編隊保持階段的航路長度在整個編隊航行過程中占比較高；在編隊非保持階段，各AUV間的航路滿足時間和空間協(xié)同的要求。

5 結(jié)語

編隊航路規(guī)劃可劃分為編隊保持階段的規(guī)劃和編隊非保持階段的規(guī)劃。在編隊保持階段，采用粒子群算法，將編隊航路看作一個粒子，對粒子進行通視-混沌初始化，通過迭代尋優(yōu)得到一組具有隊形約束的航路，且保證了編隊轉(zhuǎn)彎前后隊形和狀態(tài)的一致性；在編隊非保持階段，采用粒子群算法，以K均值聚類的方式得到一組滿足時間要求且空間分布散列的協(xié)同航路。實驗結(jié)果表明，該算法得到的編隊航路滿足設(shè)定目標要求。