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

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

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院,上海 200072)

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

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

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院,上海 200072)

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

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

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

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

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

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

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

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

1 基于橢圓的速度障礙法

1.1 速度障礙原理

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

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

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

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

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

式(4)等價(jià)表述為

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

當(dāng)存在多個(gè)障礙物時(shí),求出所有障礙物的VO集合的并集后得到總的VO集合:

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

1.2 計(jì)算橢圓切線

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

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

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

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

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

圖2 橢圓切線Fig.2 Ellipse tangents

計(jì)算切點(diǎn)T1obs,T2obs在船體坐標(biāo)系中的坐標(biāo)(如圖2中的T1boat,T2boat),求出兩個(gè)切點(diǎn)后即可計(jì)算出在船體坐標(biāo)系中障礙物相對(duì)于無人水面艇的兩條切線.

2 避障算法

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

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

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

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

圖4 避障流程Fig.4 Avoidance fowchart

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

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

動(dòng)態(tài)窗口法考慮了無人水面艇的運(yùn)動(dòng)學(xué)性能[1],只計(jì)算無人水面艇在給定時(shí)間窗口?t內(nèi)能達(dá)到的移動(dòng)速度,即速度窗口

和角速度,即角速度窗口

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

式中,θh為無人水面艇當(dāng)前的艏向.

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

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

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

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

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

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

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

2.2.1 開始避障

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

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

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

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

定義開始避障時(shí)間為tavoid,則

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

2.2.2結(jié)束避障

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

即可結(jié)束避障.式(19)中,VLOS為軌跡跟蹤規(guī)劃的速度矢量,Vdest為無人水面艇以當(dāng)前速度大小沿目標(biāo)點(diǎn)方向運(yùn)動(dòng)時(shí)的速度矢量.

2.3 虛擬障礙物

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

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

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

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

3 仿真實(shí)驗(yàn)

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

3.1 單個(gè)動(dòng)態(tài)障礙物場(chǎng)景

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

圖7中,假設(shè)障礙物的速度與運(yùn)動(dòng)方向恒定.但是,在實(shí)際場(chǎng)景中障礙物的速度與運(yùn)動(dòng)方向通過傳感器獲得,往往會(huì)有很大的誤差.為了模擬傳感器的這些誤差,對(duì)障礙物的速度與運(yùn)動(dòng)方向增加干擾,即無人水面艇獲得的是增加干擾后的障礙物的速度與運(yùn)動(dòng)方向.圖8為障礙物的實(shí)際速度、運(yùn)動(dòng)方向與無人水面艇獲得的障礙物的速度、運(yùn)動(dòng)方向.

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

圖7 單個(gè)動(dòng)態(tài)障礙物場(chǎng)景Fig.7 Single dynamic obstacle scene

圖8 障礙物速度與運(yùn)動(dòng)方向Fig.8 Speeds and courses of obstacle

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

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

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

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

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

3.2 多個(gè)動(dòng)態(tài)障礙物場(chǎng)景

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

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

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

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

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

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

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

4 結(jié)束語

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

本方法并未采用海事規(guī)則,即當(dāng)無人水面艇的最大速度小于障礙物的速度時(shí),如果無人水面艇選擇從動(dòng)態(tài)障礙物前方進(jìn)行避障則會(huì)有碰撞危險(xiǎn),因此采用海事規(guī)則的動(dòng)態(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

國(guó)家自然科學(xué)基金資助項(xiàng)目(61673254);上海市自然科學(xué)基金資助項(xiàng)目(13ZR1454300);上海市科委能力建設(shè)資助項(xiàng)目(14500500400)

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