結合速度障礙法的無人艇編隊控制策略研究

徐 楊，邵 興

（1. 中國船舶集團有限公司第七一〇研究所，湖北 宜昌 443003；2. 清江創新中心，湖北 武漢 430076）

0 引言

隨著國家對于海洋科學的不斷重視和無人裝備技術的進一步發展，水面無人艇作為一種新型的無人探測作業設備得到了快速的發展與廣泛的運用。在民用方面，水面無人艇可用于內河航運、海洋環境檢測、應急救援等領域；在軍用領域，可用作區域偵察、中繼通信、探雷探潛等用途。由于水面無人艇運動靈活性高且部署成本低，編隊集群作業是水面無人艇的一類重要作業方式，編隊控制技術也成為了水面無人艇當前研究的熱點方向。當前，編隊控制的主要方法有領航者-跟隨者算法、虛擬結構法和基于行為的編隊控制算法[1]。

領航者-跟隨者算法作為一種結構簡單、易于擴展的編隊控制算法在實際工程實踐中得到了廣泛的運用。楊震等根據領導者的位置以及航向角信息來構建出一艘參考船，使其運行軌跡能夠在固定時間內實現對參考船軌跡的跟蹤，并利用反步法以及上下界滑模法設計一個魯棒控制策略，使跟隨船的實際軌跡能夠跟隨領航船的軌跡[2]。Cui基于領航者的位置信息和預設編隊結構信息設計虛擬水下航行體，通過虛擬體產生期望軌跡和速度，并利用Lyapunov理論以及反演法設計控制律實現了跟隨船對虛擬船的軌跡實時跟蹤[3]。李良[4]等提出了一種基于改進跟隨領航者法的編隊控制方法，將對跟隨者的航向和速度控制簡化成單一的速度控制，并通過仿真實驗和湖上試驗進行了驗證

相比于領航者-跟隨者算法，虛擬結構法將編隊描述為剛體運動，轉化為時變的目標跟蹤問題。適用于外界干擾少、運動模型確定的編隊控制。潘無為等采用虛擬結構與人工勢場相結合的編隊控制算法，研究了存在障礙物條件下，多AUV系統的編隊形成、編隊保持和編隊變換問題[5]。基于行為的編隊控制算法通過定義編隊成員的行為規則約束、協調編隊動作，易于描述，但無法確定編隊系統的整體行為，同時也無法用數學手段定量分析編隊隊形的穩定性。

基于行為的無人艇編隊控制算法將無人艇的作業任務分解為多種行為（編隊保持、進行避碰、目標跟蹤等），并將各類行為分解為期望控制輸出，通過行為融合模塊控制無人艇運動。哈爾濱工程大學的姜大鵬[6]對基于行為的多AUV協調控制方法進行了研究，針對多AUV的編隊控制問題設計了多種基本行為，采用基于多目標值優化的方法進行行為融合，并采用IvP計算模型對行為決策空間進行離散化處理以保證決策的實時性。基于行為的多無人艇編隊控制基礎結構簡單，具有良好的實時性，可組成復雜的編隊行為，但缺少相應的系統穩定性分析，不利于理論計算。

本文針對領航者-跟隨者算法在編隊形成過程中的缺陷，提出將速度障礙法引入領航者-跟隨者算法，并在此基礎上運用威脅評價函數協調編隊運動，避免編隊碰撞，提高編隊控制的穩定性和安全性。

1 水面無人艇運動模型

為建立水面無人艇編隊控制仿真平臺，需建立水面無人艇的運動及動力學模型。結合水面無人艇的運動特性，本文只考慮無人艇水平面的運動模型，即只建立無人艇的縱蕩、橫蕩以及艏搖3個自由度的數學模型，如圖1所示。

圖1 無人艇運動模型Fig.1 Motion model of unmanned surface vehicle

為簡化模平面的三自由度數學模型，給出假設[7]：1）無人艇左右嚴格對稱，即滿足 Ixy=Iyz= 0 ；2）無人艇的質量均勻分布，且無人艇重心位于艇體坐標系原點；3）忽略無人艇在垂直面上的運動。

通過以上3個假設，可得出無人艇在水平面的三自由度數學模型：

式中：η表示無人艇在大地坐標系下的位置信息，即縱向位置、橫向位置和艏向角；v表示無人艇在大地坐標系下的速度信息，即縱向速度、橫向速度和艏向角速度；τ表示無人艇控制力和力矩輸入；M為慣性參數矩陣；C(v)為科氏力矩陣；D(v)為阻尼矩陣。

本文中所描述的水面無人艇為欠驅動水面無人艇，通過噴泵提供無人艇的前進推力，倒斗提供無人艇的轉艏力矩，其基本控制器包括速度控制、艏向控制，路徑跟蹤、目標跟蹤等控制算法的實現依賴于上述 2種基本控制器。水面無人艇通過舵、倒斗、噴泵為其提供前進推力和轉艏力矩，并通過運動控制器實現速度控制、艏向控制、航跡跟蹤等基本控制功能；激光雷達、光電攝像頭和導航雷達為無人艇提供環境態勢信息，并通過運動規劃實現無人艇的目標跟蹤、路徑規劃、動態避碰等功能。

2 編隊控制基本策略模型

本文采用領航者-跟隨者算法作為編隊控制的基本結構。領航者采用路徑跟蹤控制的方式跟蹤既定的航線路徑，跟隨者采用目標跟蹤的方式跟蹤虛擬目標點，虛擬目標點由領航者當前的位置和編隊控制結構計算得出[8]。領航者和跟隨者通過各自控制算法計算得出其期望速度和期望艏向，通過艏向和速度控制器實現編隊控制[9]。

2.1 領航者路徑跟蹤算法設計

在實際工程運用中，水面艇的路徑跟蹤分為直線跟蹤和圓軌跡跟蹤2種形式。本文構建目標航向漸進函數，可計算得出目標區域內任意一點對于目標路徑的趨近航向[10]。

設當前無人艇的所在位置與目標路徑的最短距離為D。

1）當目標路徑為直線段時，最短距離D為無人艇與目標直線的側向距離，目標航向lδ為目標路徑的方向角。最短距離D計算公式為

式中：目標航向可表述為 A x+By+C= 0 ；(x, y)為無人艇的所在位置。當無人艇位于目標路徑左側時，距離為負；位于目標路徑右側時，距離為正。

2）當目標路徑為圓弧段時，最短距離D為距離無人艇位置的最近圓弧切線和無人艇當前位置的距離，目標航向lδ為距離無人艇位置的最近圓弧切線的方向。最短距離D計算公式為

式中：(x, y)為無人艇的所在位置；(Ox, Oy)為目標路徑的圓心；R為目標路徑的半徑。當無人艇位于目標路徑外側時，距離為負；位于目標路徑內側時，距離為正。

圖2 直線跟蹤路徑導引Fig.2 Line tracking path guidance

圖3 圓弧跟蹤路徑導引Fig.3 Circle tracking path guidance

無人艇所在位置期望航向角dψ與目標航向lδ之間的偏差航向ψΔ可通過式（4）計算。

式中ψmax為最大偏差航向角。對于函數 f( D)來說，應該滿足特征：①函數有界，且取值范圍為（-1，1）；②函數連續可導，且在D的取值范圍內為嚴格單調遞增的奇函數。

本文選取S面函數為函數f( D)。

式中，k為航向變化參數，代表偏差航向的變化速率。其值越大，偏差航向的變化速度越大。

無人艇期望航向角dψ可計算得出：

式中a為符號函數。當目標路徑為直線段時，常值為正；當目標路徑為圓弧段時，若圓弧段的繞行方向為順時針，則a常值為正，若圓弧段的繞行方向為逆時針，則a常值為負。

上文所構建的目標航向漸進函數為外環控制器，計算得出目標航向，目標航向作為無人艇航向控制器的輸入控制無人艇的舵/斗漸進跟蹤目標路徑。

2.2 跟隨者目標跟蹤算法設計

本文采用目標跟蹤的方式實時跟蹤編隊隊形，建立目標坐標系計算跟隨者的跟蹤誤差。如圖4所示，目標坐標系將坐標原點Od建立在領航者位置處，坐標軸OXd沿領航者艏向方向，坐標軸OYd為坐標軸OXd順時針旋轉90°所得。

無人艇的跟蹤誤差可由式（7）得出：

式中：s為無人艇的縱向跟蹤距離；e為無人艇的橫向跟蹤距離； Rn(θ)為從大地坐標系到目標坐標系的旋轉矩陣；qf為跟隨者在大地坐標系下的位置；ql為領航者在大地坐標系下的實際位置。

無人艇的橫向跟蹤距離可通過調整無人艇的艏向角進行調整。根據橫向距離誤差(e -ed)，可計算無人艇的期望艏向角ψd：

式中：lθ為領航者的艏向角；Δe為前視距離；一般為艇長的2～3倍。

無人艇的縱向跟蹤距離可通過調整無人艇的航速進行調整。根據縱向距離誤差(s -sd)，可計算無人艇的期望速度vd：

式中：vl為領航者的航速；umax為最大相對速度；Δs為相對速度調整項。

3 基于威脅評價的編隊控制避碰策略

經典的領航者-跟隨者算法在實際運用中有以下不足[11]：1）過度依賴編隊領航者；2）無法靈活進行編隊變換；3）存在編隊形成過程中的避碰風險。

為了降低編隊形成過程中各編隊成員相互碰撞的風險，本文引入速度障礙法作為編隊控制過程中的避碰策略。在常規的水面無人艇的避碰中，由于無人艇所攜帶的傳感器難以精確地獲取障礙物的速度位置信息，將速度障礙法運用于無人艇避碰時往往需要實時對障礙物的運動信息進行濾波和預測后，得到相對準確的障礙物運動信息才能發揮出良好的避碰性能。而在編隊控制中，由于編隊各成員的運動信息實時已知，速度障礙法可實時準確地計算出避碰角度。

3.1 基于橢圓的速度障礙法

由于無人艇的物理外形特征，本文采用文獻[12]中的基于橢圓的速度障礙法計算無人艇的可行避碰角度。在橢圓速度障礙法中：P和V分別表示無人艇在水平面的位置和速度矢量；A和B分別表示無人艇橢圓和障礙物橢圓，兩者的半長軸分別沿著其速度矢量方向；VA表示無人艇的速度矢量；VB表示障礙物的速度矢量；VBA表示無人艇相對障礙物的速度矢量，可通過下式求得。

圖5 基于橢圓的速度障礙法示意圖Fig.5 Schematic diagram of ellipse-based velocity obstacle algorithm

假設無人艇和障礙物的運動狀態保持不變，當相對速度VBA的方向與障礙物B相交時，即可認為無人艇和障礙物在將來某一時刻t會發生碰撞。定義使得A和B碰撞的相對速度VBA的集合為RCCAB，稱之為相對碰撞區域。為方便在多個障礙物環境下得統一描述，將每個障礙物的RCC定義在無人艇的絕對速度空間，稱之為速度障礙VOAB。

當存在多個障礙物時，總的速度障礙VO為各障礙物VO的并集：

3.2 基于威脅評價的編隊避碰算法

通過速度障礙法計算出速度障礙VO后，非VO集合中的矢量方向即為無人艇可選的避碰艏向。考慮到無人艇的轉彎性能，一般選取無人艇當前編隊期望艏向角aψ附近的艏向區間作為避碰艏向選擇。即：

式中：aψ為由式（8）計算所得的期望艏向角；ψmax為最大允許轉彎角度；RAV為可行避碰區間。

在編隊成員間使用速度障礙法進行避碰需要對避碰編隊成員間的避碰艏向進行協調。本文采用編隊成員i所受勢場力的矢量和作為編隊成員i的威脅評價，并以編隊成員i的威脅評價計算編隊成員i的期望轉艏角度。

式中：Frep為編隊成員i所受的排斥力（當編隊成員i具有碰撞威脅且與具有碰撞威脅編隊成員處于距離R內時不為0）；arep為斥力系數；dis為編隊成員i與其他編隊成員的相互距離，通過求解編隊成員i的總斥力Ftrep可計算編隊成員i的碰撞威脅。

f( x)為編隊成員i的威脅評價函數[13]，通過排斥力Frep的矢量和計算編隊成員i所受的碰撞威脅程度。當編隊成員之間的相互距離越近，編隊成員i的威脅程度越高，其轉艏角度也就越大，當存在多個碰撞威脅時，按照Frep的矢量和計算編隊成員i的碰撞威脅程度。

編隊成員的期望艏向角：

式中α為符號函數。當Ftrep的矢量方向沿速度矢量p左側時，α取負；當Ftrep的矢量方向沿速度矢量p右側時，α取正。

當無人艇按照既定隊形進行編隊時，領航者-跟隨者算法作為編隊控制的基本策略導引無人艇進行編隊航行，當跟隨者附近出現障礙物時，由威脅評價函數判斷跟隨者是否有碰撞風險。若具有碰撞風險，則由速度障礙法計算器避碰艏向，跟隨者按避碰艏向規避障礙物。

4 仿真試驗

4.1 領航者-跟隨者編隊控制試驗

圖6為同一仿真試驗條件下的一字型編隊隊形下3艘無人艇的編隊控制效果，該試驗假設3艘無人艇位于同一出發點同時出發，最終在100個仿真步長下形成了穩定的編隊隊形。

圖6 一字形結構下的編隊控制效果Fig.6 Formation control effects under linear shape structure

仿真結果表明，領航者-跟隨者算法能夠有效地對指定編隊隊形進行組織和跟蹤。

4.2 編隊避碰算法仿真試驗

圖7為編隊控制時2個編隊成員的初始狀態，p和q分別表示在該編隊成員的艏向角。在該種狀態下編隊成員具有相互碰撞的風險，本文對該類初始狀態下的編隊控制效果進行對比仿真試驗。圖8為帶有避碰策略下的編隊形成過程。

圖7 編隊成員初始狀態Fig.7 Initial status of formation member

圖 8（a）中，帶有避碰策略的編隊控制算法可在50 m之內初步形成有效的編隊隊形，而圖8（b）中，則需要115 m的距離形成有效的編隊隊形。相比于無避碰策略的編隊控制算法，帶有避碰策略的編隊控制算法編隊形成速度加快，且避免了原本可能發生的碰撞編隊成員碰撞風險。

圖8 有無避碰策略的編隊形成過程對比Fig.8 Comparison of formation with and without collision avoidance strategy

5 結束語

本文通過領航者-跟隨者算法成功解決了多無人艇編隊控制問題，且分析了領航者-跟隨者算法的缺陷。通過將速度障礙法引入到編隊控制中，解決了領航者-跟隨者算法在編隊形成過程中的編隊碰撞問題。仿真試驗表明：該編隊控制避碰策略顯著改善了編隊形成過程中的相互碰撞問題，使領航者-跟隨者算法更適合實際工程試驗中的編隊控制問題，為后續的湖上試驗創造了有利的條件。