王子靜陳熙源

(1.東南大學(xué)儀器科學(xué)與工程學(xué)院，江蘇 南京210096；2.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，江蘇 南京210096)

無人水面艇(unmanned surface vehicle，USV)具有自主航行、自動作業(yè)能力，成本低、環(huán)境適應(yīng)性強等特點，可用于測繪、勘探、巡邏等領(lǐng)域[1]。 近些年來，出現(xiàn)了眾多的USV 項目，由普利茅斯大學(xué)的海洋和工業(yè)動力分析(MIDAS)研究小組于2004 年啟動研發(fā)的Springer USV 就是其中的突出代表[2]。

路徑規(guī)劃是USV 發(fā)展的關(guān)鍵技術(shù)之一，包括全局規(guī)劃和局部規(guī)劃。 全局規(guī)劃是基于已知信息建立適當(dāng)?shù)沫h(huán)境模型，并尋找符合約束條件的安全路徑。局部規(guī)劃需要根據(jù)船載環(huán)境傳感系統(tǒng)實時獲取的周圍環(huán)境信息動態(tài)調(diào)整和修正局部路徑，以確保USV在航行過程中的安全。

Dijkstra 算法是經(jīng)典的全局路徑規(guī)劃方法之一，可以在實際環(huán)境中搜索最短路徑[3]。 使用啟發(fā)式搜索的A?算法在靜態(tài)環(huán)境的路徑規(guī)劃中得到廣泛使用[4]，并可以添加安全約束[5]，提高生成路徑的安全性，或結(jié)合路徑平滑方法[6]，消除路徑中的“鋸齒”以增強可行性。 此外，許多仿生物智能算法，例如遺傳算法(GA)[7]，蟻群優(yōu)化(ACO)[8-9]，粒子群優(yōu)化(PSO)[10]，以及細(xì)菌覓食算法(BFO)[11]可用于USV 的路徑規(guī)劃。 近些年來，一些研究人員逐漸開始關(guān)注利用神經(jīng)網(wǎng)絡(luò)進(jìn)行路徑規(guī)劃[12]。 文獻(xiàn)[13]提出了一種基于模糊神經(jīng)網(wǎng)絡(luò)的最優(yōu)路徑規(guī)劃模型，該模型將網(wǎng)絡(luò)信息識別與海上航行路線的模式調(diào)度相結(jié)合。

局部路徑規(guī)劃在避障中起著至關(guān)重要的作用。人工勢場(APF)是一種眾所周知的局部路徑規(guī)劃算法，它構(gòu)造了一個虛擬的引力和斥力場，而USV 的運動由吸引力和排斥力的合力決定。 但是，局部極小點和不可達(dá)點的問題限制了該方法的應(yīng)用[14]。 陳彥杰等人提出的一種局部環(huán)境增量采樣的路徑規(guī)劃算法在未知動態(tài)環(huán)境中較傳統(tǒng)的RRT 算法具有更好的規(guī)劃性能[15]。 文獻(xiàn)[16]提出的一種使用新型約束FM方法，能夠模擬運動船舶的動態(tài)行為。

盡管現(xiàn)有很多關(guān)于無人水面艇路徑規(guī)劃的研究，但其中大多數(shù)僅考慮全局規(guī)劃或局部規(guī)劃的單一應(yīng)用場合。 針對存在動態(tài)障礙物的復(fù)雜環(huán)境中的導(dǎo)航和路徑規(guī)劃，本文提出了一種基于改進(jìn)的A?算法和動態(tài)窗口法(DWA)的混合路徑規(guī)劃算法，將全局規(guī)劃與局部規(guī)劃相結(jié)合，實現(xiàn)了動態(tài)環(huán)境下無人艇的路徑規(guī)劃，并通過仿真驗證了本算法的性能。

1 全局規(guī)劃

1.1 環(huán)境描述

為了進(jìn)行路徑規(guī)劃，首先需要選擇一個合適的模型來描述周圍環(huán)境。 本文將USV 工作環(huán)境簡化為二維平面，僅考慮該平面上USV 的偏航和位置變化，而忽略橫滾和俯仰。 并使用柵格地圖來表示，以二進(jìn)制圖像表示，其中可通行區(qū)域被視為1(白色)，而障礙物和危險區(qū)域被視為0(黑色)。

1.2 路徑搜索

全局路徑規(guī)劃基于A?算法實現(xiàn)。 本文使用8方向A?算法，該算法每次都會搜索當(dāng)前節(jié)點的8 個相鄰節(jié)點，如圖1 所示。啟發(fā)式函數(shù)定義為:

圖1 8 方向A?算法示意圖

式中:G(n)代表從初始節(jié)點通過選定節(jié)點序列到達(dá)當(dāng)前節(jié)點所經(jīng)過的路徑長度，H(n)代表從當(dāng)前節(jié)點到目標(biāo)節(jié)點的估計代價。 本文使用曼哈頓距離估計H(n)，即

式中:xg和yg表示目標(biāo)節(jié)點的橫坐標(biāo)和縱坐標(biāo)，xp和yp表示當(dāng)前節(jié)點的橫坐標(biāo)和縱坐標(biāo)。

算法過程以偽代碼形式描述如下:

算法1 A?算法

傳統(tǒng)的A?算法每次僅搜索當(dāng)前節(jié)點的8 個相鄰節(jié)點。 當(dāng)局部地圖很大且距目標(biāo)節(jié)點的距離很遠(yuǎn)時，搜索路徑所需的時間將大大增加。 因此，本文提出了一種動態(tài)改變步長的快速A?算法。

在每個搜索步驟中，檢查在當(dāng)前節(jié)點的8 個方向上距離為L的節(jié)點，并根據(jù)當(dāng)前節(jié)點到目標(biāo)節(jié)點的距離確定L:

式中:L是搜索步長的最大值，[ ]表示向下取整，ρ是比例因子，xg和yg表示目標(biāo)節(jié)點的X坐標(biāo)和Y坐標(biāo)，xp和yp表示當(dāng)前節(jié)點的X坐標(biāo)和Y坐標(biāo)。

由于快速A?算法增加了搜索步長，所以路徑上的兩個節(jié)點之間可能會有障礙，如圖2 所示。 因此，有必要檢查路徑上兩個相鄰節(jié)點之間是否有障礙。 如果兩個相鄰節(jié)點之間有障礙物，對該段進(jìn)行一次重規(guī)劃。

圖2 搜索步長較大時相鄰節(jié)點間存在障礙

為了評估動態(tài)改變搜索步長的快速A?算法相對于傳統(tǒng)A?算法的改進(jìn)，進(jìn)行仿真對比如圖3 所示，路徑長度和運行時間列入表1。

圖3 快速A?與傳統(tǒng)A?路徑規(guī)劃結(jié)果

表1 路徑長度和運行時間對比

計算機(jī)操作系統(tǒng)為Windows 10，處理器為Intel? CoreTMi7-6700HQ，主頻率為2.60 GHz，內(nèi)存為16G。 地圖尺寸為2 000 m×2 000 m，比例因子ρ＝0.05，最大搜索步長為ML＝50。 通過傳統(tǒng)A?算法得到的路徑如圖3 中實線所示，長度為2 453.708 m，耗時63.464 24 s。 而通過快速A?算法得到的路徑如圖3 中虛線所示，長度為2 474.696 m，耗時僅為2.291 18 s。 可以看出，通過快速A?算法獲得的路徑長度可能略大于傳統(tǒng)A?算法的結(jié)果，但在比例因子和最大搜索步長設(shè)定合理的情況下，路徑總長度的增加不足1%，而算法效率大大提高，時間消耗僅為傳統(tǒng)A?算法的1/30。 當(dāng)需要在較短的時間內(nèi)完成一次大范圍的路徑搜索時，快速A?算法是十分有效的。

1.3 路徑平滑

通過A?算法生成的路徑是由多段折線構(gòu)成，如圖4 中虛線所示，而受限于USV 本身的運動性能，航向不可能突變，因此需要進(jìn)行平滑處理。 平滑分為兩步，第一次去除路徑中的部分冗余節(jié)點，只保留關(guān)鍵節(jié)點，第二次對保留下來的關(guān)鍵節(jié)點進(jìn)行分段Hermite 插值，獲得可行的平滑路徑。

圖4 A?算法生成路徑及平滑前后對比

剔除冗余節(jié)點，篩選關(guān)鍵節(jié)點的方法如下:設(shè)A、B、C為通過A?算法搜索得到的初始路徑上連續(xù)的三個節(jié)點，若AC段上不存在障礙物，且滿足

dist(A-B-C)表示從A經(jīng)過B到達(dá)C的路徑長度，dist(A-C)表示從A到C的路徑長度，即可將節(jié)點B視為冗余節(jié)點，將其剔除。 從初始路徑的起點開始，遍歷所有路徑點，直到提取出所有的關(guān)鍵節(jié)點，此時的路徑如圖4 中點線所示。

Hermite 插值多項式的數(shù)學(xué)定義如下:

設(shè)f為[a，b]上充分光滑函數(shù)，對給定的插值節(jié)點，及相應(yīng)的重數(shù)標(biāo)號，當(dāng)N＋1 時，若有H(x)∈Pn滿足

則稱H(x)為f(x)關(guān)于節(jié)點及重數(shù)標(biāo)號的Hermite 插值多項式。

本文中使用分段三次Hermite 插值進(jìn)行平滑處理，即在每個節(jié)點處都有:

一階導(dǎo)數(shù)相等的物理意義是USV 的速度連續(xù)變化，符合USV 的運動約束。

對關(guān)鍵節(jié)點構(gòu)成的路徑進(jìn)行分段三次Hermite插值平滑，得到最終路徑如圖4 中實線所示。

2 局部規(guī)劃

動態(tài)窗口方法(DWA)用于局部規(guī)劃。 該方法是基于速度采樣的局部規(guī)劃方法。 它在可行空間中對多組速度(v，ω)進(jìn)行采樣，并在一定時間內(nèi)模擬USV 的軌跡。 評估功能用于評估這些軌跡，并選擇與最佳軌跡相對應(yīng)的速度來執(zhí)行。

2.1 速度采樣

根據(jù)USV 本身的限制和環(huán)境限制，可以將導(dǎo)航系統(tǒng)輸出速度的采樣空間限制在一定范圍內(nèi)。 第一個約束是每個USV 的固有屬性，定義為

式中:vmin，vmax，ωmin和ωmax分別表示速度和角速度的最小值和最大值。

由于USV 的加減速性能的限制，有一個動態(tài)窗口，其定義為

式中:vc和ωc為當(dāng)前速度和角速度，和為最大加速度和角加速度，和為最大減速度和角減速度，dt是時間間隔。

由于欠驅(qū)動USV 通常不具有隨時停止的能力，因此，為了使其在到達(dá)目標(biāo)點時能夠停上，對速度進(jìn)行限制

式中:dist 是當(dāng)前位置到目標(biāo)點的距離。

最終采樣空間是Vm，Vd和Vs的交集。

在對多組速度進(jìn)行采樣之后，根據(jù)預(yù)設(shè)的時間間隔和仿真時間，對相應(yīng)的多組軌跡進(jìn)行仿真，從中剔除不安全的軌跡，并計算出剩余軌跡的評估函數(shù)。

2.2 軌跡模擬

基于USV 的運動方程和采樣速度估算軌跡。前向仿真時間分為多個小時間窗口。 由于相鄰兩個時刻之間的時間間隔較短，因此可以將USV 的運動簡化為線性運動，并根據(jù)式(11)更新狀態(tài)。

式中:x，y和φ代表USV的X坐標(biāo)，Y坐標(biāo)和航向角，dt代表時間間隔，v和ω代表艏向線速度和角速度。

2.3 評價函數(shù)

評估函數(shù)用于評估每個軌跡，定義為:

其中:①G表示總的評價函數(shù)值，即期望最大化的目標(biāo)函數(shù)。 ②heading ＝180-θ，其中θ是USV 的航向角與目標(biāo)點方向之間的夾角，該分量的物理意義是使USV 朝向目標(biāo)行進(jìn)。 ③distance 是當(dāng)前軌跡上每個點與最近障礙物之間的最小距離。 為了防止此項的影響過大，應(yīng)設(shè)置一個最大閾值，即只考慮一定范圍之內(nèi)的障礙物。 ④velocity 是USV 的艏向線速度，在確保安全的前提下，期望USV 能夠更快地達(dá)到目標(biāo)點。 ⑤deviation 用于評估當(dāng)前軌跡與從全局規(guī)劃獲得的軌跡之間的偏差。α，β，γ和δ是各項的權(quán)重系數(shù)。

值得注意的是，在計算每個軌跡的總評估函數(shù)之前，需要對評估函數(shù)中的四個組成部分進(jìn)行歸一化，如下所示:

算法流程如圖5 所示。

圖5 動態(tài)窗口法算法流程

3 仿真分析

為驗證本算法的可行性，使用實際場景進(jìn)行仿真實驗，圖6 是新安江水庫附近水域的衛(wèi)星地圖，圖中部分對應(yīng)的實際大小為1 500 m×1 100 m，對其進(jìn)行處理得到柵格地圖如圖7 所示，分辨率為1 m/pixel。仿真所用無人艇運動參數(shù)如表2 所示。

圖6 新安江水庫衛(wèi)星地圖

圖7 柵格地圖(1 m/pixel)

表2 無人艇運動參數(shù)

3.1 靜態(tài)環(huán)境仿真

圖8 路徑規(guī)劃結(jié)果(靜態(tài)環(huán)境)

第一組仿真環(huán)境中不存在動態(tài)障礙，所有地圖信息均為先驗已知，規(guī)劃結(jié)果如圖8 所示，圖8 中實線為通過A?算法進(jìn)行全局規(guī)劃生成的預(yù)規(guī)劃路徑，長度為1 220.882 m，虛線為通過DWA 進(jìn)行實時局部規(guī)劃得到的最終路徑，長度為1 219.901 m。 顯然兩者近乎完全重合，表明在沒有未知障礙物的情況下，無人艇將沿著預(yù)規(guī)劃路徑前行。 同時，將本文算法與人工勢場法(APF)進(jìn)行對比，APF 的規(guī)劃結(jié)果如圖8 中點線所示，路徑長度為1 247.941 m。 顯然本文提出的算法規(guī)劃得到的路徑長度更短，也更加平滑。

3.2 動態(tài)環(huán)境仿真

第二組仿真在環(huán)境中添加了兩個未知的動態(tài)障礙，一個以速度v1＝7 m/s 從左向右移動，另一個以速度v2＝2 m/s 從上向下移動。 在仿真時間t＝20 s和t＝50 s 兩個時刻，無人艇分別遭遇了兩個動態(tài)障礙，并偏離預(yù)規(guī)劃的路徑(圖9 和圖10 中實線)進(jìn)行規(guī)避，避障完成后繼續(xù)沿預(yù)規(guī)劃路徑前行，成功躲避了兩個動態(tài)障礙，到達(dá)目標(biāo)位置，得到最終路徑如圖11 中虛線所示。

圖9 t＝20 s 躲避第1 個障礙

圖10 t＝50 s 躲避第2 個障礙

圖11 路徑規(guī)劃結(jié)果(動態(tài)環(huán)境)

4 結(jié)論

本文提出了一種基于改進(jìn)的快速平滑A?算法和動態(tài)窗口法的無人艇路徑規(guī)劃方法。 在傳統(tǒng)A?算法的基礎(chǔ)上，動態(tài)改變搜索步長，將大范圍搜索時的算法效率提升了約30 倍，之后剔除路徑中的冗余節(jié)點，根據(jù)關(guān)鍵節(jié)點采用分段Hermite 插值方法進(jìn)行平滑處理，得到了更加平滑的路徑。 通過在動態(tài)窗口法的評價函數(shù)中新增路徑偏差項，將全局規(guī)劃與局部規(guī)劃相結(jié)合，實現(xiàn)了動態(tài)環(huán)境中無人艇的路徑規(guī)劃。 仿真實驗表明，該算法切實有效，可以滿足無人艇的運動約束，快速規(guī)劃出可行路徑，并在行進(jìn)過程中，適時修正路徑，躲避動態(tài)障礙，使無人艇安全到達(dá)目標(biāo)位置。