一種用于群無人艇避碰的動態(tài)分組策略

王玉龍, 馬 勇

(武漢理工大學(xué) a. 航運學(xué)院， 武漢 430063； b. 內(nèi)河航運技術(shù)湖北省重點實驗室， 武漢 430063)

隨著智能控制、動態(tài)軌跡規(guī)劃等新興技術(shù)的發(fā)展，水面無人艇(Unmanned Surface Vehicle，USV)已具備了一定的自主航行能力，在海洋科考、海事管理和水上搜救等領(lǐng)域發(fā)揮了重要作用。[1]多年來以單個無人艇為中心的自主避碰已經(jīng)取得一定的研究成果[2]，但是在群無人艇避碰方面的研究尚存在諸多問題需要解決。

針對避碰過程中需考慮的因素，研究者從船舶避碰的經(jīng)驗中抽象出各種概念并加以量化運用。文獻[3]在遵守國際海上避碰規(guī)則(International Regulations for Preventing Collisions at Sea, COLREG)的基礎(chǔ)上分析航行規(guī)則。文獻[4]利用航速、航向等計算碰撞危險度。文獻[5]對船舶領(lǐng)域進行劃分并分析會遇態(tài)勢。文獻[6]采用KT方程作為船舶運動控制模型，為應(yīng)對復(fù)雜的碰撞環(huán)境，考慮操縱性能并使用PID(Proportion Integral Differential)算法作為控制策略。文獻[7]在矢量障礙物算法的基礎(chǔ)上提出最優(yōu)化互利避碰算法，進而計算出最佳的轉(zhuǎn)向角度。為進一步找出最優(yōu)的避碰路徑。文獻[8]使用蟻群優(yōu)化算法規(guī)劃靜態(tài)的避碰路徑。文獻[9]使用多目標非線性優(yōu)化方法求解最優(yōu)路徑，但是傳統(tǒng)的避碰方法存在諸多缺陷，首先靜態(tài)的路徑規(guī)劃不能適應(yīng)多變的避碰環(huán)境，其次沒有考慮避碰對象的能動性。為克服傳統(tǒng)避碰方法的缺陷，研究者從行為控制的角度研究，基于行為控制表示導(dǎo)航和任務(wù)行為[10]找出最優(yōu)的避碰方案。文獻[11]使用模糊推斷的方法預(yù)測船舶一段時間后的運動軌跡。文獻[12]采用分布式隨機搜索算法結(jié)合時間窗來預(yù)測船舶的航行狀態(tài)。當前諸多研究中僅考慮一般態(tài)勢下的無人艇避碰問題，較少關(guān)注群無人艇的避碰問題；在無人艇的態(tài)勢劃分、操縱性和航行規(guī)則應(yīng)用等方面有一定的改進空間；同時，設(shè)計的避碰算法尚未體現(xiàn)出群無人艇避碰問題中運動障礙物的能動性等實際情況。

針對無人艇避碰中存在的相關(guān)不足，本文擬提出一種動態(tài)分組策略以有效地解決群無人艇避碰問題。主要工作包括：

1) 在建立無人艇控制模型、避碰規(guī)則模型后，提出解決群無人艇復(fù)雜會遇局面下的避碰問題。

2) 提出動態(tài)分組策略，考慮交通流的特性并對無人艇行為預(yù)測，實現(xiàn)群無人艇的動態(tài)分組。

3) 開展多種場景下群無人艇避碰仿真試驗，證實動態(tài)分組策略的有效性。

1 無人艇避碰規(guī)則建模

USV是指具有自主航行能力并且操縱性良好的一種無人艇。為實現(xiàn)避碰過程中的計算，建立無人艇艇體坐標系見圖1。

圖1中無人艇具有運動屬性航向C0、航速V0和舵角δ0，以無人艇為中心，將直角坐標系和極坐標系融合，無人艇航向作為Oy軸，與其垂直的右正橫方向作為Ox軸，從航向開始順時針旋轉(zhuǎn)的極角作為相對方位角θ0。

無人艇航向操縱使用比例積分微分PID算法控制，具有通過控制舵角實現(xiàn)快速調(diào)整航向的能力。根據(jù)野本謙作方程和PID控制算法，可推導(dǎo)出離散化操舵公式為

e(k)=r(k)-ψ(k)

(1)

(2)

(3)

δk=δk-1+Δu(k)

(4)

式(1)～式(4)中：e(k)為第k次航向偏差值；r(k)為目標航向；ψ(k)為第k次航向角；K0、T0為旋回性系數(shù)和穩(wěn)定性系數(shù)；δ0為初始舵角；T為采樣周期；Δu(k)為第k次舵角增量；δk為第k次舵角。

根據(jù)無人艇航向自主控制公式，可計算其轉(zhuǎn)向進距Ad和轉(zhuǎn)向橫距Td(見圖2)。用Ck表示第k次采樣得到的無人艇航向，設(shè)初始航向為C0，在第n次采樣時航向達到目標航向Cn，由式(2)和式(3)可推導(dǎo)出Ad、Td。

Ck=Ck-1+ψ(k)

(5)

(6)

(7)

為使無人艇滿足自身的操縱性能同時遵守COLREGs，將會遇態(tài)勢進一步量化，參考英國航海協(xié)會推薦的雷達避碰操縱方法[13]在艇體坐標系下將會遇態(tài)勢劃分對遇、交叉相遇和追越等3種，效果見圖3。

圖3 無人艇艇體坐標系下的會遇態(tài)勢劃分

同時，為判斷無人艇間的碰撞危險，將無人艇轉(zhuǎn)向進距的2倍作為無人艇危險距離S，即S=2Ad，Td保證無人艇轉(zhuǎn)向過程中能安全駛過，并且考慮無人艇屬性艇長L和艇寬W，將無人艇長度L的2倍[14]作為最小安全距離Dm。在COLREGS的基礎(chǔ)上對無人艇做出以下操縱約束[15]。

1) 對于分到同一組的無人艇做同組計算。無人艇在航行過程中考慮同組無人艇密度ρ=A/d，其中A表示同組無人艇所占區(qū)域面積，d表示同組中無人艇的平均間距，如果ρ>L2/Dm，則無人艇應(yīng)當調(diào)整航速小于等于前方無人艇，使其能在給定的前進方向上不與前方無人艇形成最小安全距離Dm。

2) 避碰中采取動作的原則。無人艇根據(jù)航向、航速和周邊無人艇的位置關(guān)系調(diào)整避碰動作，在保證最小安全距離Dm的同時，盡可能采取大幅度動作避碰。

3) 無人艇會遇局面，在無人艇接近到碰撞危險距離s時判定有碰撞危險，會遇局面形成。

(1) 對遇局面。運動目標在艇艏向(-5°，5°]范圍時，認為處于對遇局面，向右轉(zhuǎn)向避碰；

(2) 交叉相遇局面。運動目標在舷角(5° 90°]和(247.5°，355°]時，向右轉(zhuǎn)向避碰，運動目標在舷角(90°，112.5°]時向左轉(zhuǎn)向避碰；

(3) 追越。避碰過程中禁止主動追越，應(yīng)當調(diào)整航速小于等于前方無人艇。同時，運動目標在舷角(112.5°，210°]時向左轉(zhuǎn)向避碰;運動目標在舷角(210°，247.5°]時，向右轉(zhuǎn)向避碰。

2 多目標動態(tài)分組策略

水上交通流是在水路交通中表現(xiàn)出來的類似于流體特征的交通工具運動總體。[16]無人艇作為水上交通流的組成部分，在交通流影響下具有相似的個體行為特征，分組算法基于交通流宏觀思想，結(jié)合幾何分析和聚類算法對群無人艇進行分組，實現(xiàn)快捷、高效的避碰。

圖4 全局坐標系x0O0y0中群無人艇位置關(guān)系

在全局坐標系x0Ο0y0中有n艘無人艇(見圖4)，其艇長均為L，艇寬均為W，第i艘無人艇為USVi，運動屬性有航行方向Ci，前進速度Vi，映射到Ο0x0軸的坐標為posxi、映射到O0y0軸的坐標為posyi。

實際中USV獲取的是周邊無人艇/船舶相對位置關(guān)系，所以需要將全局坐標USV轉(zhuǎn)換成艇體坐標系中，進而分析本艇USV0與周邊無人艇/船舶的關(guān)系(見圖5)。艇體坐標系中USVi屬性[Xpi,Ypi]表示位置，ci表示航向。vi表示航速，轉(zhuǎn)換式(8)～式(10)如下，將無人艇/船舶按照轉(zhuǎn)換公式轉(zhuǎn)換到艇體坐標系xOy中，有

[XpiYpi]=[posxi-posx0posyi-posy0]

(8)

ci=Ci-C0

(9)

vi=Vi

(10)

在艇體坐標系中可得出USVi的相對方位角θi。

圖5 轉(zhuǎn)換后USV0與其他無人艇位置關(guān)系

無人艇在同一交通流中具有相似的航行行為，具體表現(xiàn)在位置相關(guān)度高及航行軌跡相似。據(jù)此原理可推導(dǎo)出無人艇相似度μ的表達式為

μ=

(11)

式(11)中：xpi、xpj為無人艇USVi、USVj的Ox軸坐標；ypi、ypj為無人艇USVi、USVj的Oy軸坐標；ci、cj為無人艇USVi、USVj的航向；vi、vj為無人艇USVi、USVj的航速。

根據(jù)轉(zhuǎn)換后的位置關(guān)系，可得出周邊無人艇可分為一組的最大相似度值μmax為

(12)

式(12)中：α為相似航向角最大值，最小相似度值μmin為

(13)

計算無人艇USVi與周無人艇USVj的相似度為

(14)

式(14)中：0≤μij≤1，表明USVi、USVj具有相似的行為屬性而分為一組，if USVi～USVjand USVj～USVkthen USVi～USVk。

按照相似度計算公式對周邊無人艇進行分組，分為G1,G2,…，Gi,…，Gm共m個分組，通過凸包掃描算法可求取各分組邊界。

1) 將周邊USV按照相對方位角θi排序。

2) 根據(jù)式(14)將群無人艇中各無人艇分到不同的組Gi中。

3) 求取凸包邊界點序列H，將分組中的USV看作質(zhì)點，根據(jù)Graham算法依次判斷是否加入凸包棧H中，最終得到凸包點序列。

由此，根據(jù)上述分組策略，得到分組聚類效果示意見圖6。

群無人艇的運動既促成交通流的形成，同時其操縱行為又會對現(xiàn)有的交通流造成影響。為能夠更好地做出正確決策，需要對無人艇/船舶運動軌跡做出預(yù)測，前向預(yù)測根據(jù)航行規(guī)則和當前運動狀態(tài)，可計算出時間段Δt后無人艇的運動狀態(tài)和位置變化。設(shè)無人艇在時間t的狀態(tài)屬性是Q(t)，無人艇在不同會遇態(tài)勢下所做的操縱可表示為R(t)，由此可得出Δt后無人艇的運動狀態(tài)。

(15)

(16)

式(16)中:t0為t0時刻;θ為USV相對方位角;M(Q(t),R(t))表示USV在t時刻的運動狀態(tài)和航行規(guī)則時做出的定性操縱決策。

在第1節(jié)的避碰規(guī)則下，交差相遇局面的無人艇會逐漸演變?yōu)閷τ鼍置?，追越局面因其中同組安全航速規(guī)則而消除碰撞危險，對遇局面成為最難判斷的局面，所以對于對遇局面的前向預(yù)測需要進一步分析?，F(xiàn)根據(jù)前向預(yù)測來分析無人艇對遇局面的狀態(tài)變化，將兩組之間狀態(tài)的預(yù)測進行劃分，得出式(17)的3種情況(見圖7)，圖7中分別為分組G1、G2，其位置水平分量為PG1(x)、PG2(x)，速度水平分量為VG1(x)、VG2(x)。

(17)

根據(jù)圖7的狀態(tài)劃分，可計算獲得兩兩分組G1、G2間的可行角度ξ，進而為群無人艇做出可信的操縱決策。

1) 當G1、G2呈現(xiàn)聚合運動趨勢時，計算G1、G2各自凸包邊界無人艇的最近會遇距離(Distance to Closest Point of Approach, DCPA)，設(shè)最小|dCPA|一側(cè)的相對方位角為θ，則可行轉(zhuǎn)向角度ξ=θ。

2) 當G1、G2呈現(xiàn)分散運動趨勢時，計算G1、G2的最小會遇時間(Time to Closest Point of Approach, TCPA)，取最小tCPA作為預(yù)測時間間隔Δt，計算G1、G2在Δt時間后的最小夾角(ξ1,ξ2)，則ξ= (ξ1+ξ2)/2。

3) 當G1、G2呈現(xiàn)同向運動趨勢時， 計算G1、G2凸包邊界無人艇的DCPA，取形成最小|dCPA|無人艇的相對方位角為θ，則可行轉(zhuǎn)向角度ξ=θ。

根據(jù)分組策略將無人艇分成多股交通流，結(jié)合航行規(guī)則并利用前向預(yù)測對無人艇的運動狀態(tài)做出一段時間的預(yù)測。在此基礎(chǔ)上，依據(jù)速度矢量計算[7]可得出最終的轉(zhuǎn)向角度，解決群無人艇避碰問題。動態(tài)分組策略下的群無人艇避碰決策過程見圖8。

3 群無人艇避碰仿真

在全局坐標系x0O0y0下開展多種場景下的群無人艇避碰仿真，借鑒文獻[17]中的仿真分析模式，驗證本文提出的動態(tài)分組策略的有效性。

圖8 動態(tài)分組策略下的群無人艇避碰決策過程

3.1 無人艇屬性定義

無人艇艇長L=3 m，艇寬W=1 m，操縱性KT指數(shù)K0=2，T0=5，PID控制算法參數(shù)Kp=0.8，Ki=5，Kd=10，可計算出群無人艇間最小安全距離Dm= 2L= 6 m。使用此無人艇在多種場景下進行如下仿真。

3.2 多場景下的群無人艇避碰仿真

3.2.1一般會遇態(tài)勢下的群無人艇避碰仿真

為明確本文方法的優(yōu)越性，特選擇文獻[4] 中5艘USV避碰的案例進行對比，還考慮了USV操縱性、避碰規(guī)則以及危險距離等因素，在此條件下應(yīng)用本文的避碰方法進行仿真測試。群無人艇的初始參數(shù)見表1，5艘USV形成復(fù)雜的多艇會遇態(tài)勢，包含了對遇、追越、交叉相遇等3種會遇情況，測試結(jié)果見圖9。

表1 仿真案例1中的5艘無人艇初始參數(shù)

b)時段1～125 s的無人艇軌跡

c) 航向C歷時曲線

d) 舵角δ歷時曲線

e) 速度V歷時曲線

f) USV1與其它無人艇間距變化歷時曲線

圖9 5艘USV避碰過程參數(shù)變化組圖

從結(jié)果中可看出：

(1) 從本案例結(jié)果與文獻[4]的仿真結(jié)果來看，本案例避碰的動作更加自然。從圖9 a可知，起初5艘無人艇處于復(fù)雜的會遇態(tài)勢之中，但是經(jīng)過40 s后，本文的避碰算法使得整體的態(tài)勢發(fā)生了轉(zhuǎn)變，USV1、USV2與USV4形成一股交通流，USV3和USV5則按照避碰規(guī)則向右轉(zhuǎn)向，使得會遇態(tài)勢迅速簡化。相對于文獻[4]的結(jié)果，本文不僅考慮無人艇之間的態(tài)勢變化，而且兼顧航速航向，如圖9 e速度的變化，由于分組的影響，最終速度趨于一致，同時航向也有接近的趨勢，總體的避碰決策隨著周邊態(tài)勢的變化而調(diào)整；

(2) 仿真結(jié)果表明在經(jīng)過125 s后，5艘USV都能安全避碰并且駛過，同時本文中的避碰動作更加自然流暢，想比于文獻[4]中的規(guī)劃路線，本文避碰過程中不斷調(diào)整航向和航速，針對態(tài)勢的變化調(diào)整避碰決策，能夠應(yīng)對復(fù)雜多變的避碰環(huán)境。

3.2.2多組復(fù)雜交通流場景下的群無人艇避碰仿真

為進一步證實動態(tài)分組策略能夠應(yīng)對更為復(fù)雜的會遇態(tài)勢，突出對群無人艇避碰的優(yōu)勢，特針對多組群無人艇避碰進行了仿真演示?，F(xiàn)在仿真中初始化9艘USV，初始運動狀態(tài)見表2。

表2 案例2中的9艘無人艇初始狀態(tài)

由圖10可知：9艘USV自主分為3組并形成3股交通流，具有一致的避碰行為，USV1、USV2、USV3作為G1組和USV4、USV5、USV6為G2組均處在交叉相遇和對遇的態(tài)勢中， USV7、USV8、USV9為G3處在復(fù)雜交叉相遇態(tài)勢中，G1組向右轉(zhuǎn)向避讓，G2組小幅度操縱并最終和G3組形成一股交通流。86 s后會遇態(tài)勢簡化并且安全駛過。

3.3 仿真結(jié)果分析

對仿真結(jié)果進行分析后可知:案例1在一般態(tài)勢下能夠以最少的操縱方法避讓周邊無人艇/船舶，同時，協(xié)調(diào)周邊無人艇/船舶形成有規(guī)律的交通流，最終完成避碰。案例2在無人艇/船舶較多的情況下，交通流情形更為復(fù)雜，因此，可充分體現(xiàn)出動態(tài)分組策略在群無人艇避碰問題中的優(yōu)越性和有效性。動態(tài)分組策略充分考慮交通流的特性，將處于復(fù)雜態(tài)勢下的無人艇/船舶引導(dǎo)至有規(guī)律的交通流，進而有效解決群無人艇的避碰問題。

a) 時段1～40 s航行軌跡

b) 時段1～86 s航行軌跡

c) 航向C歷時曲線

d)舵角δ歷時曲線

e) 航速V歷時曲線

f) USV1與其他無人艇間距變化歷時曲線

圖10 9艘無人艇避碰過程參數(shù)變化組圖

4 結(jié)束語

本文提出解決群無人艇避碰問題的一種動態(tài)分組策略。在動態(tài)分組策略中，考慮群無人艇的運動特性及航行規(guī)則，實現(xiàn)對多個避碰目標的分組，同時形成有規(guī)律的交通流簡化會遇態(tài)勢，多個場景下的避碰仿真證實本文方法在群無人艇避碰問題中的有效性。

后續(xù)研究需要進一步考慮無人艇航跡恢復(fù)和風浪流等環(huán)境擾動避碰問題，以及構(gòu)建群無人艇試驗平臺來推廣本文的避碰方法在行業(yè)中的應(yīng)用。