(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)障礙物進行躲避的全局路徑規(guī)劃系統(tǒng)[4];另一種是實現(xiàn)USV對動態(tài)障礙物進行躲避的局部路徑規(guī)劃系統(tǒng)。

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

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

1 環(huán)境模型

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

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)的位置進行一一綁定;

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

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

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

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

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

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

(1)

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

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

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ī)劃算法時,為了表示方便,采用坐標法來表示USV的運動環(huán)境。

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

1.2 柵格粒度確定

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

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

圖2 柵格粒度Fig.2 Grid granularity

2 A*算法的改進

2.1 A*算法概述

A*算法綜合了Dijkstra算法和寬度優(yōu)先搜索算法的優(yōu)點,提出了一個類似于寬度優(yōu)先搜索算法中的啟發(fā)式函數(shù)來對路徑進行評價,并將其用來評估初始節(jié)點到當(dāng)前節(jié)點所消耗的代價以及當(dāng)前節(jié)點到目標終點預(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é)點到目標終點預(yù)計還需要的消耗代價;f(n)表示綜合兩種不同代價的全局代價值。

式(2)中的g(n)與h(n)存在互相制約的關(guān)系,其中h(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)利用改進的堆結(jié)構(gòu)OPENLIST添加數(shù)據(jù)時,所進行的操作如下:

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

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

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

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

2.3 雙向A*算法

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

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

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

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

步驟 3同時由目標點和起始點向各自的方向進行搜索,搜索方法與A*搜索一致;

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

2.4 算法仿真

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

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

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

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

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

由表1的仿真結(jié)果進行數(shù)據(jù)分析可以發(fā)現(xiàn),雙向A*算法在運行時間以及搜索的節(jié)點數(shù)量上與A*算法進行比較,發(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īng)標記的障礙物進行規(guī)劃躲避,還需要實時監(jiān)測所行駛的環(huán)境,實現(xiàn)動態(tài)障礙物的躲避,而局部路徑規(guī)劃剛好可以處理這個比較棘手的問題,因此USV的局部路徑規(guī)劃是值得去探索深究的一個方向。本文使用DWA來完成上述所提到的問題,能夠得到比較正確的USV局部路徑規(guī)劃結(jié)果,結(jié)合上文提到的改進的A*算法中的評價函數(shù),考慮到海況的級別對評價函數(shù)進行相應(yīng)的改進,確定相應(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ā)生碰撞,因此在進行路徑規(guī)劃的時候會直接排除掉這些不安全的規(guī)劃結(jié)果,然后,在那些實線的軌跡即不會碰到障礙物的安全軌跡中根據(jù)每條軌跡的評價函數(shù)得到最終的路徑規(guī)劃結(jié)果。

3.1 運動模型

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

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

USV的行進方向可以是向前也可以以任意角度進行轉(zhuǎn)向,假設(shè)USV在單位時間Δt內(nèi)的速度可以用(vt,ωt)來表示,USV的位置坐標用(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)地進行增加,k2同時也會不斷減少,這樣能夠得到比較正確的路徑規(guī)劃[20]。

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

(6)

(7)

(8)

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

3.3 DWA算法仿真

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

圖8是DWA算法對靜態(tài)障礙物避障的仿真結(jié)果,其中仿真所設(shè)置的起點坐標是(49,34),終點坐標是(17,51)。其中,圖8(a)藍色圓圈代表的是本次DWA算法仿真所設(shè)置的起始點坐標,紅色五角星代表的是本次DWA算法仿真所設(shè)置的目標點坐標,綠色部分表示的是DWA算法中所涉及到的滑動窗口。圖8(b)中的藍色路徑表示的是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è)置的起點的坐標是(59,19),終點的坐標是(38,44)。其中,圖9(a)藍色圓圈代表的是本次DWA算法仿真所設(shè)置的起始點坐標,紅色五角星代表的是本次DWA算法仿真所設(shè)置的目標點坐標,綠色部分表示的是DWA算法中所涉及到的滑動窗口。圖9(b)中的藍色路徑表示的是DWA算法最后路徑搜索結(jié)束后的路徑規(guī)劃結(jié)果。從本次的仿真結(jié)果可以看出，通過DWA 算法可以完成USV對包含動態(tài)障礙物的路徑規(guī)劃,以實現(xiàn)從起始點到終點的避障規(guī)劃。由上述的仿真結(jié)果可以看出,DWA算法能夠躲避動靜態(tài)障礙物,但是由于這種方法存在一種天然的缺陷,就是容易將局部最優(yōu)值當(dāng)成全局最優(yōu)值進行輸出,以至于USV很難到達最終目標。如圖10所示,將坐標點(29,9)定為起始點,將坐標點(50,28)設(shè)置為終點時,DWA算法暴露缺陷,陷入了局部最小點,將其當(dāng)成最優(yōu)點導(dǎo)致無法得到一條正確的規(guī)劃路徑。為了解決上述出現(xiàn)的問題,本文基于DWA及改進的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)點是能夠準確地避開靜態(tài)障礙物,但是對于動態(tài)障礙物來說,此算法還無法做到能夠準確無誤地規(guī)劃出一條完整的路徑。其二,DWA算法優(yōu)點是能夠準確地避開動態(tài)障礙物以及其附近的一些靜態(tài)障礙物,但是對于全局的靜態(tài)障礙物來說,此算法還無法做到能夠準確無誤地規(guī)劃出一條完整的路徑,可能會出現(xiàn)第3節(jié)所提到的陷入局部最優(yōu)值的問題。為了解決上述兩個問題,本文提出了一種基于A*算法和DWA算法相結(jié)合的融合算法。此混合算法不僅解決了對動靜態(tài)障礙物的躲避問題,還可以在保證最優(yōu)路徑的情況下提升路徑的平滑度,使其更貼合USV的實際航行情況,具有更強的實用性。

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

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

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

圖12 局部目標點示意圖Fig.12 Local target point diagram

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

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

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

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

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

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

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

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

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

6 結(jié) 論

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