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

徐 楊，邵 興

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

0 引言

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

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

相比于領(lǐng)航者-跟隨者算法，虛擬結(jié)構(gòu)法將編隊描述為剛體運動，轉(zhuǎn)化為時變的目標(biāo)跟蹤問題。適用于外界干擾少、運動模型確定的編隊控制。潘無為等采用虛擬結(jié)構(gòu)與人工勢場相結(jié)合的編隊控制算法，研究了存在障礙物條件下，多AUV系統(tǒng)的編隊形成、編隊保持和編隊變換問題[5]?；谛袨榈木庩牽刂扑惴ㄍㄟ^定義編隊成員的行為規(guī)則約束、協(xié)調(diào)編隊動作，易于描述，但無法確定編隊系統(tǒng)的整體行為，同時也無法用數(shù)學(xué)手段定量分析編隊隊形的穩(wěn)定性。

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

本文針對領(lǐng)航者-跟隨者算法在編隊形成過程中的缺陷，提出將速度障礙法引入領(lǐng)航者-跟隨者算法，并在此基礎(chǔ)上運用威脅評價函數(shù)協(xié)調(diào)編隊運動，避免編隊碰撞，提高編隊控制的穩(wěn)定性和安全性。

1 水面無人艇運動模型

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

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

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

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

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

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

2 編隊控制基本策略模型

本文采用領(lǐng)航者-跟隨者算法作為編隊控制的基本結(jié)構(gòu)。領(lǐng)航者采用路徑跟蹤控制的方式跟蹤既定的航線路徑，跟隨者采用目標(biāo)跟蹤的方式跟蹤虛擬目標(biāo)點，虛擬目標(biāo)點由領(lǐng)航者當(dāng)前的位置和編隊控制結(jié)構(gòu)計算得出[8]。領(lǐng)航者和跟隨者通過各自控制算法計算得出其期望速度和期望艏向，通過艏向和速度控制器實現(xiàn)編隊控制[9]。

2.1 領(lǐng)航者路徑跟蹤算法設(shè)計

在實際工程運用中，水面艇的路徑跟蹤分為直線跟蹤和圓軌跡跟蹤2種形式。本文構(gòu)建目標(biāo)航向漸進(jìn)函數(shù)，可計算得出目標(biāo)區(qū)域內(nèi)任意一點對于目標(biāo)路徑的趨近航向[10]。

設(shè)當(dāng)前無人艇的所在位置與目標(biāo)路徑的最短距離為D。

1）當(dāng)目標(biāo)路徑為直線段時，最短距離D為無人艇與目標(biāo)直線的側(cè)向距離，目標(biāo)航向lδ為目標(biāo)路徑的方向角。最短距離D計算公式為

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

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

式中：(x, y)為無人艇的所在位置；(Ox, Oy)為目標(biāo)路徑的圓心；R為目標(biāo)路徑的半徑。當(dāng)無人艇位于目標(biāo)路徑外側(cè)時，距離為負(fù)；位于目標(biāo)路徑內(nèi)側(cè)時，距離為正。

圖2 直線跟蹤路徑導(dǎo)引Fig.2 Line tracking path guidance

圖3 圓弧跟蹤路徑導(dǎo)引Fig.3 Circle tracking path guidance

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

式中ψmax為最大偏差航向角。對于函數(shù) f( D)來說，應(yīng)該滿足特征：①函數(shù)有界，且取值范圍為（-1，1）；②函數(shù)連續(xù)可導(dǎo)，且在D的取值范圍內(nèi)為嚴(yán)格單調(diào)遞增的奇函數(shù)。

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

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

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

式中a為符號函數(shù)。當(dāng)目標(biāo)路徑為直線段時，常值為正；當(dāng)目標(biāo)路徑為圓弧段時，若圓弧段的繞行方向為順時針，則a常值為正，若圓弧段的繞行方向為逆時針，則a常值為負(fù)。

上文所構(gòu)建的目標(biāo)航向漸進(jìn)函數(shù)為外環(huán)控制器，計算得出目標(biāo)航向，目標(biāo)航向作為無人艇航向控制器的輸入控制無人艇的舵/斗漸進(jìn)跟蹤目標(biāo)路徑。

2.2 跟隨者目標(biāo)跟蹤算法設(shè)計

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

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

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

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

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

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

式中：vl為領(lǐng)航者的航速；umax為最大相對速度；Δs為相對速度調(diào)整項。

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

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

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

3.1 基于橢圓的速度障礙法

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

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

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

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

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

通過速度障礙法計算出速度障礙VO后，非VO集合中的矢量方向即為無人艇可選的避碰艏向?？紤]到無人艇的轉(zhuǎn)彎性能，一般選取無人艇當(dāng)前編隊期望艏向角aψ附近的艏向區(qū)間作為避碰艏向選擇。即：

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

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

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

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

編隊成員的期望艏向角：

式中α為符號函數(shù)。當(dāng)Ftrep的矢量方向沿速度矢量p左側(cè)時，α取負(fù)；當(dāng)Ftrep的矢量方向沿速度矢量p右側(cè)時，α取正。

當(dāng)無人艇按照既定隊形進(jìn)行編隊時，領(lǐng)航者-跟隨者算法作為編隊控制的基本策略導(dǎo)引無人艇進(jìn)行編隊航行，當(dāng)跟隨者附近出現(xiàn)障礙物時，由威脅評價函數(shù)判斷跟隨者是否有碰撞風(fēng)險。若具有碰撞風(fēng)險，則由速度障礙法計算器避碰艏向，跟隨者按避碰艏向規(guī)避障礙物。

4 仿真試驗

4.1 領(lǐng)航者-跟隨者編隊控制試驗

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

圖6 一字形結(jié)構(gòu)下的編隊控制效果Fig.6 Formation control effects under linear shape structure

仿真結(jié)果表明，領(lǐng)航者-跟隨者算法能夠有效地對指定編隊隊形進(jìn)行組織和跟蹤。

4.2 編隊避碰算法仿真試驗

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

圖7 編隊成員初始狀態(tài)Fig.7 Initial status of formation member

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

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

5 結(jié)束語

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