基于改進(jìn)人工勢(shì)場(chǎng)法的多無(wú)人艇避障策略

鄒子理，孫騫，黃雨杰，李一兵

1. 哈爾濱工程大學(xué) 信息與通信工程學(xué)院，黑龍江 哈爾濱 150001

2. 哈爾濱工程大學(xué) 先進(jìn)船舶通信與信息技術(shù)工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001

水面無(wú)人艇（unmaned surface vehicle，USV）是一種通過(guò)遙控或自主方式在水面作業(yè)的智能化小型平臺(tái)，廣泛用于情報(bào)搜集、海上救援、反潛等任務(wù)，對(duì)于維護(hù)我國(guó)海上安全與海洋權(quán)益具有重大意義。無(wú)人艇實(shí)時(shí)路徑規(guī)劃是水面無(wú)人艇研究的重要內(nèi)容之一。路徑規(guī)劃算法可以分為全局規(guī)劃和局部規(guī)劃兩部分，其中經(jīng)典的全局路徑規(guī)劃方法包括A*算法、Dijkstra 算法、快速探索隨機(jī)樹（rapidly-exploring random trees，RRT）、柵格法等；局部規(guī)劃包括人工勢(shì)場(chǎng)法（artificial potential field，APF）、動(dòng)態(tài)窗口法（dynamic window approach，DWA）等[1]。全局規(guī)劃根據(jù)已知的全局環(huán)境信息，使用各種尋優(yōu)算法獲取全局最優(yōu)路徑，其能夠在任意不規(guī)則障礙物環(huán)境中計(jì)算出全局最優(yōu)解，但是存在計(jì)算量大、搜索效率低的缺陷，且對(duì)突發(fā)情況應(yīng)對(duì)能力不足，所以單純的全局規(guī)劃算法難以應(yīng)用于探索環(huán)境未知的海域。局部規(guī)劃根據(jù)智能體傳感器采集的實(shí)時(shí)障礙物與位置信息，獲取到目標(biāo)點(diǎn)的局部最優(yōu)路徑，優(yōu)點(diǎn)是實(shí)時(shí)性強(qiáng)、計(jì)算量小，能夠?qū)崿F(xiàn)對(duì)于突發(fā)事件的快速響應(yīng)，但是局部規(guī)劃容易陷入局部最優(yōu)解，降低USV 執(zhí)行任務(wù)的效率。

APF 由Khabit 于1986 年首次提出，其基本思想是仿照靜電場(chǎng)中電勢(shì)和電勢(shì)場(chǎng)的概念，在目標(biāo)點(diǎn)與障礙物位置處分別構(gòu)造引力場(chǎng)Uatt與斥力場(chǎng)Urep，其中，引力勢(shì)場(chǎng)將吸引USV 向目標(biāo)點(diǎn)移動(dòng)而斥力勢(shì)場(chǎng)幫助USV 實(shí)現(xiàn)障礙物規(guī)避。在勢(shì)場(chǎng)中，USV 視為一個(gè)質(zhì)點(diǎn)，其在當(dāng)前位置的合力作用下移動(dòng)一個(gè)指定單位步長(zhǎng)，到達(dá)下一位置后實(shí)時(shí)更新勢(shì)場(chǎng)，進(jìn)行下一步路徑規(guī)劃。APF 的顯著優(yōu)點(diǎn)之一是實(shí)時(shí)性強(qiáng)，能較好地應(yīng)對(duì)目標(biāo)的變化與環(huán)境中的動(dòng)態(tài)障礙物。但是在復(fù)雜地形多障礙物環(huán)境下若不同障礙物對(duì)USV 施加的斥力與目標(biāo)點(diǎn)引力的合力為0 時(shí)，USV 將陷入局部最優(yōu)解無(wú)法脫離導(dǎo)致路徑規(guī)劃失敗或目標(biāo)點(diǎn)不可達(dá)等問(wèn)題。同時(shí)APF 生成的規(guī)劃路徑曲率變化較大，路徑不平滑，對(duì)USV 機(jī)動(dòng)性提出挑戰(zhàn)[2]。

針對(duì)傳統(tǒng)APF 中存在的目標(biāo)點(diǎn)不可達(dá)、無(wú)效碰撞等問(wèn)題，通過(guò)對(duì)勢(shì)場(chǎng)函數(shù)進(jìn)行修正[3]，基于碰撞錐的分層人工勢(shì)場(chǎng)[4]等方法可以加以改進(jìn)。同時(shí)近年來(lái)許多工作在進(jìn)行USV 路徑規(guī)劃時(shí)考慮了國(guó)際海上避碰規(guī)則（International Regulations for Preventing Collisions at Sea，COLREGS）的要求[5?9]，但大多數(shù)是根據(jù)規(guī)則對(duì)斥力場(chǎng)函數(shù)分類討論，且對(duì)保持USV 編隊(duì)的規(guī)則情形較少關(guān)注。COLREGS規(guī)定了存在碰撞風(fēng)險(xiǎn)時(shí)應(yīng)采取的機(jī)動(dòng)類型，當(dāng)USV 在其他船只附近航行時(shí)必須遵守COLREGS，以便USV 安全避開其他海上智能體，同時(shí)給其他運(yùn)動(dòng)船只的決策提供可靠參考。

本文首先對(duì)經(jīng)典人工勢(shì)場(chǎng)法的障礙物形狀和斥力函數(shù)進(jìn)行改進(jìn)以平滑規(guī)劃路徑，以避障過(guò)程路徑消耗為指標(biāo)研究了基于改進(jìn)人工勢(shì)場(chǎng)法的多無(wú)人艇群靜態(tài)避障策略，提出一種進(jìn)行艇群會(huì)遇態(tài)勢(shì)判斷的艇群國(guó)際海上避碰規(guī)則（swarm-COLREGS，sCOLREGS），并結(jié)合改進(jìn)人工勢(shì)場(chǎng)法提出符合sCOLREGS 的偏置障礙物區(qū)域多USV動(dòng)態(tài)避障策略。最后通過(guò)仿真驗(yàn)證改進(jìn)APF 能有效平滑規(guī)劃路徑，同時(shí)證明了在特定環(huán)境下分散編隊(duì)避障將顯著降低路程消耗，所提出的偏置障礙區(qū)域算法能夠在保證實(shí)時(shí)性與安全性的情況下遵守國(guó)際海上避碰規(guī)則。

1 改進(jìn)人工勢(shì)場(chǎng)法

在經(jīng)典斥力場(chǎng)中，場(chǎng)函數(shù)是距離倒數(shù)的平方，這使得經(jīng)典APF 算法中的斥力場(chǎng)函數(shù)曲線升降過(guò)快，斥力大小在短距離內(nèi)顯著變化，導(dǎo)致規(guī)劃路徑曲率變化過(guò)大，對(duì)USV 轉(zhuǎn)向機(jī)動(dòng)性提出挑戰(zhàn)，同時(shí)造成額外的路徑消耗。

為平滑避障路徑，提高避障效率，本節(jié)通過(guò)增加緩沖區(qū)域使避障區(qū)域沿?zé)o人艇速度方向擴(kuò)展改進(jìn)斥力場(chǎng)形狀，同時(shí)設(shè)計(jì)平滑修正系數(shù)改進(jìn)斥力場(chǎng)函數(shù)。

1.1 障礙物區(qū)域建模

無(wú)人艇使用質(zhì)點(diǎn)模型建模，其運(yùn)動(dòng)狀態(tài)僅受虛擬勢(shì)場(chǎng)力及艇間通信影響，并將執(zhí)行任務(wù)環(huán)境抽象為二維平面，不考慮海浪、海流等因素的干擾。記障礙物在平面中占據(jù)的空間為點(diǎn)集Vobs。

將環(huán)境平面劃分為一系列適當(dāng)精度的正方形柵格 αi，若柵格屬于某一障礙物（島嶼、船只等），將其賦值為1，否則為0。當(dāng)劃分的柵格足夠小時(shí)，可近似將小柵格視為平面中的一點(diǎn)(x,y)，二值障礙物地圖如圖1 所示。

圖1 二值障礙物柵格地圖

1.2 改進(jìn)人工勢(shì)場(chǎng)構(gòu)建

考慮二維平面n艘無(wú)人艇集合S(n)={Si=(xi,yi)|i=1,2,···,n}， 記第i艘無(wú)人艇Si到目標(biāo)（goal）點(diǎn)g(xgoal,ygoal)的距離為則目標(biāo)點(diǎn)對(duì)Si產(chǎn)生的引力場(chǎng)Uatt為

式中 ε為對(duì)應(yīng)的引力相關(guān)正比例系數(shù)。則引力為

式中ni,g為Si指向目標(biāo)點(diǎn)的單位矢量。記Si到平面上一障礙物（obs）點(diǎn)αi(xobs,yobs)距離為

改進(jìn)后的斥力場(chǎng)由障礙物橢圓、禁止區(qū)、過(guò)渡區(qū)3 部分組成，如圖2 所示。其中 ρobs為不規(guī)則障礙物最小圓覆蓋半徑，表征障礙物大?。?ρ0為預(yù)設(shè)禁止距離，若USV 進(jìn)入此區(qū)域?qū)o(wú)法成功避碰； ρ1為過(guò)渡區(qū)半徑，當(dāng)USV 距障礙物為 ρ1時(shí)開始避碰，且從 ρ1降至 ρ0，斥力由0 以指數(shù)速度增至最大值。

圖2 改進(jìn)人工勢(shì)場(chǎng)法斥力分區(qū)

本文以ρobs為短軸長(zhǎng)度、 ρ0為長(zhǎng)軸長(zhǎng)度、USV 與障礙物中心連線為長(zhǎng)軸方向構(gòu)造改進(jìn)的橢圓形斥力場(chǎng)，通過(guò)對(duì)斥力場(chǎng)形狀進(jìn)行改進(jìn)，可以使無(wú)人艇較經(jīng)典方法提前進(jìn)行避障，提高航行安全性，同時(shí)也使路徑更為平滑，減小轉(zhuǎn)向角。

改進(jìn)后斥力場(chǎng)函數(shù)的數(shù)學(xué)表達(dá)式為

式中 λ為斥力場(chǎng)改進(jìn)系數(shù)，定義如下

式中： ηs為比例系數(shù)， γ為負(fù)實(shí)數(shù)，m為大于1 的實(shí)數(shù)，ρi1=|ρ1?ρ(i,obs)|， ρ01=|ρ1?ρ0|，由于ρ(i,obs)＞ρ0， 故在過(guò)渡區(qū)ρi1＜|ρ1?ρ0|=ρ01， 指數(shù)系數(shù)γ(1/?1/)＜0，由指數(shù)函數(shù)的特點(diǎn)知在(?∞,0]上 λ由0 隨ρ(i,obs)增速變化至 ηs，且變化較為緩慢，更容易控制。通過(guò)求解斥力場(chǎng)的梯度可以得到USV 所受斥力為

故無(wú)人艇在(xi,yi)處所受的虛擬勢(shì)場(chǎng)合力為

合勢(shì)力場(chǎng)形狀如圖3 所示。

圖3 改進(jìn)人工勢(shì)場(chǎng)法合勢(shì)力場(chǎng)形狀

2 路程評(píng)價(jià)的艇群分散避障策略

USV 在執(zhí)行任務(wù)過(guò)程中，可能會(huì)遇到大小尺寸不同的障礙物。USV 編隊(duì)在復(fù)雜地形環(huán)境中作業(yè)時(shí)，障礙物規(guī)避問(wèn)題成為USV 編隊(duì)需要面對(duì)的一大挑戰(zhàn)。由于復(fù)雜環(huán)境下障礙物數(shù)量與分布隨機(jī)，為保證安全性USV 編隊(duì)需要低速通過(guò)，故此時(shí)對(duì)于航線長(zhǎng)度的要求高于規(guī)劃路徑平穩(wěn)性，以降低避障過(guò)程能量消耗，提高USV 編隊(duì)續(xù)航能力[10]。本節(jié)首先介紹編隊(duì)控制方法，在此基礎(chǔ)上研究當(dāng)障礙物與編隊(duì)大小相當(dāng)時(shí)以編隊(duì)避障總路程為評(píng)價(jià)指標(biāo)的一種編隊(duì)變換避障策略。

2.1 基于一致性的一階分布式編隊(duì)控制

多無(wú)人艇編隊(duì)采用leader-follower 編隊(duì)形式[11]，以一虛擬領(lǐng)航者為中心，編隊(duì)成員均勻?qū)ΨQ分布于半徑為r的圓周。設(shè)編隊(duì)成員數(shù)為N的艇群為S(N)={Si|i=0,1,···,N}，其中S0為編隊(duì)虛擬領(lǐng)航者（Leader）。定義加權(quán)有向圖G=(V,E)表示編隊(duì)通信拓?fù)浣Y(jié)構(gòu)，其中節(jié)點(diǎn)集V=S(N)表示編隊(duì)成員，邊集E={eij=(Si,S j)}表示編隊(duì)成員i、j間是否存在通信關(guān)系，對(duì)應(yīng)的eij取值為1、0。定義鄰接矩陣

式中：當(dāng)eij=1時(shí)，aij=1；否則aij=0。本文編隊(duì)拓?fù)鋱D如圖4 所示。

圖4 編隊(duì)通信拓?fù)浣Y(jié)構(gòu)有向圖

定義入度矩陣

定義有向圖G的拉普拉斯矩陣為

式中L是半正定陣。

編隊(duì)一階線性連續(xù)控制方程為

式中：xi為狀態(tài)量，ui為如下線性輸入量。

使用Lyapunov 函數(shù)V=xTx/2研究此系統(tǒng)穩(wěn)定性。對(duì)狀態(tài)量求導(dǎo)，并利用L的半正定性可得

考慮=0的情形。注意到圖G聯(lián)通，即存在生成樹，故L有且僅有一個(gè)0 特征值，且其特征向量為1 向量[12]，即

由L的半正定性知xTLx=0與Lx=0n同解，所以x=x01n(x0∈R)，此時(shí)諸狀態(tài)量達(dá)到一致，且僅與L及初狀態(tài)有關(guān)。故當(dāng)狀態(tài)量不一致時(shí)由＜0狀態(tài)量將逐步收斂至x0(=0)，系統(tǒng)穩(wěn)定性得到證明。

離散時(shí)間情形下編隊(duì)一階控制方程為

編隊(duì)中跟隨者（follower）控制算法為

式中：ε ＞0為常數(shù)，N(i)為節(jié)點(diǎn)i的鄰居節(jié)點(diǎn)集合，dij為節(jié)點(diǎn)i、j間距離。

Leader 控制算法為第1 節(jié)所導(dǎo)的APF 合力，即

2.2 避障開始與結(jié)束規(guī)定

如圖5 所示，設(shè)障礙物最小覆蓋圓大小為R，單艇避碰過(guò)程中到障礙物圓的最小許可距離為D，當(dāng)虛擬領(lǐng)航者S0距離障礙物中心小于D0(＞R+D+r)時(shí)，開始避障程序。

圖5 避障開始與結(jié)束位置示意

記Si從開始避障位置相對(duì)障礙物中心駛過(guò)的角度為 θ，當(dāng)0°＜θ ＜90°時(shí)，USV 處于避障階段。當(dāng)θ=90°時(shí)，結(jié)束避障。

2.3 避障策略建模對(duì)比

若避障時(shí)USV 編隊(duì)保持結(jié)構(gòu)剛性，不進(jìn)行編隊(duì)變換，按照虛擬領(lǐng)航者所受人工勢(shì)場(chǎng)力進(jìn)行路徑規(guī)劃，整體從障礙物一側(cè)繞過(guò)，這一策略的示意如圖6 所示。

圖6 策略1 示意

避障總路程與最小許可距離D有關(guān)，設(shè)此函數(shù)關(guān)系為f(D)。

在上述策略中，由編隊(duì)隊(duì)形的對(duì)稱性，當(dāng)艇群成員數(shù)j為偶數(shù)時(shí)，在虛擬領(lǐng)航者兩側(cè)的一對(duì)無(wú)人艇行駛的總路程可以等效為虛擬領(lǐng)航者行駛總路程的2 倍，故所有編隊(duì)成員避障總路程為虛擬領(lǐng)航者的2×j/2=j倍；當(dāng)j為奇數(shù)時(shí)，設(shè)第1 艘無(wú)人艇在虛擬領(lǐng)航者正前方，則此艘艇行駛總路程與虛擬領(lǐng)航者相等，對(duì)其余j?1（偶數(shù)）艘無(wú)人艇，化歸為上述偶數(shù)情形，故所有編隊(duì)成員避障總路程為虛擬領(lǐng)航者的1+(j?1)=j倍。編隊(duì)成員數(shù)量為奇數(shù)和偶數(shù)時(shí)的隊(duì)形對(duì)稱等效示意如圖7 所示。

圖7 USV 編隊(duì)對(duì)稱性分析

綜合2 種情形，無(wú)人艇編隊(duì)避障總路程消耗為

在不考慮編隊(duì)變換的情況下，USV 編隊(duì)將保持當(dāng)前結(jié)構(gòu)從障礙物短側(cè)繞行，這種策略不僅導(dǎo)致USV 編隊(duì)的總體能量消耗提升，不利于USV 編隊(duì)進(jìn)行長(zhǎng)時(shí)間作業(yè)，而且會(huì)增大與緊鄰障礙物碰撞的風(fēng)險(xiǎn)，降低USV 航行的安全性。

下面考慮編隊(duì)分散的避障策略。當(dāng)虛擬領(lǐng)航者S0距離障礙物中心小于D0時(shí)，開始避障程序。此時(shí)編隊(duì)隊(duì)形不再保持，各艇按照距離障礙物最近距離，由小到大開始進(jìn)入避障程序，目標(biāo)點(diǎn)為引力點(diǎn)，障礙物及其他艇為斥力點(diǎn)形成的人工勢(shì)場(chǎng)中進(jìn)行路徑規(guī)劃，如圖8 所示。

圖8 編隊(duì)分散避障策略示意

上述策略中無(wú)人艇編隊(duì)避障總路程消耗為

將f(D+r)在D處進(jìn)行泰勒展開，得

當(dāng)R足夠大時(shí)，避碰段曲線f近似為二次曲線，可認(rèn)為f(i)(D)=0，i≥3。

由式(1)可知2 種策略的路程比僅與編隊(duì)半徑r、障礙物覆蓋圓半徑R(f與R有關(guān))以及安全距離D相關(guān)。對(duì)改進(jìn)APF 橢圓形斥力場(chǎng)，記安全距離為 ρ0，對(duì)1/4 橢圓弧長(zhǎng)，有如下Ramanujan 近似公式[13]：

式中λ=(ρ0?R)/(ρ0+R+2D)。對(duì)變量D求偏導(dǎo)，有λ′=?2λ2/(ρ0?R) ，λ′′=8λ3/(ρ0?R)2。

由于(ρ0?R)?(ρ0+R+2D)，所以λ ≈0，故

所以

略去 λ的高階無(wú)窮小項(xiàng)，得

綜上所述，在改進(jìn)橢圓形人工勢(shì)場(chǎng)情形下，當(dāng)編隊(duì)大小r與障礙物大小R在同一數(shù)量級(jí)時(shí)，避障總路程之比d1/d2將會(huì)顯著大于1，r與R的比值越大，2 種策略的路程比也越大，此時(shí)采用第2 種策略將大大降低USV 編隊(duì)的能量消耗，提升編隊(duì)的續(xù)航能力。

3 符合COLREGS 的偏置人工勢(shì)場(chǎng)法

本節(jié)考慮艇群與動(dòng)態(tài)障礙物(如船只)會(huì)遇的局面。通過(guò)速度障礙法判斷碰撞風(fēng)險(xiǎn)，按照艇群避碰需求對(duì)COLREGS 對(duì)遇、右交叉、左交叉3 種會(huì)遇態(tài)勢(shì)進(jìn)行修正，提出艇群國(guó)際海上避碰規(guī)則，提出一種基于改進(jìn)APF 的偏置障礙區(qū)域避碰方法。

3.1 速度障礙法避碰判斷

速度障礙法(velocity obstacle，VO)常用于無(wú)人艇緊急避碰中[14]。假設(shè)無(wú)人艇當(dāng)前的位置為PS i，速度為vS i。將障礙物抽象為一圓形區(qū)域，障礙物中心位置為Pobs，速度為vobs，無(wú)人艇與障礙物圓的2 條切線分別為T1、T2。如圖9 所示，兩者的相對(duì)速度為vuo=vS i?vobs，若max{〈vuo,T1〉,〈vuo,T2〉}≤〈T1,T2〉成立，則認(rèn)為可能碰撞，此時(shí)無(wú)人艇速度vS i稱為一個(gè)速度障礙。所有速度障礙組成的集合可以表示為

圖9 速度障礙法示意

當(dāng)速度vS i∈VVO時(shí)，為保證航行的安全性USV 優(yōu)先執(zhí)行避障任務(wù)；當(dāng)vS i?VVO時(shí)認(rèn)為此時(shí)刻無(wú)碰撞風(fēng)險(xiǎn)，USV 保持原方向與速度繼續(xù)航行。

3.2 國(guó)際海上避碰規(guī)則(COLREGS)

自1972 年提出以來(lái)，國(guó)際海上避碰規(guī)則一直是航海實(shí)踐中會(huì)遇態(tài)勢(shì)判斷與避碰策略選擇的重要依據(jù)。對(duì)于兩艇會(huì)遇局面下的COLREGS 遵守，已有較多的工作[15?16]。然而當(dāng)USV 通信距離受限，需要在保持編隊(duì)的情況下進(jìn)行動(dòng)態(tài)避障時(shí)，上述方法將難以在保證安全性的前提下嚴(yán)格遵守COLREGS。本節(jié)根據(jù)艇群避碰特點(diǎn)，提出適用于艇群與動(dòng)態(tài)障礙物會(huì)遇的sCOLREGS。

首先介紹COLREGS 對(duì)兩單艇會(huì)遇態(tài)勢(shì)與避碰責(zé)任的規(guī)定。如圖10 所示，定義會(huì)遇角φi∈(0°,360°)為本船與障礙船質(zhì)心連線與本船速度方向所成角[17]。在有碰撞風(fēng)險(xiǎn)的情況下，依照國(guó)際海上避碰規(guī)則指導(dǎo)，以α1=5°、α1=112.5°為閾值，按 φi的大小及船速比將本船采取的避障策略分為表1 中的5 種情況，對(duì)應(yīng)的策略示意如圖11 所示。

表1 會(huì)遇態(tài)勢(shì)判斷表

圖10 COLREGS 會(huì)遇態(tài)勢(shì)判斷

圖11 COLREGS 避碰策略示意

對(duì)于艇群與動(dòng)態(tài)障礙物會(huì)遇態(tài)勢(shì)判斷，由于一方從單艇膨脹為艇群，上述規(guī)則不能直接應(yīng)用。為此，分別以USV 編隊(duì)虛擬leaderS0（編隊(duì)方）與動(dòng)態(tài)障礙船Sobs（單艇方）為參考，定義如下判斷準(zhǔn)則sCOLREGS。

對(duì)于編隊(duì)方，定義 α?前向區(qū)域?yàn)榫庩?duì)沿速度方向前進(jìn)時(shí)覆蓋的區(qū)域，其中 α為區(qū)域與速度方向順時(shí)針夾角。圖12 中射線l1、l2形成的區(qū)域即為此時(shí)編隊(duì)的 0°?前向區(qū)域。

圖12 sCOLREGS 編隊(duì)方會(huì)遇態(tài)勢(shì)判斷

記編隊(duì)的α1?前向區(qū)域與(?α1)?前向區(qū)域的邊界l4、l6所夾區(qū)域?yàn)?Ω1，如圖12(a)所示。編隊(duì)的α1?前向區(qū)域與 α2前向區(qū)域的邊界l4、l8所夾區(qū)域?yàn)棣?，如圖12(b)所示。當(dāng)Sobs∈Ω1時(shí)，認(rèn)為此時(shí)相當(dāng)于COLREGS 中的對(duì)遇局面；當(dāng)Sobs∈Ω2時(shí)，認(rèn)為此時(shí)相當(dāng)于COLREGS 中的右交叉局面；當(dāng)Sobs∈時(shí)，認(rèn)為此時(shí)相當(dāng)于COLREGS 中的左交叉局面。

對(duì)于單艇方，以區(qū)域編隊(duì)成員最大累計(jì)數(shù)為指標(biāo)進(jìn)行會(huì)遇態(tài)勢(shì)劃分。以Sobs為 中心將平面按與速度夾角為(360°?α1,360°)∪(0°,α1),(α1,α2),(α2,360°?α1)分為Ω1、Ω2、Ω3共3 部分，如圖13 所示。

圖13 sCOLREGS 單艇方會(huì)遇態(tài)勢(shì)判斷

設(shè)編隊(duì)成員為S j(j=1,2,···,n)，定義區(qū)域 Ωi編隊(duì)成員累計(jì)數(shù)為σi(i=1,2,3)。

區(qū)域編隊(duì)成員最大累計(jì)數(shù)為σ=max{σ1,σ2,σ3}。若σ=σ1，認(rèn)為此時(shí)相當(dāng)于COLREGS 中的對(duì)遇局面；若σ=σ2，認(rèn)為此時(shí)相當(dāng)于COLREGS 中的右交叉局面；若σ=σ3，認(rèn)為此時(shí)相當(dāng)于COLREGS中的左交叉局面；以圖13 為例說(shuō)明，此時(shí)σ1=1,σ2=3,σ3=0,σ=max{1,3,0}=3=σ2，認(rèn)為此時(shí)為右交叉會(huì)遇態(tài)勢(shì)，本船采取右轉(zhuǎn)避讓策略。

3.3 滿足sCOLREGS 的偏置APF 動(dòng)態(tài)避碰策略

高速航行的無(wú)人艇進(jìn)行避碰，對(duì)避碰路徑的生成速率及實(shí)時(shí)性有較高要求。考慮到APF 具有實(shí)時(shí)性且運(yùn)算量較小的特點(diǎn)，本節(jié)提出一種滿足sCOLREGS 的偏置障礙區(qū)域改進(jìn)APF 實(shí)時(shí)動(dòng)態(tài)避碰方法。

設(shè)艇群在寬闊海域執(zhí)行任務(wù)，艇載雷達(dá)感知范圍為ρmax，當(dāng)有動(dòng)態(tài)障礙物與艇群成員最近距離小于ρmax時(shí)，USV 編隊(duì)將執(zhí)行避障程序。

設(shè)動(dòng)態(tài)障礙物速度為vobs，此時(shí)通過(guò)速度障礙法進(jìn)行避碰判斷。若vobs∈VVO，則認(rèn)為有碰撞風(fēng)險(xiǎn)，進(jìn)行下一步程序，否則認(rèn)為無(wú)碰撞風(fēng)險(xiǎn)，保速直行，同時(shí)重復(fù)速度障礙法避碰判斷直至動(dòng)態(tài)障礙物與所有艇群成員最近距離大于ρmax。

當(dāng)碰撞風(fēng)險(xiǎn)存在時(shí)，USV 根據(jù)sCOLREGS 判斷會(huì)遇態(tài)勢(shì)，若我方為非避讓船，則保速直行，同時(shí)重復(fù)速度障礙法避碰判斷直至動(dòng)態(tài)障礙物與艇群成員最近距離大于ρmax。若我方為責(zé)任船，則按照本節(jié)接下來(lái)提出的實(shí)時(shí)人工勢(shì)場(chǎng)策略進(jìn)行動(dòng)態(tài)避障，同時(shí)進(jìn)行速度障礙法判斷碰撞風(fēng)險(xiǎn)，直至動(dòng)態(tài)障礙物與艇群成員最近距離大于ρmax。避障策略框架如圖14 所示。

圖14 動(dòng)態(tài)避障策略流程

圖15 以右交叉會(huì)遇局面為例展示了基于改進(jìn)人工勢(shì)場(chǎng)法的無(wú)人艇動(dòng)態(tài)避碰策略。圖15 中無(wú)人艇Si位置為P(xi,yi)，速度為vS i。障礙船位置為Q(xobs,yobs), 速度為vobs，兩船間距d為‖(xi,yi)?(xobs,yobs)‖2， 會(huì)遇角φi∈(?5°,112.5°) ，vS i∈VVO，vS i/vobs＜0.95，故屬于右交叉情形，且本船為避讓船。

圖15 偏置障礙物區(qū)域示意

為實(shí)現(xiàn)國(guó)際海上避碰規(guī)則規(guī)定的右轉(zhuǎn)避讓，USV 執(zhí)行動(dòng)態(tài)避碰策略構(gòu)造左偏置斥力區(qū)域。以P為圓心、d為半徑將障礙物所在點(diǎn)Q旋轉(zhuǎn)至速度方向上的D點(diǎn)，以PD為斜邊作直角三角形OPD，其中∠OPD=α?φi， α為臨界角度112.5°。

設(shè)障礙船半徑為r=vobs·dt，代表單位時(shí)間后動(dòng)態(tài)障礙物的可能位置范圍。以O(shè)為中心、m·IOP·nOP為長(zhǎng)軸、n·r·nOD為短軸建立前文所提及改進(jìn)橢圓人工斥力場(chǎng)，其中m、n為長(zhǎng)短軸系數(shù)，nOP、nOD為 OP、 OD方向單位向量。

φi越小，即動(dòng)態(tài)障礙物位置越接近本船正前方時(shí)，橢圓斥力區(qū)域越偏離運(yùn)動(dòng)方向，右轉(zhuǎn)幅度越小。反之，障礙物越偏離本船正前方時(shí)（此時(shí)碰撞概率較大），橢圓區(qū)域越接近運(yùn)動(dòng)方向，本船右轉(zhuǎn)幅度較大，符合實(shí)際避碰需求。

為使APF 符合COLREGS 的約束，常見的工作是對(duì)斥力場(chǎng)形狀、函數(shù)根據(jù)不同會(huì)遇態(tài)勢(shì)進(jìn)行分類討論，得出的結(jié)果較為復(fù)雜，本節(jié)通過(guò)將斥力場(chǎng)位置進(jìn)行變換實(shí)現(xiàn)對(duì)COLREGS 遵守的方法較為簡(jiǎn)便。同時(shí)，多數(shù)工作并沒有考慮基于編隊(duì)保持的COLREGS 遵守[18]，本節(jié)提出的sCOLREGS考慮了艇群的編隊(duì)尺寸，較傳統(tǒng)COLREGS 更符合海上USV 編隊(duì)動(dòng)態(tài)避障的安全需求。本節(jié)依據(jù)改進(jìn)APF，在滿足動(dòng)態(tài)避障的實(shí)時(shí)性的同時(shí)，考慮了對(duì)國(guó)際海上避碰規(guī)則的遵守，具有較高的安全性與求解規(guī)劃路徑速度。

4 仿真實(shí)驗(yàn)與分析

使用科學(xué)計(jì)算平臺(tái)對(duì)本文所提出的改進(jìn)型人工勢(shì)場(chǎng)路徑規(guī)劃、編隊(duì)分散避碰策略、遵守國(guó)際海上避碰規(guī)則的多艇動(dòng)態(tài)避碰策略進(jìn)行仿真分析，證明提出算法的可行性。

4.1 編隊(duì)分散避碰策略仿真驗(yàn)證

本小節(jié)對(duì)1.2 節(jié)提出的分散編隊(duì)避障策略進(jìn)行軟件仿真驗(yàn)證，仿真程序參數(shù)如下設(shè)置：地圖大小20 n mile×20 n mile,r=2 n mile,ρobs=1.5 n mile,ρ0=2 n mile,ρ1=3 n mile,ε=100,ηs=300,γ=10，無(wú)人艇速度上限vmax=2 n mile/h，加速度上限amax=0.5 n mile/h， 角速度上限ωmax=5°/s， 出發(fā)點(diǎn)坐標(biāo)(0 n mile,0 n mile)，目標(biāo)點(diǎn)坐標(biāo)(16 nmile，17 n mile)，障礙物中心坐標(biāo)(9 n mile,9 n mile)。2 種策略的仿真航跡如圖16 所示，避障過(guò)程中的路徑消耗、最大轉(zhuǎn)向角、距障礙物最近距離如表2 所示，關(guān)鍵理論參數(shù)的實(shí)驗(yàn)結(jié)果如表3 所示。

表2 策略參數(shù)對(duì)比

表3 關(guān)鍵理論參數(shù)實(shí)驗(yàn)結(jié)果

圖16 改進(jìn)APF 靜態(tài)避障策略航跡圖

由仿真設(shè)置，編隊(duì)半徑r與實(shí)際最近距離D的比值為1.408，具有較大的距離裕度，能保證無(wú)人艇群的避障安全。圖16 顯示本文改進(jìn)的人工勢(shì)場(chǎng)法可以安全地實(shí)現(xiàn)避障，且路徑較為平滑。

由表2 知策略2 分散避障的最大轉(zhuǎn)向角略大于策略1，在復(fù)雜環(huán)境下USV 編隊(duì)速度較低，對(duì)航跡長(zhǎng)度的要求大于對(duì)轉(zhuǎn)向角的要求。

本仿真情景中r/R=1.33接近1，可以認(rèn)為無(wú)人艇編隊(duì)大小與障礙物大小相當(dāng)。此時(shí)策略1（保持編隊(duì)）的路程消耗明顯大于策略2（分散編隊(duì)），由圖16 仿真航跡及2.2 節(jié)分析，所減少的路徑消耗主要來(lái)自編隊(duì)外側(cè)無(wú)人艇。表3 顯示d1/d2與1+2r/(ρ0+R+2D)較為接近，符合2.2 節(jié)推導(dǎo)的理論式，艇群分散避障策略的建模分析得到仿真驗(yàn)證。

4.2 符合COLREGS 的多艇動(dòng)態(tài)避碰

本小節(jié)對(duì)第3 節(jié)提出的基于偏置APF 的sCOLREGS 遵守策略進(jìn)行仿真驗(yàn)證。在一次航行任務(wù)中依次設(shè)置對(duì)遇、右交叉、左交叉3 種海上會(huì)遇態(tài)勢(shì)，并在對(duì)遇、左交叉局面中設(shè)置動(dòng)態(tài)干擾艇以檢驗(yàn)算法對(duì)多移動(dòng)障礙物的有效性。仿真參數(shù)設(shè)置為：r=2 n mile,m=2.5,n=1.5，無(wú)人艇速度上限vmax、加速度上限amax、角速度上限ωmax與4.1 節(jié)一致。艇群初始中心(0 n mile,0 n mile)，地圖大小50 n mile×50 n mile，地標(biāo)位置參數(shù)設(shè)置如表4 所示。規(guī)劃生成的全局航跡如圖17（a）所示，對(duì)遇、右交叉、左交叉會(huì)遇態(tài)勢(shì)的放大局部航跡分別如圖17（b）、圖17（c）、圖17（d）所示。全程USV 編隊(duì)成員與障礙艇距離如圖18 所示。

表4 地標(biāo)位置參數(shù)設(shè)置

圖17 偏置APF-sCOLREGS仿真航跡

圖18 (sCOLREGS)USV 編隊(duì)與障礙艇距離曲線

在仿真結(jié)果圖17 中，處于對(duì)遇局面時(shí)，USV 編隊(duì)與動(dòng)態(tài)障礙船均為責(zé)任船，均采取右轉(zhuǎn)避讓措施，如圖17（b）所示；處于右交叉局面時(shí)，USV 編隊(duì)為責(zé)任船，采取右轉(zhuǎn)避讓措施，動(dòng)態(tài)障礙船非責(zé)任船，保速直行，如圖17（c）所示；處于左交叉局面時(shí)，動(dòng)態(tài)障礙船為責(zé)任船，采取右轉(zhuǎn)避讓措施，USV 編隊(duì)非責(zé)任船，保速直行，如圖17（d）所示。故第3 節(jié)提出的基于改進(jìn)APF 的偏置障礙區(qū)域sCOLREGS 遵守方法能完全符合國(guó)際海上避碰規(guī)則條例，可以實(shí)現(xiàn)USV 集群海面保持編隊(duì)進(jìn)行動(dòng)態(tài)避障。此外，在對(duì)遇、左交叉局面中設(shè)置的動(dòng)態(tài)干擾艇并沒有改變無(wú)人艇群根據(jù)sCOLREGS 選擇的避障策略，說(shuō)明所提出的方法具有一定的抗干擾能力，可以處理多移動(dòng)障礙物局面。

圖18 中從左至右分別為全程航行過(guò)程中USV 編隊(duì)與對(duì)遇障礙艇、右交叉障礙艇、左交叉障礙艇的距離曲線。由圖18 可知障礙物與USV 編隊(duì)具有較充足的距離裕度，可以保證USV 編隊(duì)動(dòng)態(tài)避碰的安全性，同時(shí)為突發(fā)情況提供了緩沖區(qū)域。此距離裕度主要是因?yàn)閟COLREGS對(duì)單艇會(huì)遇的COLREGS 進(jìn)行了物理范圍的擴(kuò)充，考慮了編隊(duì)的幾何尺寸。若不考慮sCOLREGS規(guī)則，在對(duì)遇情形下由于原COLERGS 的閾值角度為 5°，難以完全避開艇群，往往會(huì)導(dǎo)致碰撞的發(fā)生，如圖19、圖20 所示。

圖19 COLREGS 航跡

圖20 (COLREGS)USV 編隊(duì)與障礙艇距離曲線

圖21 為避障過(guò)程中USV 編隊(duì)航向角變化曲線。本文所提出的算法航向角極差約為 37°，對(duì)USV 的轉(zhuǎn)向機(jī)動(dòng)性能要求較低。由于航向角變化較小，生成的航跡曲線較為平滑，既避免了尖銳的轉(zhuǎn)向，也節(jié)省了額外的路徑消耗，提高了USV 編隊(duì)執(zhí)行任務(wù)的續(xù)航能力。圖21 中可觀察到航向角在剛開始變化時(shí)有一段維持過(guò)程，這是因?yàn)閷?duì)遇與右交叉態(tài)勢(shì)中偏置APF 障礙區(qū)域的轉(zhuǎn)向夾角(α?φi)較大，障礙物橢圓區(qū)域接近USV 編隊(duì)正左方，對(duì)USV 編隊(duì)右轉(zhuǎn)偏向斥力場(chǎng)較小，從而轉(zhuǎn)向不明顯。這一過(guò)程為平滑航跡做出了貢獻(xiàn)。

5 結(jié)束語(yǔ)

本文針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法路徑不平滑的問(wèn)題，首先對(duì)斥力場(chǎng)形狀與函數(shù)進(jìn)行修正，針對(duì)復(fù)雜環(huán)境下多無(wú)人艇群避障時(shí)的額外路徑消耗，提出一種分散避碰策略，同時(shí)結(jié)合艇群會(huì)遇態(tài)勢(shì)判斷特點(diǎn)，提出艇群國(guó)際海上避碰規(guī)則，結(jié)合改進(jìn)人工勢(shì)場(chǎng)法偏置斥力場(chǎng)區(qū)域?qū)崿F(xiàn)動(dòng)態(tài)避障。實(shí)驗(yàn)結(jié)果表明，本文提出的分散策略在設(shè)定情境中避障總路程顯著小于維持編隊(duì)策略，提高了編隊(duì)的續(xù)航能力。

本文提出的動(dòng)態(tài)避障策略，在對(duì)遇、右交叉、左交叉3 種常見海上會(huì)遇態(tài)勢(shì)中以較大的距離裕度進(jìn)行符合規(guī)則的動(dòng)態(tài)避障，編隊(duì)轉(zhuǎn)向角度范圍較小，規(guī)劃航跡平滑，能在確保實(shí)時(shí)性的同時(shí)較好地遵守國(guó)際海上避碰規(guī)則的相關(guān)條例。