基于IRRT*和DWA 的無人艇混合路徑規(guī)劃方法

張嘯天，陳熙源

(東南大學(xué)儀器科學(xué)與工程學(xué)院，江蘇 南京 210096)

水面無人艇(unmanned surface vehicle，USV)，是一種無人操作的帶有多類型傳感器的水面艦艇，具有體積小、全天時、低成本的優(yōu)點。在配備先進的控制系統(tǒng)、傳感器系統(tǒng)和通信系統(tǒng)的情況下，可用于勘探巡邏和測繪等任務(wù)，擁有著廣闊的應(yīng)用前景[1]。

路徑規(guī)劃是水面無人艇的最基本的關(guān)鍵技術(shù)之一，它的目的是通過接收傳感器所得到的信息，結(jié)合無人艇的運動狀態(tài)，確定USV 航行的最佳軌跡，并在USV 實際行駛過程中，根據(jù)實際位置與預(yù)設(shè)位置的誤差，不斷修正受到外界因素影響的行進路線，以達到相關(guān)的任務(wù)要求。路徑規(guī)劃算法的優(yōu)劣，不僅決定了其自主性水平，而且也是圓滿實現(xiàn)任務(wù)的前提[1-2]。

無人艇的路徑規(guī)劃從廣義上屬于機器人路徑規(guī)劃的一種。目前主流的路徑規(guī)劃算法可以分為三類:基于圖搜索的路徑規(guī)劃方法，基于采樣的路徑規(guī)劃方法以及基于智能算法的路徑規(guī)劃算法。其中基于隨機采樣原理的快速搜索隨機樹(RRT)算法由于無需對環(huán)境進行具體建模，以及對于具有微分約束和非線性動力學(xué)的系統(tǒng)有效的優(yōu)點備受研究者的關(guān)注[3]。由于RRT 算法產(chǎn)生的路徑不具備最優(yōu)性，Karaman 等提出了RRT*算法，在原有算法的基礎(chǔ)上加入了重布線過程，使算法具備漸進最優(yōu)性[4]。廖彬等[5]提出了一種F-RRT*算法，通過隨機創(chuàng)建父節(jié)點以及引入三角不等式對路徑成本進行優(yōu)化。Naderi[6]提出了一種RT-RRT*，通過實時構(gòu)建地圖，對搜索樹進行更新，實現(xiàn)RRT*的動態(tài)規(guī)劃。以上改進均針對RRT*算法的搜索效率進行提升，進而匹配應(yīng)用場景。

無人艇的應(yīng)用場景存在影響載體運動的風(fēng)浪流，但目前無人艇的路徑規(guī)劃研究主要局限在靜水環(huán)境，較少考慮水流影響。侯春曉等[7]提出了應(yīng)用于內(nèi)河無人船的一般曲線路徑的LOS 循跡控制算法，能夠較好地處理內(nèi)河中漲潮和落潮時的路徑規(guī)劃問題，當處于不同水流條件時規(guī)劃的路徑存在較大差別。吳博等[7]提出了一種基于速度障礙的自動避碰算法，該算法考慮了無人艇與風(fēng)浪流的相對速度，并分析其相對速度與位置的角度關(guān)系，從而進行路徑規(guī)劃?？偟膩碚f，目前解決水流影響的基本手段是將無人艇運動與水流運動進行合成，再引入具體路徑規(guī)劃算法進行求解。

全局路徑規(guī)劃通常需要根據(jù)電子海圖中海域中的障礙、危險區(qū)域信息，建立包含可以確保船只航行必要信息的環(huán)境模型[9-10]，接著從建立的模型中找出一條能滿足各種航行指標的安全路徑[11-12]。與全局路徑規(guī)劃不同，水面無人艇局部路徑規(guī)劃算法需要根據(jù)艇上多類型傳感器實時感知周圍環(huán)境中的信息，根據(jù)距離、風(fēng)浪等因素動態(tài)地修正局部路徑，確保無人艇航行路線的安全可靠[13-14]。

因此，要綜合考慮風(fēng)浪對于無人艇路徑規(guī)劃的影響，對于規(guī)劃算法進行調(diào)整，同時結(jié)合預(yù)路徑，實時給出無人艇下一時刻的運動信息。本文采用基于IRRT*和DWA 的無人艇混合路徑規(guī)劃方法。全局路徑規(guī)劃方法選擇改進的IRRT*算法。這種方法基于增量式搜索，特別適合大空間的路徑規(guī)劃問題[15]。局部路徑規(guī)劃方法選擇動態(tài)窗口法(Dynamic Window Approach，DWA)，并在此基礎(chǔ)上結(jié)合海浪影響進行改進，能夠較好地符合無人艇的實際運動情況[16-17]。

1 全局路徑規(guī)劃

針對無人艇的路徑規(guī)劃，通常為了保證算法路徑規(guī)劃的實時性和有效性，分為全局路徑規(guī)劃和局部路徑規(guī)劃兩個部分進行。全局路徑規(guī)劃通常首先通過電子海圖等環(huán)境信息，根據(jù)航行指令預(yù)規(guī)劃出一條全局路徑。在全局規(guī)劃的基礎(chǔ)上，無人艇通過自身傳感器探測裝置獲取周圍環(huán)境信息，結(jié)合無人艇具體的運動約束，再利用局部路徑規(guī)劃方法實現(xiàn)避障。這么做的好處是，通過預(yù)先花費較長時間進行全局路徑規(guī)劃得到全局路徑，避免了無人艇在局部避障過程中可能出現(xiàn)的盲目性，同時也能有效地減少局部路徑規(guī)劃算法所需要的時間，具體過程如圖1 所示。

圖1 無人艇路徑規(guī)劃流程

在進行路徑規(guī)劃過程之前，通常需要對環(huán)境進行建模，常用的方法包括柵格法、可視圖法和Voronoi 圖法，這個步驟對于其他路徑搜索算法往往是必要的。然而RRT*算法通過對狀態(tài)空間的采樣點進行碰撞檢測，從而避免在狀態(tài)空間內(nèi)顯式地構(gòu)造障礙物，提供了大量的計算節(jié)省。

1.1 RRT*算法

為了解決機器人路徑規(guī)劃問題中存在非完整性約束或微分約束時，很難逐一對軌跡片段進行求解的問題。LaValle 于1998 年提出了隨機采樣的RRT算法，他通過使用增量式的前向采樣，構(gòu)建一棵搜索樹，隨后再對樹結(jié)構(gòu)進行搜索得到路徑。具體的流程為將起點位姿點加入隨機樹后，直接在空間中先隨機采樣一個位姿點，遍歷整個隨機樹中的節(jié)點并選出距離該隨機位姿點最近的節(jié)點，在滿足約束的前提下，以一定的步長向著隨機點方向生長，直至到達目標區(qū)域。從目標位姿點出發(fā)，依次尋找當前位姿點的父節(jié)點直至回到起始位姿點，即可得到規(guī)劃路徑

RRT*算法是在RRT 算法基礎(chǔ)上提出的一種路徑優(yōu)化算法。在快速地找到初始路徑之后繼續(xù)進行采樣，不斷地進行優(yōu)化直到找到目標點或者達到設(shè)定的最大循環(huán)次數(shù)。RRT*算法過程以偽代碼形式描述如下:

算法1 RRT*算法

雖然RRT*算法相較于RRT 算法能夠給出一條漸進最優(yōu)的路線，但是在實際環(huán)境運用時也存在一些缺點，例如最優(yōu)解收斂較慢，迭代次數(shù)以及設(shè)置的運行時間會影響最后的路線等。

采樣算法的質(zhì)量與規(guī)劃算法的運行效率有著密不可分的關(guān)系，有效的采樣策略能夠節(jié)約大量的搜索時間。初始RRT*算法采取的策略是在地圖中直接進行隨機采樣，會產(chǎn)生大量與最優(yōu)路徑明顯無關(guān)的采樣點，導(dǎo)致算法的實時性不佳。后續(xù)的研究者針對采樣算法進行了改進，例如Informed-RRT*(IRRT*)，在得到可行路徑后對采樣范圍進行縮小，初始點xinit和目標點xgoal設(shè)置為橢圓的焦點，焦距為Cmin，當前的最優(yōu)路徑長度Cbest為橢圓長軸，建立橢圓作為新的搜索范圍，如圖2 所示。由于橢圓的性質(zhì)，橢圓內(nèi)的任意一點到兩個焦點的距離之和小于長軸長度，所以只對橢圓內(nèi)部進行采樣，隨著采樣過程的進行，路徑長度Cbest不斷縮短，橢圓采樣面積減小，能夠有效地有效提升收斂速度。采樣空間可以表示為:

圖2 IRRT*原理示意圖

1.2 改進隨機采樣

在IRRT*的基礎(chǔ)上，為了進一步提高搜索效率，降低算法的隨機性，在生成新節(jié)點xrand的過程中，引入啟發(fā)式函數(shù)，引導(dǎo)新節(jié)點朝向xgoal生長，減小了規(guī)劃時間。

這里的Pr是位于(0，1)的隨機數(shù)，α是給定的常數(shù)。LineTo(xgoal) 指在當前節(jié)點與xgoal的連線上進行采樣；Random(χ)指在地圖上隨機采樣；當目標點xgoal已經(jīng)包含在隨機樹中時，xrand會使用Ellipse(xinit，xgoal)在上文提到的橢圓內(nèi)部進行采樣。因此，該采樣算法能夠更有效地生成新節(jié)點，這也意味著采樣時間得到了減少，算法的實時性得到提高。

為了評估啟發(fā)式IRRT*算法相較于傳統(tǒng)RRT*算法的改進，進行仿真對比，如圖3 所示，同時將尋找路徑所需節(jié)點數(shù)和路徑長度列入表1。

圖3 3 種RRT*路徑規(guī)劃結(jié)果

表1 不同RRT*路徑長度和時間對比

模擬環(huán)境地圖尺寸為500 m×500 m，設(shè)置無人艇路徑規(guī)劃起點位于左上角，終點位于右下角，坐標分別為(25，25)和(475，475)，白色和黑色分別代表可通行區(qū)域和不規(guī)則障礙物。通過傳統(tǒng)RRT*算法所獲得的路徑如圖中實線所示，所需的擴展節(jié)點為1 450 個，路徑長度為733.32 m。使用IRRT*算法獲得路徑如圖中點劃線所示，所需的擴展節(jié)點為613 個，路徑長度為722.09 m，而加入了啟發(fā)式函數(shù)的IRRT*算法所得路徑如圖中虛線所示，所需的擴展節(jié)點為526 個，路徑長度為701.31 m。

可以看出，使用啟發(fā)式IRRT*算法能夠減少所需擴展節(jié)點數(shù)量，由于RRT*算法主要耗時在碰撞檢測過程，而每個新生成的節(jié)點都需要調(diào)用該過程，因此可減少碰撞檢測部分，以降低算法所需時間。根據(jù)式(2)，控制采樣算法的一個關(guān)鍵參數(shù)α，調(diào)整該參數(shù)能夠改變采樣算法對于目標點的趨向，這里的α＝0.3。與IRRT*算法相比，引入啟發(fā)式函數(shù)后，所需節(jié)點減少了14%。因此，當無人艇在寬闊水面進行預(yù)規(guī)劃時，啟發(fā)式IRRT*算法能夠有效地給出一條全局路徑。

2 動態(tài)窗口法

由于無人艇的航行環(huán)境往往較為開闊且復(fù)雜，不但需要對地圖上已標記的障礙物進行規(guī)劃，也需要通過無人艇所搭載的傳感器檢測到的動態(tài)障礙物信息進行實時躲避，而局部路徑規(guī)劃能夠較好地解決該問題。結(jié)合上文通過啟發(fā)式IRRT*算法給出的全局路徑，同時考慮到海流的影響對于局部路徑規(guī)劃方法進行改進，使得無人艇在運行過程中能夠更好地對水面的情況做出及時的響應(yīng)。

動態(tài)窗口法(Dynamic Window Approach，DWA)是一種基于速度采樣的局部規(guī)劃方法，在速度空間采樣多組速度，并模擬一定時間內(nèi)無人艇的軌跡，根據(jù)預(yù)設(shè)的評價函數(shù)來比較這些軌跡的優(yōu)劣，從中選取最優(yōu)軌跡對應(yīng)的速度執(zhí)行。動態(tài)窗口的具體含義是根據(jù)載體的運動特性，將其速度采樣空間限制在一個可行的動態(tài)范圍內(nèi)。

2.1 速度采樣

根據(jù)無人艇本身的限制和環(huán)境約束，可以將速度的采樣空間限定在一定范圍內(nèi)，如式(3)～式(5)所示。

式中，vmin、vmax、ωmin、ωmax分別表示速度和角速度的最小值和最大值，vc、ωc表示當前的速度、角速度，表示加速度和角加速度的最大值。

同時基于無人艇運動安全的考慮，需要在碰到障礙物前停下來，因此速度和角速度必須滿足:

式中，dist(v，ω)表示在速度(v，ω)所對應(yīng)軌跡上與障礙物最近的距離。

2.2 考慮流場評價函數(shù)

對于動態(tài)窗口法采樣得到多組速度后，根據(jù)預(yù)設(shè)的時間窗口和前向模擬時間，模擬得到對應(yīng)的多組軌跡，從中剔除不安全的軌跡，計算其他軌跡的評價函數(shù)。

式中，G(v，ω)表示處于當前速度對應(yīng)軌跡的總評價函數(shù)值，hea(v，ω)用于評價當前航向與目標點方向之間的偏差，dis(v，ω)用于評價當前軌跡上各點與障礙物之間的最小距離，vel(v，ω)用于評價當前速度的大小，wav(v，ω)用于評價當前軌跡與全局規(guī)劃得到軌跡的偏離程度，α、β、γ、η分別為各項參數(shù)。

區(qū)別于移動機器人，無人艇路徑規(guī)劃問題中不能忽略流場的影響。考慮流場的無人艇路徑規(guī)劃方法的優(yōu)勢主要在于，基于無人艇體積小續(xù)航能力不強的前提，利用流場有效地減少行駛耗能。流場類型主要分為兩種:一種流速和流向相對固定的內(nèi)河流場，另一種則是時變的海洋流場。為了反映洋流擾動的實際情況，其數(shù)學(xué)模型如下:

在式(9)中，采用與時間相關(guān)的無維度函數(shù)B(t)來表征曲流振幅的振蕩，且B(t)＝B0＋εcos(ξt＋ψ)，其中B0、ε、ξ、ψ、k、c是用來表征時變流場的參數(shù)。因此，在某時刻t，無人艇當前位置所在流場的水流流速vs在直角坐標系下的投影vsx和vsy為:

在無人艇路徑規(guī)劃中，水流的影響應(yīng)當從流向和流速兩方面考慮，流場對于無人艇的影響體現(xiàn)在其航行速度上。因此為了綜合考慮海流對于無人艇運動的影響，本文在傳統(tǒng)動態(tài)窗口法基礎(chǔ)上引入了海流評價函數(shù)wav(v，ω)

式中，〈v，vs〉代表無人艇當前速度v與流場的夾角，范圍為[0，π]。這里假定無人艇能夠克服流場帶來的不良影響，滿足:

同時，為了避免流場評價函數(shù)wav(v，ω)的某項在評價函數(shù)太占優(yōu)勢，也需要對其進行歸一化處理:

2.3 改進DWA 算法仿真

為了驗證改進動態(tài)窗口法的效果，進行對比仿真實驗，分別對原始DWA 算法和考慮流場的DWA算法進行驗證，仿真結(jié)果如圖4 和圖5 所示。

圖4 不考慮流場的DWA 仿真

圖5 考慮流場的DWA 仿真

兩種方法驗證實驗初始設(shè)置均相同，航行環(huán)境設(shè)為50 m×50 m，圖中的黑色圓點代表障礙物位置，障礙物半徑設(shè)為2 m，箭頭代表流場，其中箭頭長短代表流速大小，箭頭方向代表流速方向。起始點設(shè)為左下角，坐標為(5，5)；目標點為右上角，坐標為(45，45)，曲線代表無人艇實時航行軌跡。

對比分析圖4、圖5 軌跡，相較于傳統(tǒng)動態(tài)窗口法僅考慮路徑最短，可以明顯看出考慮流場的動態(tài)窗口法能夠使無人艇在行駛過程中盡可能避開逆流場方向，在條件允許的情況下選擇沿流場方向。雖然此方法規(guī)劃的路徑長度相比傳統(tǒng)DWA 算法更長，但是能夠在成功避障的基礎(chǔ)上，考慮到能量優(yōu)化目標，計算出一條高質(zhì)量的路徑。

3 混合路徑規(guī)劃

雖然動態(tài)窗口法在局部避障時具有良好性能，但在實際運行中規(guī)劃的路徑極易出現(xiàn)缺乏目的性的情況，基于此本文在原有動態(tài)窗口法的基礎(chǔ)上將改進IRRT*生成的全局路線作為參數(shù)輸入到DWA中，作為局部目標點，以此保證規(guī)劃器得到的路線既有良好的避障性能，也不會出現(xiàn)抵達不了目標點的情況。

為了驗證混合路徑規(guī)劃算法的可行性，利用實際場景進行仿真實驗，圖6 是海域的衛(wèi)星地圖，對其進行二值化處理得到地圖如圖7 所示，白色代表可航行區(qū)域。

圖6 無人艇航行區(qū)域衛(wèi)星地圖

圖7 二值化地圖

仿真航行區(qū)域為712 m×400 m，起始點坐標為(360，300)，目標點為(45，80)。無人艇的最大速度設(shè)為10 m/s，最大加速度為2.5 m/s，最大角速度為20°/s，水流流向為右偏下45°。

如僅采用DWA 算法進行路徑規(guī)劃，結(jié)果如圖8所示，由于行駛路徑較為復(fù)雜，算法易陷入局部最小“陷阱”，導(dǎo)致任務(wù)失敗。

圖8 僅使用DWA 路徑規(guī)劃失敗

采用改進IRRT*給出一條預(yù)規(guī)劃路徑，如圖9折線所示。路徑長度為675.018 4 m。在全局路徑的基礎(chǔ)上，使用DWA 算法進行動態(tài)驗證。規(guī)劃航行結(jié)果如另一曲線所示，長度為726.167 5 m。

圖9 基于全局路徑的混合路徑規(guī)劃結(jié)果

仿真結(jié)果能夠很好體現(xiàn)混合路徑規(guī)劃的優(yōu)勢，將改進的IRRT*算法和DWA 算法相結(jié)合，能克服局部規(guī)劃缺乏目的性，易陷入局部最優(yōu)的問題，也能實現(xiàn)根據(jù)無人艇具體的運動學(xué)約束和實時的障礙物信息進行避障的要求，更有利于USV 在復(fù)雜水面條件下實現(xiàn)路徑規(guī)劃。

4 結(jié)論

分別對路徑規(guī)劃算法中的全局路徑規(guī)劃算法和局部路徑規(guī)劃算法進行改進，提出了改進IRRT*的全局路徑規(guī)劃算法和考慮流場的DWA 算法。本文采用的改進IRRT*算法與傳統(tǒng)IRRT*算法相比，搜索節(jié)點減少了14%，有效提升了搜索效率?？紤]流場的DWA 算法在有流場情況下能夠較好地利用流場影響，規(guī)劃出相對節(jié)能的局部路徑。將兩種方法聯(lián)合使用，通過混合路徑規(guī)劃仿真驗證，能夠?qū)崿F(xiàn)無人艇在行進時對路徑進行修正，安全完成任務(wù)。