李文剛1,2, 汪流江1,2,*, 方德翔1,2, 李玉瑋1,2, 黃 郡3

(1. 西安電子科技大學(xué)通信工程學(xué)院, 陜西 西安 710071; 2. 西安電子科技大學(xué)綜合業(yè)務(wù)網(wǎng)理論及

關(guān)鍵技術(shù)國家重點實驗室, 陜西 西安 710071; 3. 國防科技大學(xué)電子對抗學(xué)院, 安徽 合肥 230037)

0 引 言

近些年來,無人系統(tǒng)受到了比較廣泛的關(guān)注,并在無人車、無人飛行器、無人潛水器、無人船等領(lǐng)域取得了長足的發(fā)展[1]。水面無人艇(unmanned surface vessel, USV)是一種新型的智能無人水面平臺,能夠在復(fù)雜的水域環(huán)境中完成復(fù)雜的工程科研任務(wù)[2]。

USV所行駛的環(huán)境比較復(fù)雜,容易受到障礙物的影響,因此USV需要很高的自主性系統(tǒng),路徑規(guī)劃是其中最為重要的系統(tǒng)之一[3]。USV的路徑規(guī)劃系統(tǒng)可以分為兩種,一種是針對靜態(tài)障礙物進(jìn)行躲避的全局路徑規(guī)劃系統(tǒng)[4];另一種是實現(xiàn)USV對動態(tài)障礙物進(jìn)行躲避的局部路徑規(guī)劃系統(tǒng)。

國內(nèi)外學(xué)者對于路徑規(guī)劃系統(tǒng)已經(jīng)研究出了很多相關(guān)的算法。鞏敦衛(wèi)[5]等人為了解決多地貌環(huán)境下的移動機器人路徑規(guī)劃問題,提出了多目標(biāo)微粒群優(yōu)化算法,但此算法不能完全適用于復(fù)雜的海洋環(huán)境。Kozynchenko[6]等人把人工智能(artificial intelligence, AI)算法與USV的路徑規(guī)劃問題進(jìn)行有機結(jié)合,解決了局部路徑規(guī)劃問題。鞏敦衛(wèi)[7]等人針對密集障礙物環(huán)境下存在控制偏差的機器人路徑規(guī)劃問題,提出了一種基于凸包和微粒群路徑規(guī)劃算法,但此算法不適用于存在動態(tài)障礙物的情形。Zhang[8]等人提出一種改進(jìn)的多目標(biāo)粒子群算法,解決了具有不確定危險源的機器人路徑規(guī)劃問題,但其對于動態(tài)環(huán)境沒有進(jìn)行相對應(yīng)的解決方案。YU[9]等人提出了一種A*算法與人工勢場法相結(jié)合的路徑規(guī)劃算法,解決了路徑規(guī)劃問題中常出現(xiàn)的陷入局部極小點以至于無法找到最佳點的問題。Geng[10]等人針對搜救領(lǐng)域的任務(wù)分配問題,提出了一種基于粒子群算法的改進(jìn)集中式算法,該算法為路徑規(guī)劃算法提供了一個新的思路。Wang[11]等人提出了一種多功能的路徑規(guī)劃器以解決復(fù)雜環(huán)境下航行的USV問題。

相對于國內(nèi)的學(xué)者來說,國外學(xué)者對于USV路徑規(guī)劃問題進(jìn)行了比較多的研究,比如將神經(jīng)網(wǎng)絡(luò)以及強化學(xué)習(xí)與路徑規(guī)劃問題有機結(jié)合起來進(jìn)行研究[12-16]。

1 環(huán)境模型

環(huán)境模型的建立對于路徑規(guī)劃系統(tǒng)而言是非常重要的。首先需要對環(huán)境信息進(jìn)行捕獲,包括靜態(tài)信息和動態(tài)信息[17]。然后對得到的環(huán)境信息進(jìn)行環(huán)境模型的建立,所建立的模型可以完全代表USV在行駛過程中的真實環(huán)境。因此環(huán)境建模是非常重要的,其結(jié)果的好壞會影響到路徑規(guī)劃時搜索路徑的長度以及搜索時間。本文綜合考慮了實驗場景的方式以及環(huán)境建模方法的便利性,考慮使用柵格法進(jìn)行實驗環(huán)境進(jìn)行建模,對于其環(huán)境建模細(xì)節(jié)部分在下面進(jìn)行了分析說明。

1.1 柵格法建模

柵格法是將整個環(huán)境劃分成多個連續(xù)但不重合的網(wǎng)格,每個網(wǎng)格的狀態(tài)對應(yīng)所處的環(huán)境信息。通過一定的數(shù)學(xué)建模方法,將復(fù)雜且連續(xù)的USV復(fù)雜路徑規(guī)劃問題簡化為離散的相鄰柵格之間的移動問題,具體的操作過程如下所示:

(1) 根據(jù)場景大小選擇合適的粒度,將場景環(huán)境劃為相同的柵格;

(2) 通過場景環(huán)境與柵格對應(yīng)的位置進(jìn)行一一綁定;

(3) 根據(jù)場景環(huán)境中的障礙物設(shè)置柵格地圖的每個狀態(tài)。

如圖1所示,場景環(huán)境空間按照柵格的粒度大小進(jìn)行劃分,將實際環(huán)境中的障礙物區(qū)域在柵格地圖中用黑色區(qū)域表示,而白色部分表示沒有障礙物的區(qū)域即為可以通行的區(qū)域。S和G分別表示初始點和目標(biāo)點。在對路徑進(jìn)行搜索時,需要對柵格位置進(jìn)行標(biāo)記,標(biāo)記的方式主要有兩種,一種為坐標(biāo)法,另一種為序列法。

圖1 柵格法示意圖Fig.1 Schematic diagram of grid method

(1) 坐標(biāo)法:以柵格地圖的兩條互相垂直的邊作為平面直角坐標(biāo)系的X軸和Y軸,柵格地圖中的任意位置都可以用坐標(biāo)(x,y)來表示。

(2) 序列法:每個網(wǎng)格按照一定的水平和垂直規(guī)則遞增標(biāo)記,每個網(wǎng)格都有相應(yīng)的編號。

對上述兩種方法進(jìn)行轉(zhuǎn)換的公式如下:

(1)

式中:x,y是利用坐標(biāo)法得到的柵格地圖的坐標(biāo);N表示利用序列法得到的柵格編號;n是柵格粒度因子;Mod是求余函數(shù);Rem表示向下取整函數(shù)。

以圖1為例,n=10,得到的可行路線已經(jīng)在圖中進(jìn)行標(biāo)記,利用上述兩種方法對得到的可行路徑可分別表示為

M1=[(0,0),(1,1),(1,2),(2,2),(3,3),(4,4),
(5,5),(6,6),(7,6),(8,6),(8,7),(8,8),(9,9)]
M2=[0,11,21,22,33,44,55,66,67,68,78,88,99]

由于上述兩種不同表示方法的表示形式,本文在設(shè)計路徑規(guī)劃算法時,為了表示方便,采用坐標(biāo)法來表示USV的運動環(huán)境。

柵格法能夠降低對復(fù)雜環(huán)境進(jìn)行抽象處理的復(fù)雜度,而且其算法原理以及實現(xiàn)過程都比較簡單。但是其在進(jìn)行柵格劃分的時候比較復(fù)雜,需要考慮很多因素以尋求得到一個平衡。

1.2 柵格粒度確定

柵格法建模過程中最重要的一步是需要確定柵格地圖的柵格粒度,其劃分粒度不同,最后通過所建立的柵格地圖的精度也會不同。不同的柵格粒度大小可以根據(jù)環(huán)境建立不同的柵格地圖模型以及利用所建立的柵格地圖進(jìn)行路徑搜索時所需要的時間也更不相同,甚至所得出的路徑也不盡相同。

柵格粒度大小是由該區(qū)域的障礙物在整個仿真環(huán)境所占的比重來確定的,當(dāng)障礙物所占的比重小時可能會產(chǎn)生不良影響,以至于得到一個不準(zhǔn)確的搜索結(jié)果。反之可以增大所確定的柵格力度大小,在得到一個比較好的路徑搜索結(jié)果時,降低算法進(jìn)行搜索時所用的時間[18]。根據(jù)上述所提到的柵格粒度處理原則,考慮使用如圖2所示的60×60的柵格地圖進(jìn)行仿真,分析所提出算法的優(yōu)勢。

圖2 柵格粒度Fig.2 Grid granularity

2 A*算法的改進(jìn)

2.1 A*算法概述

A*算法綜合了Dijkstra算法和寬度優(yōu)先搜索算法的優(yōu)點,提出了一個類似于寬度優(yōu)先搜索算法中的啟發(fā)式函數(shù)來對路徑進(jìn)行評價,并將其用來評估初始節(jié)點到當(dāng)前節(jié)點所消耗的代價以及當(dāng)前節(jié)點到目標(biāo)終點預(yù)計需要消耗的代價。評價函數(shù)的輸出用于評價當(dāng)前節(jié)點的某個鄰域內(nèi)的所有節(jié)點的輸入,其評價函數(shù)可以表示為

f(n)=g(n)+h(n)

(2)

式中:g(n)表示從初始節(jié)點到當(dāng)前節(jié)點n已經(jīng)產(chǎn)生的消耗代價;h(n)表示從當(dāng)前節(jié)點到目標(biāo)終點預(yù)計還需要的消耗代價;f(n)表示綜合兩種不同代價的全局代價值。

式(2)中的g(n)與h(n)存在互相制約的關(guān)系,其中h(n)可以認(rèn)為是啟發(fā)函數(shù),是影響A*算法的時空復(fù)雜度最為重要的因素之一[19]。A*算法在實際路徑搜索過程中的流程如圖3所示。

圖3 A*算法流程圖Fig.3 A* algorithm flow chart

2.2 堆結(jié)構(gòu)優(yōu)化OPEN列表

當(dāng)USV在廣闊的海洋環(huán)境行駛采用A*算法時,會使得需要存儲的節(jié)點個數(shù)成指數(shù)級別增長?？紤]到往OPENLIST中添加節(jié)點時,不會考慮到所添加節(jié)點的大小,當(dāng)需要得到數(shù)值最小的節(jié)點時,需要遍歷一遍存儲數(shù)值的容器,當(dāng)節(jié)點數(shù)量比較多時,需要耗費的時間比較長。因此為了降低節(jié)點增添所帶來的時間復(fù)雜度,考慮使用堆結(jié)構(gòu)來構(gòu)建OPENLIST列表,堆的根節(jié)點即為列表的最小值,增刪查改的時間復(fù)雜度可以大大降低,結(jié)構(gòu)示意圖如圖4所示。

圖4 堆結(jié)構(gòu)的存儲Fig.4 Storage of heap structures

當(dāng)利用改進(jìn)的堆結(jié)構(gòu)OPENLIST添加數(shù)據(jù)時,所進(jìn)行的操作如下:

步驟 1將所要添加的新節(jié)點插入到堆結(jié)構(gòu)的尾部;

步驟 2通過比較新節(jié)點與其插入位置的父節(jié)點大小進(jìn)行比較,如果所插入的節(jié)點值小于父節(jié)點值,則滿足交換操作,需要交換兩個節(jié)點之間的位置;

步驟 3重復(fù)進(jìn)行步驟2的操作,直到不滿足交換條件時停止操作。

當(dāng)向堆結(jié)構(gòu)優(yōu)化的列表中修改和刪除數(shù)據(jù)時,操作方法與插入數(shù)據(jù)類似,不再一一贅述。

2.3 雙向A*算法

由于海洋環(huán)境比較寬闊,導(dǎo)致所選取的目標(biāo)點與起始點的距離比較遠(yuǎn),所建立的柵格地圖中柵格的數(shù)量也比較多。因此單向A*算法尋找路徑所需要的時間會變得很長，并且存儲節(jié)點所需要的空間變得很大。為了A*算法在大規(guī)模柵格數(shù)量搜索時出現(xiàn)的問題,本文對傳統(tǒng)的A*算法進(jìn)行一定的改進(jìn),同時在目標(biāo)點和起始點進(jìn)行路徑搜索。

本文所提出的雙向A*算法的詳細(xì)步驟如下。

步驟 1建立兩個不同的OPEN列表,一個用于存儲由目標(biāo)點向起始點方向搜索過程中還沒有檢查過的點,而另一個存儲由起始點向目標(biāo)點方向搜索過程中還沒有檢查過的點。同時，兩個不同的CLOSE列表并置為空列表,一個用于存儲由目標(biāo)點向起始點方向搜索過程中已經(jīng)遍歷過的點,一個用于存儲由起始點向目標(biāo)點方向搜索過程中已經(jīng)遍歷過的點;

步驟 2將初始節(jié)點S添加到存儲由起始點向目標(biāo)點方向搜索過程中還沒有檢查過的點的OPEN列表中,將目的節(jié)點G添加到存儲由目標(biāo)點向起始點方向搜索過程中還沒有檢查過的點的OPEN列表中;

步驟 3同時由目標(biāo)點和起始點向各自的方向進(jìn)行搜索,搜索方法與A*搜索一致;

步驟 4當(dāng)某個OPEN列表的節(jié)點在另一個CLOSE列表當(dāng)中時,此時已經(jīng)得到了最終路徑,搜索完成,返回搜索結(jié)果。

2.4 算法仿真

對上述兩種算法在USV路徑規(guī)劃中進(jìn)行相應(yīng)的分析,在Matlab 2019a中進(jìn)行了相關(guān)仿真,以對比雙向A*和傳統(tǒng)的A*算法的搜索效率,仿真場景是由真實的航行環(huán)境經(jīng)過上述所提到的建模方法得到的60×60的柵格地圖。在本節(jié)的仿真實驗中,分別對兩種算法進(jìn)行同一個起始點和相同的目標(biāo)點路徑規(guī)劃,并對兩種算法的路徑規(guī)劃結(jié)果的長度以及路徑的搜索時間進(jìn)行對比,仿真結(jié)果及分析如下所示。

圖5是A*算法的仿真結(jié)果,仿真所設(shè)置的起點的坐標(biāo)是(34,54),終點的坐標(biāo)是(27,16)。其中,圖5(a)中的陰影部分表示的是A*算法在進(jìn)行路徑搜索過程中的擴展過程,圖5(b)中的紅色路徑表示的是A*算法最后路徑搜索結(jié)束后的路徑規(guī)劃結(jié)果,圖5(b)藍(lán)色圓圈代表的是本次A*算法仿真所設(shè)置的起始點坐標(biāo),圖5(b)的藍(lán)色五角星代表的是本次A*算法仿真所設(shè)置的目標(biāo)點坐標(biāo)。從本次的仿真結(jié)果可以看出,通過A*算法可以完成USV的全局路徑規(guī)劃,以實現(xiàn)從起始點到終點的避障規(guī)劃。圖6是雙向A*算法的仿真結(jié)果,仿真所設(shè)置的起點的坐標(biāo)是(34,54),終點的坐標(biāo)是(27,16)。其中,圖6(a)中的陰影部分表示的是雙向A*算法在進(jìn)行路徑搜索過程中的擴展過程,圖6(b)中的紅色路徑表示的是雙向A*算法最后路徑搜索結(jié)束后的路徑規(guī)劃結(jié)果,圖6(b)藍(lán)色圓圈代表的是本次雙向A*算法仿真所設(shè)置的起始點坐標(biāo),圖6(b)的藍(lán)色五角星代表的是本次雙向A*算法仿真所設(shè)置的目標(biāo)點坐標(biāo)。從本次的仿真結(jié)果可以看出,通過雙向A*算法可以完成USV的全局路徑規(guī)劃,以實現(xiàn)從起始點到終點的避障規(guī)劃。

圖5 A*算法仿真圖Fig.5 A* algorithm simulation diagram

在上述仿真環(huán)境中,USV起始點的坐標(biāo)為(34,54),USV的終點坐標(biāo)為(27,16),對本次仿真兩個算法的仿真結(jié)果從長度、時間與節(jié)點數(shù)量3個評價指標(biāo)進(jìn)行比較,結(jié)果如表1所示。

表1 A*算法與雙向A*算法仿真結(jié)果對比

由表1的仿真結(jié)果進(jìn)行數(shù)據(jù)分析可以發(fā)現(xiàn),雙向A*算法在運行時間以及搜索的節(jié)點數(shù)量上與A*算法進(jìn)行比較,發(fā)現(xiàn)前者的算法運行時間大致上縮短了40%,算法執(zhí)行過程中所產(chǎn)生的變量占用的內(nèi)存減少了36%。從本節(jié)的仿真結(jié)果可以看出,所提出的雙向A*算法與A*算法相比,在保證正確的搜索路徑上,還能節(jié)省程序所執(zhí)行的時間以及程序執(zhí)行過程中所生成的變量需要的空間,因此可以考慮用雙向A*算法代替經(jīng)典的A*算法完成USV的路徑規(guī)劃,以得到一條安全且無差錯的從起始點到終點的路徑。

3 動態(tài)窗口法

在比較寬闊及復(fù)雜的海域上想要進(jìn)行無差錯的行駛,需要對地圖中已經(jīng)標(biāo)記的障礙物進(jìn)行規(guī)劃躲避,還需要實時監(jiān)測所行駛的環(huán)境,實現(xiàn)動態(tài)障礙物的躲避,而局部路徑規(guī)劃剛好可以處理這個比較棘手的問題,因此USV的局部路徑規(guī)劃是值得去探索深究的一個方向。本文使用DWA來完成上述所提到的問題,能夠得到比較正確的USV局部路徑規(guī)劃結(jié)果,結(jié)合上文提到的改進(jìn)的A*算法中的評價函數(shù),考慮到海況的級別對評價函數(shù)進(jìn)行相應(yīng)的改進(jìn),確定相應(yīng)的權(quán)重系數(shù),以應(yīng)對USV在海洋環(huán)境行駛時出現(xiàn)的一系列突發(fā)情況。

圖7為DWA示意圖,USV用黑色的船來表示,障礙物用灰色矩形來表示,圖中的曲線代表USV的航行軌跡。

圖7 DWA示意圖Fig.7 Schematic diagram of DWA

如圖7所示,虛線代表預(yù)測到的軌跡會與障礙物發(fā)生碰撞,因此在進(jìn)行路徑規(guī)劃的時候會直接排除掉這些不安全的規(guī)劃結(jié)果,然后,在那些實線的軌跡即不會碰到障礙物的安全軌跡中根據(jù)每條軌跡的評價函數(shù)得到最終的路徑規(guī)劃結(jié)果。

3.1 運動模型

DWA的主要思路是根據(jù)預(yù)測軌跡來確定下一時刻的行進(jìn)方向及行進(jìn)速度,因此確定行進(jìn)軌跡的模型就非常重要,本節(jié)所設(shè)計的模型主要是為了確定位置與速度、時間間隔等之間的關(guān)系。

在本節(jié)中可以考慮USV的線速度vt與角速度ωt是不受對方干擾的,也就是說這二者是相互獨立的,而USV的行進(jìn)方向也主要由這兩者確定,因此可以考慮用速度對(vt,ωt)來確定行進(jìn)的軌跡。在本文中,僅考慮USV在單位時間間隔內(nèi)處于勻速運動的情況,因此可以根據(jù)速度的改變判斷行進(jìn)方向的改變。

USV的行進(jìn)方向可以是向前也可以以任意角度進(jìn)行轉(zhuǎn)向,假設(shè)USV在單位時間Δt內(nèi)的速度可以用(vt,ωt)來表示,USV的位置坐標(biāo)用(x0,y0)來表示,USV的方向角用θt來表示, USV的位置以及方向角的更新方程為

(3)

3.2 軌跡評價函數(shù)

本文在傳統(tǒng)的DWA評價函數(shù)中考慮了外部因素的影響,增加了外部因素對評價軌跡的影響權(quán)重,其定義如下:

(4)

由式(4)可以看出,最終評價值是由n_head(u,r)、n_dist(u,r)和n_vel(u,r)這3個評價函數(shù)得到的[20]。式(4)中的α,β,γ分別是n_head(u,r)、n_dist(u,r)和n_vel(u,r)的初始化的權(quán)重值,k1,k2分別表示海面狀況系數(shù)的權(quán)重系數(shù),定義如下:

(5)

式中:F表示不同的海況環(huán)境的級別,(ρ,η)∈(0,1)。由式(5)可以看出,當(dāng)目前的環(huán)境不適合航行時,k1會相應(yīng)地進(jìn)行增加,k2同時也會不斷減少,這樣能夠得到比較正確的路徑規(guī)劃[20]。

為了讓得到的路徑轉(zhuǎn)折點數(shù)量更小,需要對前文中所提到的評價函數(shù)進(jìn)行歸一化操作,具體流程如下所示:

(6)

(7)

(8)

式中:head(i)是航向角;dist (i)表示當(dāng)前位置距離障礙物之間的距離;v(i)表示USV此時的行駛速度。

3.3 DWA算法仿真

對上述所提到的DWA算法在USV路徑規(guī)劃中進(jìn)行可行性分析,在Matlab 2019a中進(jìn)行了相關(guān)仿真,以驗證DWA算法在擁有動態(tài)障礙物和靜態(tài)障礙物兩種不同場景下的性能,仿真場景是由真實的航行環(huán)境經(jīng)過所提建模方法得到的柵格地圖。在本節(jié)的仿真實驗中,分別對DWA算法設(shè)置動態(tài)障礙物和靜態(tài)障礙物兩種場景進(jìn)行仿真分析,仿真結(jié)果如下所示。

圖8是DWA算法對靜態(tài)障礙物避障的仿真結(jié)果,其中仿真所設(shè)置的起點坐標(biāo)是(49,34),終點坐標(biāo)是(17,51)。其中,圖8(a)藍(lán)色圓圈代表的是本次DWA算法仿真所設(shè)置的起始點坐標(biāo),紅色五角星代表的是本次DWA算法仿真所設(shè)置的目標(biāo)點坐標(biāo),綠色部分表示的是DWA算法中所涉及到的滑動窗口。圖8(b)中的藍(lán)色路徑表示的是DWA算法最后路徑搜索結(jié)束后的路徑規(guī)劃結(jié)果。從本次的仿真結(jié)果可以看出,通過DWA 算法可以完成USV的路徑規(guī)劃,以實現(xiàn)從起始點到終點的避障規(guī)劃。

圖8 DWA未受到動態(tài)障礙物影響仿真圖Fig.8 Simulation diagram of DWA not affected by dynamic obstacle

圖9的仿真場景所設(shè)置的仿真環(huán)境包含了動態(tài)障礙物,這與圖8中的靜態(tài)障礙物的仿真場景不同,但其余的條件與圖8的仿真場景基本相同。圖9是DWA算法對動態(tài)障礙物避障的仿真結(jié)果,仿真所設(shè)置的起點的坐標(biāo)是(59,19),終點的坐標(biāo)是(38,44)。其中,圖9(a)藍(lán)色圓圈代表的是本次DWA算法仿真所設(shè)置的起始點坐標(biāo),紅色五角星代表的是本次DWA算法仿真所設(shè)置的目標(biāo)點坐標(biāo),綠色部分表示的是DWA算法中所涉及到的滑動窗口。圖9(b)中的藍(lán)色路徑表示的是DWA算法最后路徑搜索結(jié)束后的路徑規(guī)劃結(jié)果。從本次的仿真結(jié)果可以看出，通過DWA 算法可以完成USV對包含動態(tài)障礙物的路徑規(guī)劃,以實現(xiàn)從起始點到終點的避障規(guī)劃。由上述的仿真結(jié)果可以看出,DWA算法能夠躲避動靜態(tài)障礙物,但是由于這種方法存在一種天然的缺陷,就是容易將局部最優(yōu)值當(dāng)成全局最優(yōu)值進(jìn)行輸出,以至于USV很難到達(dá)最終目標(biāo)。如圖10所示,將坐標(biāo)點(29,9)定為起始點,將坐標(biāo)點(50,28)設(shè)置為終點時,DWA算法暴露缺陷,陷入了局部最小點,將其當(dāng)成最優(yōu)點導(dǎo)致無法得到一條正確的規(guī)劃路徑。為了解決上述出現(xiàn)的問題,本文基于DWA及改進(jìn)的A*算法提出了一種全新的路徑規(guī)劃算法。

圖9 DWA受到動態(tài)障礙物影響仿真圖Fig.9 Simulation diagram of DWA affected by dynamic obstacle

圖10 DWA陷入局部最小點Fig.10 DWA falling into local minimum

4 混合路徑規(guī)劃系統(tǒng)

4.1 混合路徑規(guī)劃系統(tǒng)建立

USV在海面航行時的真實環(huán)境錯綜復(fù)雜,體現(xiàn)在島嶼、大型水下石頭等靜態(tài)障礙物,還有大型海洋生物等會對USV造成影響的動態(tài)障礙物。在規(guī)劃USV路徑時二者均需考慮。其一, A*算法的優(yōu)點是能夠準(zhǔn)確地避開靜態(tài)障礙物,但是對于動態(tài)障礙物來說,此算法還無法做到能夠準(zhǔn)確無誤地規(guī)劃出一條完整的路徑。其二,DWA算法優(yōu)點是能夠準(zhǔn)確地避開動態(tài)障礙物以及其附近的一些靜態(tài)障礙物,但是對于全局的靜態(tài)障礙物來說,此算法還無法做到能夠準(zhǔn)確無誤地規(guī)劃出一條完整的路徑,可能會出現(xiàn)第3節(jié)所提到的陷入局部最優(yōu)值的問題。為了解決上述兩個問題,本文提出了一種基于A*算法和DWA算法相結(jié)合的融合算法。此混合算法不僅解決了對動靜態(tài)障礙物的躲避問題,還可以在保證最優(yōu)路徑的情況下提升路徑的平滑度,使其更貼合USV的實際航行情況,具有更強的實用性。

圖11為本文所提出的混合路徑規(guī)劃系統(tǒng)設(shè)計框架,主要分為全局路徑規(guī)劃部分以及局部路徑規(guī)劃部分,二者相互融合進(jìn)行路徑規(guī)劃，其主要步驟如下。

步驟 1通過實際海洋地圖經(jīng)過本文的柵格處理方法得到的柵格地圖來確定全局環(huán)境中的動態(tài)障礙物的分布情況,利用雙向A*算法得到USV的全局路徑;

步驟 2使用外部感知傳感器實時感知的數(shù)據(jù)來更新局部地圖,并結(jié)合步驟1中的全局路徑以及得到的局部地圖確定局部目標(biāo)點,使用DWA算法對動態(tài)障礙物進(jìn)行躲避生成局部路徑規(guī)劃路徑,通過這種方式不斷更新局部最優(yōu)點使USV最終到達(dá)全局目標(biāo)點,最后得到最佳的規(guī)劃路徑。為了獲得較遠(yuǎn)的安全距離,在DWA算法中優(yōu)化評價函數(shù),加入與海面實時狀況級別相關(guān)的系數(shù)。當(dāng)海面航行的實時條件不斷變化時,實時調(diào)整相關(guān)的系數(shù),以使得USV能夠比較安全地在海面環(huán)境上進(jìn)行航行。

圖11 混合路徑規(guī)劃系統(tǒng)Fig.11 Hybrid path planning system

對于本文所提出的USV混合路徑規(guī)劃系統(tǒng),要做到的是充分融合A*算法以及DWA算法的優(yōu)點,確保能夠解決單獨使用這兩種算法時出現(xiàn)的缺點。圖12展示了本文所提出的混合路徑規(guī)劃算法的實現(xiàn)步驟,不斷通過局部最優(yōu)點以及全局路徑進(jìn)行融合更新,最終得到一條安全的規(guī)劃路徑。

圖12 局部目標(biāo)點示意圖Fig.12 Local target point diagram

4.2 混合路徑規(guī)劃算法仿真

對上述所提到的USV混合路徑規(guī)劃算法中進(jìn)行可行性分析,在Matlab 2019a中進(jìn)行了相關(guān)仿真,以驗證所提算法相較于經(jīng)典路徑規(guī)劃算法的性能,仿真場景是由真實的航行環(huán)境經(jīng)過上述所提方法得到的柵格地圖。如圖10所示,將坐標(biāo)點(29,9)定為起始點,將坐標(biāo)點(50,28)設(shè)置為終點時,DWA算法暴露缺陷,陷入了局部最小點,將其當(dāng)成最優(yōu)點導(dǎo)致無法得到一條正確的規(guī)劃路徑。通過設(shè)置與圖10相同的仿真條件來驗證本文所提出的混合路徑規(guī)劃算法的優(yōu)越性。

圖13與圖10的仿真將坐標(biāo)點(29,9)定為起始點,將坐標(biāo)點(50,28)設(shè)置為終點,通過雙向A*算法得到的全局路徑在圖13中用紅色路徑表示,藍(lán)色路徑表示的是通過DWA算法同時結(jié)合全局搜索路徑得到的最優(yōu)路徑。

圖13 混合路徑規(guī)劃算法仿真Fig.13 Hybrid path planning algorithm simulation

表2展示了兩種算法得到路徑的路徑長度和路徑拐點,通過兩組數(shù)據(jù)分析得出混合算法相較于改進(jìn)的A*算法,其路徑長度縮短了21%,路徑拐點降低了59%。

表2 混合算法與A*算法仿真結(jié)果對比

由圖13和表2的結(jié)果可以看出,本文所提算法能夠得到一條完整的規(guī)劃路徑,其結(jié)合了雙向A*算法以及DWA算法的優(yōu)點,同時彌補了這兩種算法的缺點,得到的路徑的拐點數(shù)還較少,更方便USV在海面上進(jìn)行安全行駛。

5 算法時間復(fù)雜度分析

傳統(tǒng)的A*算法在最壞的情形下,即評價函數(shù)中實際代價函數(shù)發(fā)揮主要作用,此時算法被簡化為Dijkstra算法,從而計算量增加,此算法在運行過程中會遍歷還沒到達(dá)的節(jié)點,因此整個算法的運行時間為O(n2)[21](其中n為節(jié)點的個數(shù)),而本文所提算法的運行時間主要是由堆排序決定的,在排序過程中的時間復(fù)雜度小于或等于滿二叉樹的高度log2n,故整個算法運行時間的復(fù)雜度為O(nlog2n)。由于DWA遍歷運行的時間復(fù)雜度是線性的,因此本文提出的混合路徑規(guī)劃算法的時間復(fù)雜度取決于本文提出的改進(jìn)A*算法。

6 結(jié) 論

USV路徑規(guī)劃系統(tǒng)主要分為兩種路徑規(guī)劃系統(tǒng),一種為全局路徑規(guī)劃系統(tǒng),另一種為局部路徑規(guī)劃系統(tǒng),本文對這兩種路徑規(guī)劃算法進(jìn)行改進(jìn),提出一種基于DWA和改進(jìn)A*的混合路徑規(guī)劃算法。本文所采用的改進(jìn)A*即雙向A*算法在運行時間以及算法搜索的節(jié)點數(shù)量上與傳統(tǒng)A*算法進(jìn)行比較,發(fā)現(xiàn)前者的算法運行時間大致上縮短了40%,算法所產(chǎn)生的變量所占用的內(nèi)存減少了36%?；贒WA和改進(jìn)A*的混合路徑規(guī)劃算法相較于改進(jìn)的A*算法,其路徑長度縮短了21%以及路徑拐點降低了59%,性能得到大幅度提升。