變化海流環(huán)境下AUV能量最優(yōu)三維路徑規(guī)劃

李亞南, 黃海濱, 陳亮名, 莊宇飛,*, 王曉麗

(1. 哈爾濱工業(yè)大學(xué)(威海)信息科學(xué)與工程學(xué)院, 山東 威海 264209;2. 南洋理工大學(xué)機(jī)械與航天工程學(xué)院, 新加坡 639788)

0 引 言

近年來(lái),隨著海洋自主觀測(cè)和數(shù)據(jù)收集設(shè)備的需求增加,海洋資源呈現(xiàn)出巨大價(jià)值,海洋的勘探和開發(fā)也越來(lái)越受到關(guān)注[1]。作為水下勘探的主要工具之一,自主水下航行器(autonomous underwater vehicle, AUV)具備高度的自主性和靈活性[2]。AUV在執(zhí)行許多水下任務(wù)時(shí)都發(fā)揮著重要作用,例如沿海生態(tài)系統(tǒng)數(shù)據(jù)收集[3-5]、掃雷[6-8]、海底勘測(cè)[9-11]、通信中繼[12-14]等。在上述任務(wù)背景中,AUV路徑規(guī)劃是最基本且最關(guān)鍵的問(wèn)題之一[15]。因此,非常有必要研究AUV的路徑規(guī)劃問(wèn)題。

對(duì)于AUV的路徑規(guī)劃問(wèn)題,相關(guān)研究者提出了許多不同類型的算法,包括A*算法、快速探索隨機(jī)樹(rapidly-exploring random trees, RRTs)以及其他基于圖搜索的水下路徑規(guī)劃算法。A*算法將路徑規(guī)劃問(wèn)題轉(zhuǎn)換為圖搜索問(wèn)題,文獻(xiàn)[16]采用A*算法為滑翔機(jī)在海洋環(huán)境下規(guī)劃了運(yùn)動(dòng)路徑。雖然A*算法的執(zhí)行效率高,但由于A*算法是一種啟發(fā)式的算法,其無(wú)法保證所規(guī)劃路徑的全局最優(yōu)性。RRTs算法廣泛用于路徑規(guī)劃研究中,為了考慮航行器運(yùn)動(dòng)的方向性以及動(dòng)態(tài)海流的影響,可以通過(guò)在RRTs中添加偏差的算法實(shí)現(xiàn)路徑規(guī)劃。盡管RRTs可以有效地對(duì)高維和非凸的空間進(jìn)行搜索,但是其預(yù)測(cè)的路徑通常不是最優(yōu)的[17]。此外,動(dòng)態(tài)海流的影響通常不能準(zhǔn)確地被并入到RRTs的計(jì)算公式中。因此,RRTs不能很容易地適用于高精度和存在海流下的水下路徑規(guī)劃。圖形搜索算法是在20世紀(jì)60～80年代之間提出的,已成功應(yīng)用于許多實(shí)際任務(wù)中。然而,基于網(wǎng)格的圖搜索算法獲得的狀態(tài)是離散的,因此限制了AUV在運(yùn)動(dòng)過(guò)程中的航向[18]。

上述算法存在的不足是不能獲得全局最優(yōu)路徑,并且在考慮海流對(duì)路徑規(guī)劃的影響時(shí)不夠精確。然而,很多應(yīng)用需要獲得最優(yōu)路徑,再加上海流廣泛存在于海底,忽略海流的影響可能使所獲得的結(jié)果難以應(yīng)用于實(shí)際工程中。因此,考慮存在海流影響的情況下,開發(fā)既精確又有效的算法來(lái)規(guī)劃路徑并使所規(guī)劃的路徑滿足所設(shè)定的優(yōu)化指標(biāo),是很有研究意義和價(jià)值的[19]。

目前,為了獲得航行器在水下環(huán)境影響下的最優(yōu)路徑,部分研究已經(jīng)取得了一些進(jìn)展。文獻(xiàn)[20]采用并行群搜索算法研究了時(shí)變海流下的最優(yōu)路徑規(guī)劃問(wèn)題。Lolla等人在文獻(xiàn)[21]中提出了一種用于獲得水下最優(yōu)運(yùn)動(dòng)路徑的算法,該算法可以準(zhǔn)確地描述動(dòng)態(tài)環(huán)境海流對(duì)航行器運(yùn)動(dòng)的影響。此外,這項(xiàng)工作也已擴(kuò)展到如何獲得所需能量最少的路徑[22]。文獻(xiàn)[23]在多航行器任務(wù)分配的研究中,采用最優(yōu)控制理論設(shè)計(jì)了一種海流影響下的路徑規(guī)劃算法。注意到,上述工作僅考慮了二維(two-dimensional, 2D)的運(yùn)動(dòng)環(huán)境,并將AUV的路徑限定在恒定水深下。然而,海流的大小通常會(huì)隨水深的變化而變化。因此,2D情況下的規(guī)劃算法不適用于不規(guī)則海床以及隨深度變化很大的復(fù)雜海流等三維(three-dimensional,3D)環(huán)境下的AUV運(yùn)動(dòng)規(guī)劃。

為了解決3D環(huán)境下的AUV運(yùn)動(dòng)規(guī)劃問(wèn)題,有研究為了提高規(guī)劃效率并減少內(nèi)存需求,將3D環(huán)境投影到2D空間。然而,由于海流和障礙物的存在,這樣投影得到的2D空間無(wú)法完全體現(xiàn)3D環(huán)境下的所有信息[24]。文獻(xiàn)[19]首次采用水平集算法解決完全適用于3D的海洋環(huán)境下AUV路徑規(guī)劃問(wèn)題,但是并未將此工作擴(kuò)展到能量最優(yōu)路徑規(guī)劃。文獻(xiàn)[25]研究AUV實(shí)時(shí)最優(yōu)路徑規(guī)劃問(wèn)題時(shí)采用最優(yōu)控制理論得到近似解析解,并在3D海流環(huán)境下產(chǎn)生近似最優(yōu)路徑，但是此算法不適用于AUV的速度很低的情況?；谝陨暇C述可以得出,對(duì)3D下存在海流影響的AUV能量最優(yōu)路徑規(guī)劃問(wèn)題還有待進(jìn)一步研究。

在上述相關(guān)工作的基礎(chǔ)上,本文首先對(duì)AUV提出了一種從起始位置行駛至目標(biāo)位置的3D能量最優(yōu)路徑規(guī)劃算法。通過(guò)該算法,可以獲得從起始位置到目標(biāo)位置的能量最優(yōu)路徑。其次,在這項(xiàng)工作中考慮了更加接近實(shí)際工程背景的變化海流的影響,即海流與位置成固定關(guān)系。最后,利用一般線性系統(tǒng)的狀態(tài)空間理論,得到了最優(yōu)控制律的顯式解。與現(xiàn)有的迭代和離散算法(例如圖搜索和進(jìn)化算法)相比,基于最優(yōu)控制的算法可以在3D動(dòng)態(tài)海流的影響下,可以生成能量最優(yōu)路徑和速度控制律,并獲得全局最優(yōu)路徑。由于本文研究的AUV運(yùn)動(dòng)模型為非線性,這對(duì)于求得最優(yōu)解析解帶來(lái)了難度。

1 問(wèn)題描述

1.1 運(yùn)動(dòng)學(xué)模型

考慮AUV的運(yùn)動(dòng)模型如圖1所示,定義慣性坐標(biāo)系G-xyz和體坐標(biāo)系O-xbybzb。在慣性坐標(biāo)系中,G為坐標(biāo)原點(diǎn),Gx置于海平面,且指向地理正東方向,Gz指向地心。在體坐標(biāo)系中,O為坐標(biāo)原點(diǎn),并固定在AUV的重心,Oxb軸選取在AUV的縱中剖面內(nèi),正向指向船首,Ozb垂直于水平面指向地心方向。慣性坐標(biāo)系和體坐標(biāo)系都遵循右手螺旋法則。

圖1 參考坐標(biāo)系與AUV六自由度運(yùn)動(dòng)Fig.1 Reference coordinate system and six degrees of freedom movement of AUV

向量vc=[vcx,vcy,vcz]T用于描述慣性坐標(biāo)系下的海流速度,假設(shè)海流大小僅隨G-xyz下的位置而變化。忽略側(cè)傾運(yùn)動(dòng)的影響,欠驅(qū)動(dòng)AUV的運(yùn)動(dòng)學(xué)模型可以由以下方程表示:

(1)

1.2 問(wèn)題定義

2 能量最優(yōu)控制

(2)

定義相應(yīng)的哈密頓函數(shù)為

(3)

(4)

(5)

(6)

由于式(5)和式(6)對(duì)于所有t≥t0滿足對(duì)時(shí)間求導(dǎo)等于0,所以可以分別得到：

(7)

(8)

由式(4)可得

(9)

將式(7)變形為

(10)

將式(8)變形為

(11)

整理式(10)和式(11)得

(12)

(13)

當(dāng)滿足式(12)和式(13)時(shí),AUV的運(yùn)動(dòng)滿足最小化性能指標(biāo)J。注意到式(3)～式(13)采用最優(yōu)控制理論的龐特里亞金極小值原理,對(duì)第1.2節(jié)所提出的問(wèn)題,得到了全局最優(yōu)解所需要滿足的條件。根據(jù)文獻(xiàn)[27]中定理4.1,若找到一組符合式(12)和式(13)的解,那么該解可以全局優(yōu)化性能指標(biāo)式(2)。

3 最優(yōu)航速及初始導(dǎo)航角求解

(14)

(15)

(16)

進(jìn)一步得到

(17)

(18)

將式(17)、式(18)以及海流向量代入式(1),得到:

(19)

(20)

可得初始時(shí)刻為t0，初始狀態(tài)為x(t0),y(t0),z(t0)時(shí)，其解為

(21)

式中：B=u*(0)I3，

首先求狀態(tài)轉(zhuǎn)移矩陣：

(22)

將B、T(t)以及xtf、ytf和ztf代入,求解上述線性定常系統(tǒng)非齊次方程，得到：

(23)

為了得到最優(yōu)的運(yùn)動(dòng)路徑,最優(yōu)航向和最優(yōu)航速需分別滿足式(15)和式(16)。而式(15)和式(16)的求解則依賴于非線性代數(shù)方程式(23)。注意到非線性代數(shù)方程式(23)的已知參數(shù)是AUV的運(yùn)動(dòng)初始位置、末端位置和海流的大小。這就意味著最終能否順利求解得到最優(yōu)航向和最優(yōu)航速,以及求解得到的最優(yōu)航向和最優(yōu)航速能否物理實(shí)現(xiàn),取決于所給定的AUV運(yùn)動(dòng)初始位置、末端位置和海流的大小。當(dāng)所給條件過(guò)于苛刻,將面臨著最優(yōu)航速、航向不可物理實(shí)現(xiàn)問(wèn)題。

4 仿真驗(yàn)證

4.1 仿真參數(shù)設(shè)置

以下是AUV在海流的影響下從給定起始點(diǎn)到終止目標(biāo)點(diǎn)的連續(xù)能量最優(yōu)路徑的仿真,選取起始時(shí)間t0=0,航行終止時(shí)間為tf,起始點(diǎn)(0,0,70),終止點(diǎn)為(70,80,0),海流向量vc=10-3[2x-y,x+2y,2z]T,此海流是變化的,表現(xiàn)為不同位置的海流大小是不同的,考慮到海流的穩(wěn)定性,因此假設(shè)在一定時(shí)間內(nèi)海流并不隨時(shí)間變化。

4.2 能量最優(yōu)AUV路徑規(guī)劃仿真結(jié)果

4.2.1 海流影響下能量最優(yōu)路徑規(guī)劃

首先進(jìn)行第一種情況仿真,即AUV在海流下由起始點(diǎn)到終止點(diǎn)的能量最優(yōu)路徑規(guī)劃仿真。給定AUV俯仰角θ=π/3,將初始條件、海流大小以及給定起始點(diǎn)和目標(biāo)點(diǎn)代入到式(23)中,通過(guò)數(shù)值求解方法得到最優(yōu)初始導(dǎo)航角、最優(yōu)航速和航行終止時(shí)間,如下所示:

(24)

再將式(24)中計(jì)算所得u*(0)以及航行終止時(shí)間tf代入式(2)中求得所需能量消耗。在第4.1節(jié)中給定的仿真參數(shù)下,得到如圖2～圖4所示的仿真結(jié)果。圖2為海流影響下的AUV在x,y,z方向上的能量最優(yōu)路徑,其中橫軸為航行終止時(shí)間,縱軸為AUV在x,y,z方向上的位移變化,仿真結(jié)果驗(yàn)證了AUV在能量最優(yōu)性能指標(biāo)下能夠精確地到達(dá)目標(biāo)位置。圖3為海流影響下基于最優(yōu)控制的AUV航速及艏向角變化曲線,基于最優(yōu)控制的能量最優(yōu)路徑規(guī)劃所求速度保持恒定以及所求艏向角呈線性變化,因此可保持推進(jìn)器的推力為常值,基于最優(yōu)控制規(guī)劃的能量最優(yōu)路徑更易物理實(shí)現(xiàn)。

圖2 海流影響下的AUV能量最優(yōu)路徑Fig.2 Energy-optimal path of AUV in presence of ocean current

圖3 海流影響下基于最優(yōu)控制的AUV航速及艏向角變化曲線Fig.3 Velocity and yaw angle changing curve of AUV based on optimal control in presence ocean current

圖4為海流影響下的3D AUV路徑,其中藍(lán)色實(shí)線是AUV由起始點(diǎn)到終止點(diǎn)的路徑,紅色箭頭表示海流。實(shí)驗(yàn)結(jié)果驗(yàn)證了基于最優(yōu)控制的算法能有效解決AUV面向能量消耗的路徑規(guī)劃問(wèn)題,在給定條件下獲得全局能量最優(yōu)路徑。

圖4 海流影響下的AUV能量最優(yōu)3D路徑Fig.4 Energy-optimal 3D AUV path in presence of ocean current

為說(shuō)明基于最優(yōu)控制優(yōu)化算法的效果,用非優(yōu)化的負(fù)反饋控制器進(jìn)行仿真對(duì)比。第二種情況給出AUV采用負(fù)反饋控制器在海流下由起始點(diǎn)到終止點(diǎn)的路徑仿真。AUV運(yùn)動(dòng)學(xué)模型為式(1)所描述,海流變量仍為vc=10-3·[2x-y,x+2y,2z]T。

基于負(fù)反饋的路徑規(guī)劃的設(shè)計(jì)過(guò)程如下。首先設(shè)計(jì)輔助控制量:

(25)

式中：k0為正常數(shù)。由此得到負(fù)反饋控制器的運(yùn)動(dòng)過(guò)程及其控制變量：

(26)

將起始點(diǎn)和終止點(diǎn)代入,k0取0.03(k0>0均可),仿真結(jié)果如圖5所示。圖5為基于負(fù)反饋控制器的航速和艏向角以及俯仰角變化曲線,雖然其航速和艏向角均可物理實(shí)現(xiàn),但是對(duì)比圖3和圖5,基于負(fù)反饋控制所求速度需實(shí)時(shí)變化,而基于最優(yōu)控制的能量最優(yōu)路徑規(guī)劃所求速度保持恒定以及所求艏向角呈線性變化,因此可以始終保持推進(jìn)器的推力是一個(gè)常值,驗(yàn)證了基于最優(yōu)控制所規(guī)劃的能量最優(yōu)路徑更易物理實(shí)現(xiàn)。

圖5 海流下基于負(fù)反饋的AUV艏向角、俯仰角及航速變化曲線Fig.5 AUV yaw angle, pitch angle and velocity changing curvebased on negative feedback under ocean current

圖6為基于負(fù)反饋控制器與基于最優(yōu)控制的AUV能量消耗對(duì)比圖,基于負(fù)反饋控制能量消耗遠(yuǎn)大于基于最優(yōu)控制優(yōu)化的路徑規(guī)劃所需能量消耗,因此驗(yàn)證了基于最優(yōu)控制的能量最優(yōu)路徑規(guī)劃算法的優(yōu)越性。為說(shuō)明海流變化程度對(duì)所提優(yōu)化算法的影響,接下來(lái)擴(kuò)大海流大小,針對(duì)AUV在海流下從(0,0,70)到(70,80,0)的能量最優(yōu)路徑規(guī)劃仿真。給定AUV俯仰角θ=π/3,得到最優(yōu)航速、航行終止時(shí)間和能量,如表1所示。當(dāng)海流為原始大小時(shí),在起始點(diǎn)、終止點(diǎn)的海流大小分別為0.14 m/s和0.24 m/s,此海流的最大值為0.28 m/s,得到的航速為0.26 m/s。海流與AUV航速一般設(shè)置為同一量級(jí)大小[29],而AUV航速大小一般為0～10 kN,得到的航速小于AUV實(shí)際最大運(yùn)行速度,因此算法在物理上可實(shí)現(xiàn)。

圖6 基于負(fù)反饋控制器與基于最優(yōu)控制的AUV能量消耗對(duì)比圖Fig.6 Comparison of AUV energy consumption based on negativefeedback controller and optimal control

表1 不同海流下的控制量計(jì)算結(jié)果

海流分別擴(kuò)大5倍、10倍時(shí),得到的航速小于AUV實(shí)際最大運(yùn)行速度,可以物理實(shí)現(xiàn)。

當(dāng)海流擴(kuò)大20倍時(shí),即20vc=10-3[40x-20y,20x+40y,40z]T,在起始點(diǎn)、終止點(diǎn)的海流大小分別為2.8 m/s和4.75 m/s,此海流的最大值為5.52 m/s,得到的航速為5.21 m/s,得到的航速稍大于AUV的實(shí)際最大運(yùn)行速度。因此當(dāng)逆向海流過(guò)大時(shí),導(dǎo)致所求航速過(guò)大,面臨著不可物理實(shí)現(xiàn)問(wèn)題。為了驗(yàn)證最優(yōu)控制的可實(shí)現(xiàn)性問(wèn)題,考慮當(dāng)海流擴(kuò)大100倍時(shí),無(wú)法求得可行解,這是由于海流過(guò)大造成的。因此,當(dāng)所給條件較苛刻時(shí),將面臨著物理不可實(shí)現(xiàn)問(wèn)題。

4.2.2 大深度情況下能量最優(yōu)路徑規(guī)劃

為了面向海底管道檢修、掃雷以及水下傳感器網(wǎng)絡(luò)數(shù)據(jù)采集任務(wù),需要大深度情況下的能量最優(yōu)路徑規(guī)劃[30-31]。接下來(lái)進(jìn)行AUV在海流下從(0,0,700)到(700,800,0)的能量最優(yōu)路徑規(guī)劃仿真,驗(yàn)證了本文所提出的能量最優(yōu)路徑規(guī)劃算法同樣適用于大深度下的路徑規(guī)劃需求。并進(jìn)行了在俯仰角分別為π/3 rad,π/4 rad,0.99 rad下的能量最優(yōu)路徑規(guī)劃,仿真結(jié)果如圖7所示。

圖7 不同俯仰角下的3D AUV能量最優(yōu)路徑Fig.7 3D AUV energy-optimal path at different pitch angles

圖7中藍(lán)色實(shí)線為θ=π/3 rad下的能量最優(yōu)路徑,黃色實(shí)線為θ=π/4 rad下的能量最優(yōu)路徑,紫色實(shí)線為θ=0.99 rad下的能量最優(yōu)路徑。表2為不同俯仰角下最優(yōu)航速、最優(yōu)導(dǎo)航角和航行終止時(shí)間的計(jì)算結(jié)果。由表2看出，起始點(diǎn)和終止點(diǎn)相同的情況下,由于俯仰角不同,仿真結(jié)果不同。其中，在θ=0.99 rad下為連接起始點(diǎn)和終止點(diǎn)的俯仰角,但由于海流的原因,其所需的能量并不是最少的。在θ=π/4 rad下所求航速不可物理實(shí)現(xiàn),且需要更多的能量,因此可以根據(jù)不同俯仰角下的計(jì)算結(jié)果,選取適當(dāng)?shù)母┭鼋恰?/p>

表2 大深度下不同俯仰角的控制量計(jì)算結(jié)果

接下來(lái)進(jìn)行AUV在海流下從(0,0,700)到(700,800,0)的基于負(fù)反饋控制器的仿真。將起始點(diǎn)和終止點(diǎn)代入,得到所需能量消耗為7 759.4 J,其大于采用優(yōu)化算法的能量消耗,因此驗(yàn)證了此優(yōu)化算法的優(yōu)越性?；谪?fù)反饋控制器在大深度下的仿真結(jié)果如圖8所示,與基于最優(yōu)控制的能量最優(yōu)路徑規(guī)劃所求速度保持恒定以及所求艏向角呈線性變化,基于最優(yōu)控制所規(guī)劃的能量最優(yōu)路徑更易物理實(shí)現(xiàn)。接下來(lái)進(jìn)行針對(duì)AUV在海流下從(0,0,700)到(700,800,0)的基于負(fù)反饋控制器的仿真。將起始點(diǎn)和終止點(diǎn)代入,得到所需能量消耗為7 759.4 J,其大于采用優(yōu)化算法的能量消耗,因此驗(yàn)證了此優(yōu)化算法的優(yōu)越性?；谪?fù)反饋控制器在大深度下的仿真結(jié)果如圖8所示,與基于最優(yōu)控制的能量最優(yōu)路徑規(guī)劃所求速度保持恒定以及所求艏向角呈線性變化,基于最優(yōu)控制所規(guī)劃的能量最優(yōu)路徑更易物理實(shí)現(xiàn)。

圖8 大深度下基于負(fù)反饋的AUV艏向角、俯仰角及航速變化曲線Fig.8 AUV yaw angle, pitch angle and velocity changing curve based on negative feedback under large depth

5 結(jié) 論

水下環(huán)境的復(fù)雜性是AUV路徑規(guī)劃的巨大挑戰(zhàn),本文提出了一種3D環(huán)境下的AUV能量最優(yōu)路徑規(guī)劃算法,采用了最優(yōu)控制理論來(lái)得出最優(yōu)航速變化律以及艏向角變化律,并考慮海流與水下位置成固定函數(shù)關(guān)系,獲得從初始位置到目標(biāo)位置的能量最優(yōu)路徑,能夠有效地克服海流對(duì)AUV路徑規(guī)劃的影響。最后通過(guò)仿真對(duì)比了多組不同參數(shù)情況下的最優(yōu)路徑規(guī)劃過(guò)程,仿真結(jié)果表明基于最優(yōu)控制的算法下AUV能夠精確地到達(dá)目標(biāo)位置,驗(yàn)證了基于最優(yōu)控制的優(yōu)化算法的有效性。此外,基于最優(yōu)控制的算法有效減少能量消耗,且規(guī)劃的路徑更易物理實(shí)現(xiàn)。未來(lái)的工作將進(jìn)一步研究更復(fù)雜的海洋環(huán)境情況,例如存在障礙物和更加復(fù)雜海流的對(duì)AUV路徑規(guī)劃的影響。此外,將進(jìn)一步研究在有限能量以及時(shí)變、非線性海流情況下的路徑規(guī)劃問(wèn)題。