基于RRT和DWA的欠驅(qū)動(dòng)UUV路徑規(guī)劃

嚴(yán)浙平，黃俊儒，吳 迪

（哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001）

0 引言

水下無人潛航器（Unmanned Underwater Vehicle，UUV）的運(yùn)動(dòng)規(guī)劃是UUV核心技術(shù)之一，本文重點(diǎn)討論水平面規(guī)劃。路徑規(guī)劃以功能和側(cè)重點(diǎn)分為全局路徑規(guī)劃和局部路徑規(guī)劃，兩者有區(qū)別但又可以相互轉(zhuǎn)化。全局路徑規(guī)劃是在已知周圍環(huán)境的前提下可以快速規(guī)劃出一條最優(yōu)路徑，但全局路徑規(guī)劃有時(shí)不考慮動(dòng)力學(xué)約束，并且水下環(huán)境復(fù)雜，環(huán)境信息獲取并不完全，容錯(cuò)率較低；局部路徑規(guī)劃側(cè)重于局部環(huán)境規(guī)避未知障礙物的能力，但其尋找到最優(yōu)全局路徑的效率低下。如何使 UUV快速安全地到達(dá)目標(biāo)點(diǎn)是一個(gè)重要的研究方向。

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

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

1 UUV水平面數(shù)學(xué)模型

欠驅(qū)動(dòng)UUV路徑規(guī)劃為水平面的規(guī)劃，所以不考慮垂直方向運(yùn)動(dòng)，水平面模型由欠驅(qū)動(dòng) UUV六自由度模型在水平面方向解耦得到。

水平面運(yùn)動(dòng)學(xué)模型：

水平面動(dòng)力學(xué)模型：

其中

動(dòng)態(tài)窗口算法中UUV未來時(shí)域的狀態(tài)由狀態(tài)空間模型得到，由式（1）和式（2）轉(zhuǎn)換為 UUV的水平面狀態(tài)空間方程：

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

2 路徑規(guī)劃算法

2.1 RRT全局路徑規(guī)劃算法

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

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

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

1）策略概率p依據(jù)目標(biāo)點(diǎn)的距離給定，以一個(gè)隨機(jī)數(shù)確保概率的有效性。距離目標(biāo)點(diǎn)越近生成Qrad的概率越高，否則隨機(jī)樹將聚集在一個(gè)區(qū)域，不能很好地向目標(biāo)點(diǎn)方向伸展探索空間。p選取為

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

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

②設(shè)置循環(huán)；

③隨機(jī)生成Qrad，設(shè)置全局終點(diǎn)附近生成Qrad的概率p；

④選取距離Qrad最近的隨機(jī)樹節(jié)點(diǎn)Qnear向Qrad方向伸展，伸展長度為距離參數(shù)qdist，伸展后得到新節(jié)點(diǎn)Qnew；

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

⑥檢測循環(huán)次數(shù)，若達(dá)到循環(huán)次數(shù)則回到③重新構(gòu)建隨機(jī)樹，未超出次數(shù)則繼續(xù)擴(kuò)展隨機(jī)樹；

⑦檢測Qnew與Qgol之間的距離是否小于Qset，若小于說明到達(dá)目標(biāo)點(diǎn)則跳出循環(huán)，并輸出隨機(jī)樹節(jié)點(diǎn)以及路徑，若不小于則回到③；

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

2.2 DWA局部路徑規(guī)劃算法

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

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

DWA算法的核心主要有2部分，第1部分為動(dòng)態(tài)窗口的生成，依據(jù)以下幾點(diǎn)。

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

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

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

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

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

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

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

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

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

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

3 基于RRT和DWA的改進(jìn)路徑規(guī)劃算法

針對全局算法和局部算法的優(yōu)缺點(diǎn)以及UUV性能與安全性的需求，提出了RRT和DWA改進(jìn)的融合算法。該算法主要有兩層規(guī)劃設(shè)計(jì)，一層外部框架和一層內(nèi)部框架。外部框架由RRT算法實(shí)現(xiàn)，內(nèi)部框架將由RRT算法得到全局路徑分段，再由DWA在線規(guī)劃。融合算法的框架如圖2所示。具體流程如下：

1）首先全局框架加載全局地圖，當(dāng)獲取到起始點(diǎn)和目標(biāo)點(diǎn)信息后規(guī)劃出一條全局路徑。傳感器進(jìn)入待機(jī)狀態(tài)，傳感器的范圍即是局部框架動(dòng)態(tài)軌跡窗口的范圍。

2）將得到的全局路徑發(fā)送給局部框架，利用DWA算法檢驗(yàn)全局路徑是否滿足 UUV的運(yùn)動(dòng)學(xué)規(guī)律，若不滿足則重新生成全局路徑。此外，當(dāng)傳感器得到的局部地圖中有全局地圖中未知的障礙物，并且此障礙物不在安全距離之外時(shí)則會(huì)發(fā)生碰撞，發(fā)出碰撞警告。

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

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

表1 動(dòng)態(tài)窗口仿真參數(shù)Table 1 Dynamic window simulation parameters

4 仿真驗(yàn)證分析

針對以上設(shè)計(jì)的算法，構(gòu)建1個(gè)1 000 m2的仿真地圖，圖中黑色區(qū)域?yàn)檎系K物；目標(biāo)的起始點(diǎn)為原點(diǎn)，終點(diǎn)為地圖右上角。首先給定3組參數(shù)驗(yàn)證單獨(dú)使用動(dòng)態(tài)窗口的全局路徑規(guī)劃性能。

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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