一種動態環境下無人艇局部路徑規劃方法

容 煜 何正偉 王森杰

(武漢理工大學航運學院 武漢 430063)

0 引 言

水面無人艇(unmanned surface vehicle, USV)路徑規劃是無人艇應用中的一個關鍵問題[1].然而,真實航行環境復雜多變,動態環境[2]不僅包括島嶼、海岸線、錨地等靜態礙航因素,還包括運動船舶、漂浮物等動態目標.因此,需要路徑規劃系統實時理解動態環境并為無人艇提供自動避碰路徑.

全局路徑規劃是指根據水域環境設計出一條起點到終點最優路徑,雖然解決了路線可達問題,但是僅考慮了靜態障礙物.因此需要局部路徑規劃技術通過傳感器獲取附近障礙物信息,實時修正當前航線.當前局部路徑技術根據方法原理主要分為傳統算法、速度障礙域、動態窗口法、強化學習四種類型.傳統算法是改進常用的路徑規劃算法用于滿足船舶避碰的需求.Fuat等[3]針對狹窄水帶提出了一種動態環境下的人工勢場法,分析了船舶對遇和追越兩種避讓情況的處理方式.Wang等[4]改進蟻群算法提出了滿足《國際海上避碰規則》的蟻群算法.速度障礙法是通過計算最近會遇點、最近會遇距離和最近會遇時間來進行碰撞區域建模和避碰的方法.Xia等[5]提出了一種非線性有限時間速度障礙法,通過高斯混合模型和高斯回歸算法預測動態障礙物的軌跡,能夠處理非線性運動的障礙物.動態窗口法(dynamic window approach,DWA)是一種對無人艇運動狀態參數采樣生成一系列軌跡,然后利用評價函數得到最優路徑的方法.該方法將軌跡規劃和軌跡避碰進行了分開建模,因此易與其它算法結合,如文獻[2]改進DWA算法,在DWA的評價函數中增加了考慮海況的權重系數,在海況級別變高時,通過降低速度的權重并增加距離的權重來確保USV的安全性.基于深度強化學習的規劃方法是將代理機器人放在環境中自由探索,通過設置與環境交互的獎勵,讓網絡模型端到端地學會自主避碰.封佳祥等[6]基于Q-learning算法對無人艇路徑規劃任務進行了建模,該算法實現了在簡單環境下的無人艇自主避碰問題.

現有方法主要研究在滿足算法實時性的前提下如何實現無人艇的動態避碰問題,其重心放在了避碰時機和安全性上,沒有考慮動態障礙物不確定性和實時變化的問題,導致研究結果存在局限性.

文中針對動態障礙物不確定性的問題,構建軌跡障礙域模型對局部窗口內多條船舶未來航行范圍同時進行估計,并考慮《避碰規則》提出改進的動態窗口算法,高效地求解局部避碰路徑,并設計真實水域環境驗證該局部路徑規劃方法的適應性和實用性.

1 問題描述與分析

假設無人艇可以準確地、實時地獲取環境信息,并且路徑規劃系統輸出路徑可以得到運動控制系統良好執行.

圖1為局部路徑規劃場景,由圖1可知:無人艇在視野范圍內發現有動態障礙物,局部路徑規劃系統開始計算與分析,在計算出的障礙物未來軌跡范圍不影響無人艇安全航行時,在線局部路徑規劃模塊的輸出路徑和全局規劃的路徑重合,實現按照既定路線行駛,在存在碰撞風險時,在線局部路徑規劃系統在局部區域內重新規劃路徑完成動態避碰,并在沖突結束后,重新回到全局路線上,實現復航.

圖1 局部路徑規劃場景示意圖

因此局部路徑規劃算法需要實現:

1) 在線局部路徑規劃算法僅在出現動態障礙物時修改路徑,否則按照全局路徑航行.

2) 能夠對動態障礙物的軌跡進行精準估計.

3) 無人艇在動態環境中能自動避開障礙物的前提下,選擇最優路徑完成復航.

2 基于改進動態窗口的局部路徑規劃方法

文中提出了圖2的改進動態窗口算法(improved dynamic window algorithm, IDWA),利用軌跡采樣預測一系列無人艇局部路徑,為了解決規劃需要滿足無人艇操縱性能、航行安全的問題,采用無人艇性能約束對采樣軌跡的最大速度、最大回旋半徑、加速度等屬性鑒別,刪除超出操縱性能的軌跡,采用無人艇碰撞約束用于檢查規劃軌跡是否侵入它船的安全領域.過濾得到可行軌跡后,改進動態窗口算法為了實現無人艇的動態避碰與復航,設計了碰撞風險函數,實時計算路徑的動態碰撞風險,添加《避碰規則》代價用于解決避碰復雜性的問題,采用全局路徑跟隨代價對規劃路徑的偏航程度和航速進行評價.

圖2 IDWA的局部路徑規劃方法框架圖

由于動態障礙物的位置是動態變化的,無法直接確定動態障礙物的未來的位置,因此給路徑的避碰決策帶來困難.為了解決這個問題,文中提出軌跡障礙域模型(trajectory obstacle area,TOA)用于實時對非線性運動的動態障礙物進行精準估計.

2.1 環境模型

路徑規劃需要避開靜態障礙物.通過柵格化導航區域的離線地圖,構建環境模型,根據無人艇的視野范圍決定地圖的分辨率,因此,無人艇在環境模型中被視為一個柵格點.柵格化的地圖由二維數組構成,網格的值為255表示可行區域,0表示障礙物區域,可以被保存用于判斷路徑是否與障礙物發生碰撞.

2.2 軌跡采樣

軌跡采樣用于獲取多組預測的局部路徑,將笛卡爾坐標系的位置轉換到Frenet坐標系中.Moritz等[7]將軌跡采樣過程分解為

(1)

式中:s(t)為無人艇從起始點沿著全局路徑行駛的弧長;d(t)為無人艇當前位置偏離全局路徑的距離;(α0,α1,…,α5)為s(t)多項式待求解系數;(β0,β1,…,β5)為d(t)多項式待求解系數.

(2)

同樣的,可以得到關于dt的方程組,因此可以通過求解方程組(2)得到st和dt的方程系數解.通過求解過程發現,給定無人艇的當前狀態作為初始狀態參數.

(3)

可以通過采樣一系列無人艇的目標狀態

(4)

來求取方程得到無人艇未來的一系列軌跡點.每次采樣tmax為最大預測時間,ttick為時間刻度,預測每個ttick時的無人艇軌跡,tmax時間內無人艇軌跡點集合代表一組采樣軌跡,根據采樣區域大小Wmax與刻度s可以得到Wmax/s組軌跡.

2.3 軌跡障礙域模型

圖3為軌跡障礙域模型的網絡圖,模型使用GRU網絡對船舶樣本軌跡序列X進行建模解決多步長連續預測的問題,利用全連接層神經網絡實現群體船舶相對運動信息的提取,多條軌跡在經過編碼器和解碼器的雙層神經網絡結構后可以實現多條軌跡時空特征和全局交通態勢特征的融合Y′.鑒別器用于鑒別生成軌跡X+Y′和真實軌跡X+Y的真假.TOA作為一種非線性預測模型,適用于會遇局面下船舶未來航行區域的推斷.

圖3 軌跡障礙域模型網絡結構

2.4 約束項

算法經過軌跡采樣之后,需要設計約束項對結果進行過濾,篩選出滿足無人艇操作性能、無碰撞的可行路徑.設置了無人艇性能約束項和無人艇碰撞約束項來滿足以上需求:

1)Pfilter1為USV性能約束.

Pfilter1(L)={li|li∈L,φv(li)≤

(5)

式中:L為采樣軌跡集合;li為第組采樣軌跡;φv(li)為跟隨軌跡li的需求速度;φc(li)為跟隨軌跡li的需求曲率;φa(li)為跟隨軌跡li的需求加速度;Vmax為USV的最大航行速度;R為USV最小回旋半徑;amax為USV的最大加速度.

2) 船舶領域是保障船舶安全的最小領域[8],因此文中將無人艇安全領域內存在障礙物的采樣軌跡點均視為碰撞軌跡.四元船舶領域邊界[9]到船體中心的距離為r,由前后左右四個方向的半徑決定.Cfilter2為USV碰撞約束.

Cfilter2(L)={li|li∈L,φd(li)≥r}

(6)

式中:φd(li)為USV按照軌跡li航行時與目標船的距離,需要軌跡障礙域模型對目標船的航行范圍進行預測并計算歐氏距離得到.

2.5 代價函數

1) 在四元船舶領域模型的基礎上,考慮障礙物距離領域邊界越遠越安全,因此將軌跡li的碰撞風險J1定義為所有障礙物與無人艇領域邊界距離的倒數和.

(7)

2) 全局路徑跟隨代價函數J2為實現無人艇避碰后復航的關鍵函數.定義采樣軌跡與全局路線的偏差程度Δd為

(8)

定義采樣軌跡速度與期望航速的偏差程度Δv為

(9)

J2(li)=Δd+Δv

(10)

式中:Δd給予偏航越大的路徑更高的代價,保證規劃軌跡盡量沿著全局航線行駛.Δv給予與期望速度偏差越大的路徑更高的代價,保證規劃軌跡盡量保持期望航速行駛,避碰時首先考慮轉向來進行避碰,僅靠轉向無法避碰時才通過改變航速來實現避碰,更加符合實際需求.

3) 《避碰規則》代價函數是實現規劃路徑滿足《避碰規則》約束的關鍵.首先人為定義代價項J3為

(11)

(12)

2.6 評價模型

本文為了適用于船舶沖突局面,設計了四種綜合代價函數.

1) 對遇當USV處于對遇局面時,雙方都有避讓責任,雙方應該右轉從雙方的左舷經過.因此路徑的總代價函數應該包含J3項,對不滿足規則的路徑進行懲罰.

2) 右舷交叉當USV處于小角度右舷交叉相遇局面和大角度右舷交叉相遇局面時,USV作為避讓船,有義務右轉從目標船的船尾經過或直線減速.因此路徑的總代價函數應該包含J3項,對不滿足規則的路徑進行懲罰.

3) 追越當USV處于左舷追越局面和右舷追越局面時,USV作為讓路船需要根據風險決定從目標左舷還是右舷追越,此時代價項J1和J2就可以滿足避碰要求,因此不需要添加J3即J3=0.

4) 左舷交叉當USV處于被追越、小角度左舷交叉相遇局面和大角度左舷交叉相遇局面時,此時USV作為直航船沒有避讓義務,應保持航速航向,但是緊急局面下可以右轉避碰.此時路徑代價項J1和J2就可以滿足避碰要求.

表1 不同會遇局面下的代價函數

綜上,動態環境下局部路徑規劃可以被表述為一個條件排序問題,最優的局部避碰路徑Lf可以被表示為代價函數最小值時的取值.

L*=Filter2(Filter1(L))

(13)

Lf=argminJ(l|l∈L*)

(14)

式中:L={l1l2…lm}為USVm組采樣軌跡;Filter1為USV性能約束;Filter1為USV碰撞約束;L*為可行路徑的集合.

3 實驗及分析

選取舟山港岱山附近水域作為實驗場景,實現了下列三種路徑規劃算法進行對比分析:

A*算法根據離線地圖規劃全局最優路徑.

AD*算法(anytime dynamic A*)根據初始環境,規劃出無碰撞最短路徑,在障礙物的位置發生變化時,重新規劃局部路徑.

IDWA在線規劃局部路徑.軌跡采樣設置無人艇當前速度為10 kn,每過20 s計算一次無人艇狀態,一次采樣最大預測時間為500 s,設置最大采樣寬度為200 m,每次采樣160條軌跡.評價模型參數k1=1,k2=1,k3=1.TOA模型訓練每次batch的大小設置為32,訓練迭代次數設置為1 800.軌跡序列長度設置為11,預測序列長度設置為11.采樣時間間隔為10 s.

設置USV參數見表2,Vmax為最大航速,Rmax為最小回轉半徑,amax為最大加速度,lusv為船長.起點坐標是(122°10.439′,30°12.151′),柵格點為(261,607),位于柵格圖中左側.終點是(122°14.723′,30°11.793′),柵格點為(795,669),位于柵格圖中右下側.文中在環境中的動態障礙物信息見表3,驗證避碰有效性僅測試算法能否避碰船舶A,船舶B、C用于驗證IDWA算法在不同會遇局面下的有效性.

表2 無人艇綜合實驗參數

表3 動態障礙物運動狀態參數

表4為在路徑長度、轉向點數量、與障礙物最小距離、計算時間共四種指標上對不同算法性能進行了統計.其中計算時間是在CPU R6-2600,32g內存電腦上執行10次算法的平均結果.

表4 AD*、改進動態窗口算法質量的評價結果

A*算法規劃路徑見圖4.規劃出一條從起點到終點的全局路徑,但無法避開動態障礙物,在實驗第22 min與船舶A發生了碰撞.

圖4 A*實驗

AD*算法規劃路徑見圖5.無人艇根據當前水域環境規劃出一條無碰撞最優路徑,在第33 min無人艇與船舶A相遇,算法重新規劃了避碰路線,然而,規劃的路徑轉向點過多且曲率不平滑,避碰路徑緊貼靜態障礙物邊界.在第40 min,無人艇抵達終點.

圖5 AD*實驗

IDWA見圖6.全局路徑為起點至終點的直線,無人艇能夠有效跟隨初始路徑,同時處理靜態和動態障礙物的復雜環境,在與它船構成沖突局面時能夠及時做出符合《避碰規則》的避碰決策,并在沖突消失后復航到全局路徑上,在路徑中詳細狀態見表5.

表5 IDWA路徑規劃狀態

圖6 IDWA實驗結果圖

由圖6可知:A*算法無法實現動態避障,AD*算法規劃出最終的路徑波動較為明顯,最大曲率為0.785,規劃的路徑彎曲程度較大,在避碰過程中需要在設定的20 s內轉向約45°,且與障礙物最短距離為26 m,路徑安全性無法保障.改進動態窗口法規劃出的路徑曲率連續符合無人艇的操縱性能,為無人艇的后續運動控制和路徑跟蹤提供良好條件,與障礙物最短距離為128 m.在實時規劃避碰路徑的響應速度方面,AD*算法計算平均時間是0.62 s,改進動態窗口算法平均時間是0.28 s,所提算法在計算時間和路徑質量上都優于AD*算法.同時,實驗結果證明在線局部路徑規劃算法可以同時處理靜態障礙物和動態障礙物,能夠有效解決動態環境下的無人艇避碰和路徑規劃問題.

4 結 束 語

針對航行環境中動態障礙物不確定性和避碰復雜性的問題,文中提出了一種新的動態環境下的無人艇局部路徑規劃算法,構建了面向船舶實時避碰的軌跡障礙域模型,以實現局部窗口內交通態勢提取,為無人艇的避碰決策和實時路徑規劃提供了基礎.基于柵格化環境模型,引入四元船舶領域和《避碰規則》改進動態窗口算法,解決了動靜態環境下路徑規劃一體化的問題.設計無人艇性能約束和全局路徑跟隨代價函數,實現了避碰后自動復航的需求.實驗結果表明,用改進動態窗口算法可以有效實現不同會遇局面下的避碰與復航,規劃的路徑較A*、AD*算法更平滑,更符合無人艇操縱性能,實時規劃的計算時間更短.后續研究考慮將風、浪、流等環境信息加入仿真環境中,結合船舶動力學模型和船舶運動學模型,以適用于更復雜的場景.