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

徐 楊，邵 興

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

0 引言

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

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

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

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

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

1 水面無人艇運(yùn)動模型

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

圖1 無人艇運(yùn)動模型Fig.1 Motion model of unmanned surface vehicle

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

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

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

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

2 編隊控制基本策略模型

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

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

在實(shí)際工程運(yùn)用中，水面艇的路徑跟蹤分為直線跟蹤和圓軌跡跟蹤2種形式。本文構(gòu)建目標(biāo)航向漸進(jìn)函數(shù)，可計算得出目標(biāo)區(qū)域內(nèi)任意一點(diǎn)對于目標(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)跟蹤的方式實(shí)時跟蹤編隊隊形，建立目標(biāo)坐標(biāo)系計算跟隨者的跟蹤誤差。如圖4所示，目標(biāo)坐標(biāo)系將坐標(biāo)原點(diǎn)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)系下的實(shí)際位置。

無人艇的橫向跟蹤距離可通過調(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)航者-跟隨者算法在實(shí)際運(yùn)用中有以下不足[11]：1）過度依賴編隊領(lǐng)航者；2）無法靈活進(jìn)行編隊變換；3）存在編隊形成過程中的避碰風(fēng)險。

為了降低編隊形成過程中各編隊成員相互碰撞的風(fēng)險，本文引入速度障礙法作為編隊控制過程中的避碰策略。在常規(guī)的水面無人艇的避碰中，由于無人艇所攜帶的傳感器難以精確地獲取障礙物的速度位置信息，將速度障礙法運(yùn)用于無人艇避碰時往往需要實(shí)時對障礙物的運(yùn)動信息進(jìn)行濾波和預(yù)測后，得到相對準(zhǔn)確的障礙物運(yùn)動信息才能發(fā)揮出良好的避碰性能。而在編隊控制中，由于編隊各成員的運(yùn)動信息實(shí)時已知，速度障礙法可實(shí)時準(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è)無人艇和障礙物的運(yùn)動狀態(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ā)點(diǎn)同時出發(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)航者-跟隨者算法更適合實(shí)際工程試驗中的編隊控制問題，為后續(xù)的湖上試驗創(chuàng)造了有利的條件。