海流環(huán)境下基于改進D*算法的AUV 動態(tài)路徑規(guī)劃①

李世奇 孫 兵 朱蟋蟋

(上海海事大學 智能海事搜救與水下機器人上海工程技術研究中心 上海 201306)

0 引言

自治水下機器人(autonomous underwater vehicle,AUV)是一種依靠自主導航、決策系統(tǒng)航行并完成任務的海洋探索與研究工具。為了使AUV 在復雜的海洋環(huán)境中能夠安全有效地到達預定區(qū)域,合適的路徑規(guī)劃算法是不可或缺的[1-3]。AUV 在運行過程中,海流作為能量流會在其行進過程中產生很大的影響,當遇到強海流時甚至會危害到任務的順利完成。由于AUV 自帶能源有限,在大范圍海洋環(huán)境航行中必須考慮海流對其能耗的影響,使AUV 盡量保持順流行駛并在減少逆流和側流的情況下充分利用海流以減少能耗[4]。文獻[5]采用了量子粒子群優(yōu)化算法進行路徑規(guī)劃,考慮了海流和障礙物的影響,但其優(yōu)化目標是航行時間,沒有考慮能量消耗。文獻[6]利用稀疏A*算法對AUV 進行了全局路徑規(guī)劃,將航行總距離作為優(yōu)化目標,并建立了相應的約束條件,但同樣沒有考慮能耗代價。文獻[7]將遺傳算法與粒子群優(yōu)化算法進行結合,使AUV 在考慮海流因素航行時減少耗能,但其存在計算時間長且計算量大的問題[8]。而D*算法[9-11]具有速度快、尋路優(yōu)等特點,在AUV 的路徑規(guī)劃領域中得到了廣泛的應用。

另外,路徑規(guī)劃通常都會把機器人當作質點處理,而AUV 自身體積較大,一般小型AUV 身長是1～2 m 左右,按照規(guī)劃路徑行駛可能會與障礙物邊緣碰撞,因此更安全地避障也是AUV 路徑規(guī)劃需要注意的問題。文獻[12]將支持向量機(support vector machine,SVM)應用到D*算法中,保證無人車規(guī)劃路徑的安全與平滑,但此方法不適用于障礙物密集的環(huán)境。文獻[13]在D*算法中加入碰撞因子,應用于無人機室內路徑規(guī)劃,使規(guī)劃路徑更安全,但該方法不能解決路徑偏向角大的問題,且僅考慮到二維環(huán)境。文獻[14]將粒子群優(yōu)化算法與D*算法進行結合來解決路徑規(guī)劃中的門限狀態(tài),但沒有考慮機器人的性能約束和三維航跡規(guī)劃問題。

本文針對上述問題提出了一種基于D*算法的改進算法,主要貢獻如下。

(1)建立海流影響能耗代價模型,使得AUV 能充分利用海流,減少能耗。

(2)添加障礙物威脅約束,使得規(guī)劃出的路徑盡量躲避障礙物邊緣,以保證AUV 行駛更安全。

(3)采用三次均勻B 樣條曲線擬合對規(guī)劃的路徑進行平滑處理,使生成的路徑更加符合AUV 的行駛條件。

1 改進D*路徑規(guī)劃算法

AUV 在復雜的海洋環(huán)境中運行時,影響其航行的因素有很多,其中海流的影響是很大的。特別當AUV 進行遠距離航行時,能耗問題就變得尤為顯著。海流是海水較大規(guī)模相對穩(wěn)定的非周期性流動,隨季節(jié)、氣候、海域、地形、深度變化而變化,是時間和空間的復雜函數,目前很難用精確的數學表達式描述其運動規(guī)律[15-16]。但在有限的海域及時段內,海流的流速和流向都是比較穩(wěn)定的,因此本文研究主要考慮海流定常流動和對水下機器人的定常干擾。

1.1 海流模型

建立海流的數學模式時需要知道速度勢和流函數的概念。對于函數方程:

其中σ 即為速度勢,φ 即為流函數。于是可得:

進一步可得如下Cauchy-Rieman 方程:

結合式(5):

最后可得:

同理可得:

以上方程均是基于二維平面的,同理可得出三維方程:

在二維恒定流場中,如果速度平行x 軸,則速度勢和流函數為

方向平行于x 軸,速度大小為1 m/s 的流場仿真模型如圖1 所示。

圖1 平行于x 軸的定常海流模型

類似地,在三維環(huán)境中平行于z 軸的定常流場仿真模型如圖2 所示。

圖2 平行于z 軸的定常海流模型

在海流情況下,不能簡單地通過AUV 航行的總路程來衡量規(guī)劃算法的效果。因為在無海流環(huán)境中,最短路徑等價于最小能量消耗,但是在海流環(huán)境下,通過文獻[17]定義一個海流能量函數來衡量路徑規(guī)劃算法的效果。定義Xi-1Xi是連接Xi與Xi-1的路徑,di代表Xi-1Xi的長度,ei是沿著Xi-1Xi方向的單位向量,V 是AUV 的額定速度。先定義如下速度向量:

式中vi(x,y) 是(x,y) 處的AUV 速度向量,vc(x,y) 是(x,y) 處的海流速度向量,以此為基礎定義AUV 航行的海流能量函數,用于評估AUV 路徑規(guī)劃的性能。

其中,Ji表示Xi-1Xi段的能量消耗,J 表示整個AUV航行的總能量消耗,m 是AUV 所規(guī)劃路徑的段數。

1.2 海流影響能耗代價模型

海上大風浪航行的船舶為增強適航性和安全性都會開足馬力以最大航速逆流行駛,因為這樣可以加大螺旋槳效率,增強舵的水動力性能,提升船舶操縱性水平。

同樣,如果AUV 的最大速度與海流相比有較大余量,逆流航行會保持較為穩(wěn)定的艏向角,從而減小事故發(fā)生的可能性;但如果作業(yè)任務要求AUV 做較長距離的航行時,若海流大小適中,在滿足AUV 操縱性的基礎上則可以充分利用海流,順流行駛以減少能耗。

當海流的方向不同于AUV 首尾方向,即AUV側流行駛時,海流力會對AUV 產生轉艏力矩;另外當AUV 受到流壓時,自身將朝流向漂移,從而產生阻止其漂移的水的反作用力,水阻力也給AUV 以轉艏力矩。對于這種由海流產生的轉艏作用,AUV 需要消耗大量的能量來抵抗其產生的力和力矩,同時還要保持既定航向。當AUV 首尾線方向與海流方向垂直時,AUV 便很難按照預定軌跡航行。因此在進行路徑規(guī)劃時,應盡量減少橫向海流力的作用,以此節(jié)約AUV 運行所需的能耗。

綜上,在AUV 遠距離航行時,按照順流最好、頂流次之、側向流最差的原則進行路徑規(guī)劃可以充分利用海流節(jié)約能耗。

根據以上海流對AUV 的能耗影響可建立代價模型,應用到D*算法中。在D*算法的代價函數上添加能耗代價項,如式(12)所示。

其中,G(s) 表示從起始點到當前柵格的實際代價,H(s) 為啟發(fā)函數,它表示當前柵格到目標點的估計代價,δ(s) 表示柵格i 和j 間的海流影響權重。

因為假定AUV 行駛在速度大小均勻的海流中,所以只需考慮海流的方向信息。海流影響因子設計是根據AUV 運動方向與海流方向的夾角大小確定的,在二維環(huán)境中如圖3 所示,在三維環(huán)境中如圖4 所示。則定義柵格i 和j 間的海流影響權重δi,j(s) 如下:

圖3 海流方向與AUV 運動方向夾角示意圖

圖4 海流方向與AUV 運動方向夾角示意圖

其中,a 為一個常量因子,可由實際的海洋環(huán)境確定,用于調節(jié)δi,j(s) 大小范圍使其適合代價函數公式。a 的值越大,表示海流能耗代價項占總代價項的比重越大,規(guī)劃出的路徑相對越順流。本文仿真部分a 的值統(tǒng)一設定為1。δk為根據海流方向與AUV運動方向的夾角| w1 -w2| 制定一個用來近似表示海流對AUV 能耗影響的權重。根據順流最好、頂流次之、側向流最差的原則定義δ1=0,δ2=2,δ3=4,δ4=3,δ5=1。

同理,在三維水下環(huán)境中,柵格i 和j 間的海流影響權重δi,j(s) 可設置為

根據順流最好、頂流次之、側向流最差的原則定義δ1=0,δ2=2,δ3=2.3,δ4=4,δ5=3.3,δ6=3,δ7=1。

1.3 障礙物威脅約束模型

基于傳統(tǒng)D*算法規(guī)劃出的路徑存在緊貼障礙物邊緣的問題,考慮到AUV 自身大小,這種情況下對AUV 安全航行是有一定威脅的,因此在D*算法的代價函數上添加障礙物威脅約束[18-19],改進后的D*算法代價值計算方法如式(15)所示。

其中,H(s) 表示啟發(fā)函數,它表示當前柵格到目標點的估計代價,G(s) 表示從起始點到當前柵格發(fā)生的實際代價,W(s) 表示周圍障礙物對當前柵格產生的威脅代價。

障礙物對AUV 航行的威脅是一個復雜的問題,本文將其做了簡化,如圖5 所示,不同形狀的黑色圖形為障礙物,灰色柵格代表水下機器人的位置。

圖5 障礙物威脅模型圖

以柵格中心點為中心,以AUV 當前位置到障礙物的距離為自變量d,建立威脅約束模型如式(16)所示。

其中,rmax為最大效用半徑,當d 大于rmax這個距離,障礙物對柵格不再產生威脅作用;rmin為最小限制半徑,當d 到達距離障礙物距離為這個值時,對它產生最大限制。當d 在兩個距離之間,產生的代價與距離呈現負指數關系,k 為常系數。

關于常系數k 對路徑規(guī)劃的影響,k 的值越大,表示障礙物威脅項占總代價項的比重越大。其物理意義是,障礙物威脅部分約束影響力越大,規(guī)劃出的路徑越相對遠離障礙物邊緣。本文仿真部分k 的值統(tǒng)一設定為3。

1.4 B 樣條曲線平滑

AUV 行駛過程中,當規(guī)劃好的路徑是折線段時,由于轉向角度的限制,AUV 可能無法行駛。而且轉角會導致航行路徑長度的增加,造成能量浪費,如果將規(guī)劃出的折線路徑作平滑處理,則更能適應AUV 的水下航行。本文采用三次均勻B 樣條曲線[20-21]對規(guī)劃后的折線進行平滑處理,基于三次均勻B 樣條曲線擬合模型如圖6 所示。

圖6 擬合曲線圖

給定m+n +1 個平面或空間頂點Pi(i=0,1,…,m+n),稱n 次參數曲線段:

為第k 段n 次B 樣條曲線段(k=0,1,…,m),這些曲線段的全體稱為n 次B 樣條曲線,其中基函數Gi,n(t) 定義為

取n=3,則有三次B 樣條曲線的基函數如下:

三次B 樣條曲線段P0,3(t) 為

1.5 改進后算法的實現

由于改進D*算法是基于原D*算法的,因此,改進D*算法涉及到的大部分符號與功能函數和原D*算法相同。下面主要介紹Current_cost()和Danger_cost()2 個功能函數。

Current_cost()是海流影響能耗代價函數,在路徑規(guī)劃過程中,計算所要拓展柵格由于海流影響而產生的能耗代價值,按照順流最好、頂流次之、側向流最差的原則進行取值。

Danger_cost()是障礙物威脅代價函數,在路徑規(guī)劃過程中,計算所要拓展柵格由于障礙物的威脅影響而付出的代價值。它的值與到最近障礙物邊界的距離有關,距離越近,付出的代價越大,反之,則越小。

由于這2 個函數主要是在Process_state2()中調用,因此在圖7 中介紹Process_state2()的算法流程。

圖7 Process_ state2()函數流程圖

2 仿真及結果分析

本文通過柵格法將環(huán)境地圖離散化,將環(huán)境中包含障礙物的區(qū)域設置為障礙柵格,其他的設置為自由柵格,仿真圖中的五角星表示目標點,菱形表示起始點,灰色柵格是靜態(tài)障礙物,黑色柵格是動態(tài)障礙物,點劃線是未出現動態(tài)障礙物之前規(guī)劃的路徑,帶有“x”標記的虛線是重規(guī)劃路徑,實線是將折線平滑后的結果。在Matlab 2016a 中針對二維障礙物海流環(huán)境和三維障礙物海流環(huán)境分別進行全局路徑規(guī)劃仿真實驗,以驗證結果的有效性。

2.1 相關工作仿真對比

為了進一步驗證所提出算法的有效性,加入人工勢場法[22]與所提出D*算法進行對比分析。人工勢場方法的核心思想是創(chuàng)建一個虛擬的力場,將地圖中的目標設置為引力源,障礙物設置成斥力源,并通過建立引力場函數、斥力場函數來進行路徑規(guī)劃。人工勢場方法的優(yōu)點是:規(guī)劃的路徑一般比較平滑,算法模型實現簡單。但它的缺點也很明顯,那就是存在局部最優(yōu)的問題,并且對于局部最優(yōu)問題一直還沒有一個通用高效的解決方案。二者的仿真結果分別如圖8 和圖9 所示。

圖8 人工勢場路徑規(guī)劃

圖9 D*算法路徑規(guī)劃

表1 是AUV 航行中能量消耗與總路程對比,通過對比發(fā)現D*算法障礙物環(huán)境下的路徑規(guī)劃的總能量消耗和路徑長度均小于人工勢場方法。

表1 AUV 能量消耗對比

2.2 二維海流環(huán)境下的路徑規(guī)劃

將改進D*算法進行仿真驗證,圖10 為未考慮海流影響的路徑規(guī)劃仿真圖,圖11 表示的是考慮海流影響的路徑規(guī)劃仿真圖。

通過對比圖10 和圖11 可以看出,改進前后的算法都能夠使AUV 從起始點到達目標點,但2 種算法的路徑長度與能耗如表2 所示,即考慮海流影響的路徑長度比未考慮海流影響的路徑長度稍長,但是由于利用了海流以及減少了側流的情況,使得改進后的路徑能耗更低。

表2 AUV 能量消耗對比

圖10 未考慮海流影響路徑規(guī)劃

圖11 考慮海流影響路徑規(guī)劃

將改進后的算法繼續(xù)進行仿真,圖12 表示的是初始路徑規(guī)劃完成后臨時出現了動態(tài)障礙物時的路徑重規(guī)劃,可以看到重規(guī)劃的路徑能夠避開動態(tài)障礙物。

圖12 出現動態(tài)障礙物時的路徑規(guī)劃

但此時規(guī)劃的路徑會緊靠障礙物邊緣,考慮到AUV 自身的大小,現實情況中可能會造成與障礙物碰撞的后果,圖13 表示的是添加障礙物威脅約束后的仿真。

圖13 添加障礙物威脅約束的路徑規(guī)劃

如圖13 可以看出,規(guī)劃出的路徑距離靜態(tài)障礙物和動態(tài)障礙物都有一段距離,這樣就使得規(guī)劃出來的路徑更安全。不過此時的路徑仍是由折線段組成的,考慮到水下機器人立即改變方向的不可操作性,在實際作業(yè)過程中難以實現,因此路徑平滑尤為重要。圖14 中的實線是既添加了障礙物威脅約束,又對路徑進行平滑后的結果。

2.3 三維海流環(huán)境下的路徑規(guī)劃

三維海流環(huán)境下的處理方式與二維海流環(huán)境下相似,仿真結果如圖15～19 及表3 所示。通過結果對比可以得出和二維環(huán)境下類似的結論,即考慮海流影響的路徑長度稍長但是能耗更低。按照改進后的算法依次進行障礙物安全距離和路徑平滑的改進,結果符合預期。

圖15 未考慮海流影響路徑規(guī)劃

圖16 考慮海流影響路徑規(guī)劃

圖17 出現動態(tài)障礙物時的路徑規(guī)劃

圖18 添加障礙物威脅項的路徑規(guī)劃

圖19 添加障礙物威脅項、有平滑處理

通過仿真分析可知,本文所采用的路徑規(guī)劃算法適用于海流信息已知的離散化柵格地圖,模型建立方便,避免了復雜的計算,運行時間短,提高了AUV 路徑規(guī)劃的效率。但是,當海流信息不確定時,需要做進一步改進設計。

3 結論

目前大多數路徑規(guī)劃算法對AUV 的針對性不強,具有一定的局限性。本文針對海流環(huán)境下AUV動態(tài)路徑規(guī)劃問題,提出了一種基于D*算法的改進設計。通過建立海流影響能耗代價模型,使AUV能夠利用海流,減少能耗;加入障礙物威脅約束,使AUV 能盡量遠離障礙物邊緣,保證行駛安全;對規(guī)劃路徑作平滑處理,使生成的路徑更符合AUV 的行駛條件。這是本文工作的主要創(chuàng)新點也是區(qū)別于其他機器人路徑規(guī)劃的方面,實驗仿真驗證了本文提出改進算法的有效性。

由于海洋環(huán)境以及作業(yè)任務的復雜化程度不斷提高,單個AUV 很難高效地完成制定的任務。因此,多AUV 協同的路徑規(guī)劃及任務分配問題將成為未來工作的研究方向。