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

張嘯天，陳熙源

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

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

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

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

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

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

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

1 全局路徑規劃

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

圖1 無人艇路徑規劃流程

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

1.1 RRT*算法

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

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

算法1 RRT*算法

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

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

圖2 IRRT*原理示意圖

1.2 改進隨機采樣

在IRRT*的基礎上，為了進一步提高搜索效率，降低算法的隨機性，在生成新節點xrand的過程中，引入啟發式函數，引導新節點朝向xgoal生長，減小了規劃時間。

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

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

圖3 3 種RRT*路徑規劃結果

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

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

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

2 動態窗口法

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

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

2.1 速度采樣

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

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

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

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

2.2 考慮流場評價函數

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

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

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

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

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

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

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

2.3 改進DWA 算法仿真

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

圖4 不考慮流場的DWA 仿真

圖5 考慮流場的DWA 仿真

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

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

3 混合路徑規劃

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

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

圖6 無人艇航行區域衛星地圖

圖7 二值化地圖

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

如僅采用DWA 算法進行路徑規劃，結果如圖8所示，由于行駛路徑較為復雜，算法易陷入局部最小“陷阱”，導致任務失敗。

圖8 僅使用DWA 路徑規劃失敗

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

圖9 基于全局路徑的混合路徑規劃結果

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

4 結論

分別對路徑規劃算法中的全局路徑規劃算法和局部路徑規劃算法進行改進，提出了改進IRRT*的全局路徑規劃算法和考慮流場的DWA 算法。本文采用的改進IRRT*算法與傳統IRRT*算法相比，搜索節點減少了14%，有效提升了搜索效率。考慮流場的DWA 算法在有流場情況下能夠較好地利用流場影響，規劃出相對節能的局部路徑。將兩種方法聯合使用，通過混合路徑規劃仿真驗證，能夠實現無人艇在行進時對路徑進行修正，安全完成任務。