基于RRT和DWA的欠驅動UUV路徑規劃

嚴浙平，黃俊儒，吳 迪

（哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001）

0 引言

水下無人潛航器（Unmanned Underwater Vehicle，UUV）的運動規劃是UUV核心技術之一，本文重點討論水平面規劃。路徑規劃以功能和側重點分為全局路徑規劃和局部路徑規劃，兩者有區別但又可以相互轉化。全局路徑規劃是在已知周圍環境的前提下可以快速規劃出一條最優路徑，但全局路徑規劃有時不考慮動力學約束，并且水下環境復雜，環境信息獲取并不完全，容錯率較低；局部路徑規劃側重于局部環境規避未知障礙物的能力，但其尋找到最優全局路徑的效率低下。如何使 UUV快速安全地到達目標點是一個重要的研究方向。

全局路徑規劃有很多成熟的算法，早在 1958年就有了Dijsktra算法，改進后又有了A*和D*算法，以及蟻群算法、貪心算法、粒子群算法等。丁帥以減少水下機器人能量消耗為前提，基于RRT*算法設計了能耗最低的路徑規劃算法[1]；孫波針對移動機器人的路徑規劃提出了粒子群優化算法[2]；鄧超針對UUV三維空間移動提出了雙種群粒子群算法[3]。局部路徑規劃一般配備了各類傳感器以獲取局部地圖或者探測未知障礙物，其算法也有人工勢場法以及動態窗口法。同時也有衍生的改進和結合的算法：黃辰考慮到機器人的尺寸，通過激光雷達獲取局部地圖，改進了DWA算法[4]；何壯壯在ROS環境下提出了針對兩輪機器人的 D*和 DWA結合算法等[5]。

針對欠驅動UUV的研究也比較成熟，張偉針對垂直面的路徑跟蹤目標設計了基于模型預測連續系統跟蹤控制器，避免離散化，提高了控制器的泰勒展開階次，提高了精度[6]。趙玉飛針對UUV近海底的路徑規劃問題，提出了改進粒子群算法[7]。該算法可以有效規避靜態障礙物，但是無法規避動態障礙物，針對這一問題，本文提出了快速擴展隨機樹以及動態窗口的融合算法框架。該算法一方面繼承了 RRT算法的全局規劃能力，一方面繼承了DWA算法的實時避障能力，實現了實時避障的全局路徑規劃。通過修改外部框架的距離參數可以提高全局路徑的精度，同時通過修改內部框架的評價函數參數可以使UUV應對不同的需求和地形，最后設計了融合參數μ，通過修改μ來確定內外框架的融合度。

1 UUV水平面數學模型

欠驅動UUV路徑規劃為水平面的規劃，所以不考慮垂直方向運動，水平面模型由欠驅動 UUV六自由度模型在水平面方向解耦得到。

水平面運動學模型：

水平面動力學模型：

其中

動態窗口算法中UUV未來時域的狀態由狀態空間模型得到，由式（1）和式（2）轉換為 UUV的水平面狀態空間方程：

式中：x∈R6，u∈R2，y∈R3，狀態空間方程的狀態量為UUV的位置姿態，輸出量為UUV的實際位置坐標；x，y為UUV水平面坐標；u，v為橫、縱向速度；ψ，r為艏向角和角速度；Xprop、τr為縱向推力以及轉向力矩。

2 路徑規劃算法

2.1 RRT全局路徑規劃算法

在全局路徑規劃的算法中，快速擴展隨機樹（Rapidly-exploring Random Trees，RRT）算法由LaValle教授在20世紀末提出。RRT算法相對于其他全局算法的優點在于可以更快更靈活地規劃出路徑，并且不要求障礙物形狀的規則性。但是缺點也尤為明顯：它的效率并不穩定，由于其迭代過程沒有目標性，所以每次的路徑選取并不相同。經過類似迷宮的狹窄區域很難找到出口，需要大量反復計算，浪費時間和資源。并且所需路徑越平滑，計算量也越大。一些改進的 RRT算法彌補了部分缺點，且可通過插值修正曲線。

初始化的地圖只包含隨機樹的根節點也就是起始點Qint和目標點Qgol。首先按照策略概率p生成一個隨機點Qrad，p的生成由一個隨機數保證。隨機樹上離該隨機點最近的節點Qnear（初始化的情況下為起始點）向Qrad矢量方向生成一個新節點Qnew。Qnear和Qnew的長度為距離參數qdist，修改此參數可以修改RRT算法的精度和計算量。之后對隨機樹上的節點Qnear以及Qnew的路線做障礙物碰撞檢測，若發生碰撞則放棄此條路徑，若沒有發生碰撞則將節點Qnew加入到隨機樹節點中，Qnear和Qnew加入到路徑矩陣。重復以上步驟直到Qnew與Qgol之間的距離小于設定值Qset，即說明到達目標點。最后依據貪心算法簡化得到的路徑，從起點Qint起依次尋找與Qgol節點滿足碰撞檢測函數的節點q*，用此路徑替代之前的路徑，再以q*為終點依次尋找得到一個最簡路徑。同時需要注意以下幾點。

圖1 RRT算法示意圖Fig. 1 RRT algorithm diagram

1）策略概率p依據目標點的距離給定，以一個隨機數確保概率的有效性。距離目標點越近生成Qrad的概率越高，否則隨機樹將聚集在一個區域，不能很好地向目標點方向伸展探索空間。p選取為

2）為了保證算法的效率和可控性，需要設定每次搜索的循環次數，在給定次數之內完成循環，否則放棄此次搜索。以下為RRT算法的具體步驟：

①加載全局地圖（獲取障礙物信息，給定全局路徑起點Qint，終點Qgol），設置距離參數qdist，范圍參數Qset；

②設置循環；

③隨機生成Qrad，設置全局終點附近生成Qrad的概率p；

④選取距離Qrad最近的隨機樹節點Qnear向Qrad方向伸展，伸展長度為距離參數qdist，伸展后得到新節點Qnew；

⑤對Qnear與Qnew之間的路徑進行碰撞檢測，若此路徑與障礙物存在交集則舍棄該節點，若不存在交集則將Qnew加入到隨機樹節點中；

⑥檢測循環次數，若達到循環次數則回到③重新構建隨機樹，未超出次數則繼續擴展隨機樹；

⑦檢測Qnew與Qgol之間的距離是否小于Qset，若小于說明到達目標點則跳出循環，并輸出隨機樹節點以及路徑，若不小于則回到③；

⑧通過碰撞檢測簡化輸出的隨機樹節點和路徑。從起點依次尋找與終點Qgol滿足未碰撞的節點，找到未碰撞的節點q*后即舍棄該節點之后的節點，簡化此節點到終點的路徑，以此節點為終點重復以上過程繼續向前尋找未碰撞節點得到最簡路徑。

2.2 DWA局部路徑規劃算法

相對于全局路徑規劃來說，局部路徑規劃并不要求對周圍環境百分之百的了解。全局路徑規劃為移動機器人在已知全局地圖的前提下規劃出從一個區域到另一個區域的具體路線，而局部路徑規劃的地圖多為臨時獲取，由獲得的局部地圖規劃出一條臨時路線，隨著機器人的移動，局部地圖實時更新，路線也實時更新。局部路徑規劃往往和各類傳感器協同工作，由傳感器獲取周圍環境的具體信息，進而得到局部地圖進行規劃[8]。局部路徑規劃的缺點是受參數以及位置環境物體影響，隨機性較高，無法生成一個固定高效的全局路線[9-10]。

動態窗口算法（Dynamic Window Approach，DWA）同樣是20世紀末提出，其使用的是在線規劃的方法：根據機器人本身的運動學模型得到各類約束，利用運動學約束根據當前的狀態得出下一拍的動態區域，區域內包括機器人下一拍能實現的全部運動軌跡和狀態。常用的移動機器人運動學模型在相鄰拍數的軌跡一般用直線代替，用圓弧代替相比于直線更精確一些，而UUV的受力十分復雜，需要UUV專用的模型。之后通過對區域內的所有軌跡進行評價分析，得到一條得分最高的即為最優軌跡。

DWA算法的核心主要有2部分，第1部分為動態窗口的生成，依據以下幾點。

1）速度與角速度約束，由UUV本身的性能得到最大與最小速度。

2）加速度約束，UUV的加速度受螺旋槳的推力以及水流情況的影響，所能達到的最小與最大加速度，在一拍時間內與最大最小速度和角速度共同組成了一個軌跡范圍，生成了一拍時間內的動態窗口。

3）碰撞檢測，為了使 UUV避免碰撞到障礙物，相應的距離需要小于對應的速度。

式中：Ddist(v,ω)為UUV與障礙物之間的軌跡距離，若在一定距離內小于對應速度則該條軌跡保留，否則去除該條軌跡。

第2部分為軌跡的評分策略函數G(v,)ω，根據生成的動態窗口得到若干條軌跡，分析每一條軌跡的效率和安全性，評分主要依據以下幾點。

1）艏向Eheading(v,ω)評分，UUV軌跡的實際航向越接近目標方向，則該評分越高。

2）安全系數Esdist評分，UUV軌跡距離障礙物越遠，則評分越高；若該條軌跡距離障礙物小于對應速度的安全距離則直接舍棄；全路路徑的安全性首先由此函數檢驗。

3）效率Evelocity(v,ω)評分，根據UUV軌跡的曲率、角加速度以及效率進行評分，越平滑速度越快，評分越高。

由于三類分屬于不同的系統，為了防止某一參數占比過高導致不同路徑的評分占比差距過大，將三類評價函數統一可以得到Eevaluate(i)。

加入可調整的占比得到最終的評分函數。α為艏向評分占比，β為安全系數評分占比，γ為效率評分占比。

3 基于RRT和DWA的改進路徑規劃算法

針對全局算法和局部算法的優缺點以及UUV性能與安全性的需求，提出了RRT和DWA改進的融合算法。該算法主要有兩層規劃設計，一層外部框架和一層內部框架。外部框架由RRT算法實現，內部框架將由RRT算法得到全局路徑分段，再由DWA在線規劃。融合算法的框架如圖2所示。具體流程如下：

1）首先全局框架加載全局地圖，當獲取到起始點和目標點信息后規劃出一條全局路徑。傳感器進入待機狀態，傳感器的范圍即是局部框架動態軌跡窗口的范圍。

2）將得到的全局路徑發送給局部框架，利用DWA算法檢驗全局路徑是否滿足 UUV的運動學規律，若不滿足則重新生成全局路徑。此外，當傳感器得到的局部地圖中有全局地圖中未知的障礙物，并且此障礙物不在安全距離之外時則會發生碰撞，發出碰撞警告。

圖2 RRT-DWA融合算法框架Fig. 2 RRT-DWA fusion algorithm framework

3）局部框架加載全局路徑后，由融合參數μ確定全局路徑插值，μ參數即為由RRT算法得到的最終全局路徑的分段比例，根據路徑的長短以及障礙物的密度進行修改，這里取為0.05。再由DWA算法在線生成局部路徑，直到到達目標點完成循環，得到修正后的路徑。

表1 動態窗口仿真參數Table 1 Dynamic window simulation parameters

4 仿真驗證分析

針對以上設計的算法，構建1個1 000 m2的仿真地圖，圖中黑色區域為障礙物；目標的起始點為原點，終點為地圖右上角。首先給定3組參數驗證單獨使用動態窗口的全局路徑規劃性能。

圖3為利用DWA算法得到的路徑，改變DWA算法中評價函數中艏向評分占比，安全系數評分占比以及效率評分占比將得到不同的路徑。圖中藍色和綠色兩條軌跡的航向角評分分別為0.5和0.45，占比較高，在（300，300）坐標附近時為保證航向角評分路徑的生成朝向目標點，UUV從障礙物上方行駛。下方紅色軌跡的安全性評分為0.6，占比更高，所以在同一位置為保證安全性的評分，遠離障礙物，UUV向下行駛。這就導致了單純的DWA算法受很多因素影響，很難找到一條簡潔的全局軌跡。

圖3 基于DWA算法的局部規劃Fig. 3 Local planning based on DWA algorithm

圖4和圖5為利用RRT算法得到的全局路徑，兩次生成的全局路徑并不相同，說明該算法有較高的靈活性。由于 RRT算法得到的全局路徑未考慮UUV的運動學約束，在圖4以及圖5中部分路徑與障礙物相對較近，小于10 m的安全距離，安全性得不到保證。

圖4 基于RRT算法的全局規劃（1）Fig. 4 Global planning based on RRT（1）

圖5 基于RRT算法的全局規劃（2）Fig. 5 Global planning based on RRT（2）

圖6和圖7為RRT-DWA融合算法生成的路徑，其中綠色軌跡為圖4和圖5中利用RRT得到的全局路徑，紅色軌跡為RRT-DWA融合算法軌跡。由圖可知，該條軌跡由于采用了RRT算法，其軌跡比較靈活，很容易找到一條簡潔的全局路徑。同時后加入的DWA算法使得該條路徑滿足UUV的運動學規律，可以時刻與障礙物保持距離，保證了安全，繼承了DWA算法的躲避移動或未知障礙物的能力。

圖6 基于融合算法的全局規劃（1）Fig. 6 Global planning based on RRT-DWA（1）

圖7 基于融合算法的全局規劃（2）Fig. 7 Global planning based on RRT-DWA（2）

表2中RRT1和RRT-DWA1分別對應圖4和圖 6中一段范圍內的最近障礙物的距離，RRT2、RRT-DWA2對應圖5和圖7中障礙物距離。由表知RRT算法得到的軌跡無法實時保證安全距離；融合算法得到的軌跡可以始終保證10 m的安全距離，說明了融合算法具有良好的避障能力。

表2 最近障礙物距離Table 2 Nearest obstacle distance

5 結束語

為保證UUV水平面規劃的效率以及安全性，以UUV水平面狀態空間方程為研究對象，針對傳統局部路徑規劃和全局路徑規劃導航方式的優缺點，提出了RRT-DWA融合算法。融合算法的外部框架不考慮動力學約束，仿真結果顯示有較好的全局路徑搜索能力和靈活性；內部框架結合UUV的運動模型、動力學模型以及傳感器實時獲取局部信息，通過評價函數可以實現障礙物的規避。仿真結果顯示，融合算法始終可以保持10 m的安全距離，保證了安全，通過內部框架判斷全局路徑的可行性。同時應對不同的海底區域可以通過調整局部框架的評價函數參數來滿足安全性或者效率的需求。綜上，RRT-DWA融合算法可以實現以上功能，滿足UUV現實需求。