較強(qiáng)海流中的低速水下機(jī)器人路徑優(yōu)化

朱心科，侯 斐，孟 肯，羅建超

（自然資源部第二海洋研究所海底科學(xué)重點(diǎn)實(shí)驗(yàn)室，浙江 杭州 310012）

以水下滑翔機(jī)為代表的低航速水下機(jī)器人由于長續(xù)航力方面的優(yōu)勢，在海洋觀測中的應(yīng)用越來越多。從1997年開始，美國就開始了自主海洋采樣網(wǎng)（Autonomous Ocean Sampling Network，AOSN）試驗(yàn)，試驗(yàn)中使用了17個水下滑翔機(jī)，其中12個為Slocum水下滑翔機(jī)，5個為Spray水下滑翔機(jī)，分別搭載溫鹽深（Conductivity，Temperature，Depth）、葉綠素、熒光計(jì)等傳感器對蒙特利海灣（Monterey Bay）上升流進(jìn)行了40天的調(diào)查[1-4]。丹麥的卑爾根大學(xué)（University of Bergen）利用3臺水下滑翔機(jī)在格陵蘭島海域?qū)Υ笪餮笈c北歐之間的海水交換問題進(jìn)行長期觀測[5]。BOSSE A等[6]利用水下滑翔機(jī)對Lofoten海盆的渦旋進(jìn)行連續(xù)15個月的觀測，對其造成的海洋生態(tài)環(huán)境參數(shù)動態(tài)變化進(jìn)行了記錄。然而水下滑翔機(jī)的長續(xù)航力是以犧牲滑翔速度為代價的，一般來說水下滑翔機(jī)的速度為0.5～1 kn，在較強(qiáng)的海流環(huán)境中，很難完成設(shè)定的觀測軌跡。SOULIGNAC M等[7]設(shè)計(jì)了SWE（Symbolic Wavefront Expansion）算法，在機(jī)器人出發(fā)時間窗內(nèi)選擇最佳的出發(fā)時刻，使得完成使命的時間最短。

水下機(jī)器人（Autonomous Underwater Vehicle，AUV）路徑規(guī)劃問題一直是研究的熱點(diǎn)，涌現(xiàn)出了各種各樣的方法。傳統(tǒng)的AUV水下路徑規(guī)劃是以安全為主要出發(fā)點(diǎn)，目的是AUV在執(zhí)行任務(wù)的過程中，能夠成功避開障礙物或者危險區(qū)域[8-10]。隨著AUV智能性的提升，AUV路徑規(guī)劃研究的目標(biāo)除了避障之外，還要考慮任務(wù)的完成質(zhì)量，例如AUV在較強(qiáng)的海流環(huán)境中完成指定的任務(wù)花的時間最短、走的路程最小、消耗的能量最少或者幾種的折中。ALVAREZ A等[11]以完成任務(wù)消耗的能量為優(yōu)化準(zhǔn)則，采用動態(tài)規(guī)劃的方法，在不同空間尺度的海流環(huán)境中對AUV的路徑進(jìn)行優(yōu)化。A*算法也常被用作AUV的路徑規(guī)劃[12-13]。

傳統(tǒng)的AUV路徑規(guī)劃算法中，一般假設(shè)海流為常值，不隨時間和位置發(fā)生變化，而在實(shí)際的海流場中，海流是時空變化的。KRUGER D[14]等設(shè)計(jì)了優(yōu)化算法克服了快速變化的雙向潮汐流，能夠讓AUV“騎”著海流穿梭運(yùn)動，克服遠(yuǎn)大于自身速度的海流速度。對于以水下滑翔機(jī)為代表的低航速水下機(jī)器人來說，為了提高續(xù)航力必須降低功耗，一般不安裝測流速傳感器，無法實(shí)時獲得海流信息。海流預(yù)報模型輸出的數(shù)據(jù)可以用來指導(dǎo)水下路徑規(guī)劃。THOMPSON R等[15]依據(jù)海流預(yù)報數(shù)據(jù)，利用Wavefront算法設(shè)計(jì)了在時變海流情況下的水下滑翔機(jī)路徑規(guī)劃策略，取得了不錯的效果。

對于海洋預(yù)報來說，海流的預(yù)報時間是有時限的，只能夠提供一定時間內(nèi)的海流預(yù)報，而且預(yù)報準(zhǔn)確度隨著預(yù)報時間增長而降低[4]。對于利用海流預(yù)報數(shù)據(jù)進(jìn)行水下路徑規(guī)劃來說，走完從出發(fā)位置與目標(biāo)位置之間路程花費(fèi)時間有可能超過單個預(yù)報周期。本文將針對較強(qiáng)海流場中海流的時變性以及預(yù)報周期有限的問題，以水下滑翔機(jī)為例，開展低航速水下機(jī)器人路徑規(guī)劃方法研究。

1 問題描述

一般情況下，水下滑翔機(jī)進(jìn)入穩(wěn)定滑翔狀態(tài)后，浮力調(diào)節(jié)系統(tǒng)不再發(fā)生變化。因此，滑翔速度的大小為定值，滑翔方向是可變的。假設(shè)水下滑翔機(jī)在穩(wěn)態(tài)滑翔時速度為vr，海流速度為vc。

當(dāng)k?2時，海流速度對水下滑翔機(jī)的運(yùn)動軌跡影響微乎其微，路徑規(guī)劃可以忽略海流影響；當(dāng)k＜0.5時，由于水下滑翔機(jī)的速度太小，在海流場中基本上處于隨波逐流的狀態(tài)，失去了路徑規(guī)劃的意義；當(dāng)0.5≤k≤2時，海流對滑翔機(jī)的運(yùn)動軌跡會有較大的影響，但通過合適的路徑規(guī)劃算法，滑翔機(jī)能夠克服、利用海流，從而獲得一條最優(yōu)的觀測路徑，論文針對這種情況開展研究。

論文基于水平面內(nèi)的二維海流為假設(shè)前提。在二維的離散空間，海洋環(huán)境定義在n×p個規(guī)則的網(wǎng)格上，空間中任意點(diǎn)x=（h，k）為其中的一個網(wǎng)格，0≤h≤n，0≤k≤p，h和k分別代表該網(wǎng)格的橫、縱坐標(biāo)。從出發(fā)點(diǎn)S到目標(biāo)點(diǎn)D的路徑P定義為一組有序點(diǎn)的集合Γ=S，…，xi-1，xi，…，D中相鄰兩點(diǎn)之間的連線。

水下滑翔機(jī)在靜水中的對地水平滑翔速度為vr，海流速度為vc（x，y，t），表示在t時刻位置（x，y）處的海流速度。以水下滑翔機(jī)以最短的時間到達(dá)目的地為路徑規(guī)劃目標(biāo)如下。

2 水下滑翔機(jī)運(yùn)動模型

假設(shè)水下滑翔機(jī)在靜水中的水平滑翔速度為vr，大小為定值，方向可變；海流速度為vc（x，y，t）（在不致混淆情況下，用vc，表示），那么水下滑翔機(jī)的對地速度v如下。

海流方向?yàn)棣萩，水下滑翔機(jī)靜水中的速度方向?yàn)棣萺，對地速度方向?yàn)棣取＜僭O(shè)二維空間中每個網(wǎng)格中海流速度是定值，如果從一個網(wǎng)格運(yùn)動到另一個網(wǎng)格，水下滑翔機(jī)有8個方向可選，如圖1所示。因此，水下滑翔機(jī)的對地速度可以寫為

圖1 可移動方向

由于水下滑翔機(jī)速度方向是可變的，可以是任意值。通過式（4）和式（5）消去θr可得關(guān)于對地速度的方程如下。

由式（6）表示的方程可以判斷滑翔機(jī)能否從一個網(wǎng)格運(yùn)動到另一個網(wǎng)格。若方程有一個正數(shù)解，表示水下滑翔機(jī)能夠從當(dāng)前網(wǎng)格運(yùn)動到下一個網(wǎng)格；如果方程的解為有負(fù)數(shù)或者復(fù)數(shù)，表示水下滑翔機(jī)無法抵制強(qiáng)海流從當(dāng)前網(wǎng)格運(yùn)動到下一個網(wǎng)格。如果有兩個正數(shù)解，選擇較大的v作為水下滑翔機(jī)的合速度。

3 路徑規(guī)劃算法

對于離散空間中的路徑規(guī)劃問題，Wavefront算法和A*搜索是較常用算法，分別應(yīng)用這兩種算法對水下滑翔機(jī)進(jìn)行路徑規(guī)劃，并對結(jié)果進(jìn)行比較。

3.1 Wavefront算法

傳統(tǒng)的Wavefront算法[6]主要分為兩個部分，首先，從出發(fā)點(diǎn)開始向相鄰節(jié)點(diǎn)擴(kuò)展，一直擴(kuò)展到目標(biāo)節(jié)點(diǎn)為止，在擴(kuò)展的過程中，記錄每個被擴(kuò)展到的節(jié)點(diǎn)需要的代價；然后，從目標(biāo)節(jié)點(diǎn)開始，根據(jù)記錄的每個被擴(kuò)展到節(jié)點(diǎn)的代價，利用爬山法反向構(gòu)建最優(yōu)路徑。在這里，進(jìn)行兩個方面的改變：①代價最小的節(jié)點(diǎn)優(yōu)先向前擴(kuò)展。②每個節(jié)點(diǎn)指定一個唯一的父節(jié)點(diǎn)。通過修改擴(kuò)展規(guī)則，可以大幅度減小節(jié)點(diǎn)的擴(kuò)展數(shù)量，同時有利于反向構(gòu)造最優(yōu)路徑，提升算法速度。Wavefront算法的擴(kuò)展過程如下。

（1）A→B，其中A為父節(jié)點(diǎn)，B為子節(jié)點(diǎn)，表示為P（A，B）。每個節(jié)點(diǎn)前向擴(kuò)展過程中可以有多個子節(jié)點(diǎn)，但只能有一個父節(jié)點(diǎn)；

（2）T（A，B）表示從水下滑翔機(jī)節(jié)點(diǎn)A運(yùn)動到節(jié)點(diǎn)B花費(fèi)的時間；T（B）表示從水下滑翔機(jī)出發(fā)點(diǎn)運(yùn)動到節(jié)點(diǎn)B花費(fèi)的總時間；

（3）如果B的父節(jié)點(diǎn)為A，即P（A，B），如果滿足T（B）＞T（C）+T（C，B），則B的父節(jié)點(diǎn)由A改變?yōu)镃，即P（C，B），并且從出發(fā)點(diǎn)運(yùn)動到B點(diǎn)的花費(fèi)的總時間更新成T（B）=T（C）+T（C，B）；

（4）每次前向擴(kuò)展總是從min{T（x）}的節(jié)點(diǎn)開始，直至到達(dá)目標(biāo)節(jié)點(diǎn)；

（5）從目標(biāo)點(diǎn)開始，按照代價最低原則反向?qū)ふ易约旱母腹?jié)點(diǎn)，直至到達(dá)出發(fā)點(diǎn)為止。

利用Wavefront算法進(jìn)行路徑規(guī)劃時，只要從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的路徑存在，就一定能夠找到一條全局最優(yōu)路徑。但是，由于Wavefront算法無啟發(fā)式搜索，搜索的過程中擴(kuò)展的節(jié)點(diǎn)較多，因此搜索時間較長。

3.2 A*算法

A*算法與Wavefront算法最大不同之處是代價評價函數(shù)中加入了啟發(fā)函數(shù)。啟發(fā)函數(shù)體現(xiàn)了當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的關(guān)系，因此能夠加速朝著目標(biāo)節(jié)點(diǎn)方向進(jìn)行搜索。

A*算法的評價函數(shù)定義如下。

式中，T（n）為出發(fā)點(diǎn)運(yùn)動到當(dāng)前節(jié)點(diǎn)的代價；h（n）為當(dāng)前節(jié)點(diǎn)運(yùn)動到目標(biāo)節(jié)點(diǎn)代價的估計(jì)值。

式中，s（d，n）為當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的歐氏距離；vmax為水下滑翔機(jī)的最大速度。當(dāng)h（n）≤h*（n）（h*（n）為當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)代價的真值）時，h（n）稱作可采納啟發(fā)函數(shù)。在這種情況下，只要從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的路徑存在，那么A*算法就一定能搜索到全局最優(yōu)解。

與Wavefront算法相比，由于A*算法具有啟發(fā)性，評價函數(shù)中體現(xiàn)了當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間的關(guān)系，只要保證啟發(fā)函數(shù)是可采納啟發(fā)，那么在節(jié)點(diǎn)的擴(kuò)展過程中，只需要朝著當(dāng)前評價函數(shù)最小的節(jié)點(diǎn)擴(kuò)展而無需擴(kuò)展當(dāng)前節(jié)點(diǎn)所有的相鄰節(jié)點(diǎn)，就能夠搜索到全局最優(yōu)解，從而提升了搜索效率。

3.3 算法仿真

3.3.1 海流模型

以流函數(shù)描述的周期性變化的雙曲流為例驗(yàn)證路徑規(guī)劃算法[4]。雙曲流定義如下。

其中，

海流的速度場如下。

圖2 雙曲海流

3.3.2 仿真結(jié)果

假設(shè)在路徑規(guī)劃過程中，已經(jīng)通過海洋預(yù)報獲得了海流信息。路徑規(guī)劃的出發(fā)點(diǎn)為s（10，20），目標(biāo)點(diǎn)為d（80，30），分別利用Wavefront算法和A*算法在對水下滑翔機(jī)在海流環(huán)境中進(jìn)行路徑規(guī)劃，結(jié)果如圖3所示。

圖3 路徑規(guī)劃結(jié)果

圖3 （a）中，棕色網(wǎng)格表示W(wǎng)avefront算法在搜索過程中父節(jié)點(diǎn)沒有發(fā)生過改變，黃綠色網(wǎng)格表示該節(jié)點(diǎn)的父節(jié)點(diǎn)發(fā)生過改變，二者共同表示在算法搜索過程中擴(kuò)展到的節(jié)點(diǎn)。圖3（b）中，棕色網(wǎng)格表示A*算法在搜索過程中該節(jié)點(diǎn)沒有被反復(fù)搜索過，黃綠色網(wǎng)格表示該節(jié)點(diǎn)被重復(fù)搜索過。從圖3可以看出，A*算法搜索過程擴(kuò)展的節(jié)點(diǎn)個數(shù)明顯小于Wavefront算法，從而搜索速度相對較快。在本仿真實(shí)例中，A*算法的搜索速度是Wavefront算法的3倍。由于A*算法的啟發(fā)函數(shù)是可采納的，因此和Wavefront算法搜索到的路徑完全一樣，都是最優(yōu)的。

4 分段路徑規(guī)劃

4.1 分段路徑規(guī)劃算法

數(shù)值海洋預(yù)報根據(jù)預(yù)報的范圍和空間分辨率不同，預(yù)報的時效也不同，從數(shù)小時到幾十個小時不等。當(dāng)出發(fā)點(diǎn)與目標(biāo)點(diǎn)間的距離相對較近時，完成觀測任務(wù)需要的時間相對較短，根據(jù)單個周期的海洋預(yù)報提供的海流信息，利用Wavefront算法或者A*算法均可搜索到最優(yōu)路徑，稱之為完整路徑規(guī)劃；當(dāng)兩個點(diǎn)距離較遠(yuǎn)，單個預(yù)報周期提供的海流信息不足以完成觀測任務(wù)的路徑規(guī)劃時，就需要根據(jù)預(yù)報周期分段進(jìn)行路徑規(guī)劃，稱之為分段路徑規(guī)劃[4]。

Wavefront算法不適合分段路徑規(guī)劃，因?yàn)橹荒艿玫匠霭l(fā)點(diǎn)到目標(biāo)點(diǎn)之間的最優(yōu)路徑，無法得到中間值。對于A*算法來說，算法具有啟發(fā)性，能夠評價中間點(diǎn)和目標(biāo)點(diǎn)之間的相互關(guān)系，因此，適合用來進(jìn)行分段路徑規(guī)劃。

當(dāng)觀測時間超過海流預(yù)報周期時，可以利用A*算法先找到單個預(yù)報周期內(nèi)最優(yōu)的路徑，然后再以這條最優(yōu)路徑的終點(diǎn)作為下一段路徑優(yōu)化的起點(diǎn)進(jìn)行路徑規(guī)劃，直至到達(dá)目標(biāo)點(diǎn)為止。算法偽代碼如表1所示。

表1 分段路徑規(guī)劃偽代碼

4.2 仿真結(jié)果

路徑規(guī)劃算法的海流環(huán)境仿真參數(shù)同3.1節(jié)中的參數(shù)，水下滑翔機(jī)的出發(fā)點(diǎn)為S（170，60），目標(biāo)點(diǎn)為D（10，20），分別用完整路徑規(guī)劃算法和分段路徑規(guī)劃算法兩種方式進(jìn)行。進(jìn)行分段路徑規(guī)劃時，假設(shè)海流的預(yù)報周期為1個時間單位，即10個時間步長的海流預(yù)報，仿真結(jié)果如圖4所示。

圖4 完整路徑規(guī)劃與分段路徑規(guī)劃比較

結(jié)果顯示，無論是利用完整還是分段路徑規(guī)劃算法，水下滑翔機(jī)都能克服海流從出發(fā)點(diǎn)順利到達(dá)目標(biāo)點(diǎn)，二者的差別主要體現(xiàn)在走完該段路徑用時不同。完整路徑規(guī)劃得到的路徑長度為2.19個距離單位，從出發(fā)點(diǎn)到目標(biāo)點(diǎn)用時5.25個時間單位；分段路徑規(guī)劃得到的路徑長度為2.25個距離單位，用時5.73個時間單位。對比發(fā)現(xiàn)，分段路徑規(guī)劃獲得路徑長度僅比最優(yōu)路徑多了2.7%，而用時卻多了9%。這是由于基于A*算法的分段路徑規(guī)劃算法是“貪婪”式搜索，當(dāng)啟發(fā)函數(shù)是可采納的情況下，每段的路徑都是最優(yōu)的，但是組合起來后就不一定是全局最優(yōu)；同時，由于兩種算法規(guī)劃出來的路徑不同造成滑翔機(jī)遭遇到的海流速度（方向和大?。┮膊煌?，因此增加的耗時與路徑長度并不一定成正比。但總的看來，走完分段路徑規(guī)劃得到路徑需要的時間僅僅超過最優(yōu)路徑需要時間的9%，這個偏差結(jié)果在可接受的范圍之內(nèi)，表明基于A*算法的分段路徑規(guī)劃算法能夠滿足時變海流中的低航速水下機(jī)器人的路徑規(guī)劃。

5 結(jié)論與展望

5.1 結(jié)論

針對以水下滑翔機(jī)為代表的低航速水下機(jī)器人航跡易受海流的影響問題，研究了在較強(qiáng)海流下路徑規(guī)劃方法。

（1）當(dāng)海流預(yù)報周期小于滑翔機(jī)水下觀測時間時，Wavefront算法和A*算法均能夠克服較強(qiáng)海流對滑翔機(jī)觀測路徑的影響，獲得最優(yōu)的觀測路徑。

（2）當(dāng)海流預(yù)報周期超出滑翔機(jī)水下觀測時間時，Wavefront算法只能得到出發(fā)點(diǎn)與目標(biāo)點(diǎn)之間的最優(yōu)路徑，而無法得到中間值，無法進(jìn)行路徑規(guī)劃。

（3）設(shè)計(jì)了分段式A*路徑規(guī)劃算法，解決了觀測時間超出海洋預(yù)報周期問題。

仿真結(jié)果顯示，分段式A*路徑規(guī)劃算法獲得較為合理的觀測路徑。該方法可為水下滑翔機(jī)為代表的低航速水下機(jī)器人在海洋觀測中的應(yīng)用提供理論指導(dǎo)。

5.2 展望

分段路徑規(guī)劃的本質(zhì)是“貪婪”式規(guī)劃，每一段都選擇當(dāng)前的局部最優(yōu)。局部最優(yōu)組合起來的完整路徑往往與全局最優(yōu)路徑會有一定的差距，未來將會考慮用拉格朗日相干結(jié)構(gòu)（Lagrangian Coherent Structures，LCS）指導(dǎo)路徑規(guī)劃。LCS描述了流體的平均運(yùn)動，與變化較快的海流場相比，LCS具有長期的穩(wěn)定性，而且能將海流場明顯地分為不同的區(qū)域，水下機(jī)器人沿著LCS能夠較容易地從一個區(qū)域到達(dá)另外一個區(qū)域。同時，水下機(jī)器人沿著LCS運(yùn)動一方面能夠節(jié)省能量，另一方面也能夠提高對地速度，是一條比較理想的軌跡，在最大程度上減小與全局最優(yōu)解的偏差?；贚CS的路徑規(guī)劃算法對被動式海洋觀測設(shè)備，如Argo浮標(biāo)、表面漂流浮標(biāo)的投放位置、間距的選擇也有一定的指導(dǎo)作用，可以避免設(shè)備在海流作用下過早地漂到近岸或者聚集在一起，失去大范圍觀測的意義。