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

余耀宇 蔡 超

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

1 引言

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

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

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

2.1 編隊航行問題

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

圖1 編隊航行示意圖

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

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

圖2 編隊轉彎前后的隊形和狀態一致性

2.2 編隊航行問題

實驗中所涉及的環境區域主要有限制區、風浪區、威脅區、陸地和島嶼。不同類型的區域有不同的約束限制。限制區是限制航行的區域，也即規劃的航路不能通過限制區。風浪區則是指海平面上的波浪區域，禁止AUV在風浪區進行上浮，因此上浮點不能位于風浪區內。威脅區一般為敵方軍艦，這些區域對于航行器來說具有一定威脅性，有一定概率被摧毀，可將威脅代價定義為

其中l表示AUV穿過威脅區的路徑長度，ε為威脅系數。

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

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

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

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

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

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

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

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

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

3.1 編隊保持階段的航路規劃

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

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

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

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

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

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

圖3 通視分析原理示意圖

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

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

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

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

2）粒子適應度

如圖4中導航點的定義，假設有K個AUV，N個一級導航點，N-2個轉彎點，則對應會有N-2個轉彎開始點和轉彎結束點，需要優化的目標為

圖4 編隊規劃航路示意圖

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

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

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

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

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

3.2 編隊非保持階段的航路規劃

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

1）混沌粒子初始化

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

2）粒子適應度

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

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

4 編隊航路規劃實驗分析

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

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

圖5 復雜環境下三角形編隊規劃航路

表1 復雜環境下三角形編隊時間代價

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

圖6 復雜環境下菱形編隊規劃航路

表2 復雜環境下菱形編隊時間代價

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

表3 一字型編隊時間代價

圖7 一字型編隊規劃航路

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

5 結語

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