基于動態目標點協同的無人艇編隊路徑規劃

郝金玉,范 雪,楊少龍,趙三飛,盧憶文

(1.駐大連地區第一軍事代表室， 遼寧 大連 116000； 2.華中科技大學船舶與海洋工程學院， 武漢 430074； 3.中國艦船研究設計中心， 武漢 430064)

1 引言

隨著各國對海洋開發重視程度日益提高，基于水面無人艇(unmanned surface vehicles,USV)的海洋自主作業成為探索開發海洋資源的重要利器。隨著智能化程度提升，無人艇可實現自主航行、環境感知、目標探測、自主避障等功能。相比有人海上作業裝備，無人艇具有體積小、速度快、機動性強等優勢，可完成高風險、重復性、大范圍的海上作業任務，在海上偵察、海事搜救、海洋調查等場景有良好應用前景。隨著應用需求日益廣泛，無人艇編隊理念也應時而生。相比于單艇作業，編隊作業在覆蓋范圍、容錯率、實效性等方面具有更好表現，在大范圍海洋作業方面具有重要意義。

無人艇編隊如何進行路徑規劃，提高編隊的隊形穩定性和安全性，一直是該領域的研究熱點。目前，國內外已有較多專家學者針對該問題展開研究并取得進展。歐陽子路等針對無人艇編隊穩定性問題，在經典快速搜索隨機樹算法基礎上，提出了一種非嚴格保形修正向量與非嚴格保形控制圓區域，能夠使無人艇編隊安全避碰并最大程度地保證隊形的穩定性。李賀等為解決復雜擾動下的無人艇編隊控制問題，設計一種基于有限時間擾動觀測器的固定時間編隊控制策略，有效提高無人艇編隊系統的精確性和魯棒性。謝偉杰等考慮到基于質點模型的蜂擁算法直接應用到欠驅動無人艇模型的困難性，提出一種基于目標跟蹤的轉化策略，能夠有效地將蜂擁算法間接地應用到無人艇集群編隊中。

在編隊控制領域，相比于以上相關論文中所用算法，領航者-跟隨者控制算法是實現多機器人編隊最經典、最簡單的控制算法。核心思想是基于領航者的實時位置，按一定的編隊位置關系得出跟隨者的實時位置，進而實現整個機器人集群的協同編隊。該算法的缺點就是過度依賴領航者。在確定每一個跟隨者的位置時只考慮了與領航者的位置關系，沒有考慮跟隨者之間的位置關系，有可能造成跟隨者機器人碰撞等問題出現。

人工勢場算法是另一多機器人編隊控制領域的經典算法。人工勢場算法的優點在于可以實現機器人集群運動中不發生碰撞，且勢場中力的計算是根據機器人之間的實時相對位置，時效性強。相比于領航者-跟隨者算法，編隊效果更加靈活。人工勢場的缺點主要有兩點：第一，存在“零勢能點或區域”，機器人集群在勢場力的作用下，向零勢能區域運動，當機器人到達此區域后，不再受勢場力的作用，此時機器人將無法繼續運動；第二，人工勢場算法只保證機器人集群之間不發生碰撞，它具有改變編隊隊形的能力而沒有維持隊形的能力，這與我們期望能夠實現多機器人的固定編隊運動有偏差。

在前人研究基礎上，結合以上兩種算法的優缺點，提出將領航者-跟隨者和人工勢場兩種算法融合的動態目標點編隊路徑規劃策略，同時考慮到將用于質點模型算法應用到無人艇編隊中的困難性，提出目標追蹤的轉化策略，進一步構建無人艇分層控制架構，設計通用視線角制導律(line of sight,LOS)與經典比例-積分-微分(proportion integration differentiation,PID)控制律的航路跟蹤算法，將多無人艇編隊任務分解成領航船的目標航線跟蹤和跟隨艇的動態目標點跟蹤。根據領航船的實時位置，更新動態目標點的位置，并保證跟隨艇的持續追蹤。仿真結果表明，該策略可較好地實現多無人艇的直線和圓形等基本航路單元的編隊任務，保持剛性編隊穩定性同時使碰撞沖突對編隊穩定性的影響降至最低。

2 協同編隊路徑規劃設計

無人艇協同編隊路徑規劃策略以系統中各個無人艇的行為和相互作用研究為出發點，為突出算法的準確性和降低工作復雜性，用圖論描述多無人艇編隊系統。如圖1所示，用節點1、2、3、4描述4艘無人艇，用雙箭頭連線描述兩兩對象的通信信息流。為模擬真實自組網通信拓撲方式(如ZigBee、Wifi組網)的準確性，設定任意2個節點間都可實現信息交互，即多無人艇系統內任何一個個體都可收到其他個體發送的信息流。

圖1 節點模型描述無人艇編隊系統示意圖Fig.1 Node model representation of USV formation

根據圖論中圖的結構和性質可用矩陣描述，定義矩陣=[]代表系統中節點聯系，當兩節點,之間有信息流時，=1；定義對角矩陣=diag；定義圖拉普拉斯矩陣=-。

結合圖1實例，則有：

(1)

式(1)中3個矩陣是協同編隊策略的關鍵變量。

2.1 基于領航者-跟隨者的編隊群體運動算法

節點模型如式(2)：

(2)

式中:、分別代表節點的當前狀態量、輸入控制量；∈，代表系統中的節點個數，這里取4。輸入控制量由式(3)得到：

(3)

式中:為編隊鄰接矩陣；為節點的相鄰集合。將式(3)代入式(2)，同時引入全局狀態向量=[,…,]∈，整理得式(4)。

(4)

式(4)可見編隊穩定性由矩陣決定。由于矩陣每一行元素相加為零，所以當且僅當有唯一的特征向量，保證=0，其特征值為零，該結論對實現節點編隊控制有重要意義。式(5)可實現編隊穩定性，最終狀態為：

(5)

式中:為特征向量；為可逆矩陣；(0)為節點在0時刻的狀態量。圖1中，假設節點4為領航者，節點1、2、3為跟隨者。編隊中跟隨者的控制函數為：

(6)

式中:>0為常數;為時刻；為跟隨者節點和跟隨者節點之間的相對位置。編隊中領航者控制算法為

(7)

式中:()為時刻領航者節點與跟隨者節點之間的相對距離；和為定值；()為在時刻領航者與期望航點之間的距離。根據式(6)和式(7)，可實現節點集群按照一定固定編隊隊形運動。

2.2 基于人工勢場的編隊個體間避障算法

在領航者-跟隨者編隊運動算法基礎上，融入人工勢場法，保證在形成和維持編隊隊形過程中節點之間不發生碰撞。假設在時刻，節點掃描到了節點處于危險接觸范圍內，此時節點會對節點產生一個斥力滿足

(8)

式中:為節點個數，()=||-()||，為探測距離，為一常數，在時刻避碰響應()對節點速度產生影響。

則考慮編隊個體間斥力影響下，編隊中跟隨者節點的控制函數更新為：

(9)

式中:為常數。編隊中領航者節點的控制算法更新為

(10)

引入避碰斥力分量，式(9)和式(10)協同能夠實現單個節點在集群的形成和維持編隊隊形過程中避免碰撞。

3 無人艇編隊分層控制架構

3.1 分層控制架構設計

為實現面向多無人艇的協同編隊自主航行任務，在協同編隊路徑規劃層輸出動態目標點，引入無人艇單艇控制層，如圖2所示，通過分層控制思想將無人艇編隊自主航行任務解耦成基于協同編隊路徑規劃策略的動態質點編隊控制的規劃層和基于LOS-PID底層控制算法的無人艇動態目標點跟蹤的控制層。

圖2 無人艇編隊控制層設計框圖Fig.2 Hierarchical control design framework of USV formation

控制原理如圖3所示，在某時刻下，協同編隊控制策略中的式(9)依據領航者的實時位置(,)和動態目標點和在上一時刻末的位置，產生動態目標點和在該單位時間內的運動速度，目標點的初始位置和運動速度累加，實現不斷更新目標點的位置。動態目標點與領航者連線構成的陣型即為無人艇編隊的期望隊形，將動態目標點和作為無人艇和的跟蹤目標，最終達到多無人艇協同編隊的效果。

圖3 分層控制原理示意圖Fig.3 Schematic diagram of layered control

3.2 LOS-PID底層控制算法

LOS制導律是無人艇航路跟蹤控制的經典理論，具有邏輯簡潔、參數易調、穩定性好等優點。LOS制導原理如圖4所示，只與無人艇的當前位置和任務目標點有關，能夠將二維航路跟蹤問題轉化為無人艇的一維航向跟蹤問題，實現無人艇逐步平滑逼近目標航線及跟蹤目標點。

圖4 LOS制導原理示意圖Fig.4 LOS guidance principle diagram

在慣性坐標系下由一系列的目標點(=1,2,3,…)組成期望航線，無人艇的路徑跟蹤任務按順序依次到達各航路點并沿著相鄰2個航點之間的連線跟蹤，當無人艇到達某航點附近時執行到點切換邏輯，實現順次跟蹤多航路點目標。

以航點和 + 1 之間的航線任務為例，無人艇當前位置為()，以無人艇為圓心設定半徑為的視線圓，以視線圓與+1的交點()為期望位置。根據反正弦函數可求出()()和()()之間的夾角。當無人艇與航線間的距離大于時，直接取無人艇到航線的垂線方向作為期望航向。根據該邏輯，則期望航向可表示為式(11)：

(11)

式中:的選取直接影響無人船對目標航線的跟蹤響應速度，當越小則期望航向越趨近于垂直航線方向，系統對目標航線的橫向偏差收斂響應越快，這種情況下無人艇位置會快速逼近目標航線上，但同時也會帶來振蕩和不穩定現象。當較大時則系統對橫向誤差的響應較慢，但控制過程會相對穩定。

當根據動態目標點生成無人艇期望航向后，底層控制器根據期望航向產生舵角控制指令，控制無人艇航向逼近期望航向。設計PID航向控制器，由于結構簡單、調試方便、工程實現度高，廣泛應用于實船航向控制。

以LOS制導產生的期望航向與無人艇當前航向之間的偏差信號為PID的輸入，根據航向偏差分別進行比例、積分、微分運算，為防止長時間積分使積分項過大導致執行器指令飽和，對于積分運算進行了飽和限制，即設定一個最大值，當積分運算結果達到這個值后，不再進行累加。如式(12)所示，PID控制律將比例、積分、微分三項運算結果相加得到舵角控制指令。

(12)

式中:()為舵角控制指令；()為航向偏差；為離散采樣點，=0,1,2,…；為比例系數；為積分系數，一般取0；為微分系數。

4 Matlab仿真驗證

根據第3節所設計的動態目標點協同編隊路徑規劃策略、LOS-PID制導控制底層算法，設計無人艇編隊執行仿真系統，流程如圖5所示。

圖5 無人艇編隊控制仿真流程框圖Fig.5 USV formation control simulation process framework

按照執行任務的先后邏輯，首先為領航船單艇的路徑跟蹤任務，如圖5中箭頭、、所示，根據目標航線和領航船的當前位置，根據LOS-PID制導控制算法輸出舵角控制指令，同時設定無人船轉速保持恒定且不為0，根據舵角和轉速更新領航船的實時位置。然后，協同編隊控制算法根據所設計的期望隊形和領航船的實時位置計算出3個動態目標點的位置，如圖5中箭頭、所示。最后，3個跟隨艇實時跟蹤并不斷縮短與各自目標點的差距，如圖5中箭頭、、所示，最終形成無人艇編隊的期望隊形。本章的Matlab仿真驗證設計也將按如圖5所示流程執行(本文采用Matlab R2020a進行仿真驗證)。

4.1 控制層驗證

首先進行單艇路徑跟蹤仿真試驗，初始以(0,0)為起始點，設計了由9個航點組成的期望航線，如圖6，該航線包含了基本的直線與轉向路徑，可較好地確定參數值。

圖6 期望航線Fig.6 Desired routes

設定LOS制導律中視線圓半徑為4 m，到點判斷距離為1 m。PID控制器、、三個參數進行Matlab仿真，依據單一變量原則，依次確定3個參數值。

首先，防止長時間積分下不能收斂為一有限值，設置飽和限制，為簡化參數取值，將積分系數設為0。其次，比例系數對系統控制影響最大，系數越大，動態反應越快，消除的能力越強，但由于慣性作用，系數過大時會產生振蕩不穩定，在參數選取時首先確定；最后，微分系數可以優化反應速度，避免過大帶來的不穩定效應，主要起輔助穩定的作用，參數取值中最后確定。

從無人艇的追蹤軌跡、產生期望航向和當前航向的響應以及無人艇相對于期望軌跡的實時位置偏差3個方面，對控制算法進行驗證。

經過Matlab數值仿真確定=6,=0，且發現當=6,=0,5,10時，的數值變化對反應速度的優化效果不明顯，對系統減小超調，克服振蕩，加快系統的響應速度，減小調節時間，改善系統的動態性能的作用較低，為減少參數個數，取=0。

經Matlab仿真，如圖7所示，使用LOS制導律和PID控制器能夠完成單艇的路徑跟蹤任務，LOS制導律產生期望航向，當期望航向與當前航向產生差值時，在PID控制的作用下，對無人艇模型產生舵角控制信號，實現無人艇沿照期望航線運動。此外，從圖8可以看出，無人艇的期望航向和當前航向僅在轉彎處產生一定超調和震蕩，系統可以及時做出反應并迅速消除差值，能夠實現無人艇的及時轉向跟蹤。如圖9所示，無人艇模型在跟蹤圖6軌跡時，僅在轉彎處有明顯的橫向偏差，但最大值不超過2 m，可說明單艇的跟蹤航線能力較好。

圖7 仿真無人艇追蹤軌跡Fig.7 USVfollowing trajectory

圖8 期望航向和當前航向曲線Fig.8 Curve of desired course and current course

圖9 橫向位置偏差曲線Fig.9 lateral position deviationcurve

4.2 規劃層驗證

單艇的航路跟蹤控制算法驗證完成后，驗證協同編隊控制算法的有效性，即根據領航船與跟隨船的質點模型能否實現固定編隊運動，并且運動過程中質點之間不會發生碰撞。

初始位置設定：領航船(0,0)、目標點1(-1.5,1.5)、目標點2(1.5,1.5)、目標點3(-3,0)。在經過一段時間穩定后，領航船位置為(,)時，3個動態目標點分別在軸正向、軸正向、軸負向上相差20，期望隊形可由領航船與動態目標點的位置共同構成三角形編隊。式(9)和式(10)的參數如表1所示。

表1 算法主要參數Table 1 Main parameters of the algorithm

首先測試直線編隊，領航船以1 m/s速度沿軸直線運動。結果如圖10(a)所示。然后，測試圓軌跡編隊，領航船以1 m/s速度沿半徑為10 m的圓做圓周運動。結果如圖10(b)所示。

圖10(a)展示了在直線編隊任務中領航船和3個目標點的運動軌跡，實現了在協同編隊控制算法下實現某一初始位置的質點模型編隊，當一段時間后編隊處于穩定狀態，按照期望的相對位置協同前進。圖10(b)展示了曲線運動任務，領航船和3個目標點的運動軌跡和相對位置，曲線編隊穩定性不如直線，但基本符合預期位置。因此，規劃層中協同編隊控制算法對質點模型編隊的控制效果符合期望預期。

(虛線和○代表領航船和3個目標點的運動軌跡和相對位置；實線和△代表無人艇編隊的運動軌跡和3個跟隨者的相對位置)圖10 多無人艇協同編隊仿真曲線Fig.10 Simulation verification of multi-USVs formation

4.3 規劃層與控制層聯合驗證

在單獨驗證規劃層和控制層基礎上，將2部分級聯驗證分層控制架構及編隊任務的可行性，3艘跟隨艇與目標點初始位置一致，初始航速為1 m/s，初始航向沿軸正向為0°。同規劃層驗證類似，驗證直線和曲線航行編隊。結果如圖10(c)和10(d)所示。圖10(c)展示了3艘無人艇跟蹤動態目標點行進軌跡和多無人艇編隊的構型，從行進軌跡上可見無人艇編隊較好地追蹤動態目標點的運動軌跡，實現了避碰和編隊效果。

圖10(d)所示為曲線行進軌跡，穩定后，3個跟隨艇相對于領航船分別在軸正向、軸正向、軸負向上相差20 m，較好地保持無人艇的期望隊形。

經過3個階段驗證，所提算法對多無人艇編隊運動控制是理論可行的，既能保證無人艇編隊的固定隊型，也能實現在行進過程中不發生碰撞。

對仿真結果進行分析，將其與使用人工勢場算法控制無人艇柔性編隊運動的策略進行對比，如圖11所示，該算法通過引入一個虛擬領航者，使其沿軸直線運動，采用分層控制策略，規劃出一系列目標點引領整個集群運動，該算法可以實現無人艇集群沿軸正向運動，并且無人艇之間不會發生碰撞，但該算法無法實現無人艇集群形成并保持固定的編隊隊形，無人艇之間位置沒有明確相對關系。所提協同編隊控制算法相比于該人工勢場算法，如圖10(c)和(d)所示，在能夠實現無人艇之間避碰的基礎上，同時實現無人艇集群按照期望隊形進行編隊，3個跟隨者相對于領航者在軸正向、軸正向、軸負向上分別相差20 m，無人艇之間有明確的相對位置關系，更易于無人艇編隊協同作業。故所提算法相比于該人工勢場算法，從編隊形成和維持編隊的穩定性上都體現出來更好的性能。

圖11 人工勢場柔性編隊無人艇運動仿真曲線Fig.11 The artificial potential field algorithm controls the swarm motion of USVs

5 結論

提出了融合領航者-跟隨者協同編隊與人工勢場避碰的多無人艇編隊路徑規劃策略。設計分層控制架構，在規劃層實現質點模型編隊輸出3個跟隨艇的動態目標點，控制層利用LOS-PID算法實現無人艇目標點跟蹤。所提方法將編隊任務解耦成動態目標點的規劃與跟蹤2個子問題，較好地實現了穩定且安全的協同編隊任務。經Matlab仿真驗證，所提算法及系統能夠對直線、圓弧兩類基本路徑單元實現較好的協同編隊，為進一步開展復雜路徑協同編隊研究提供了良好支撐。

協同編隊算法和分層控制的復雜度低、穩定性好、通用性強，不僅可用于多無人艇編隊任務，也可拓展至異構集群，為實際海上應用提供了一種簡潔有效的協同編隊策略。