欠驅(qū)動無人艇集群協(xié)同編隊路徑規(guī)劃仿真

王藝凱,孫 俊,王 偲

(江南大學(xué)人工智能與計算機學(xué)院,江蘇 無錫 214122)

1 引言

無人艇(Unmanned Surface Vehicle,USV)是一種具備自主運行能力的水面艇,作為新一代信息技術(shù)的產(chǎn)物,無人艇不僅是軍事無人作戰(zhàn)系統(tǒng)的重要組成部分,而且為提升國家科技創(chuàng)新能力提供了重要支撐[1]。由于海洋環(huán)境日益復(fù)雜、作業(yè)任務(wù)日益多樣,無人艇系統(tǒng)處理復(fù)雜任務(wù)往往需要相互間的協(xié)作才能更有效率地、更高質(zhì)量地完成。在海軍作戰(zhàn)系統(tǒng)中,一艘無人艇可以實現(xiàn)有限的功能,而多艘USV協(xié)同編隊不僅可以提高效率,還可以完成許多更復(fù)雜的任務(wù),如協(xié)同防護、編隊集結(jié)等,因此無人艇集群化必然是無人艇發(fā)展的重要方向[2,3]。編隊控制問題作為無人艇集群化研究領(lǐng)域的重要課題,引起了國內(nèi)外眾多學(xué)者的關(guān)注。目前國內(nèi)外對多智能體集群編隊控制問題的研究方法主要有領(lǐng)航者-跟隨者(Leader-Follower,L-F)法[4]、基于行為(Behaviour-Based)法[5]、虛擬結(jié)構(gòu)(Virtual Structure)法[6]、人工勢場(Artificial Potential Field,APF)法[7]。

領(lǐng)航跟隨法的主要實現(xiàn)形式是編隊中設(shè)置一艘無人艇作為領(lǐng)航者,其余無人艇作為跟隨者,跟隨者與領(lǐng)航者保持固定的相對位置形成隊形[8]。Sun等[9]采用基于有限控制集模型預(yù)測控制方法提高了跟隨者的自治性。Park等[10]提出一種基于連通性保持和避障性能函數(shù)的魯棒主從編隊跟蹤設(shè)計。領(lǐng)航跟隨法的優(yōu)點是隊形穩(wěn)定性高,容易設(shè)計實現(xiàn),缺點是過度依賴領(lǐng)導(dǎo)者,跟隨者反饋差,避障效果差。虛擬結(jié)構(gòu)法要求機器人以剛體上的相應(yīng)點的位置和姿態(tài)信息作為各自的跟蹤目標形成隊形[11]。Tank等[12]提出了虛結(jié)構(gòu)法,該方法能取得較高的精度和軌跡跟蹤效。Ren[13]采用信息一致性算法,提出了一種分布式虛擬結(jié)構(gòu)的多智能體編隊算法。虛擬結(jié)構(gòu)法的優(yōu)點是隊形保持性能良好、系統(tǒng)穩(wěn)定性容易保證,缺點是隊形不靈活、避障不易實現(xiàn)。人工勢場法是通過設(shè)置勢場函數(shù),無人艇在勢場的吸引力、排斥力作用下,得到相應(yīng)的隊形。Sang等[14]提出在人工勢場模型中引入A*算法,生成全局最優(yōu)路徑,降低無人艇陷入局部極小值的概率。人工勢場法的缺點在于,隊形的變化需要對勢場函數(shù)的重新配置,因而不夠靈活。基于行為控制方法則是將任務(wù)整體分解成簡單的子任務(wù),系統(tǒng)內(nèi)的智能體根據(jù)自身的性能被分配到不同的子任務(wù),就需要有不同的行為來完成子任務(wù)。Tucker 和 Ronald[15]最早采用自下而上構(gòu)造系統(tǒng)的方式,提出了一種基于行為的控制方法,將隊形控制任務(wù)分解為一系列的基本行為,通過行為的綜合來實現(xiàn)運動控制。基于行為法的優(yōu)點則是可解決多任務(wù)問題、避障效果好,缺點是隊形控制的穩(wěn)定性得不到保證。

本文采用零空間行為法,對欠驅(qū)動無人艇集群編隊路徑規(guī)劃問題進行研究。零空間行為法的核心是將低級任務(wù)向量向高級任務(wù)的零空間進行投影,將綜合的任務(wù)輸出作為最終的任務(wù)輸出,在完成高級任務(wù)的同時部分或全部完成低級任務(wù),具有實時性強、任務(wù)之間不沖突的優(yōu)點[16]。

本文在國內(nèi)外研究的基礎(chǔ)上,對整個問題進行任務(wù)分解,在任務(wù)分解完成后對不同子任務(wù)完成任務(wù)函數(shù)的處理,將虛擬結(jié)構(gòu)法與零空間行為法相結(jié)合,保證了無人艇集群編隊的穩(wěn)定性,通過仿真驗證所述算法在無人艇集群編隊路徑規(guī)劃問題上的可行性。

2 零空間行為法建模

行為法的主要思想是首先設(shè)定一些智能體的期望行為,每個行為都有其對應(yīng)的目標或任務(wù)。在欠驅(qū)動無人艇集群編隊路徑規(guī)劃任務(wù)中,零空間行為法首先將任務(wù)分解為多個可以獨立完成的子任務(wù)并分別計算出各個子行為的任務(wù)函數(shù)。為解決各子任務(wù)之間可能存在的任務(wù)沖突,首先對各子任務(wù)劃分優(yōu)先級,其次將低優(yōu)先級任務(wù)的任務(wù)向量向高優(yōu)先級任務(wù)的零空間投影,從而抵消低優(yōu)先級任務(wù)對高優(yōu)先級任務(wù)的任務(wù)沖突,最終得到任務(wù)綜合的輸出函數(shù),傳輸給底層的無人艇運動控制器控制無人艇的運動。

定義σ為要控制的任務(wù)變量,則其函數(shù)模型可表示為

σ=f(r)

(1)

對應(yīng)的微分關(guān)系為

(2)

圖1 無人艇集群運動模型

其中J(r)是r的輸出雅可比矩陣。

在無人艇運動過程中通過求取最小范數(shù)速度,從而得到最小二乘解

(3)

在任務(wù)的完成過程中,無人艇的運動軌跡可以通過vd的時間積分來獲得。為消除離散時間積分導(dǎo)致的無人艇坐標位置偏移,可將式(3)改寫為如下形式

(4)

由式(4)可知,在多任務(wù)的情況下,各任務(wù)速度計算公式如下

(5)

其中下標i表示第i個子任務(wù)。如果下標也表示任務(wù)的優(yōu)先級(即任務(wù)1是最高優(yōu)先級的任務(wù)),在n個子任務(wù)的情況下,則式(5)可以被改寫為

vd=v1+N1[v2+…+Nn-1vn]

(6)

式(6)的幾何解釋為每個任務(wù)速度的計算是單獨進行的,在計算該速度對總的速度貢獻之前,用I表示適當(dāng)維數(shù)的單位矩陣,將第i優(yōu)先級的任務(wù)通過式(7)投影到上一優(yōu)先級任務(wù)的零空間上,以抵消該任務(wù)對高優(yōu)先級任務(wù)的影響

(7)

一方面,基于零空間的行為控制是在非奇異配置下完成最高優(yōu)先級的任務(wù)。另一方面,較低優(yōu)先級的任務(wù)僅在子空間中完成,在子空間中,它們不與具有較高優(yōu)先級的任務(wù)沖突,也就是說,每個任務(wù)達到次優(yōu)條件,該次優(yōu)條件根據(jù)由最高優(yōu)先級任務(wù)施加的約束來優(yōu)化任務(wù)。

3 任務(wù)處理

在欠驅(qū)動無人水面艇集群編隊任務(wù)中,子任務(wù)包括以下三項:避障任務(wù)、質(zhì)心任務(wù)、隊形任務(wù)。下面分別對這三項子任務(wù)進行任務(wù)處理。

3.1 避障任務(wù)

避障任務(wù)是優(yōu)先級最高的任務(wù),對于保證無人艇集群的完整性至關(guān)重要。欠驅(qū)動無人艇集群運動過程中可能遇到兩種障礙物第一種為環(huán)境中存在的礁石一類集群外障礙物,第二種為集群中除自身外的其它無人艇,因此欠驅(qū)動無人艇集群的避障任務(wù)不是一項集群任務(wù),而是每艘無人艇的單獨任務(wù)。

在前進方向存在障礙物時,無人艇避障的目的就是使無人艇與障礙物保持安全距離。因此,避障任務(wù)將無人艇——障礙物方向上的速度作為輸出,該速度保證無人艇與障礙物之間的距離不小于安全距離。

當(dāng)無人艇遇到多個障礙物時,將無人艇與障礙物之間距離作為避障優(yōu)先級排序依據(jù),距離最近的障礙物優(yōu)先級最高,分別計算無人艇對每個障礙物的避障速度。

設(shè)無人艇與障礙物之間距離為σo,對于形狀不規(guī)則的障礙物,σo表示無人艇距離障礙物質(zhì)心的距離,障礙物質(zhì)心坐標為

(8)

安全距離為d,則對第i優(yōu)先級的障礙物有

(9)

對應(yīng)的雅可比矩陣為

(10)

由上述條件可得,對于無人艇遇到的第i個障礙物的避障任務(wù)速度輸出為

(11)

其中λ為避障任務(wù)的增益矩陣。避障任務(wù)的零空間向量為

(12)

最終避障任務(wù)速度輸出

vo=v1+N1[v2+…+Ni-1vi]

(13)

3.2 質(zhì)心任務(wù)

質(zhì)心任務(wù)可以描述為欠驅(qū)動無人艇集群的質(zhì)心到達目標點。在二維水面的情況下,質(zhì)心定義如式(14)

(14)

根據(jù)先前的公式推導(dǎo),可以得出

(15)

其中雅可比矩陣Jb∈2×2n為

(16)

根據(jù)式(4)質(zhì)心任務(wù)的輸出函數(shù)是

(17)

3.3 隊形任務(wù)

由于零空間行為法存在編隊穩(wěn)定性不足的問題,因此本文采用虛擬結(jié)構(gòu)法完成無人艇集群隊形任務(wù)。將無人艇集群編隊視為一個虛擬剛體,編隊參考坐標系以集群質(zhì)心為原點,正北方向為y軸正方向,正東方向為x軸正方向。參考坐標系運動則編隊開始運動。因此隊形任務(wù)可視為無人艇運動到質(zhì)心的相對位置。任務(wù)函數(shù)定義為

(18)

其中pi是第i艘無人艇的坐標,pb=σb是無人艇集群質(zhì)心的坐標。

簡單起見,將向量寫為

[η1,1,…ηn,1,η2,1,…ηn,2]T

(19)

雅可比矩陣Jb∈2×2n為

(20)

其中Jb∈2×2n為

(21)

σf表示無人艇集群的隊形信息,一旦定義了隊形σf就代表每艘無人艇在質(zhì)心參考坐標系下的坐標。

在固定期望隊形的情況下,隊形任務(wù)的輸出為

(22)

3.4 任務(wù)綜合

在得到各個子任務(wù)的輸出函數(shù)后,為避免低優(yōu)先級任務(wù)與高優(yōu)先級任務(wù)之間的沖突,根據(jù)子任務(wù)優(yōu)先級順序,將隊形任務(wù)向量向質(zhì)心任務(wù)零空間投影,得到隊形任務(wù)與質(zhì)心任務(wù)的綜合輸出,再將該向量投影到避障任務(wù)的零空間,得到最終的速度輸出向量。任務(wù)綜合后無人艇姿態(tài)示意圖如圖2。

圖2 無人艇集群編隊運動姿態(tài)示意圖

根據(jù)式(6)以及上述子任務(wù)輸出函數(shù)可以得到期望速度與期望角度如下

vx,y=vo+N1(vb+N2vf)

(23)

(24)

(25)

4 仿真試驗

通過以上分析,本文使用MATLAB環(huán)境與基于QT Creat環(huán)境的無人艇集群控制系統(tǒng)軟件作為實船仿真平臺,驗證本文所述方法的有效性,其中基于QT Creat環(huán)境的系統(tǒng)仿真軟件模擬真實無人艇集群試驗效果。

4.1 Matlab仿真結(jié)果

為測試無人艇集群編隊功能,設(shè)置三艘無人艇組成無人艇集群,分別進行一字隊形編隊與三角隊形編隊,在仿真環(huán)境中設(shè)置三個半徑不同的靜態(tài)障礙物,此時可以認為對于集群中的單艘無人艇來說,環(huán)境內(nèi)存在兩個動態(tài)障礙物(即集群內(nèi)其余兩艘無人艇)。

無人艇集群初始化參數(shù)見表1,仿真環(huán)境信息見表2。

表1 無人艇參數(shù)初始化

表2 仿真環(huán)境信息

4.1.1 一字隊形

無人艇集群一字隊形仿真結(jié)果如圖4所示,圖5為無人艇集群角度圖像。整個運動過程完成時間為102s,開始運動時,無人艇集群首先完成隊形任務(wù),到達指定編隊位置后集群繼續(xù)向目標點前進。第20s時無人艇1、2分別遇到障礙物,當(dāng)障礙物中心與無人艇距離小于安全距離時,觸發(fā)避障任務(wù)無人艇1、2開始自適應(yīng)調(diào)整速度與角度完成避障在避障過程中,編隊隊形會被短暫破壞,但是能夠保證無人艇編隊安全航行。避障任務(wù)完成后,無人艇集群會迅速恢復(fù)編隊隊形。第48s時無人艇1遇到第二個障礙物,再次觸發(fā)避障任務(wù),無人艇1繼續(xù)調(diào)整速度與角度。此時其余兩艘無人艇雖然沒有遇到障礙物,但是由于無人艇1避障隊形發(fā)生改變導(dǎo)致集群質(zhì)心偏離期望航線,其余兩艘無人艇也會產(chǎn)生較小的角度變化,直至無人艇1避障完成,質(zhì)心回歸期望航線,集群恢復(fù)編隊隊形并且保持隊形到達目標點。

圖4 一字隊形編隊

圖5 一字隊形角度圖像

4.1.2 三角隊形

三角隊形仿真結(jié)果如圖6所示,角度變化圖像如圖7。由圖6可以看出,欠驅(qū)動無人艇集群首先調(diào)整角度完成隊形任務(wù),第15s無人艇2最先遇到障礙物,開始避障任務(wù),避障過程花費7s,無人艇1在第26s遇到障礙物,避障過程6s,避障成功后集群恢復(fù)編隊。第57s無人艇1遇到第二個障礙物,避障時間為8s。第110s質(zhì)心到達目標點,無人艇集群停止運動。

圖6 三角隊形編隊結(jié)果

圖7 三角隊形角度圖像

通過對以上MATLAB仿真結(jié)果分析可知,本文所述算法在理想狀況下能夠完成不同隊形的編隊任務(wù),且隊形的改變不會影響避障效果,保障了避障任務(wù)的最高優(yōu)先級,集群能夠在質(zhì)心到達目標點時停止。

4.2 水面無人艇集群控制系統(tǒng)仿真結(jié)果

上述仿真驗證了本文所述算法在理想條件下的有效性(即每艘無人艇可以在下一時刻調(diào)整為期望的速度與航向角,到達期望位置),但真實的欠驅(qū)動水面無人艇存在角度調(diào)整較慢,且為非線性加速,容易出現(xiàn)實際角度跟不上期望角度的問題。因此,為驗證本算法在實船環(huán)境下的有效性,本文選擇在完成MATLAB仿真后,使用基于QT Creat環(huán)境的欠驅(qū)動無人艇集群控制系統(tǒng)軟件進行更為真實的仿真,仿真界面如圖8所示,輸入無人艇參數(shù)見表3。

表3 無人艇參數(shù)

圖8 仿真系統(tǒng)顯示界面部分截圖

在欠驅(qū)動無人艇集群控制軟件中,無人艇集群初始化參數(shù)見表4,仿真環(huán)境信息見表5。考慮到實際水面無人艇存在欠驅(qū)動性,當(dāng)無人艇遇到障礙物航向角發(fā)生大角度變化時,無人艇航向角存在跟隨延時,因此設(shè)置安全距離略大于障礙物半徑,給予無人艇較大避障空間保證無人艇集群避障安全。

表4 無人艇仿真參數(shù)初始化

表5 仿真環(huán)境信息

圖9、10為實船仿真中一字隊形編隊仿真路線圖與無人艇2角度跟隨誤差圖像。圖11、12為三角隊形編隊仿真路線圖和無人艇2角度跟隨誤差圖像。

圖9 一字隊形仿真路線

圖10 一字隊形角度跟隨圖像

圖11 三角隊形仿真路線

圖12 三角隊形角度跟隨圖像

由圖9、11可以看出,不同的編隊隊形對無人艇的避障路線選擇會有影響但對避障任務(wù)的完成效果沒有影響,無人艇集群在不同隊形約束下都能夠完成避障任務(wù),質(zhì)心任務(wù)以及隊形任務(wù)。由圖10、12分析可知,在遇到障礙物時,無人艇航向角變化雖然不能實時達到期望角度,但是跟隨狀況理想,能夠保證在限制水域內(nèi),完成避障任務(wù)。

5 結(jié)語

本文基于零空間行為法,研究分析了欠驅(qū)動無人艇集群編隊避障問題,零空間行為法可以有效避免無人艇集群處理復(fù)雜任務(wù)時各任務(wù)之間的沖突問題。針對無人艇編隊避障問題,本文將該任務(wù)分解為三個優(yōu)先級不同的子任務(wù),并建立相應(yīng)的任務(wù)輸出函數(shù),得到無人艇集群整體的任務(wù)輸出模型。針對零空間行為法存在的無人艇集群編隊穩(wěn)定性差的問題,引入虛擬結(jié)構(gòu)法,將無人艇集群質(zhì)心作為編隊坐標原點,集群內(nèi)各艘無人艇只需與質(zhì)心保持相對坐標即可完成編隊。最后通過兩種仿真,驗證了算法的有效性與可行性,結(jié)果表明:對于不同隊形要求的欠驅(qū)動無人艇集群,均可順利完成集群編隊,并在運動過程中避開環(huán)境中存在的障礙物,最終到達目標點。