瞿 棟,彭 艷,蒲華燕,羅 均, 黃承義,柯 俊

(1.上海大學(xué)機(jī)電工程與自動化學(xué)院,上海200444；2.國家海洋局北海海洋工程勘察研究院,山東青島266061)

無人艇在水面航行時(shí)需對運(yùn)動船只進(jìn)行規(guī)避,保證自身的航行安全.動態(tài)避碰技術(shù)是無人艇自主化程度的重要體現(xiàn).然而,海面目標(biāo)檢測和跟蹤十分困難.目前環(huán)境感知手段包括導(dǎo)航雷達(dá)、激光雷達(dá)、攝像機(jī).導(dǎo)航雷達(dá)探測范圍廣,但是分辨率、探測頻率較低.激光雷達(dá)分辨率較高,但是探測范圍只有百米級,且易受船體顛簸的影響.攝像機(jī)能獲得豐富的紋理和色彩信息,但是無法準(zhǔn)確估計(jì)目標(biāo)的方位和距離,且易受光照條件影響.另外,不同于地面無人車,船舶本身就是一個(gè)欠驅(qū)動、大慣性、強(qiáng)時(shí)滯的系統(tǒng),航行過程中因受風(fēng)浪涌流干擾導(dǎo)致無人艇姿態(tài)不穩(wěn),也會對無人艇傳感探測造成影響.因此,在目前的技術(shù)手段下無人艇難以觀測到精確的目標(biāo)運(yùn)動信息,使得無人艇在動態(tài)避碰過程中出現(xiàn)策略跳變,導(dǎo)致運(yùn)動狀態(tài)不穩(wěn)定,影響航行安全.本工作將傳感技術(shù)局限性導(dǎo)致的獲得障礙物信息存在不確定性的現(xiàn)象,統(tǒng)稱為障礙信息不確定性,其中障礙速度的不確定性是影響避碰成功率的關(guān)鍵.

針對無人艇動態(tài)避碰問題,一般有2種解決辦法.一種是基于全局或者部分信息已知的路徑規(guī)劃方法,又稱為零階避碰方法.代表性算法包括勢場法[1]、A*[2]、神經(jīng)網(wǎng)絡(luò)方法[3]等.另一種是基于行為的避碰方法,又稱為一階避碰方法,該方法能對無人艇和障礙物運(yùn)動情況作出實(shí)時(shí)反應(yīng),通過改變無人艇自身運(yùn)動狀態(tài)實(shí)現(xiàn)規(guī)避動作.代表性算法包括速度障礙物[4]、動態(tài)窗口法[5]等.

針對障礙速度不確定性條件下的避碰問題,Kuwata等[6]采用速度障礙(velocity obstacle,VO)算法和國際海事避碰規(guī)則(International Regulations for Preventing Collisions at Sea,COLREGS)進(jìn)行動態(tài)目標(biāo)避碰,并將障礙速度不確定性融入VO區(qū)域中,建立新的VO區(qū)域,稱為最差情況下的速度障礙(worst case velocity obstacle,WVO),但是并未給出WVO數(shù)學(xué)模型.Fulgenzi等[7]提出概率速度障礙法,在環(huán)境地圖中將傳感器檢測到的目標(biāo)信息以概率占據(jù)的方式進(jìn)行建模,解決障礙速度不確定性對避碰過程帶來的影響,該方法適用于室內(nèi)移動機(jī)器人.張洋洋等[8]提出了建立4個(gè)虛擬障礙物,投射到無人艇的速度空間,模擬障礙速度不確定性對VO區(qū)的影響,該方法缺乏有效的理論支撐.楊秀霞等[9-10]以無人飛行器為研究對象,在VO算法的基礎(chǔ)上,將動態(tài)不確定性通過威脅障礙速度矢量方向角的偏差來表示,并建立了動態(tài)不確定性VO模型,該方法未進(jìn)一步對無人機(jī)速度空間中障礙速度不確定性進(jìn)行建模.張成鋼等[11]考慮了2個(gè)不確定性來源,即運(yùn)動速度上的不確定性和運(yùn)動方向上的不確定性,用概率統(tǒng)計(jì)的方法為不確定性建模,該方法同樣適用于室內(nèi)移動機(jī)器人.盧艷爽[12]用極大似然估計(jì)法計(jì)算運(yùn)動障礙物航速和航向誤差,依據(jù)計(jì)算結(jié)果對障礙物進(jìn)行二次膨化,將不確定性轉(zhuǎn)為確定性,該方法難以從根本上解決障礙速度不確定性對無人艇避碰的影響.Johansen等[13]采用模型預(yù)測控制方法,根據(jù)動力學(xué)模型及相應(yīng)的控制指令計(jì)算無人艇在位姿空間中的狀態(tài),同時(shí)對障礙物的運(yùn)行狀態(tài)進(jìn)行預(yù)測,并構(gòu)建評估函數(shù)進(jìn)行碰撞風(fēng)險(xiǎn)評估及控制指令擇優(yōu),該方法對傳感器和艇體運(yùn)動的不確定性進(jìn)行了考量,但是在實(shí)驗(yàn)過程中仍然無法避免無人艇避碰策略的劇烈跳動.Park等[14]針對運(yùn)動障礙物軌跡不確定性問題,通過目標(biāo)跟蹤濾波器輸出的估計(jì)協(xié)方差進(jìn)行不確定性建模,并采用概率統(tǒng)計(jì)方法對碰撞風(fēng)險(xiǎn)進(jìn)行計(jì)算,通過在仿真平臺下避讓單個(gè)/多個(gè)運(yùn)動目標(biāo),驗(yàn)證了該算法的可行性.Larson等[15]在無人艇的二維柵格地圖中將障礙物速度不確定性通過障礙物投影區(qū)(projected obstacle area,POA)的方式進(jìn)行建模,采用局部路徑規(guī)劃的方式對含有速度不確定性的障礙物進(jìn)行規(guī)避.

綜上所述,目前國內(nèi)外針對無人艇在障礙信息存在不確定性條件下的避碰方法研究較少,絕大多數(shù)研究尚停留在理論階段,或者在仿真/半實(shí)物仿真狀態(tài)下取得了較好的效果,但在真實(shí)應(yīng)用場景下的避碰效果還有待進(jìn)一步驗(yàn)證.本工作在已有研究基礎(chǔ)上,提出了一種面向障礙速度不確定性的速度障礙(uncertain velocity obstacle,UVO)避碰方法,從宏觀和微觀兩個(gè)層面解決速度不確定性條件下的避碰問題,并利用仿真和海試實(shí)驗(yàn)對該方法的有效性進(jìn)行驗(yàn)證.

1 理論基礎(chǔ)

1.1 VO原理

速度障礙法是1998年由Fiorini等[16]提出的.在北東地(north east down,NED)坐標(biāo)系下,假定無人艇的位置為pA,速度為vA,障礙物位置為pB,速度為vB.無人艇加上障礙物的尺寸為r,如圖1所示.

圖1 速度障礙法原理Fig.1 Principle of velocity obstacle method

以無人艇的位置pA為起點(diǎn),對障礙物圓作兩條切線,左切線角度為λL,右切線角度為λR,如圖1中綠線所示.經(jīng)過障礙物的圓心pB作兩根切線的垂線TL和TR.若無人艇相對于障礙物的速度vAB=vA-vB處于λL和λR之間,則將與障礙物發(fā)生碰撞.將發(fā)生碰撞的無人艇速度矢量集合稱為VO,表示為

在實(shí)際應(yīng)用中,對無人艇的參考速度空間進(jìn)行離散化,(vi,θj)即無人艇的1個(gè)參考速度矢量(見圖2),速度大小被劃分為m份,速度方向被劃分為n份.

1.2 最近會遇點(diǎn)方法

如圖3所示,采用最近會遇點(diǎn)(closest point of approach,CPA)方法評估己方船只與其他來船的碰撞風(fēng)險(xiǎn)[17],計(jì)算2個(gè)參數(shù)即最近會遇距離dcpa和最快會遇時(shí)間tcpa,

圖2 速度空間離散化Fig.2 Discretization of velocity space

式中,dcpa表示兩船會遇過程中的最近距離,tcpa表示兩船到達(dá)最近會遇點(diǎn)所需的時(shí)間,ε為設(shè)定的極小值.文獻(xiàn)[18]在無人艇避碰中用CPA方法度量無人艇和障礙物船只的碰撞風(fēng)險(xiǎn).將dcpa和tcpa與設(shè)定的閾值εDCPA和εTCPA進(jìn)行比較,若滿足式(3),則視為無人艇與障礙物之間存在碰撞風(fēng)險(xiǎn),

圖3 最近會遇點(diǎn)原理Fig.3 Principle of closest point of approach

1.3COLREGS

文獻(xiàn)[19-21]將COLREGS應(yīng)用于無人艇避碰中,其中在超越、對遇和右舷來船3種場景下,己方船只應(yīng)從右側(cè)避讓來船；在左舷來船場景下,己方船只應(yīng)保持直行,由來船采取避讓措施(見圖4).

對超越、對遇、左舷來船、右舷來船4種海事規(guī)則場景的定義(見圖5)如下(φB表示無人艇相對于障礙物船首的方向)：

(1)超越場景,-180°＜ φB＜ -112.5°或112.5°＜ φB＜ 180°；

(2)對遇場景,-15°＜ φB＜ 15°；

(3)左舷來船場景,15°＜φB＜112.5°；

(4)右舷來船場景,-112.5°＜φB＜-15°.

圖4 COLREGS場景Fig.4 COLREGS scenarios

圖5 COLREGS場景判斷方法Fig.5 Judgment method of COLREGS

2 不確定性條件下的速度障礙法

2.1 基于自適應(yīng)閾值的CPA和COLREGS計(jì)算方法

用CPA方法計(jì)算dcpa和tcpa用于評估碰撞風(fēng)險(xiǎn),若滿足式(3),則無人艇應(yīng)進(jìn)入避碰狀態(tài)；否則回歸設(shè)定航路繼續(xù)航行.因此,CPA評估結(jié)果的穩(wěn)定性十分重要.然而,由于障礙速度的不確定性,導(dǎo)致dcpa和tcpa在設(shè)定的閾值附近波動,進(jìn)而導(dǎo)致無人艇在避碰和回歸航路2種狀態(tài)之間來回切換,對艇體自身的穩(wěn)定性和避碰安全性產(chǎn)生較大影響.

本工作采用自適應(yīng)閾值的方法,將靜態(tài)閾值εDCPA和εTCPA設(shè)定為動態(tài)閾值,提高碰撞風(fēng)險(xiǎn)評估結(jié)果的穩(wěn)定性,

式中,ΔTcpa和ΔDcpa為設(shè)定的常數(shù),k為自適應(yīng)系數(shù),與無人艇上一時(shí)刻的狀態(tài)相關(guān)聯(lián),滿足式(4),則視為當(dāng)前幀存在碰撞風(fēng)險(xiǎn).將式(4)的計(jì)算結(jié)果稱為初始評估狀態(tài),設(shè)為pi.pi是一個(gè)二值化狀態(tài),0代表無風(fēng)險(xiǎn),1代表潛在風(fēng)險(xiǎn).為了保證避碰過程的連續(xù)性,進(jìn)一步提高碰撞風(fēng)險(xiǎn)評估結(jié)果的穩(wěn)定性,采用滾動滑窗法對過去一段時(shí)間的碰撞風(fēng)險(xiǎn)評估結(jié)果進(jìn)行計(jì)算,滑窗中的元素為i時(shí)刻的初始評估pi.滑窗的長度選取依賴于無人艇的控制頻率,本工作采用10幀.在滑窗中采用7/10法則,若過去一段時(shí)間內(nèi)超過70%的時(shí)刻被判定為存在碰撞風(fēng)險(xiǎn),則最終碰撞風(fēng)險(xiǎn)評估結(jié)果記為Ri=1,無人艇進(jìn)入避碰狀態(tài)；否則,無人艇退出避碰狀態(tài),記為Ri=0(見圖6).

圖6 滑窗方法計(jì)算最終碰撞風(fēng)險(xiǎn)Fig.6 Calculation of the final collision risk by the sliding window method

同理,由于在計(jì)算COLREGS場景的過程中以動態(tài)障礙物船首方向?yàn)閰⒖?因此判斷結(jié)果極易在4個(gè)邊界角度(±15°,±112.5°)附近出現(xiàn)變換.本工作在以上4個(gè)邊界角度附近設(shè)置4個(gè)邊界緩沖區(qū)(見圖7),邊界緩沖區(qū)的大小為2δ,lb和rb分別為緩沖區(qū)的左邊界和右邊界.通過設(shè)置以上4個(gè)邊界緩沖區(qū),并且關(guān)聯(lián)上一時(shí)刻無人艇的COLREGS類型,無人艇COLREGS類型切換的邏輯如表1所示,其中T表示當(dāng)前時(shí)刻,T-1表示上一時(shí)刻.

圖7 邊界緩沖區(qū)設(shè)置規(guī)則Fig.7 Setting rules of boundary buffer

2.2 UVO數(shù)學(xué)模型

文獻(xiàn)[6]中對障礙物不確定性條件下產(chǎn)生的VO區(qū)進(jìn)行了闡述,新的VO區(qū)(即WVO)是一個(gè)包含原VO區(qū)的錐形.文獻(xiàn)[9]構(gòu)建了動態(tài)不確定性速度障礙模型,并運(yùn)用幾何法進(jìn)行了推導(dǎo)和證明.本工作在無人艇的速度空間中對障礙物不確定性條件下的速度障礙區(qū)進(jìn)行了建模,將新的VO區(qū)稱為UVO(見圖8).

表1 COLREGS場景切換邏輯Table 1 Switching logic of COLREGS types

圖8 無人艇參考速度空間中的UVOFig.8 UVO in reference velocity space

假定障礙速度不確定性度量為ΔvB和ΔφB,對ΔvB和ΔφB進(jìn)行離散處理,分別離散成N和M份,則障礙物的實(shí)際速度可以看成是一個(gè)速度集合：

在無人艇的參考速度空間中,將障礙物的速度vB疊加到無人艇后,即可將障礙物看作是靜止的.以障礙物速度大小的不確定量ΔvB為半徑作圓(見圖8),紅圈即可等效為障礙物在速度不確定性條件下的速度集合(見式(6)).因此,可以將UVO區(qū)等效為障礙物在一個(gè)確定的速度下得到的,則障礙速度不確定性轉(zhuǎn)化為確定性,

2.3 代價(jià)空間設(shè)計(jì)與策略優(yōu)化

對于無人艇的參考速度空間(v,θ),需要計(jì)算每個(gè)參考矢量(vi,θj)的代價(jià)值,即評價(jià)函數(shù)J(vi,θj),該函數(shù)是一個(gè)關(guān)于v和θ的二元函數(shù).文獻(xiàn)[22]給出了典型的評價(jià)函數(shù)形式.本工作主要考慮以下3個(gè)因素：①是否遵守COLREGS；②是否屬于UVO區(qū)；③與參考矢量的偏離程度.將違反COLREGS和屬于UVO區(qū)內(nèi)參考矢量對應(yīng)的代價(jià)值全部置為最大代價(jià)值1,即不可行空間；然后,在其他可行空間中根據(jù)梯度下降法尋找局部最優(yōu)解,避免因?yàn)閷ふ胰肿顑?yōu)而犧牲了避碰策略的穩(wěn)定性.

評價(jià)函數(shù)J(vi,θj)設(shè)計(jì)為

以上代價(jià)空間是一個(gè)關(guān)于(v,θ)的二元曲面,采用梯度下降方法,從代價(jià)空間中選取一個(gè)初始點(diǎn)(vi,θj)作為中心點(diǎn),同時(shí)向4個(gè)方向進(jìn)行搜索,得到4個(gè)備選點(diǎn)(vi-1,θj),(vi+1,θj),(vi,θj-1),(vi,θj+1),分別計(jì)算4個(gè)點(diǎn)的梯度值,選擇梯度下降最快的點(diǎn)作為第二次迭代的中心點(diǎn),再次向4個(gè)方向搜索.以(vi,θj)向(vi,θj+1)搜索為例,梯度值

重復(fù)以上過程,則經(jīng)過N次迭代,若滿足某點(diǎn)向任意方向搜索得到的梯度值均小于所設(shè)定的閾值Δthre,搜索過程結(jié)束,該點(diǎn)即為代價(jià)空間中的一個(gè)局部極小值點(diǎn).將該極小值點(diǎn)對應(yīng)的(vi,θj)作為最佳避碰策略.每次迭代過程都將上一時(shí)刻的最佳避碰策略對應(yīng)的矢量(v,θ)作為當(dāng)前時(shí)刻梯度下降法搜索的初始點(diǎn),這樣就保證了前后2幀之間的輸出策略盡可能相似,避免了由于避碰策略的急劇變化而導(dǎo)致的無人艇運(yùn)動的不穩(wěn)定性.

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

3.1 仿真平臺對比

為了驗(yàn)證本工作提出的UVO算法相比于VO算法具有更好的穩(wěn)定性,在仿真系統(tǒng)中設(shè)置相同的實(shí)驗(yàn)場景和參數(shù),對2種算法進(jìn)行對比分析.在仿真過程中,設(shè)置對遇、左舷來船、右舷來船、超越4種場景,每種場景下VO算法和UVO算法各運(yùn)行10次,共計(jì)40次.統(tǒng)計(jì)無人艇在避碰過程中的3個(gè)重要指標(biāo).

(1)避碰策略跳動次數(shù)Nc,前后2幀避碰角度變化超過20°視為1次策略跳動,該指標(biāo)直觀反映避碰策略的穩(wěn)定性,跳動次數(shù)越少越穩(wěn)定；

(2)避碰成功率Ps,即成功避碰次數(shù)/總實(shí)驗(yàn)次數(shù)；

(3)與障礙物的最近會遇距離dsafe,表示避碰過程中無人艇離障礙物最近時(shí)刻兩船之間的相對距離.

以無人艇與障礙物對遇實(shí)驗(yàn)為例,對比2種不同算法下的避碰效果,如圖9和10所示.

圖9 避碰軌跡對比圖Fig.9 Comparison of the trajectory in collision avoidance

圖10會遇距離和避碰角度對比Fig.10 Comparison of safe distance in collision avoidance and collision angle

圖9 為2種算法對應(yīng)的避碰軌跡,可見VO算法和UVO算法均能有效避開障礙物.圖10為2種算法對應(yīng)的避碰角度和會遇距離變化,可見UVO算法的避碰角度更加穩(wěn)定,前后2幀之間跳動很小,只在避碰結(jié)束時(shí)產(chǎn)生了2次達(dá)到20°以上的跳動,而VO算法則有明顯的角度跳動產(chǎn)生,達(dá)到8次.從最近會遇距離曲線的對比也能看出,UVO算法(53 m)優(yōu)于VO算法(31 m).將40次仿真的結(jié)果進(jìn)行統(tǒng)計(jì),結(jié)論如表2所示.

表2 仿真結(jié)果統(tǒng)計(jì)Table 2 Statistical data of simulation results

從表2可以看出,在相同仿真條件下,基于多次實(shí)驗(yàn)結(jié)果進(jìn)行統(tǒng)計(jì)分析,UVO算法在Nc,Ps和dsafe這3個(gè)主要指標(biāo)上均優(yōu)于VO算法.這是由于在仿真平臺下對障礙物添加一定量的“噪聲”干擾后,將在以下3個(gè)方面影響避碰過程：①開始與結(jié)束避碰的時(shí)刻；②避碰的宏觀策略(無人艇從障礙物的左側(cè)還是右側(cè)規(guī)避)；③避碰的微觀策略(無人艇以多大角度對障礙物進(jìn)行規(guī)避).而本工作提出的UVO算法是基于以上3個(gè)關(guān)鍵點(diǎn)對CPA,COLREGS和VO算法進(jìn)行改進(jìn),從而盡可能降低障礙速度不確定性對無人艇避碰過程的影響,保持艇體運(yùn)動的穩(wěn)定性.另一方面,從dsafe這項(xiàng)指標(biāo)可以看出,UVO算法相比于VO算法繞行距離更遠(yuǎn),如圖9所示.這是由2.2節(jié)所述UVO算法的數(shù)學(xué)模型決定的,UVO算法的本質(zhì)就是將障礙速度的不確定性通過擴(kuò)大VO區(qū)的方式映射到無人艇的速度空間中,從而損失了更多的可行空間,但是對于無人艇的安全航行具有重要意義.

3.2 典型運(yùn)動場景避碰實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證本工作所提UVO算法的有效性,以上海大學(xué)“精海3號”無人艇為實(shí)驗(yàn)平臺,采用激光雷達(dá)作為環(huán)境感知的主要手段[23],以有人駕駛輔助艇作為障礙物,在山東青島某港口進(jìn)行了真實(shí)環(huán)境下的避碰實(shí)驗(yàn)(見圖11,關(guān)注“上海大學(xué)學(xué)報(bào)自然科學(xué)版”微信公眾號,使用菜單欄“小程序”掃描左圖觀看視頻).

圖11 “精海3號”無人艇Fig.11 “JingHai-3” unmanned surface vessel

本實(shí)驗(yàn)設(shè)計(jì)了左舷來船、右舷來船、對遇、超越4種典型會遇場景.圖12為4種場景下的避碰過程.實(shí)驗(yàn)結(jié)果表明：①在以上4種典型會遇場景下,無人艇均能有效避開運(yùn)動障礙物,避碰結(jié)束后繼續(xù)回歸航跡線航線；②在實(shí)驗(yàn)過程中,單次實(shí)驗(yàn)的平均避碰策略跳動次數(shù)為1次,避碰過程平穩(wěn)；③在4組實(shí)驗(yàn)中,無人艇與障礙物最近會遇距離的平均值為44.6 m,避碰

圖12 4種典型會遇場景下的避碰實(shí)驗(yàn)Fig.12 Four types of collision avoidance experiments under typical encounter scenarios

過程安全.通過無人艇實(shí)艇海試,驗(yàn)證了本工作所提UVO算法的有效性.實(shí)際上,在實(shí)艇避碰的過程中,無人艇運(yùn)動也存在一定的不確定性.與此同時(shí),受到海上風(fēng)、浪、涌、流的影響,無人艇運(yùn)動軌跡與仿真平臺下的運(yùn)動軌跡也存在明顯差異,表現(xiàn)為艇艏抖動及路徑不平滑.如圖12(b)和(d)所示.以上因素客觀上也造成了傳感器觀測的不穩(wěn)定,因此在實(shí)艇避碰過程中,無法完全避免避碰策略的跳動.實(shí)驗(yàn)結(jié)果顯示以上4組避碰實(shí)驗(yàn)中,平均避碰策略跳動次數(shù)為小于等于1次.本工作提出的UVO算法通過犧牲時(shí)間和效率換取避碰策略的穩(wěn)定性,在一定程度上能夠避免避碰策略的頻繁跳動.未來,還需結(jié)合傳感器觀測和無人艇運(yùn)動控制兩方面因素共同解決障礙物信息不確定性條件下的避碰問題,進(jìn)一步提升避碰效率,增加安全性和穩(wěn)定性.

4 結(jié)束語

本工作針對無人艇動態(tài)避碰過程中由于障礙速度不確定性導(dǎo)致避碰策略不穩(wěn)定的問題,從宏觀和微觀兩個(gè)尺度上基于VO算法進(jìn)行改進(jìn),仿真對比和實(shí)艇實(shí)驗(yàn)驗(yàn)證了本工作提出的UVO算法具有較好的穩(wěn)定性、安全性和有效性.無人艇的避碰性能未來還有很大的提升空間,在現(xiàn)有的技術(shù)手段下,首先要解決的問題是提高傳感觀測信息的準(zhǔn)確性和穩(wěn)定性；其次,是融合一階和二階避碰方法,從航路規(guī)劃的層面解決運(yùn)動障礙物的規(guī)避問題；最后,在避碰方法中加入人工駕駛的經(jīng)驗(yàn)和方法,提升避碰手段的智能化程度.