基于RRT＊算法的水下機器人全局路徑規劃方法

丁 帥，陳苗苗，王 猛，谷志珉，任 峰

(1. 上海交通大學 水下工程研究所，上海 200240；2. 國家海洋局 北海海洋技術保障中心，青島 266033)

0 引 言

隨著海洋資源的開發與利用，自主水下機器人作用愈發突出，而精細化作業的需求對潛器智能控制及自主導航的要求也越來越高。路徑規劃作為關鍵技術之一，直接影響到潛器的性能。潛器的路徑規劃具體可描述為：在具有障礙物的水下環境中，依據預設的評價標準，規劃出一條從起點到終點最優或準優的無碰撞路徑。

Dijkstra 算法是最早被發明出來的規劃搜索算法，它主要利用貪心思想在圖里尋找最短路徑；1968 年，Hart 在Dijkstra 算法的基礎上加入了啟發式函數而創造出了A*算法，加快了搜索速度[1]；Wang Z 在2017 年成功的將A*算法應用在水下機器人全局路徑規劃上[2]。Fast Marching（FM）算法用到了非線性程函方程的一階數值近似，它可被認為是Dijkstra 算法的連續版本；Yu H 在2015 年實現了FM 算法在水下機器人路徑規劃上的應用[3]。人工勢場法是一種虛擬力法，它是假設目標點對機器人存在吸引力，障礙物對機器人存在斥力，由此建立一個虛擬的勢場，并讓機器人沿著勢場下降最快的方向前進直到目標點；然而該算法存在局部最小值問題[4]，楊建在2017 年成功的將人工勢場法應用到了水下機器人避障運動上[5]。繼而有學者提出進化算法，例如粒子群算法，蟻群算法和遺傳算法，這些算法在智能潛器中也得到了相關的應用[6-10]。PRM 算法和RRT 算法則是在許可范圍內，通過犧牲最優、隨機采樣實現快速可行的路徑規劃算法。PRM 算法的基本思想是在空間里隨機撒點并連線形成一個圖，然后在該圖上運行A*等搜索算法來尋找路徑。RRT 算法則是以起始點作為一棵樹的根節點，通過不斷的隨機擴展這棵樹上的節點，直到擴展的節點接近目標點，則認為在這棵樹上找到了一條唯一的路徑到達目標點，該算法的優點是計算速度較快，且可以在規劃過程中加入動力學約束。但也有明顯的缺點，找到的路徑不一定最優。Yu L 在2017 年成功地將RRT 算法應用到水下機器人的路徑規劃里[11]。2010 年，S.Karaman 通過引入代價函數來優化RRT 算法，經過逐次迭代來改善之前的路徑，優化后的算法被定義為RRT*算法[12]。RRT*算法不僅繼承了RRT 算法的優點，還克服了RRT 算法的缺點，最終會得到一條最優或準優的路徑，這額外的最優性使得RRT*算法在高維和實時工況下能快速有效的實現規劃。RRT*算法在水下機器人路徑規劃中的應用鮮有實現。

實際應用中海洋環境比較復雜，有時還會存在洋流因素，洋流的速度和方向都會對水下機器人的能量消耗產生影響。因此在路徑規劃時，除了要考慮路徑長度、障礙物信息外，還應考慮能耗問題。本文根據RRT*算法的優點，考慮洋流對水下機器人能耗的影響，設計實現基于RRT*的路徑最短和能耗最低的路徑規劃算法，并通過相應的仿真模擬和實驗室平臺的真機實現驗證了該算法的可行性。

1 RRT 算法

1.1 問題定義

讓 C ?Rn表示結構空間，其中 n表示結構空間的維數。 結構空間可進一步被劃分為障礙物空間Cobs?C 和非障礙 物空間Cfree=CCobs。讓T =(V,E)表示隨機生長樹，其中 V表示節點， E表示連接點的邊。讓qinit和 qgoal表示初始狀態和目標狀態。讓p(qi,qj)表示連接任意2 個狀態qi∈Cfree和qj∈Cfree的路徑。

問題1（可行路徑）：在非障礙物區域內找到一條從qinit到 qgoal的路徑。

問題2（最優路徑）：在非障礙物區域內找到一條從qinit到 qgoal的最優路徑，使路徑代價花費最小。

1.2 RRT 算法

RRT 算法是一種基于概率采樣的搜索方法，圖1為RRT 算法程序流程圖，具體實現如算法1 所示。圖2為隨機樹的關鍵擴展過程（Extend 函數）程序流程圖，具體實現如算法2 所示。

圖1 RRT 算法程序流程圖Fig. 1 Program flow diagram of RRT algorithm

圖2 擴展過程（Extend 函數）程序流程圖Fig. 2 Program flow diagram of Extend function

RRT(qinit)

算法1：

算法2：Extend(T,qrand)

其中，各函數含義及作用如下：

S ample(i)：這個程序返回非障礙物區域內一個獨立的均勻分配的隨機點 qrand∈Cfree。

Nearest(T,qrand)：這個程序返回一個在樹中離qrand歐式距離最近的點qnearest

S teer(qnearest,qrand)： 這程序返回一個新的點qnew使 d(qnew,qrand)最 小，并滿足條件 d(qnew,qnearest)＜η，η是我們設定的一個值。

ObstacleFree(p(qnearest,qnew))這程序檢查路徑p(qnearest,qnew) 是否屬于非障礙物區域Cfree。如果路徑p(qnearest,qnew) 屬于Cfree，則會返回真值，否則返回假值。

從RRT 算法過程中可以看出隨機樹的擴展偏向于未被訪問過的區域。這使得隨機樹在剛開始時擴展得很快，并能完全覆蓋結構空間。隨機樹上的節點在結構空間里是均勻的。如果可行路徑存在，那在節點數量足夠的前提下，RRT 算法就一定能找到一條從起始點到目標點的可行路徑。通過上述分析可知，RRT 算法所找到的可行路徑不一定最優。

2 基于RRT*的路徑規劃算法

針對RRT 算法存在的問題，S.Karaman 提出通過引入代價函數來優化，經過逐次迭代來改善之前的路徑，從而得到一條最優或準優的路徑。

2.1 RRT＊算法

在R R T 算法內容基礎上擴展定義如下：1）d(qi,qj) 表示任意2 個狀態 qi∈C 和 qj∈C之間的正歐式距離；2） φq,r:={qi∈C:d(q,qi)≤r} 表 示 中 心 點 在 q，半徑為 r ∈R,r ＞0 的閉環球形區域，其中 q ∈C可以是任 意 狀 態；3） c(qi,qj) 表 示 從 狀 態 qi∈Cfree到 狀 態qj ∈Cfree的路徑代價。

RRT*算法的初始化過程同算法1 中一致，而不同的是RRT*算法在它的擴展過程中引入了代價函數，并通過代價函數來判斷是否會更新之前的路徑，以此來改善路徑。圖3 則為RRT*算法中擴展過程（Extend 函數）程序流程圖，具體實現如算法3 所示，接下來的則是一些在擴展過程中被調用的函數。

圖3 擴展過程（Extend 函數）程序流程圖Fig. 3 Program flow diagram of Extend function

算法3：RRT* Extend(T,qrand)

算法4：Getsortedlist(qnew,Cnear)

算法6：InsertVertex(qnew,qmin,T)

其中，各函數含義及作用如下：Cnear ?V：更精確一點，Cnear={q ∈V:d(q,qnew)≤γ(logi/i)1/n}，其中i是節點數量， n是結構空間的維數， γ是個常數。

Getsortedlist(qnew,Cnear)：這個程序建立了一個列表 Ls，其中每一個元素都由(q′,C′,p(q′,qnew))組成，并使列表Ls按照代價{c(qinit,q′)+c(q′,qnew)}的升序順序排序。

ChooseBestParent(Ls)：這程序被用來搜索列表Ls的一個狀態qmin∈Ls，它能提供一條從qinit到qnew經過 qmin的代價最低并無碰撞的路徑。

InsertVertex(qnew,qmin,T)：這個程序將qnew點插入到了樹上形成一個新的節點。

RewireVertices(qnew,Ls,E)：這個程序被用來更新q′∈Cnear的父節點如果一些明確的條件被滿足的話。

如同算法1 的開始一樣，在初始化后RRT*算法開始它的迭代過程。先通過從非障礙物空間里隨機采樣qrand狀態，然后進入擴展過程。在擴展過程里，qnearest最先通過 Nearest(T,qrand) 程序被獲得，然后 qnew通過S teer(qnearest,qrand)程序被產生。之后，通過NearVertices(T,qnew,r)程序發現qnew點在樹上的鄰近點集合Cnear。如果Cnear集合為空，那么Cnear將會被賦值qnearest。集合 Cnear被用在G etsortedlist(qnew,Cnear)程序里來生成一個元素為(q′,C′,p(q′,qnew))并按代價{c(qinit,q′)+c(q′,qnew)} 升序排序的列表 Ls。然后這列表 Ls被用在ChooseBestParent(Ls) 程序里來返回一個狀態qmin∈Ls使{c(qinit,q′)+c(q′,qnew)}最小并提供一個從 q′到qnew的無碰撞路徑。如果qmin不為空，qmin將會成為qnew的父節點，而且 qnew點 則通過 InsertVertex(qnew,qmin,T)程序被插入到這棵樹中。然后執行RewireVertices(qnew,Ls,E)改寫程序，這程序將會檢查每一個節點 q′∈Cnear。如果 現存的從 qinit到q′的路徑的代價多于從 qinit到 qnew再到q′的路徑的代價且路徑p(qnew,q′)屬于非障礙物區域，那么 qnew將 會成為 q′的父節點。如果這些條件并不被滿足，那么這棵樹不會發生變化，程序則會繼續去檢查下一個在Cnear中的節點。

2.2 路徑距離代價

本文只考慮歐式空間，兩點間的路徑距離代價則為兩點間的正歐式距離，可表示為：

2.3 考慮洋流因素的水下機器人路徑能耗代價

基于Fossen 模型[13]，對于左右對稱的低速水下機器人來說，非線性阻尼矩陣可以被忽略，而忽略垂蕩、橫搖和縱搖后的線性化的阻尼矩陣可被寫成：

式中，X{.}，Y{.}和N{.}為經典的水動力系數；u和v分別為水下機器人的縱向速度和橫向速度；r為水下機器人的角速度。

在真實的海洋環境中，有時還會存在洋流因素，而洋流的速度和方向都會對水下機器人路徑上的能量消耗產生影響。

圖4 水下機器人體坐標系下2D 無旋洋流速度分量示意Fig. 4 The velocity component of 2D irrotational ocean current in ROV body coordinate system

如圖4 所示，當存在一個緩變的速度為 vc，流向為 β的2D 無旋洋流 (rc=0) 時， uc和 vc分別為洋流投影到水下機器人縱向和橫向的速度，可表示為：

其中 φ是水下機器人的首向角。

設vr=v-vc是 相對速度，此時在2D 無旋洋流環境下水下機器人所受到的阻尼力和阻尼力矩[13]則為：

假設在水下機器人路徑跟蹤里會使水下機器人的艏向永遠與規劃出的路徑方向保持一致，并保持勻速行駛，則水下機器人橫向上沒有位移，可認為水下機器人橫向上的阻力不做功，則水下機器人在路徑上的能量消耗只需要考慮水下機器人在縱向上所受到的阻力做功和原地旋轉時受到的阻力矩做功。則從 qi點到與它直接相連的 qj點的路徑能耗代價可表示為：

式中：φ2為水下機器人在路徑p(qi,qj)上的首向角；φ1為水下機器人在路徑p(qparent,qi)上的首向角；qparent為qi的父節點。

若將式（1）與式（5）結合到一起，則可得到總的路徑代價函數表示為：

當 α=1時，則式（6）變成水下機器人在2D 無旋洋流下的能耗最低路徑規劃算法中的代價函數。當α=0時，則式（6）變成水下機器人路徑最短路徑規劃算法中的代價函數。

3 仿真

3.1 RRT＊算法和RRT 算法的仿真對比

基于前文理論分析及算法設計，利用Matlab 編程實現RRT 算法和RRT*算法（路徑最短），并進行仿真對比，仿真結果如圖5 和圖6 所示。

圖中矩形區域（黑色）代表障礙物，Starting Point 代表起始點，圓形區域則代表目標區域，粗實線則為所尋找到的路徑，將兩圖中的路徑長度通過計算得到表1。

從仿真結果的對比可以看出，RRT 算法能找到一條可行路徑，但不一定是最優或準優路徑；RRT*算法因代價函數的引入，找到的路徑不僅是可行路徑，相對RRT 算法所得路徑更優，可視為準優路徑。

圖5 RRT 算法仿真結果Fig. 5 Simulation results of RRT algorithm

圖6 RRT*算法仿真結果Fig. 6 Simulation results of RRT* algorithm

表1 兩種算法路徑長度對比Tab. 1 Comparison of path length between two algorithms

3.2 2D 無旋洋流環境中路徑最短與能耗最低路徑規劃的仿真模擬

假設流場環境為2D 無旋洋流，流速為 vc，流向β；仿真計算時設定水下機器人艏向會保持與規劃出的路徑方向一致，并保持勻速行駛；仿真水動力參數則根據實驗室現有的水下機器人（ROV）以往的經驗值設定,設置仿真條件如表2 所示，仿真結果如圖7 所示。

從圖7 中可以看出，當無洋流因素影響時，ROV 能耗最低路徑規劃得到的路徑（b）與ROV 路徑最短路徑規劃得到的路徑（a）一致，為左邊路徑。而當存在洋流時，ROV 能耗最低路徑規劃得到的路徑（c）和（d）則始終為右邊路徑。將這左、右2 條路徑在上述3 種情況下的能耗和長度通過計算并整理得到表3。

表2 仿真參數表Tab. 2 Simulation parameter table

圖7 ROV 兩種路徑規劃的仿真結果Fig. 7 Simulation results of two path planning of ROV

表3 兩條路徑的能耗和長度對比表Tab. 3 Comparison of energy consumption and length of two paths

可以看出，左邊路徑的長度要比右邊路徑短。然而當洋流速度越大，左邊路徑所需要的能耗就越大，而右邊路徑所需要的能耗則越小。當不存在洋流影響時，左邊路徑的能耗要低于右邊路徑，故ROV 能耗最低路徑規劃得到的路徑是左邊路徑；當洋流速度為1 kn 或2 kn 時，左邊路徑的能耗要高于右邊路徑，故ROV 能耗最低路徑規劃得到的路徑是右邊路徑。該仿真結果驗證了ROV 路徑最短和能耗最低路徑規劃算法可行有效。

4 RRT*算法的實驗室實現

4.1 實驗條件

本次實驗是在上海交通大學拖曳水池進行的，實驗水池水深7.5 m，長300 m，寬16 m。實驗對象則是采用實驗室現有的水下機器人（ER3K，見圖8），該水下機器人的基本性能參數見表4。地形環境則通過搭載在水下機器人本體上的聲吶（Super SeaKing DST，見圖9）獲取，聲吶性能參數見表5。

圖8 水下機器人Fig. 8 Remote operated vehicle

4.2 實驗方案

在本次實驗中，采用2 個油桶來充當障礙物，然后利用搭載在水下機器人上的實驗聲吶去掃描檢測獲取環境里的障礙物信息，最后再利用基于RRT*算法的路徑最短和能耗最低路徑規劃算法得到準優路徑。圖10 為本次實驗示意圖，圖11 為本次實驗現場圖。

表4 水下機器人基本性能Tab. 4 Basic performance of remote operated vehicle

圖9 實驗聲吶Fig. 9 Experimental sonar

表5 Super SeaKing DST 聲吶基本屬性Tab. 5 Basic performance of sonar

4.3 實驗結果

利用水下機器人上所搭載的實驗聲吶進行掃描檢測，獲得的聲吶圖像如圖12 所示。

圖12 中，可看出圖中深色區域為障礙物或雜波。在實際情況中，水下機器人并無法分辨出深色區域是障礙物還是雜波，所以此處將深色區域全部視為障礙物以此來規劃路徑。將聲吶圖像進行圖像處理后得到如圖13 所示。

圖10 實驗示意圖Fig. 10 Experimental schematic diagram

圖11 實驗現場Fig. 11 Experimental scene

圖12 聲吶圖像Fig. 12 Sonar image

圖13 圖像處理后的地圖Fig. 13 Map after image processing

圖中黑色區域被認為是障礙物，白色區域則被認為是可通行區域。進一步為了安全考慮，此處通過將圖13 中的障礙物擴大來保證水下機器人通行的安全距離，得到的最終地圖如圖14 所示。受實驗條件所限，實驗環境為無流水池，則使水下機器人在圖14 上分別實施路徑最短和能耗最低（無洋流情況下）路徑規劃算法，規劃出的路徑結果如圖15 所示。

從圖15 可看出，水下機器人在該環境下實施路徑最短路徑規劃和能耗最低路徑規劃得出的路徑完全一致。該水池實驗路徑規劃結果表明了所設計的基于RRT*的路徑最短和能耗最低的路徑規劃算法可行有效。

圖14 擴大障礙物后的地圖Fig. 14 Map after enlarging the obstacle

圖15 水下機器人路徑規劃結果Fig. 15 Path planning results of underwater vehicle

5 結 語

本文針對復雜海洋環境中水下機器人路徑規劃問題展開研究，綜合考慮規劃路徑長度與全程能耗，設計出基于RRT*的路徑最短和能耗最低的路徑規劃算法，繼而進行仿真模擬和實驗室現有水下機器人平臺的真機實現，仿真及真機驗證結果顯示所設計的路徑規劃算法可行有效，為后期海上應用奠定基礎。