張洋洋,瞿棟,柯俊,李小毛

(上海大學機電工程與自動化學院,上海 200072)

基于速度障礙法和動態(tài)窗口法的無人水面艇動態(tài)避障

張洋洋,瞿棟,柯俊,李小毛

(上海大學機電工程與自動化學院,上海 200072)

速度障礙法是一種普遍應用在無人水面艇(unmanned surface vehicle,USV)上的動態(tài)避障方法,但傳統(tǒng)的速度障礙法未考慮無人水面艇運動學性能與障礙物運動信息誤差的影響,也未明確何時開始避障與結(jié)束避障.在速度障礙法的基礎上,用橢圓表示無人水面艇與障礙物,給出一種求解橢圓切線的方法;將動態(tài)窗口法與速度障礙物法相結(jié)合,考慮無人水面艇的運動學性能,只使用無人水面艇在給定時間內(nèi)能到達的速度和方向進行避障計算;通過比較碰撞時間與無人水面艇避開障礙物所需的時間來確定何時開始避障,并提出一種結(jié)束避障的判斷方法;為了減小障礙物運動信息誤差的影響,提出了一種虛擬障礙物方法.最后,通過仿真實驗驗證了該避障方法的可行性與有效性.

無人水面艇;動態(tài)避障;速度障礙;動態(tài)窗口;虛擬障礙物

無人水面艇(unmanned surface vehicle,USV)是一種輕型智能水面運載工具,具有體積小、造價低、速度快、機動性強等特點.近年來,隨著控制、傳感、無線通信技術(shù)的不斷進步,無人水面艇得到了迅速發(fā)展.通過搭載不同的設備,無人水面艇可以應用在不同的領域,如環(huán)境監(jiān)測、水質(zhì)采樣、海岸保護、海底測繪、軍事等.

如果要保證無人水面艇能夠在海洋中安全地航行,那么無人水面艇必須能夠?qū)叫羞^程中遇到的島嶼、暗礁、燈塔、浮標和航行的船只等其他障礙物進行自主避障.無人水面艇自主避障歸類于平面移動機器人路徑規(guī)劃領域,經(jīng)過國內(nèi)外學者多年的研究,目前已經(jīng)有很多成熟的算法用于移動機器人的路徑規(guī)劃.

Fox等[1]將路徑規(guī)劃分為兩類：全局路徑規(guī)劃和局部路徑規(guī)劃.全局路徑通常假設在完全已知環(huán)境信息的情況下,離線計算出一條從起點到終點的完全路徑;但當不完全可知環(huán)境信息時全局路徑規(guī)劃就不能規(guī)劃出安全的路徑,且實時性不好,環(huán)境一旦改變就不能快速計算出新的路徑.局部路徑規(guī)劃通過傳感器獲得的部分環(huán)境信息在線實時計算出可行路徑.局部路徑規(guī)劃可分為兩類:①當障礙物為靜態(tài)時稱為靜態(tài)避障;②當障礙物為動態(tài)時稱為動態(tài)避障,動態(tài)避障算法也可用于靜態(tài)避障.動態(tài)避障算法比較有難度,因為通過移動機器人自身攜帶的傳感器很難精確地獲得動態(tài)障礙物的運動信息,無法對障礙物的運動趨勢進行準確的預測.局部路徑規(guī)劃計算量小,實時性好,但由于不完全可知環(huán)境信息,因此容易陷入極小值點.根據(jù)全局路徑與局部路徑的特點,目前通常采用全局與局部相結(jié)合的多層避障結(jié)構(gòu).Campbell等[2]提出了一種混合的避障結(jié)構(gòu),即首先通過已知的環(huán)境信息(通常為地圖),離線規(guī)劃出一條從起點到目標點的可行路徑,該路徑應避開地圖上已知的靜態(tài)障礙物,機器人沿著該路徑行駛,當在行駛過程中通過傳感器檢測到新的障礙物時,再通過傳感器獲得的障礙物的詳細信息進行局部避障.Casalino等[3]提出了一種3層的避障結(jié)構(gòu),即在局部避障中又增加了一層反應式的避障結(jié)構(gòu),來應對障礙物運動信息不可知的避障場景.

目前,常用的全局路徑規(guī)劃算法主要有Dijkstra’s算法[4]、A?算法[5]及其改進算法[6-9]、D?算法[10]、進化算法[11-12]、神經(jīng)網(wǎng)絡算法[13]和快速擴展隨機樹算法[14-16]等.

目前,局部路徑規(guī)劃也有很多成熟的算法,文獻[17-19]介紹了bug系列算法中的Tangent-Bug避障算法和不同bug算法的性能對比,bug系列算法中機器人主要有兩種模式:朝目標點行走模式和繞障礙物行走模式.不同bug算法之間的主要區(qū)別在于在兩種模式之間切換的條件不同,TangentBug避障算法在判斷機器人陷入極小值點時切換到繞障礙物行走模式.文獻[20-24]介紹了基于bug算法的改進算法及算法的實際應用.人工勢場法[25-26]是一種虛擬力場法,即障礙物產(chǎn)生一個對機器人的斥力使得機器人與障礙物保持安全距離,目標點產(chǎn)生一個對機器人的引力,使得機器人駛向目標點,引力和斥力的合力決定著機器人的運動方向和運動速度.人工勢場法計算量小,實時性好,但容易出現(xiàn)局部極小值點.文獻[27]中通過使用一種帶記憶功能的“沿邊法”來解決極小值問題.Zohaib等[28]將文獻[29]提出的FGM(follow the gap method)算法與bug算法相結(jié)合,提出了一種IFGM(intelligent FGM)避障算法,可用于在U形和H形障礙物環(huán)境中避障,可以解決由U形和H形障礙物引起的極小值問題.一般避障算法在算法層面上并不考慮機器人的運動學性能,故Fox等[1]和Brock等[30]考慮機器人的運動學約束,提出了一種動態(tài)窗口法,該方法只計算在給定的時間間隔內(nèi)機器人能達到的速度,這些可達的速度構(gòu)成一個速度空間(v,w)的集合,這個集合即為動態(tài)窗口,通過構(gòu)建的目標函數(shù),從動態(tài)窗口中選擇出最優(yōu)的可行速度.Seder等[31]通過結(jié)合D*算法與動態(tài)窗口并進行一些改進,提出了一種用于動態(tài)障礙物的動態(tài)窗口法,即將動態(tài)障礙物建模為移動的柵格,通過預測障礙物與機器人的碰撞點構(gòu)建一個虛擬的障礙物,機器人應該避過該虛擬障礙物.Tang等[32]在動態(tài)窗口的基礎上提出了一種OAABHW(obstacle avoidance algorithm based on heading window)避障算法,該算法將動態(tài)窗口轉(zhuǎn)化為艏向和速度窗口,構(gòu)建兩個函數(shù)分別用來選擇避障速度和避障方向.Zhang等[33]基于文獻[32],結(jié)合Sarsa在策略強化學習算法,提出了一種新穎的適用于無人水面艇的自適應避障算法,該算法基于Sarsa自適應學習模塊處理海風和涌流的影響,通過對航向角進行補償來減小海風和涌流的干擾,提高無人水面艇的避障性能.在Casalino等[3]提出的3層避障結(jié)構(gòu)的第2層中,障礙物被定義為一個矩形邊界框,通過假設無人水面艇與障礙物的速度恒定,分別計算出無人水面艇與矩形邊界框4個頂點的碰撞位置,將無人水面艇位置點、目標點與這些碰撞位置點連接,構(gòu)成多條路徑,并通過A*算法計算出最優(yōu)路徑.Simetti等[34]對文獻[3]中的算法進行了一些改進,即在矩形邊界框的外部又增加了一個支持邊界框來減小誤差的干擾,同時采用了保持避障方向策略,一旦計算出一個初始避障角度,如果沒有出現(xiàn)新的危險影響無人水面艇,則無人水面艇一直沿著這個避障方向運動,直到認為避開障礙物為止.Wu等[35]提出了一種基于方向權(quán)重的無人水面艇動態(tài)避障算法,通過相對速度計算出與障礙物最近接近點(closest point of approach,CPA)的距離(distance of CPA, DCPA)、方位,以及到達該點的時間(time to CPA,TCPA).將不同的方向賦予權(quán)重,其中無人水面艇當前航向方向權(quán)重最大,其兩邊方向的權(quán)重逐漸減小;障礙物方向與CPA方向的權(quán)重最小,CPA方向兩邊方向的權(quán)重逐漸增大;當采用海事規(guī)則時,根據(jù)海事規(guī)則不可行的方向權(quán)重最小.綜合計算這些權(quán)重,求出權(quán)重最大的方向作為避障的方向.Chakravarthy等[36]對速度進行切向和徑向分解,預測點與點之間的碰撞,然后推廣到兩個不規(guī)則目標之間的碰撞,提出了一種碰撞錐的避障方法.Chakravarthy等[37]通過將障礙物建模為2次曲面,提出了一種可用于3維空間的碰撞錐動態(tài)避障方法.另外,速度障礙法是目前廣泛應用在無人水面艇上的動態(tài)避障算法,由Fiorini[38]提出,該算法對每個障礙物在速度空間上構(gòu)建一個錐形區(qū)域,只要速度矢量落入該區(qū)域即認定會發(fā)生碰撞,從非錐形區(qū)域中選擇一個最優(yōu)的速度矢量進行避障. Kuwata等[39]將速度障礙法與海事規(guī)則相結(jié)合,用于無人水面艇的動態(tài)避障,并通過相對速度計算出無人水面艇與障礙物最近接近點的距離和到達該點的時間.當DCPA和TCPA都滿足給定的約束條件時才認定會發(fā)生碰撞,并通過構(gòu)建一個代價函數(shù),從可行速度空間里選擇一個能使代價函數(shù)最小的速度矢量作為避障的速度矢量.文獻[40-41]中都將海事規(guī)則與避障算法相結(jié)合,用于無人水面艇避障.

上述文獻中提出的局部避障算法基本都假設障礙物為圓形,且完全已知障礙物的位置、運動信息.然而,無人水面艇在海上航行時遇到的障礙物大部分為船只,而船只具有長寬比大的特點,圓形不能形象地表示船只,會將船只在寬度方向放大很多;而船只的形狀與橢圓很相似,因此使用橢圓可以較好地表示出船只障礙物.故本工作在文獻[38]速度障礙法的基礎上,使用橢圓表示障礙物,并結(jié)合動態(tài)窗口法考慮無人水面艇的運動學約束,改進速度障礙法用于無人水面艇的靜態(tài)、動態(tài)避障.在無人水面艇實際航行時,通過無人水面艇自身攜帶的傳感器很難精確獲得障礙物的運動信息,為此本工作通過增加虛擬障礙物來減小障礙物運動信息誤差的影響.

1 基于橢圓的速度障礙法

1.1 速度障礙原理

文獻[38]中詳細介紹了速度障礙法的基本原理,本工作對速度障礙法進行簡單回顧.分別用P∈R2和V∈R2表示在t時刻一個機器人在2維平面中的位置和速度矢量.在以下的分析中將障礙物和無人水面艇的形狀都簡化為橢圓.

如圖1所示,綠色橢圓A為無人水面艇,PA為無人水面艇的位置矢量,紅色橢圓B為“膨化”后的障礙物,即將無人水面艇的尺寸疊加到障礙物的尺寸上,為了簡化分析并保證無人水面艇的安全,將無人水面艇橢圓的半長軸分別疊加到障礙物橢圓的半長軸和半短軸上,這樣就將無人水面艇簡化為一個質(zhì)點;PB表示障礙物的位置矢量.圖1中綠色箭頭VA、VBA分別為無人水面艇的速度矢量、無人水面艇相對于障礙物的速度矢量,紅色箭頭VB為障礙物的速度矢量,VBA通過式(1)求得

定義從一點P沿V方向發(fā)出的射線為

當求得相對速度VBA后,通過相對速度可以將障礙物B看作靜止.假設無人水面艇速度VA與障礙物速度VB保持不變,則當從PA點發(fā)出沿VBA方向的射線λBA與障礙物B相交,即射線λBA落在障礙物B相對于無人水面艇A的兩條切線夾角內(nèi)時,可認為無人水面艇會在將來某一時刻t1與障礙物發(fā)生碰撞.定義使A會與B碰撞的相對速度VBA的集合為RCCAB(relative collision cone),RCCAB稱為在A與B的相對速度空間里的相對碰撞區(qū)域:式中,RCCAB即為障礙物B相對于無人水面艇A的左右切線λL,λR中間的灰色區(qū)域.

相對碰撞區(qū)域RCC定義在無人水面艇A與障礙物B的相對速度空間里,但是如果存在多個障礙物時,則需要將每個障礙物的RCC轉(zhuǎn)換到統(tǒng)一的速度空間里表述.定義在無人水面艇的絕對速度空間里,會與障礙物B發(fā)生碰撞的無人水面艇速度VA的集合為VOAB(velocity obstacle),稱VOAB為速度障礙:

式(4)等價表述為

式(4)中,⊕為閔可夫斯基矢量和運算.如圖1所示,VOAB即為圖中的藍色陰影區(qū)域,相當于將RCCAB沿VB方向平移,當速度矢量VA的箭頭落在該藍色區(qū)域內(nèi)時,無人水面艇A會與障礙物B發(fā)生碰撞.

當存在多個障礙物時,求出所有障礙物的VO集合的并集后得到總的VO集合:

對于無人水面艇A,非VO集合中的速度矢量不會與障礙物發(fā)生碰撞,即在給定的時間內(nèi)將無人水面艇當前的速度矢量VA調(diào)整到非VO區(qū)域無人水面艇即可避開障礙物.

1.2 計算橢圓切線

在速度障礙法中,如果要判斷無人水面艇是否會與障礙物發(fā)生碰撞,則必須要求出障礙物相對于無人水面艇的兩條切線.為了方便求出橢圓障礙物的切線,本工作建立了無人水面艇船體和障礙物坐標系,在障礙物坐標系中用橢圓標準方程表示橢圓障礙物.首先將無人水面艇位置轉(zhuǎn)換到障礙物坐標系中,即在障礙物坐標系中求得兩條切線的切點,再將切點轉(zhuǎn)換到船體坐標系,這樣便求出障礙物相對于無人水面艇的兩條切線.

如圖2所示,xOy為無人水面艇船體坐標系,x′O′y′為障礙物坐標系;紅色橢圓為障礙物,綠色橢圓為無人水面艇,已簡化為質(zhì)點;點O為無人水面艇中心;O′為障礙物中心.無人水面艇在船體坐標系中的坐標為USVboat=[0,0]T,則無人水面艇在障礙物坐標系中的坐標USVobs=[m,n]T為

式中,C為障礙物中心點在船體坐標系中的坐標,R為障礙物坐標系相對于船體坐標系的旋轉(zhuǎn)矩陣

當求得無人水面艇在障礙物坐標系中的坐標后,在障礙物坐標系中聯(lián)立標準橢圓方程與橢圓切線方程：

即可求得在障礙物坐標系中切點的坐標(xT,yT)(如圖2中的T1obs,T2obs).通過

圖2 橢圓切線Fig.2 Ellipse tangents

計算切點T1obs,T2obs在船體坐標系中的坐標(如圖2中的T1boat,T2boat),求出兩個切點后即可計算出在船體坐標系中障礙物相對于無人水面艇的兩條切線.

2 避障算法

根據(jù)無人水面艇與障礙物之間的距離,將無人水面艇周圍的區(qū)域分為3個:安全、常規(guī)避障和緊急避障區(qū)域(見圖3),其中可認定在安全區(qū)域內(nèi)的障礙物安全,不會對無人水面艇造成威脅;同時由于安全區(qū)域范圍大,可能會存在大量障礙物,故為了減小計算量,對安全區(qū)域內(nèi)的障礙物不進行避障計算處理.在緊急避障區(qū)域內(nèi)的障礙物對無人水面艇會產(chǎn)生嚴重威脅,為此無人水面艇需要采取緊急措施.本工作主要介紹無人水面艇在常規(guī)避障區(qū)域內(nèi)的避障.

圖3 避障區(qū)域Fig.3 Avoidance zones

無人水面艇在執(zhí)行任務時,通過軌跡跟蹤算法規(guī)劃速度和方向,保證無人水面艇能沿著全局路徑規(guī)劃計算出的航跡線行駛,因此本算法中無人水面艇采用Breivik等[42-43]提出的視線制導(line of sight,LOS)軌跡跟蹤算法,其基本思想就是在規(guī)劃的軌跡線上選擇一個虛擬的目標點,無人水面艇沿著該目標點行駛即可回歸到航跡線上.

當無人水面艇將與障礙物發(fā)生碰撞時,本算法能根據(jù)無人水面艇避開障礙物所需時間來判斷何時開始避障.開始避障后通過動態(tài)窗口法與速度障礙法計算避障速度與方向,并通過增加虛擬障礙物來減少障礙物運動信息誤差帶來的影響;當滿足結(jié)束避障條件時,無人水面艇結(jié)束避障.圖4為從開始避障到結(jié)束避障的流程.

圖4 避障流程Fig.4 Avoidance fowchart

2.1 基于動態(tài)窗口的速度障礙法

通過速度障礙法計算出會碰撞的VO集合后,非VO集合中的數(shù)據(jù)都為無人水面艇可選的避障速度矢量.然而,由于無人水面艇動力學性能的約束,無人水面艇在給定的時間內(nèi)速度和方向不會改變很大,因此非VO集合內(nèi)的很多速度矢量對于無人水面艇來說是不可及的,故不需要對這些速度矢量進行計算.

動態(tài)窗口法考慮了無人水面艇的運動學性能[1],只計算無人水面艇在給定時間窗口?t內(nèi)能達到的移動速度,即速度窗口

和角速度,即角速度窗口

式(11)中,vc為無人水面艇當前的移動速度,˙v為無人水面艇的加速度;式(12)中,ω為無人水面艇當前的角速度,˙ω為無人水面艇的角加速度.通過繼續(xù)對式(12)進行計算,可以求出無人水面艇在?t內(nèi)艏向可以轉(zhuǎn)到的角度,即艏向窗口[32]

式中,θh為無人水面艇當前的艏向.

通過式(11),(13)即可確定無人水面艇在給定?t內(nèi)可以達到的速度大小vd和方向θd,但vd和θd集合中的元素都是連續(xù)的,不利于實際工程中的計算.為此本算法在實施過程中將vd速度窗口和θd艏向窗口離散化,即將vd離散為M個速度,θd離散為N個艏向,離散后的一個速度vi和一個艏向θi組成一個速度矢量(vi,θi),那么共有M×N個速度矢量.將這些無人水面艇在?t時間內(nèi)可達到的離散的速度矢量組成的集合稱為RV(reachable velocity)(見圖5(a)).RV集合中滿足式(5)的速度矢量會與障礙物發(fā)生碰撞,從RV集合中排除會碰撞的速度矢量即可得到無人水面艇在?t時間內(nèi)可達到的、安全的速度矢量集合,稱為RAV (reachable avoidance velocity)(見圖5(b)):

圖5中,紅色區(qū)域為VO區(qū)域,RV與VO相交的部分即為會發(fā)生碰撞的速度矢量,RAV部分為安全的速度矢量.

圖5 RV與RAV速度Fig.5 RV and RAV velocity

求出RAV集合后,需要在RAV集合中選出最優(yōu)的速度矢量進行避障.根據(jù)不同的任務有不同的選取準則,本算法中采用如下評價函數(shù)來選取,

式中:vc為無人水面艇當前的速度;θh為無人水面艇當前的艏向;(vi,θi)為RAV集合中的某一速度矢量,將能使Gij最大的(vi,θi)作為無人水面艇避障的速度矢量.一旦選擇了一個避障速度矢量,無人水面艇會一直沿著這個速度矢量行駛,直到出現(xiàn)新的危險或避開障礙物時才以新的速度矢量行駛.

2.2 開始與結(jié)束避障

在判斷無人水面艇是否會與障礙物發(fā)生碰撞時,需要計算何時開始避障,一旦開始避障后就需要判斷何時結(jié)束避障.本算法中采用兩個條件明確開始避障和結(jié)束避障時間.

2.2.1 開始避障

當判斷無人水面艇是否會與障礙物發(fā)生碰撞時,在碰撞時刻無人水面艇與障礙物之間的距離最小,各自的位置稱為CPA,無人水面艇到達其CPA點所需的時間稱為tCPA.如圖6所示,紅色與綠色實線橢圓分別為障礙物與無人水面艇在當前t0時刻的位置,Vobs與VUSV分別為障礙物與無人水面艇在t0時刻的速度矢量,紅色與綠色虛線橢圓分別為障礙物與無人水面艇在tCPA時刻的位置,CPAobs點與CPAUSV點分別為在tCPA時障礙物與無人水面艇中心點的位置:

圖6 距離最小點和開始避障點Fig.6 Close point approach and start avoidance point

假設保持無人水面艇速度大小不變,即可以恒定的角加速度轉(zhuǎn)彎.圖6中,無人水面艇以恒定速度和方向駛向CPAUSV點的過程中,在某點B以恒定角加速度左轉(zhuǎn)彎,恰好沿曲線軌跡B—PL與障礙物相切于PL點.無人水面艇從B點行駛到PL點所需的時間為tL,則無人水面艇在tCPA=tL時刻開始左轉(zhuǎn)避障,剛好會與障礙物相切于PL點:式中:(x,y),(xB,yB)分別為PL點、B點在障礙物坐標系中的坐標;v,θ0,ω0,α分別為無人水面艇當前速度、艏向、角速度和角加速度;PLC1+PLC2為在障礙物坐標系中PL點到橢圓兩焦點的距離和;a為橢圓半長軸.

同理,無人水面艇在某點A以恒定角加速度右轉(zhuǎn)彎,恰好沿曲線軌跡A—PR與障礙物相切于PR點.無人水面艇從A點行駛到PR點所需的時間為tR,則無人水面艇在tCPA=tR時刻開始右轉(zhuǎn)避障,剛好會與障礙物相切于PR點.

定義開始避障時間為tavoid,則

式中,k為大于1的系數(shù),保證無人水面艇的安全且轉(zhuǎn)彎平緩.無人水面艇在tCPA=tavoid時開始避障,可以安全平緩地避開障礙物.

2.2.2結(jié)束避障

當無人水面艇開始避障后需要判斷何時結(jié)束避障,本算法中無人水面艇執(zhí)行任務時需要沿規(guī)劃的航跡行駛到終點,故只要無人水面艇能安全地沿軌跡跟蹤規(guī)劃的速度與方向行駛,且安全地沿目標點方向行駛即可結(jié)束避障,也就是說無人水面艇在避障過程中滿足

即可結(jié)束避障.式(19)中,VLOS為軌跡跟蹤規(guī)劃的速度矢量,Vdest為無人水面艇以當前速度大小沿目標點方向運動時的速度矢量.

2.3 虛擬障礙物

速度障礙法設定完全已知障礙物的運動信息,而在實際的航行過程中,無人水面艇通過自身攜帶的雷達、視覺、激光等傳感器而獲得的障礙物的運動信息會有誤差,利用有誤差的信息進行避障可能會引起無人水面艇與障礙物碰撞,故需要考慮障礙物運動信息的誤差.

假設通過傳感器獲得的障礙物的速度矢量為Vt,障礙物速度矢量誤差集合為VE,則障礙物實際可能的速度矢量集合

假設VP集合中的每個元素都為一個虛擬障礙物的速度矢量,虛擬障礙物的位置、尺寸與原障礙物相同,則虛擬障礙物的速度矢量為

求出一個障礙物及與其關聯(lián)的每個虛擬障礙物的VO集合,求這些集合的并集,即為一個障礙物最終的VO集合,該集合考慮了障礙物運動信息的誤差.

3 仿真實驗

為了驗證本算法的避障性能,在Matlab環(huán)境下分別對單個和多個動態(tài)障礙物場景進行仿真分析.

3.1 單個動態(tài)障礙物場景

圖7為無人水面艇與單個動態(tài)障礙物相遇時的場景.圖中綠色直線為規(guī)劃的無人水面艇的航跡線,紅色橢圓為障礙物以速度3.5 m/s、相對于正北方向90°運動,藍色五邊形為無人水面艇以速度5 m/s、沿正北方向運動.

圖7中,假設障礙物的速度與運動方向恒定.但是,在實際場景中障礙物的速度與運動方向通過傳感器獲得,往往會有很大的誤差.為了模擬傳感器的這些誤差,對障礙物的速度與運動方向增加干擾,即無人水面艇獲得的是增加干擾后的障礙物的速度與運動方向.圖8為障礙物的實際速度、運動方向與無人水面艇獲得的障礙物的速度、運動方向.

圖9(a)為采用虛擬障礙物時的避障過程,其中藍色為無人水面艇軌跡,紅色為障礙物軌跡,無人水面艇從障礙物后方成功避開障礙物.圖9(b)為未采用虛擬障礙物方法時的避障過程,由于障礙物運動信息誤差的影響,無人水面艇與障礙物碰撞.

圖7 單個動態(tài)障礙物場景Fig.7 Single dynamic obstacle scene

圖8 障礙物速度與運動方向Fig.8 Speeds and courses of obstacle

圖9 避障過程Fig.9 Processes of avoidance

圖10為在相同場景下采用與未采用虛擬障礙物時無人水面艇輸出的避障方向,其中在0～15 s期間為避障過程.可以看出,未采用虛擬障礙物時無人水面艇輸出的避障方向抖動嚴重,而采用虛擬障礙物時無人水面艇輸出的避障方向穩(wěn)定.

圖11為在相同場景下采用與未采用虛擬障礙物時無人水面艇與障礙物的距離,可見采用虛擬障礙物時最近距離為23 m.由于無人水面艇船長為10 m,速度為5 m/s,該最小距離可以保證無人水面艇的安全;未采用虛擬障礙物時最近距離為9 m,無人水面艇與障礙物碰撞.

圖10 避障輸出方向Fig.10 Output courses of avoidance

圖11 無人水面艇與障礙物的距離Fig.11 Distances between USV and obstacle

3.2 多個動態(tài)障礙物場景

在多個障礙物場景下,同樣對每個障礙物的速度與運動方向增加了干擾.圖12為存在2個動態(tài)障礙物時采用虛擬障礙物方法時的避障過程,可以看出無人水面艇正常避過2個動態(tài)障礙物.圖13(a)為在相同場景下,未采用虛擬障礙物方法時無人水面艇與障礙物1發(fā)生碰撞,而圖13(b)為未采用虛擬障礙物方法時無人水面艇成功避過障礙物2.

圖12 采用虛擬障礙物時多障礙物避障過程Fig.12 Process of avoidance with multiple obstacles using virtual obstacles

圖13 未采用虛擬障礙物時多障礙物避障過程Fig.13 Processs of avoidance with multiple obstacles without virtual obstacles

圖14為在避障過程中無人水面艇輸出的避障方向,圖中0～15 s為無人水面艇躲避障礙物1的避障過程,30～45 s為無人水面艇躲避障礙物2的過程.采用虛擬障礙物方法時,無人水面艇在躲避2個障礙物過程中避障輸出方向穩(wěn)定.未采用虛擬障礙物方法時,無人水面艇在躲避障礙物1的過程中避障方向抖動嚴重,且與障礙物1發(fā)生碰撞;在躲避障礙物2的過程中,雖然無人水面艇成功避開障礙物,但輸出的避障方向仍然抖動嚴重.

圖14 輸出的避障方向Fig.14 Output courses of avoidances

圖15(a)為無人水面艇在行駛過程中采用與未采用虛擬障礙物法時與障礙物1的距離,其中采用虛擬障礙物時無人水面艇距離障礙物的最小距離為20 m,而未采用虛擬障礙物時無人水面艇與障礙物的最小距離為8 m;圖15(b)為無人水面艇行駛過程中無人水面艇和障礙物2的距離,其中采用虛擬障礙物時無人水面艇距離障礙物的最小距離為18 m,而未采用虛擬障礙物時無人水面艇距離障礙物的最小距離為17 m.

圖15 多障礙物時無人水面艇與障礙物的距離Fig.15 Distances between USV and obstacles with multiple obstacles

4 結(jié)束語

本工作改進了速度障礙法并用于無人水面艇動態(tài)避障,用橢圓表示無人水面艇和障礙物,考慮無人水面艇的運動學性能,只將無人水面艇在給定時間內(nèi)能到達的速度和方向用于速度障礙法計算.另外,通過比較碰撞與避障所需時間來確定何時開始避障,當回歸航跡與沿目標點運動都安全時可結(jié)束避障,并且使用虛擬障礙物來減小障礙物運動信息誤差對避障過程的影響.通過仿真實驗驗證了本避障方法的效果.

本方法并未采用海事規(guī)則,即當無人水面艇的最大速度小于障礙物的速度時,如果無人水面艇選擇從動態(tài)障礙物前方進行避障則會有碰撞危險,因此采用海事規(guī)則的動態(tài)避障可以更好地保證無人水面艇的安全.

[1]FOX D,BuRGARD W,THRuN S.The dynamic window approach to collision avoidance[J].IEEE Robotics&Automation Magazine,1997,4(1):23-33.

[2]CAMPBELL S,NAEEM W,IRWIN G W.A review on improving the autonomy of unmanned surface vehicles through intelligent collision avoidance manoeuvres[J].Annual Reviews in Control,2012, 36(2):267-283.

[3]CAsALINO G,TuRETTA A,SIMETTI E.A three-layered architecture for real time path planning and obstacle avoidance for surveillance USVs operating in harbour felds[C]//Oceans.2009: 1-8.

[4]DIJKsTRA E W.A note on two problems in connexion with graphs[J].Numerische Mathematik, 1959,1(1):269-271.

[5]HART P E,NILssON N J,RAPHAEL B.A formal basis for the heuristic determination of minimum cost paths[J].IEEE Transactions on Systems Science and Cybernetics,1968,4(2):100-107.

[6]KIM H,KIM D,SHIN J U,et al.Angular rate-constrained path planning algorithm for unmanned surface vehicles[J].Ocean Engineering,2014,84:37-44.

[7]KOENIG S,LIKHACHEV M,FuRCY D.Lifelong planning A*[J].Artifcial Intelligence,2004, 155(1):93-146.

[8]LIKHACHEV M,FERGusON D I,GORDON G J,et al.Anytime dynamic A*:an anytime,replanning algorithm[C]//Fifteenth International Conference on Automated Planning and Scheduling. 2005:262-271.

[9]LIKHACHEV M,KOENIG S.A generalized framework for lifelong planning A*search[C]//Fifteenth International Conference on Automated Planning and Scheduling.2005:99-108.

[10]STENTZ A.The focussed D*algorithm for real-time replanning[C]//International Joint Conference on Artifcial Intelligence.2000:3310-3317.

[11]BOTEA A,M¨uLER M,SCHAEFFER J.Near optimal hierarchical path-fnding[J].Journal of Game Development,2004,1(7):7-28.

[12]LEIGH R,LOuIs S J,MILEs C.Using a genetic algorithm to explore A*-like pathfnding algorithms[C]//IEEE Symposium on Computational Intelligence and Games.2007:72-79.

[13]YANG S X,LuO C.A neural network approach to complete coverage path planning[J].IEEE Transactions on Systems,Man,and Cybernetics,Part B(Cybernetics),2004,34(1):718-724.

[14]MELCHIOR N A,SIMMONs R.Particle RRT for path planning with uncertainty[C]//IEEE International Conference on Robotics and Automation.2007:1617-1624.

[15]Hsu D,KINDEL R,LATOMBE J C,et al.Randomized kinodynamic motion planning with moving obstacles[J].International Journal of Robotics Research,2002,21(3):233-255.

[16]RODRIGuEZ,TANG X,LIEN J M,et al.An obstacle-based rapidly-exploring random tree[C]// IEEE International Conference on Robotics and Automation.2006:895-900.

[17]KAMON I,RIVLIN E,RIMON E.A new range-sensor based globally convergent navigation algorithm for mobile robots[C]//IEEE International Conference on Robotics&Automation.1995: 429-435.

[18]NG J,BR¨ANL T.Performance comparison of bug navigation algorithms[J].Journal of Intelligent and Robotic Systems,2007,50(1):73-84.

[19]KAMON I,RIMON E,RIVLIN E.TangentBug:a range-sensor-based navigation algorithm[J]. International Journal of Robotics Research,1998,17(9):934-953.

[20]MARAVALL D,LOPE J D,FuENTEs J P.Visual bug algorithm for simultaneous robot homing and obstacle avoidance using visual topological maps in an unmanned ground vehicle[M].New York:Springer International Publishing,2015:301-310.

[21]ZOHAIB M,PAsHA S M,JAVAID N,et al.Intelligent bug algorithm(IBA):a novel strategy to navigate mobile robots autonomously[C]//Springers International Multi Topic Conference. 2013.

[22]HAsHMI U,AFsHAN F,RAFIQ M.Performance analysis of diferent optimal path planning bug algorithms on a client server based mobile surveillance UGV[C]//International Conference on Intelligent Systems Modelling&Simulation.2013:30-35.

[23]BuNIYAMIN N,WAN N W,SARIFF N,et al.A simple local path planning algorithm for autonomous mobile robots[J].International Journal of Systems Applications,Engineering& Development,2011,5(2):151-159.

[24]MOHAMED E F,ELMETWALLY K,HANAFY A.An improved Tangent Bug method integrated with artifcial potential feld for multi-robot path planning[C]//International Symposium on Innovations in Intelligent Systems and Applications.2011:555-559.

[25]KHATIB M,CHATILA R.An extended potential feld approach for mobile robot sensor-based motions[C]//Intelligent Autonomous Systems.1995:490-496.

[26]KHATIB O.Real-time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics Research,1986,5(1):90-98.

[27]HuANG Y,Hu H,LIu X.Obstacles avoidance of artifcial potential feld method with memory function in complex environment[C]//Intelligent Control and Automation.2010:6414-6418.

[28]ZOHAIB M,PAsHA S M,JAVAID N,et al.An improved algorithm for collision avoidance in environments having U and H shaped obstacles[J].Studies in Informatics&Control,2014, 23(1):97-106.

[29]SEZER V,GOKAsAN M.A novel obstacle avoidance algorithm:“follow the gap method”[J]. Robotics&Autonomous Systems,2012,60(9):1123-1134.

[30]BROCK O,KHATIB O.High-speed navigation using the global dynamic window approach[C]// IEEE International Conference on Robotics and Automation.1999:341-346.

[31]SEDER M,PETROVIC I.Dynamic window based approach to mobile robot motion control in the presence of moving obstacles[C]//IEEE International Conference on Robotics and Automation. 2007:1986-1991.

[32]TANG P,ZHANG R,LIu D,et al.Research on near-feld obstacle avoidance for unmanned surface vehicle based on heading window[C]//24th Chinese Control and Decision Conference.2012: 1262-1267.

[33]ZHANG R,TANG P,Su Y,et al.An adaptive obstacle avoidance algorithm for unmanned surface vehicle in complicated marine environments[J].IEEE/CAA Journal of Automatica Sinica,2014, 1(4):385-396.

[34]SIMETTI E,TORELLI S,CAsALINO G,et al.Experimental results on obstacle avoidance for high speed unmanned surface vehicles[C]//Oceans.2014:1-6.

[35]Wu G,SHI D,GuO J.Deliberative collision avoidance for unmanned surface vehicle based on the directional weight[J].Journal of Shanghai Jiaotong University(Science),2016,21(3):307-312. [36]CHAKRAVARTHY A,GHOsE D.Obstacle avoidance in a dynamic environment:a collision cone approach[J].Systems&Humans,1998,28(5):562-574.

[37]CHAKRAVARTHY A,GHOsE D.Collision cones for quadric surfaces[J].IEEE Transactions on Robotics,2011,27(6):1159-1166.

[38]FIORINI P.Motion planning in dynamic environments using velocity obstacles[J].International Journal of Robotics Research,1998,17(7):760-772.

[39]KuWATA Y,WOLF M T,ZARZHITsKY D,et al.Safe maritime autonomous navigation with colregs,using velocity obstacles[J].IEEE Journal of Oceanic Engineering,2014,39(1):110-119. [40]JOHANsEN T A,PEREZ T,CRIsTOFARO A.Ship collision avoidance and colregs compliance using simulation-based control behavior selection with predictive hazard assessment[J].IEEE Transactions on Intelligent Transportation Systems,2016,17:1-16.

[41]NAEEM W,IRWIN G W,YANG A.COLREGs-based collision avoidance strategies for unmanned surface vehicles[J].Mechatronics,2012,22(6):669-678.

[42]BREIVIK M,HOVsTEIN V,FOssEN T.Straight-line target tracking for unmanned surface vehicles[J].Modeling Identifcation and Control,2008,29(4):131-149.

[43]BREIVIK M,FOssEN T I.Guidance laws for planar motion control[C]//IEEE Conference on Decision and Control.2008:570-577.

Dynamic obstacle avoidance for USV based on velocity obstacle and dynamic window method

ZHANG Yangyang,QU Dong,KE Jun,LI Xiaomao
(School of Mechatronic Engineering and Automation,Shanghai University,Shanghai 200072,China)

Velocity obstacle is a dynamic obstacle avoidance algorithm widely used on an unmanned surface vehicle(USV).However,traditional velocity obstacle does not consider the efect of USV’s kinematics and obstacle’s motion information error,and does not know when to start and terminate avoidance.Based on velocity obstacle,USV and obstacle are represented by an ellipse.A method for solving ellipse tangent is proposed.Considering the kinematics performance of USV,a dynamic window method and velocity obstacle are combined.Only the speed and course that the USV can reach in a given time are used in calculation.By comparing the collision time and the time required for the USV to fnish avoidance to determine when to start avoidance,a method to terminate obstacle avoidance is proposed.A virtual obstacle method is then proposed to reduce infuence of obstacle’s motion information error.Feasibility and efectiveness of the method are verifed by simulation.

unmanned surface vehicle(USV);dynamic avoidance;velocity obstacle; dynamic window;virtual obstacle

TP 242

A

1007-2861(2017)01-0001-16

10.3969/j.issn.1007-2861.2016.07.021

2017-01-07

國家自然科學基金資助項目(61673254);上海市自然科學基金資助項目(13ZR1454300);上海市科委能力建設資助項目(14500500400)

李小毛(1981—),男,研究員,研究方向為圖像處理、雷達數(shù)據(jù)處理、無人艇環(huán)境感知、導航和控制及其總體技術(shù).E-mail:lixiaomao@shu.edu.cn