欠驅(qū)動(dòng)無(wú)人艇集群協(xié)同編隊(duì)路徑規(guī)劃仿真

王藝凱,孫 俊,王 偲

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

1 引言

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

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

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

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

2 零空間行為法建模

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

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

σ=f(r)

(1)

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

(2)

圖1 無(wú)人艇集群運(yùn)動(dòng)模型

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

在無(wú)人艇運(yùn)動(dòng)過(guò)程中通過(guò)求取最小范數(shù)速度,從而得到最小二乘解

(3)

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

(4)

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

(5)

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

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

(6)

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

(7)

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

3 任務(wù)處理

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

3.1 避障任務(wù)

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

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

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

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

(8)

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

(9)

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

(10)

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

(11)

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

(12)

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

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

(13)

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

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

(14)

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

(15)

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

(16)

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

(17)

3.3 隊(duì)形任務(wù)

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

(18)

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

簡(jiǎn)單起見(jiàn),將向量寫為

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

(19)

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

(20)

其中Jb∈2×2n為

(21)

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

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

(22)

3.4 任務(wù)綜合

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

圖2 無(wú)人艇集群編隊(duì)運(yùn)動(dòng)姿態(tài)示意圖

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

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

(23)

(24)

(25)

4 仿真試驗(yàn)

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

4.1 Matlab仿真結(jié)果

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

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

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

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

4.1.1 一字隊(duì)形

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

圖4 一字隊(duì)形編隊(duì)

圖5 一字隊(duì)形角度圖像

4.1.2 三角隊(duì)形

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

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

圖7 三角隊(duì)形角度圖像

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

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

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

表3 無(wú)人艇參數(shù)

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

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

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

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

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

圖9 一字隊(duì)形仿真路線

圖10 一字隊(duì)形角度跟隨圖像

圖11 三角隊(duì)形仿真路線

圖12 三角隊(duì)形角度跟隨圖像

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

5 結(jié)語(yǔ)

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