基于CPSO算法的海空跨域無人航行器集群協(xié)同作業(yè)路徑規(guī)劃

殷 翔，李培正，駱曹飛，馬賽男，馬 勇

（1.武漢理工大學航運學院，武漢 430063；2.內河航運技術湖北省重點實驗室，武漢 430063；3.浙江嘉藍海洋電子有限公司，舟山 316000）

1 引言

隨著人工智能、信息技術的快速發(fā)展，無人航行器自主航行能力得到顯著提升，能夠完成如環(huán)境勘測、應急救援等任務，被廣泛地應用到相關行業(yè)并發(fā)揮了重要作用。隨著應用場景的多元化，單個無人航行器已不能滿足復雜任務情形的需求，跨域無人航行器集群可協(xié)同完成復雜場景下的任務需求。

文獻[1]基于人工勢場法設計了無人機（Unmanned Aerial Vehicle,UAV）與目標的動態(tài)博弈關系，設計了一種群UAV 圍捕方法。文獻[2]提出一種融合粒子群（Particle Swarm Optimization,PSO）與改進蟻群算法的無人潛航器（Autonomous Underwater Vehicle,AUV）路徑規(guī)劃算法PSO-ACO，實現(xiàn)了AUV 的三維路徑規(guī)劃。文獻[3]提出了一種新型的k度平滑的協(xié)同UAV協(xié)調路徑規(guī)劃方法。文獻[4]為無人艇（Unmanned Surface Vehicle,USV）設計了基于非線性模型預測控制的軌跡跟蹤控制器，實現(xiàn)了跟隨者對領導者目標軌跡的實時估計。文獻[5]提出了一種考慮避碰和同步打擊時間的UAV 集群多目標同時打擊協(xié)同優(yōu)化方法。文獻[6]為AUV 提出了一種基于Octree 模型和改進蟻群算法的路徑規(guī)劃方法。文獻[7]為AUV 路徑規(guī)劃問題設計了一種基于四叉樹的蟻群算法。文獻[8]提出了一種基于PSO算法改進的蟻群優(yōu)化算法，以尋找復雜海底環(huán)境中AUV 到達指定目的地的最優(yōu)路徑。文獻[9]設計了一種無人船艇協(xié)商協(xié)議來實現(xiàn)群無人艇避碰。文獻[10]提出了一種基于深度強化學習的無人船艇編隊路徑跟蹤方法。文獻[11]設計了一種UAV 和USV 協(xié)作清污協(xié)作系統(tǒng)。文獻[12]采用有限角度的A*算法為USV 集群規(guī)劃最優(yōu)路徑。文獻[13]針對四旋翼水下航行器協(xié)同包圍控制問題，設計了自適應反步法控制器和魯棒反步法控制器分別處理航行器受到的未知常值干擾和時變干擾。文獻[14]建立聲學通信和定位系統(tǒng)來提升USV 和AUV 導航和控制性能。文獻[15]設計了一種包括搜索階段和航行階段的分步動態(tài)目標追蹤方法，并使用IPSO 算法來生成航行器路徑。文獻[16]利用可變柔性機械臂配合攔阻索的機構輔助實現(xiàn)在狹小空間和惡劣條件下對微小型固定翼無人機平臺的自主釋放與回收，形成了一種全新的跨域異構協(xié)作的實現(xiàn)方式。文獻[17]設計了一種用于UAV-USV 集群的協(xié)作通信框架。文獻[18]設計了一種分層著陸引導點生成算法，通過引導點序列實現(xiàn)了UAV 在USV 甲板上的穩(wěn)定著陸。

分析可知，在同一類型無人航行器集群協(xié)同路徑規(guī)劃方面已取得了大量成果，但在無人機協(xié)同無人艇、無人艇協(xié)同無人潛航器等跨域集群無人航行器協(xié)同作業(yè)規(guī)劃方面成果相對較少。文獻[19]明確了跨域協(xié)同的基本內涵：在不同領域互補性地而不是簡單疊加性運用多種能力，使各領域之間互補增效，從而在多領域建立優(yōu)勢，獲得完成任務所需要的行動自由。不同的無人航行器集群，如UAV 集群、USV 集群、AUV 集群等，由于存在航行環(huán)境、結構性能、運動模型等差異，不同類型的無人航行器集群勝任的任務場景不盡相同。為此，在跨域協(xié)同路徑規(guī)劃中，需要為各類無人航行器集群設置獨立的作業(yè)任務，從而多個領域互補增效，實現(xiàn)跨域無人航行器集群協(xié)同作業(yè)路徑規(guī)劃的有機耦合。

UAV 集群在空中航行，以其機動靈活、空間障礙物少、探測視野開闊等特點，可為USV 高效完成水面航行環(huán)境探測任務；AUV 在水下航行，隱蔽性強，但水下航行環(huán)境復雜，導致其環(huán)境探測效率較低，適合執(zhí)行短距離水下目標探測任務；USV 在水面航行，具有小型輕量、操縱靈活、自主性強、無人駕駛等優(yōu)點，在UAV 協(xié)同下水面航行環(huán)境情況相對更容易獲得，可搭載AUV 等可承擔較遠距離的作業(yè)任務。

為應對復雜作業(yè)任務需求，需要為UAV、USV 和AUV 集群協(xié)同規(guī)劃高質量的路徑。為此，本文提出一種基于協(xié)同粒子群優(yōu)化（Cooperative Particle Swarm Optimization,CPSO）的路徑規(guī)劃算法，以增進?？湛缬驘o人航行器集群協(xié)同作業(yè)能力。

2 ?？湛缬驘o人航行器集群協(xié)同作業(yè)路徑規(guī)劃模型

2.1 ?？湛缬驘o人航行器集群協(xié)同系統(tǒng)描述

針對?？湛缬驘o人航行器集群協(xié)同作業(yè)路徑規(guī)劃問題，設計一個UAV 集群、USV 集群、AUV集群協(xié)同路徑規(guī)劃系統(tǒng)，在考慮避碰和轉彎機動的基礎上，區(qū)別不同UAV 集群的特性，給定不同無人航行器集群不同的航行任務，設計CPSO算法實現(xiàn)各無人航行器分步路徑規(guī)劃，最終來協(xié)同完成整體的目標追蹤任務。整個系統(tǒng)運行過程為分步的三個階段，每個階段都對相應的跨域無人集群進行與之能力相適應的任務規(guī)劃。第一階段，UAV 集群起飛至航行高度，基于已知的航道信息，使用CPSO 算法規(guī)劃UAV 集群路徑，航行過程中使用傳感器來沿途探測USV 航行區(qū)域障礙物，將障礙物的信息與已有的航道信息結合，生成完整的USV 通航環(huán)境圖；第二階段，利用完整的障礙物位置信息，通過CPSO 算法，規(guī)劃USV集群的協(xié)同航行路徑；第三階段，USV 集群攜帶AUV 到達規(guī)劃位置后，利用傳感器探測水下航行環(huán)境，生成可用的水下航行環(huán)境圖。AUV 集群結合航行環(huán)境圖利用CPSO 算法規(guī)劃集群協(xié)同水下路徑，并從USV 處釋放來追蹤到目標位置。

?？湛缬驘o人航行器集群的路徑協(xié)同規(guī)劃過程，實際上是分步的三次無人航行器集群協(xié)同路徑規(guī)劃過程，每個集群的航行都為下一個集群的規(guī)劃提供關鍵的航行信息。每個集群的路徑規(guī)劃過程中，航行器的起始位置和終點位置已知，通過在兩個已知信息點之間插入合適數(shù)量的路徑點來形成一條完整的航行路徑。航行場景的復雜程度決定了航行路徑點的數(shù)量。

2.2 懲罰函數(shù)設計

在三個階段的無人航行器集群路徑規(guī)劃的過程中，需要考慮航行中避碰因素、無人設備轉彎性能因素[18]，同時協(xié)同航行的最終優(yōu)化目標為整體航行時間最短。本文將障礙物碰撞約束、轉彎性能約束、航行時間函數(shù)設計為懲罰函數(shù)。根據(jù)實際航行中無人航行器性能的差異，對不同的航行器設計不同的懲罰函數(shù)判斷閾值和相關參數(shù)。為實現(xiàn)航線實際可行，將碰撞約束和轉彎性能約束的懲罰函數(shù)設置較大的權重，使所有的規(guī)劃航線不發(fā)生碰撞且同時滿足無人航行設備轉彎能力。

2.2.1 轉彎懲罰函數(shù)

無人航行器在航行的過程中受到其結構和性能的約束，轉彎過程的航跡通常較為平滑而且旋回半徑有最小限制，因此需要加入獨立的轉彎性能的懲罰來使得每個航行器的航行軌跡實際可行。具體的設計方法為，每三個相鄰且距離較近的航行路徑點會形成一個轉彎的夾角，而航行器的最小轉彎半徑需要小于規(guī)劃路徑的轉彎夾角的最大內切圓半徑，才能使得無人設備能夠依照規(guī)劃路徑完成轉向。

轉彎懲罰函數(shù)的計算過程如下，三個相鄰路徑點構成的轉彎夾角為：

取三個路徑點的兩段路徑中最短的一段來設置內切圓，較短的路徑為：

此時的最大內切圓半徑計算為：

式中，α為每三個相鄰路徑點確定的路徑夾角，l1是前兩個路徑點的距離，l2是后兩個路徑點的距離，l3是第一個和第三個路徑點的距離，ρ是邊l的最大內切圓半徑，即極限轉彎半徑。

通過比較無人航行器的最小轉彎半徑和規(guī)劃的轉彎角的最大內切圓半徑得到懲罰函數(shù)值tu為：

式中，無人航行器的最小轉彎半徑為R，且Pk–1，Pk，Pk–2為三個相鄰的路徑點，ρ≥R即表示規(guī)劃的路徑滿足了航行設備的轉彎性能約束。一段完整路徑中每相鄰三個路徑點都需要進行轉彎懲罰函數(shù)計算，從而得到完整路徑的轉彎懲罰函數(shù)值Atu為：

對于一條完整的路徑若至少有一組懲罰函數(shù)取值為0，即發(fā)生過至少一次碰撞，則該路徑的轉彎懲罰函數(shù)值取0。

2.2.2 碰撞懲罰函數(shù)

?？湛缬驘o人航行器集群規(guī)劃航行的路徑需要避免所有障礙物的碰撞。為簡化避碰過程，將障礙物膨化為不同軸長的橢球。具體的碰撞懲罰設計方法為：首先根據(jù)航行器航行的高度，取得對應高度上障礙物切片信息，然后對于每兩個相鄰路徑點構成的路徑段都需要判斷本路徑段是否與障礙物切片相交。判斷方法是計算路徑線段上選取的樣本點與障礙物切片圓心的距離，比較所有距離數(shù)值與障礙物切片半徑的大小關系。

具體計算過程如下：

根據(jù)路徑和障礙物的尺度確定航行器第k個路徑線段的采樣點數(shù)量n，生成采樣點矩陣P：

第i個樣本點與其對應的第j個障礙物切片圓心的距離為：

對于第i個樣本點和第j個障礙物的碰撞懲罰數(shù)為：

航行器完整路徑的碰撞懲罰函數(shù)值Acoll為：

2.2.3 時間懲罰函數(shù)

跨域無人航行器集群由于航行環(huán)境和性能的差異，實際航行速度有一定差異，這就導致最短的規(guī)劃路徑方案并不一定是最有效的方案。因此將航行時間最優(yōu)而不是航行距離最優(yōu)作為規(guī)劃路徑的評價指標。具體的設計方法是：對每個航行器的速度進行設置，根據(jù)規(guī)劃的路徑，計算每個航行器的航行時間，作為懲罰數(shù)值。

第i個航行器的近似航行距離為所有相鄰的路徑點的距離和，表示為如下：

則第i個航行器的航行時間為：

3 CPSO 協(xié)同作業(yè)路徑規(guī)劃算法

3.1 粒子群算法基本原理

粒子群優(yōu)化算法是一種成熟的仿生算法，來源于對鳥類捕食過程的研究。粒子群算法采用群體進化的方法，需要建立N個維度的搜索區(qū)域，其中每個粒子都具有位置和速度兩個矢量信息。在迭代過程中，會記錄每個粒子的最佳位置和全局的最佳粒子位置，并共享信息到全局，根據(jù)每次迭代結果的共享信息來決定下一次迭代優(yōu)化的方向。

具體的粒子群算法過程如下：

（1）首先建立維度M的初始粒子種群，為第i個粒子的當前位置信息矩陣；為第i個粒子進化速度矩陣。

（3）根據(jù)問題需求，確定適應度優(yōu)劣評價標準，保存當前全局最優(yōu)適應度的粒子位置和每個粒子最優(yōu)適應度的位置，粒子個體最優(yōu)歷史確定如下式：

全局最優(yōu)粒子確定如下式：

（4）根據(jù)已知的全局最優(yōu)粒子和粒子個體最優(yōu)歷史，來確定粒子群的進化方向，具體進化方程包括速度更新和位置更新，速度更新方程如下式：

位置更新方程如下式：

式中，c1、c2為學習因子，用來分配全局最優(yōu)粒子和粒子個體最優(yōu)歷史對于進化步長影響的權重；r1、r2為一對取值區(qū)間[0,1]的均勻分布隨機數(shù)；Xbestij為第i個粒子個體最優(yōu)歷史的第j個數(shù)據(jù)值；Xallbestj為全局最優(yōu)粒子的第j個數(shù)據(jù)值。

（5）重復步驟（2）～步驟（4），直到迭代次數(shù)達到預設代數(shù)，停止優(yōu)化，輸出當前全局最優(yōu)粒子。

3.2 CPSO 算法和求解過程

粒子群優(yōu)化算法具有較快的收斂速度，能高效處理簡單的目標優(yōu)化過程，廣泛適用于不同領域的優(yōu)化問題。但是隨著優(yōu)化目標的復雜度不斷提高，普通的粒子群優(yōu)化方法更加容易陷入局部最優(yōu)解的情形。因此，本文使用協(xié)同粒子群優(yōu)化方法，將復雜的優(yōu)化目標分解為不同的粒子群的細化優(yōu)化目標。協(xié)同粒子群算法基本思想為：建立一個有多個粒子種群的復合粒子群，每個獨立粒子群代表了一個分解出來的細化優(yōu)化目標，每個粒子群都具有M個維度，并在M個維度下進行優(yōu)化，每一次的迭代優(yōu)化，各個種群之間保持信息共享，直到達到整體的優(yōu)化目標。

基于協(xié)同粒子群算法的求解過程如下：

（1）根據(jù)同類無人航行器的數(shù)量建立N個協(xié)同進化的粒子群，每個粒子群中包含M個粒子。建立每個粒子的位置矩陣和進化速度矩陣為第k個粒子群，第i個粒子的當前位置信息和當前進化速度。

（2）構造全局問題解C以及全局適應度函數(shù)：

式中，α、β、γ為不同懲罰函數(shù)的設計權重，權重的設計考慮到避碰和轉彎約束的必要性，在平衡不同懲罰函數(shù)值量級的基礎上，將α、β設計為較大權值，γ設計為較小權值。Atu、Acoll、γT分別為本文第二部分設計的懲罰函數(shù)。

（3）通過依次替換解中對應粒子群的粒子，搜索并保存每個粒子群的全局最優(yōu)適應度的粒子位置和每個粒子最優(yōu)適應度的位置。

第k個粒子群每個粒子個體最優(yōu)歷史確定如下：

第k個粒子群粒全局最優(yōu)粒子確定如下：

（4）重復步驟（3），直到得到N個粒子群的所有粒子個體最優(yōu)歷史和全局最優(yōu)粒子，全局問題解更新為：

然后通過式(17)、(18)對整個種群所有粒子的進化速度和當前位置進行更新迭代。

（5）重復步驟（3）～步驟（4），直到迭代次數(shù)達到預設代數(shù)，停止優(yōu)化，輸出并保存當前最優(yōu)粒子群，即為此同類無人航行器最優(yōu)路徑規(guī)劃方案。

（6）根據(jù)得到的同類航行器航行最優(yōu)規(guī)劃方案執(zhí)行航行任務，并沿途更新其他集群航行器航行必要環(huán)境信息數(shù)據(jù)，完成之后，重復步驟（1）～步驟（6）來進行順次的下一個同類無人航行器集群路徑規(guī)劃。直至3 次順次的同類無人航行器路徑規(guī)劃完成后，得到并實現(xiàn)了最終的協(xié)同航行路徑規(guī)劃方案。

4 仿真結果及分析

4.1 ?？湛缬驘o人航行器集群參數(shù)設置

為了驗證所提出的群無人航行器海空協(xié)同路徑規(guī)劃策略的有效性，本文建立了一個多障礙物的航行場景模型，并以其為任務區(qū)域進行模擬仿真實驗。整個區(qū)域為一個水深100 m、空域高度100 m、長寬各為1000 m 的長方體區(qū)域。整個區(qū)域中，共設置大小、位置不同的11 個橢球形障礙物。

?？湛缬驘o人航行器集群中包含 3 類無人航行器集群，每個集群根據(jù)任務需求來設置航行器數(shù)量；本文協(xié)同集群設計為：UAV 集群共兩臺，記為a1、a2；USV 集群共4 臺，記為s1、s2、s3、s4；AUV 集群共兩臺，記為u1、u2。具體每臺航行器的參數(shù)和狀態(tài)設置如表1、表2所示。

表1 UAV 和AUV 基礎參數(shù)Table 1 General parameters of UAV and AUV

表2 USV 基礎參數(shù)Table 2 Basic parameters of USV

4.2 協(xié)同作業(yè)路徑規(guī)劃仿真

CPSO 算法需要預設最大迭代次數(shù)，迭代次數(shù)的設置不僅會影響優(yōu)化過程探索全局解的能力，也會影響仿真規(guī)劃的效率。較低次數(shù)的迭代雖然能快速得到可行解，但是收斂過早，對全局最優(yōu)解的發(fā)現(xiàn)能力較差；較高次數(shù)的迭代雖然能夠得到全局最優(yōu)解，但是探索過程緩慢，效率低下。本文選擇使用線性遞減的慣性因子，來保證算法前期全局尋優(yōu)能力強，后期局部尋優(yōu)能力強。

仿真模擬過程中，進行多次仿真模擬過程，對比不同仿真迭代次數(shù)下算法的有效性。每次仿真實驗預設不同的最大迭代次數(shù)，包括：10 次、30 次、50 次、100 次、200 次、500 次、800 次、1000 次、2000 次最大迭代數(shù)。得到的適應度測試結果見表3、圖1、圖2。同時，展示了10次、100 次、800 次的仿真規(guī)劃方案，如圖3～圖5所示。

圖1 不同迭代次數(shù)下的最優(yōu)適應度值折線圖Fig.1 Line diagram of optimal fitness value under different iterations

圖2 整體最優(yōu)適應度值折線圖Fig.2 Line diagram of the overall optimal fitness values

圖5 最大迭代次數(shù)為800 次的協(xié)同路徑Fig.5 Collaborative path graph with maximum number of iterations of 800

表3 不同迭代數(shù)下適應度Table 3 Adaptation table under different iterations

由仿真結果可以看出：

（1）如圖3所示，在本次仿真場景實驗的條件下，10 次迭代優(yōu)化就可以得到可行的協(xié)同作業(yè)路徑規(guī)劃方案。但是由于迭代次數(shù)較小對于全局解的探索能力較小，導致此時的協(xié)同作業(yè)規(guī)劃方案適應度較差，雖然完全滿足了避碰約束，但僅部分滿足了轉彎的約束，同時航行路徑有往復，耗費了較多的航行時間。

圖3 最大迭代次數(shù)為10 次的協(xié)同路徑Fig.3 Collaborative path graph with maximum number of iterations of 10

（2）如圖4所示，在100 次的最大迭代次數(shù)的條件下可以得到較優(yōu)的協(xié)同作業(yè)路徑規(guī)劃方案，尤其是UAV 和USV 集群協(xié)同航行路徑，完全滿足了轉彎和避碰的約束要求，同時航跡平滑沒有往復。此時的UAV 集群和USV 集群的協(xié)同適應度迭代結果也達到最優(yōu)水平，如圖1所示。但是由于AUV 集群的規(guī)劃路徑區(qū)別于其他集群，需要考慮到航行深度的變化，導致路徑規(guī)劃更加復雜。從而，AUV 集群的規(guī)劃路徑依然有急轉彎、往復的現(xiàn)象。相應的AUV 集群的協(xié)同適應度迭代結果（見圖1）并未達到最優(yōu)水平。

圖4 最大迭代次數(shù)為100 次的協(xié)同路徑Fig.4 Collaborative path graph with maximum number of iterations of 100

（3）如圖5所示，在最大迭代次數(shù)設置為800次的條件下，得到了全局最優(yōu)的協(xié)同作業(yè)路徑規(guī)劃方案。UAV 集群、USV 集群和AUV 集群均得到了最優(yōu)的協(xié)同路徑，即完全滿足了轉彎、避碰的約束條件，同時航跡線十分平滑。此時，如表3、圖1、圖2所示，各跨域無人集群的協(xié)同適應度和全局適應度均達到了最優(yōu)水平。

仿真結果表明，在UAV 集群和USV 集群的協(xié)同作業(yè)路徑規(guī)劃過程中，能在50～100 次的最大迭代次數(shù)下較為穩(wěn)定地收斂到最小適應度值，即得到這兩個跨域協(xié)同集群作業(yè)方案的全局最優(yōu)解；而AUV 集群的協(xié)同作業(yè)路徑規(guī)劃過程，需要500～800 次的迭代次數(shù)來收斂到較為穩(wěn)定的最小適應度值。如圖5所示的仿真結果，充分地體現(xiàn)出了本算法對于解決海空跨域無人航行器集群在復雜水域環(huán)境下協(xié)同作業(yè)以追蹤水下目標問題的優(yōu)越性和有效性。

5 結論

本文面向?？湛缬驘o人航行器集群協(xié)同作業(yè)任務提出了一種基于CPSO 的路徑規(guī)劃算法。在CPSO 算法設計過程中，將無人航行器轉彎性能、航行時效和避碰等約束條件以懲罰函數(shù)形式體現(xiàn)出來，建立全局的適應度優(yōu)化函數(shù)實現(xiàn)非線性迭代求解，為各個無人航行器建立獨立的粒子群來優(yōu)化，并行處理同類航行器的優(yōu)化過程，共享優(yōu)化信息，減少了迭代次數(shù)，提升了協(xié)同作業(yè)路徑規(guī)劃的效率。仿真結果表明，CPSO 算法為UAV、USV、AUV 無人航行器集群規(guī)劃了有效協(xié)同作業(yè)路徑，實現(xiàn)了水下目標的有效跟蹤。