面向障礙速度不確定性的無人艇動態避碰

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

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

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

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

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

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

1 理論基礎

1.1 VO原理

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

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

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

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

1.2 最近會遇點方法

如圖3所示,采用最近會遇點(closest point of approach,CPA)方法評估己方船只與其他來船的碰撞風險[17],計算2個參數即最近會遇距離dcpa和最快會遇時間tcpa,

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

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

圖3 最近會遇點原理Fig.3 Principle of closest point of approach

1.3COLREGS

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

對超越、對遇、左舷來船、右舷來船4種海事規則場景的定義(見圖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 基于自適應閾值的CPA和COLREGS計算方法

用CPA方法計算dcpa和tcpa用于評估碰撞風險,若滿足式(3),則無人艇應進入避碰狀態；否則回歸設定航路繼續航行.因此,CPA評估結果的穩定性十分重要.然而,由于障礙速度的不確定性,導致dcpa和tcpa在設定的閾值附近波動,進而導致無人艇在避碰和回歸航路2種狀態之間來回切換,對艇體自身的穩定性和避碰安全性產生較大影響.

本工作采用自適應閾值的方法,將靜態閾值εDCPA和εTCPA設定為動態閾值,提高碰撞風險評估結果的穩定性,

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

圖6 滑窗方法計算最終碰撞風險Fig.6 Calculation of the final collision risk by the sliding window method

同理,由于在計算COLREGS場景的過程中以動態障礙物船首方向為參考,因此判斷結果極易在4個邊界角度(±15°,±112.5°)附近出現變換.本工作在以上4個邊界角度附近設置4個邊界緩沖區(見圖7),邊界緩沖區的大小為2δ,lb和rb分別為緩沖區的左邊界和右邊界.通過設置以上4個邊界緩沖區,并且關聯上一時刻無人艇的COLREGS類型,無人艇COLREGS類型切換的邏輯如表1所示,其中T表示當前時刻,T-1表示上一時刻.

圖7 邊界緩沖區設置規則Fig.7 Setting rules of boundary buffer

2.2 UVO數學模型

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

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

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

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

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

2.3 代價空間設計與策略優化

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

評價函數J(vi,θj)設計為

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

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

3 仿真與實驗

3.1 仿真平臺對比

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

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

(2)避碰成功率Ps,即成功避碰次數/總實驗次數；

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

以無人艇與障礙物對遇實驗為例,對比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種算法對應的避碰軌跡,可見VO算法和UVO算法均能有效避開障礙物.圖10為2種算法對應的避碰角度和會遇距離變化,可見UVO算法的避碰角度更加穩定,前后2幀之間跳動很小,只在避碰結束時產生了2次達到20°以上的跳動,而VO算法則有明顯的角度跳動產生,達到8次.從最近會遇距離曲線的對比也能看出,UVO算法(53 m)優于VO算法(31 m).將40次仿真的結果進行統計,結論如表2所示.

表2 仿真結果統計Table 2 Statistical data of simulation results

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

3.2 典型運動場景避碰實驗

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

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

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

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

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

4 結束語

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