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

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

0 引 言

隨著海洋資源的開發(fā)與利用，自主水下機(jī)器人作用愈發(fā)突出，而精細(xì)化作業(yè)的需求對(duì)潛器智能控制及自主導(dǎo)航的要求也越來(lái)越高。路徑規(guī)劃作為關(guān)鍵技術(shù)之一，直接影響到潛器的性能。潛器的路徑規(guī)劃具體可描述為：在具有障礙物的水下環(huán)境中，依據(jù)預(yù)設(shè)的評(píng)價(jià)標(biāo)準(zhǔn)，規(guī)劃出一條從起點(diǎn)到終點(diǎn)最優(yōu)或準(zhǔn)優(yōu)的無(wú)碰撞路徑。

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

實(shí)際應(yīng)用中海洋環(huán)境比較復(fù)雜，有時(shí)還會(huì)存在洋流因素，洋流的速度和方向都會(huì)對(duì)水下機(jī)器人的能量消耗產(chǎn)生影響。因此在路徑規(guī)劃時(shí)，除了要考慮路徑長(zhǎng)度、障礙物信息外，還應(yīng)考慮能耗問題。本文根據(jù)RRT*算法的優(yōu)點(diǎn)，考慮洋流對(duì)水下機(jī)器人能耗的影響，設(shè)計(jì)實(shí)現(xiàn)基于RRT*的路徑最短和能耗最低的路徑規(guī)劃算法，并通過相應(yīng)的仿真模擬和實(shí)驗(yàn)室平臺(tái)的真機(jī)實(shí)現(xiàn)驗(yàn)證了該算法的可行性。

1 RRT 算法

1.1 問題定義

讓 C ?Rn表示結(jié)構(gòu)空間，其中 n表示結(jié)構(gòu)空間的維數(shù)。 結(jié)構(gòu)空間可進(jìn)一步被劃分為障礙物空間Cobs?C 和非障礙 物空間Cfree=CCobs。讓T =(V,E)表示隨機(jī)生長(zhǎng)樹，其中 V表示節(jié)點(diǎn)， E表示連接點(diǎn)的邊。讓qinit和 qgoal表示初始狀態(tài)和目標(biāo)狀態(tài)。讓p(qi,qj)表示連接任意2 個(gè)狀態(tài)qi∈Cfree和qj∈Cfree的路徑。

問題1（可行路徑）：在非障礙物區(qū)域內(nèi)找到一條從qinit到 qgoal的路徑。

問題2（最優(yōu)路徑）：在非障礙物區(qū)域內(nèi)找到一條從qinit到 qgoal的最優(yōu)路徑，使路徑代價(jià)花費(fèi)最小。

1.2 RRT 算法

RRT 算法是一種基于概率采樣的搜索方法，圖1為RRT 算法程序流程圖，具體實(shí)現(xiàn)如算法1 所示。圖2為隨機(jī)樹的關(guān)鍵擴(kuò)展過程（Extend 函數(shù)）程序流程圖，具體實(shí)現(xiàn)如算法2 所示。

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

圖2 擴(kuò)展過程（Extend 函數(shù)）程序流程圖Fig. 2 Program flow diagram of Extend function

RRT(qinit)

算法1：

算法2：Extend(T,qrand)

其中，各函數(shù)含義及作用如下：

S ample(i)：這個(gè)程序返回非障礙物區(qū)域內(nèi)一個(gè)獨(dú)立的均勻分配的隨機(jī)點(diǎn) qrand∈Cfree。

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

S teer(qnearest,qrand)： 這程序返回一個(gè)新的點(diǎn)qnew使 d(qnew,qrand)最 小，并滿足條件 d(qnew,qnearest)＜η，η是我們?cè)O(shè)定的一個(gè)值。

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

從RRT 算法過程中可以看出隨機(jī)樹的擴(kuò)展偏向于未被訪問過的區(qū)域。這使得隨機(jī)樹在剛開始時(shí)擴(kuò)展得很快，并能完全覆蓋結(jié)構(gòu)空間。隨機(jī)樹上的節(jié)點(diǎn)在結(jié)構(gòu)空間里是均勻的。如果可行路徑存在，那在節(jié)點(diǎn)數(shù)量足夠的前提下，RRT 算法就一定能找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑。通過上述分析可知，RRT 算法所找到的可行路徑不一定最優(yōu)。

2 基于RRT*的路徑規(guī)劃算法

針對(duì)RRT 算法存在的問題，S.Karaman 提出通過引入代價(jià)函數(shù)來(lái)優(yōu)化，經(jīng)過逐次迭代來(lái)改善之前的路徑，從而得到一條最優(yōu)或準(zhǔn)優(yōu)的路徑。

2.1 RRT＊算法

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

RRT*算法的初始化過程同算法1 中一致，而不同的是RRT*算法在它的擴(kuò)展過程中引入了代價(jià)函數(shù)，并通過代價(jià)函數(shù)來(lái)判斷是否會(huì)更新之前的路徑，以此來(lái)改善路徑。圖3 則為RRT*算法中擴(kuò)展過程（Extend 函數(shù)）程序流程圖，具體實(shí)現(xiàn)如算法3 所示，接下來(lái)的則是一些在擴(kuò)展過程中被調(diào)用的函數(shù)。

圖3 擴(kuò)展過程（Extend 函數(shù)）程序流程圖Fig. 3 Program flow diagram of Extend function

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

算法4：Getsortedlist(qnew,Cnear)

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

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

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

ChooseBestParent(Ls)：這程序被用來(lái)搜索列表Ls的一個(gè)狀態(tài)qmin∈Ls，它能提供一條從qinit到qnew經(jīng)過 qmin的代價(jià)最低并無(wú)碰撞的路徑。

InsertVertex(qnew,qmin,T)：這個(gè)程序?qū)new點(diǎn)插入到了樹上形成一個(gè)新的節(jié)點(diǎn)。

RewireVertices(qnew,Ls,E)：這個(gè)程序被用來(lái)更新q′∈Cnear的父節(jié)點(diǎn)如果一些明確的條件被滿足的話。

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

2.2 路徑距離代價(jià)

本文只考慮歐式空間，兩點(diǎn)間的路徑距離代價(jià)則為兩點(diǎn)間的正歐式距離，可表示為：

2.3 考慮洋流因素的水下機(jī)器人路徑能耗代價(jià)

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

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

在真實(shí)的海洋環(huán)境中，有時(shí)還會(huì)存在洋流因素，而洋流的速度和方向都會(huì)對(duì)水下機(jī)器人路徑上的能量消耗產(chǎn)生影響。

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

如圖4 所示，當(dāng)存在一個(gè)緩變的速度為 vc，流向?yàn)?β的2D 無(wú)旋洋流 (rc=0) 時(shí)， uc和 vc分別為洋流投影到水下機(jī)器人縱向和橫向的速度，可表示為：

其中 φ是水下機(jī)器人的首向角。

設(shè)vr=v-vc是 相對(duì)速度，此時(shí)在2D 無(wú)旋洋流環(huán)境下水下機(jī)器人所受到的阻尼力和阻尼力矩[13]則為：

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

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

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

當(dāng) α=1時(shí)，則式（6）變成水下機(jī)器人在2D 無(wú)旋洋流下的能耗最低路徑規(guī)劃算法中的代價(jià)函數(shù)。當(dāng)α=0時(shí)，則式（6）變成水下機(jī)器人路徑最短路徑規(guī)劃算法中的代價(jià)函數(shù)。

3 仿真

3.1 RRT＊算法和RRT 算法的仿真對(duì)比

基于前文理論分析及算法設(shè)計(jì)，利用Matlab 編程實(shí)現(xiàn)RRT 算法和RRT*算法（路徑最短），并進(jìn)行仿真對(duì)比，仿真結(jié)果如圖5 和圖6 所示。

圖中矩形區(qū)域（黑色）代表障礙物，Starting Point 代表起始點(diǎn)，圓形區(qū)域則代表目標(biāo)區(qū)域，粗實(shí)線則為所尋找到的路徑，將兩圖中的路徑長(zhǎng)度通過計(jì)算得到表1。

從仿真結(jié)果的對(duì)比可以看出，RRT 算法能找到一條可行路徑，但不一定是最優(yōu)或準(zhǔn)優(yōu)路徑；RRT*算法因代價(jià)函數(shù)的引入，找到的路徑不僅是可行路徑，相對(duì)RRT 算法所得路徑更優(yōu)，可視為準(zhǔn)優(yōu)路徑。

圖5 RRT 算法仿真結(jié)果Fig. 5 Simulation results of RRT algorithm

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

表1 兩種算法路徑長(zhǎng)度對(duì)比Tab. 1 Comparison of path length between two algorithms

3.2 2D 無(wú)旋洋流環(huán)境中路徑最短與能耗最低路徑規(guī)劃的仿真模擬

假設(shè)流場(chǎng)環(huán)境為2D 無(wú)旋洋流，流速為 vc，流向β；仿真計(jì)算時(shí)設(shè)定水下機(jī)器人艏向會(huì)保持與規(guī)劃出的路徑方向一致，并保持勻速行駛；仿真水動(dòng)力參數(shù)則根據(jù)實(shí)驗(yàn)室現(xiàn)有的水下機(jī)器人（ROV）以往的經(jīng)驗(yàn)值設(shè)定,設(shè)置仿真條件如表2 所示，仿真結(jié)果如圖7 所示。

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

表2 仿真參數(shù)表Tab. 2 Simulation parameter table

圖7 ROV 兩種路徑規(guī)劃的仿真結(jié)果Fig. 7 Simulation results of two path planning of ROV

表3 兩條路徑的能耗和長(zhǎng)度對(duì)比表Tab. 3 Comparison of energy consumption and length of two paths

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

4 RRT*算法的實(shí)驗(yàn)室實(shí)現(xiàn)

4.1 實(shí)驗(yàn)條件

本次實(shí)驗(yàn)是在上海交通大學(xué)拖曳水池進(jìn)行的，實(shí)驗(yàn)水池水深7.5 m，長(zhǎng)300 m，寬16 m。實(shí)驗(yàn)對(duì)象則是采用實(shí)驗(yàn)室現(xiàn)有的水下機(jī)器人（ER3K，見圖8），該水下機(jī)器人的基本性能參數(shù)見表4。地形環(huán)境則通過搭載在水下機(jī)器人本體上的聲吶（Super SeaKing DST，見圖9）獲取，聲吶性能參數(shù)見表5。

圖8 水下機(jī)器人Fig. 8 Remote operated vehicle

4.2 實(shí)驗(yàn)方案

在本次實(shí)驗(yàn)中，采用2 個(gè)油桶來(lái)充當(dāng)障礙物，然后利用搭載在水下機(jī)器人上的實(shí)驗(yàn)聲吶去掃描檢測(cè)獲取環(huán)境里的障礙物信息，最后再利用基于RRT*算法的路徑最短和能耗最低路徑規(guī)劃算法得到準(zhǔn)優(yōu)路徑。圖10 為本次實(shí)驗(yàn)示意圖，圖11 為本次實(shí)驗(yàn)現(xiàn)場(chǎng)圖。

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

圖9 實(shí)驗(yàn)聲吶Fig. 9 Experimental sonar

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

4.3 實(shí)驗(yàn)結(jié)果

利用水下機(jī)器人上所搭載的實(shí)驗(yàn)聲吶進(jìn)行掃描檢測(cè)，獲得的聲吶圖像如圖12 所示。

圖12 中，可看出圖中深色區(qū)域?yàn)檎系K物或雜波。在實(shí)際情況中，水下機(jī)器人并無(wú)法分辨出深色區(qū)域是障礙物還是雜波，所以此處將深色區(qū)域全部視為障礙物以此來(lái)規(guī)劃路徑。將聲吶圖像進(jìn)行圖像處理后得到如圖13 所示。

圖10 實(shí)驗(yàn)示意圖Fig. 10 Experimental schematic diagram

圖11 實(shí)驗(yàn)現(xiàn)場(chǎng)Fig. 11 Experimental scene

圖12 聲吶圖像Fig. 12 Sonar image

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

圖中黑色區(qū)域被認(rèn)為是障礙物，白色區(qū)域則被認(rèn)為是可通行區(qū)域。進(jìn)一步為了安全考慮，此處通過將圖13 中的障礙物擴(kuò)大來(lái)保證水下機(jī)器人通行的安全距離，得到的最終地圖如圖14 所示。受實(shí)驗(yàn)條件所限，實(shí)驗(yàn)環(huán)境為無(wú)流水池，則使水下機(jī)器人在圖14 上分別實(shí)施路徑最短和能耗最低（無(wú)洋流情況下）路徑規(guī)劃算法，規(guī)劃出的路徑結(jié)果如圖15 所示。

從圖15 可看出，水下機(jī)器人在該環(huán)境下實(shí)施路徑最短路徑規(guī)劃和能耗最低路徑規(guī)劃得出的路徑完全一致。該水池實(shí)驗(yàn)路徑規(guī)劃結(jié)果表明了所設(shè)計(jì)的基于RRT*的路徑最短和能耗最低的路徑規(guī)劃算法可行有效。

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

圖15 水下機(jī)器人路徑規(guī)劃結(jié)果Fig. 15 Path planning results of underwater vehicle

5 結(jié) 語(yǔ)

本文針對(duì)復(fù)雜海洋環(huán)境中水下機(jī)器人路徑規(guī)劃問題展開研究，綜合考慮規(guī)劃路徑長(zhǎng)度與全程能耗，設(shè)計(jì)出基于RRT*的路徑最短和能耗最低的路徑規(guī)劃算法，繼而進(jìn)行仿真模擬和實(shí)驗(yàn)室現(xiàn)有水下機(jī)器人平臺(tái)的真機(jī)實(shí)現(xiàn)，仿真及真機(jī)驗(yàn)證結(jié)果顯示所設(shè)計(jì)的路徑規(guī)劃算法可行有效，為后期海上應(yīng)用奠定基礎(chǔ)。