面向AUV水下移動(dòng)對(duì)接的實(shí)時(shí)路徑規(guī)劃方法

時(shí)常鳴，劉開周，趙 洋，趙寶德

(1. 中國(guó)科學(xué)院沈陽自動(dòng)化研究所 機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽 110016；2. 中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110016；3. 中國(guó)科學(xué)院大學(xué)，北京 100049)

0 引 言

自主水下機(jī)器人（Autonomous Underwater Vehicle，AUV）是探索海洋空間的有力工具之一，在海洋科學(xué)考察和海底資源勘測(cè)等領(lǐng)域獲得廣泛應(yīng)用。利用水下對(duì)接平臺(tái)對(duì)AUV進(jìn)行自主對(duì)接，可以實(shí)現(xiàn)AUV能源補(bǔ)充、數(shù)據(jù)上傳和使命下載，從而增強(qiáng)AUV的續(xù)航力與作業(yè)能力，降低AUV的應(yīng)用成本。而AUV與移動(dòng)對(duì)接平臺(tái)之間的實(shí)時(shí)路徑規(guī)劃技術(shù)，是AUV與水下對(duì)接平臺(tái)自主對(duì)接的使能技術(shù)之一。

Ken T等[1-2]提出一種基于TSK模糊理論的AUV對(duì)接控制方法，利用AUV與水下固定式對(duì)接平臺(tái)之間的橫向誤差、縱向誤差、AUV的期望縱向速度和艏向角建立相應(yīng)的模糊策略，通過仿真實(shí)驗(yàn)和實(shí)際實(shí)驗(yàn)結(jié)果證明，該方法可使AUV在沒有障礙物和海流未知的情況下回收到水下固定對(duì)接平臺(tái)內(nèi)；Jantapremjit P等[3-4]將對(duì)接階段分為2個(gè)階段，利用切換加權(quán)系數(shù)的人工矢量場(chǎng)法，使AUV沿著水下固定對(duì)接平臺(tái)的中軸線運(yùn)動(dòng)，達(dá)到對(duì)接所需方向和位置最終實(shí)現(xiàn)AUV的安全對(duì)接；李曄和姜言清等[5-6]提出一種基于3次B樣條和遺傳算法的AUV小范圍靜態(tài)路徑規(guī)劃方法，該方法滿足AUV對(duì)接過程中所需的終端約束和運(yùn)動(dòng)約束。上述方法都是AUV與水下固定對(duì)接平臺(tái)之間的路徑規(guī)劃方法，存在容易陷入局部極值或運(yùn)算復(fù)雜規(guī)劃時(shí)間長(zhǎng)等缺點(diǎn)，無法滿足環(huán)境未知的情況下，AUV與水下移動(dòng)平臺(tái)對(duì)接的實(shí)時(shí)性需求。

本文針對(duì)復(fù)雜動(dòng)態(tài)環(huán)境下AUV與水下移動(dòng)平臺(tái)對(duì)接的實(shí)時(shí)性和終端姿態(tài)需求，研究了一種基于混合整數(shù)線性規(guī)劃[7]（Mixed Integer Linear Programming，MILP）的AUV與水下移動(dòng)平臺(tái)對(duì)接路徑規(guī)劃方法，根據(jù)對(duì)接階段的需求設(shè)計(jì)了不同的目標(biāo)優(yōu)化函數(shù)，實(shí)現(xiàn)了移動(dòng)對(duì)接目標(biāo)函數(shù)建模，對(duì)障礙物約束和AUV本體約束進(jìn)行了相應(yīng)的線性化，建立起由線性化約束和目標(biāo)函數(shù)組成的線性規(guī)劃模型，在相對(duì)速度坐標(biāo)系下實(shí)現(xiàn)對(duì)AUV加速度的優(yōu)化，實(shí)時(shí)規(guī)劃出AUV相應(yīng)的縱向速度和艏向角，并充分考慮AUV實(shí)際的動(dòng)力學(xué)模型，最終在AUV與水下移動(dòng)平臺(tái)之間得到滿足所有約束且目標(biāo)函數(shù)最優(yōu)的實(shí)時(shí)優(yōu)化路徑，利用仿真實(shí)驗(yàn)驗(yàn)證該方法的有效性。

1 問題描述

本文將對(duì)接過程分為2個(gè)階段：歸航階段和終端導(dǎo)引階段[8-9]。第1階段，AUV通過超短基線等傳感器快速到達(dá)對(duì)接平臺(tái)前方一定距離位置[10]；第2階段，AUV通過高精度傳感器（聲、光、磁等）進(jìn)行精確導(dǎo)航定位，根據(jù)當(dāng)前的位置和艏向，以及水下對(duì)接平臺(tái)的位置及艏向，最終以期望的航向姿態(tài)完成欠驅(qū)動(dòng)AUV與水下移動(dòng)對(duì)接平臺(tái)之間的精確移動(dòng)對(duì)接。

MILP是在線性規(guī)劃（Linear Programming，LP）的基礎(chǔ)上發(fā)展而來的一種優(yōu)化方法，它融入了整數(shù)變量，可以解決具有復(fù)雜邏輯關(guān)系的實(shí)數(shù)-整數(shù)混合優(yōu)化問題。為了利用MILP優(yōu)化方法更好地解決AUV與水下移動(dòng)平臺(tái)之間的實(shí)時(shí)優(yōu)化路徑規(guī)劃問題，在此，進(jìn)行相關(guān)假設(shè)和簡(jiǎn)化：

圖 1 AUV與水下移動(dòng)平臺(tái)對(duì)接過程示意圖Fig. 1 Diagram of docking process between AUV and underwater mobile platform

1）設(shè)AUV、障礙物和水下對(duì)接平臺(tái)的當(dāng)前時(shí)刻的相對(duì)位置和相對(duì)速度已知，且下一時(shí)刻的相對(duì)位置和相對(duì)速度未知；

2）設(shè)在極小時(shí)間周期內(nèi)，障礙物和水下移動(dòng)平臺(tái)的速度保持不變；

3）設(shè)對(duì)接階段水下對(duì)接平臺(tái)航行速度小于AUV的速度；

4）設(shè)水下對(duì)接平臺(tái)沿著水下移動(dòng)平臺(tái)的中軸線方向移動(dòng)。

2 約束建模

2.1 障礙物約束的線性化

如圖2所示，設(shè)E-ξη為固定坐標(biāo)系，點(diǎn)A表示AUV,點(diǎn)O表示為障礙物的圓心；VA和VO分別表示為AUV和障礙物的當(dāng)前速度矢量，VAO表示為AUV和障礙物之間的相對(duì)速度矢量,VAO=VA-VO；LAO表示為AUV和障礙物之間的相對(duì)位置矢量； γAO表示為之間的夾角，以VAO在LAO右側(cè)為正，取值范圍為 (-π,π]， 同理，與邊界AM或AN的夾角，表示為單位時(shí)間內(nèi)γAOC的改變量。

圖 2 AUV-障礙物示意圖Fig. 2 Diagram of AUV-obstacle

為了使AUV可以避開多個(gè)障礙物，障礙物約束的線性化模型可以寫成：

其中：i=1,2,...N。

為了解決AUV避開障礙物時(shí)的轉(zhuǎn)向問題，可以將式(2)寫成：

其 中ai+bi=1，ai和bi都 為 二 進(jìn) 制 變 量 ，κ 為 遠(yuǎn) 大 于 不等式左側(cè)的正實(shí)數(shù)。

當(dāng)ai=0,bi=1時(shí)，式（3）中前2個(gè)等式成立，表示AUV從障礙物的右側(cè)避開障礙物；

當(dāng)ai=1,bi=0時(shí)，式（3）中后2個(gè)等式成立，表示AUV從障礙物的左側(cè)避開障礙物。

2.2 AUV本體約束

考慮 AUV動(dòng)力學(xué)模型，由于螺旋槳的轉(zhuǎn)速極限約束，欠驅(qū)動(dòng)AUV航速不會(huì)太大，由AUV操縱性知識(shí)可知，過低的航速會(huì)導(dǎo)致方向舵失效。假設(shè)在單個(gè)采樣周期Δt內(nèi)自主水下機(jī)器人的速度和加速度保持不變，且滿足約束：

聯(lián)立式（4）和式（5）得加速度分量約束如下：

3 目標(biāo)函數(shù)建模

目標(biāo)函數(shù)是根據(jù)AUV與水下移動(dòng)平臺(tái)對(duì)接的任務(wù)需求來確定的。在AUV回收的第1階段，目的是盡快縮短AUV與水下移動(dòng)平臺(tái)的距離，并快速到達(dá)大致預(yù)定位置，這里與其他AUV路徑規(guī)劃問題類似，只需要解決目標(biāo)點(diǎn)的到達(dá)問題；第2階段，目的是AUV以期望的航向姿態(tài)盡快與水下移動(dòng)平臺(tái)完成對(duì)接，這里與其他AUV路徑規(guī)劃問題不同的是，不僅需要解決目標(biāo)點(diǎn)的到達(dá)問題，還要保證AUV是以期望方向到達(dá)目標(biāo)點(diǎn)。

3.1 距離收斂

為了使AUV與水下移動(dòng)平臺(tái)之間的距離越來越近（見圖3），即，則可以寫作：

其中，分別表示為AUV與水下移動(dòng)對(duì)接平臺(tái)之間的相對(duì)位置矢量和相對(duì)速度矢量，表示在單個(gè)采樣周期內(nèi)的變化量。

圖 3 相對(duì)速度的速度分解Fig. 3 Velocity decomposition of relative velocity

為了方便計(jì)算，式（7）寫作：

其中，j表示維數(shù)。

3.2 時(shí)間最優(yōu)

為了盡快縮短AUV與水下移動(dòng)對(duì)接平臺(tái)之間的相對(duì)距離，AUV與目標(biāo)的相對(duì)速度VAG在沿著相對(duì)位置矢量LAG上的分量應(yīng)該最大，即

經(jīng)過線性化[11]以及為了與距離收斂目標(biāo)函數(shù)式（8）相對(duì)應(yīng)，式（9）最終為：

3.3 姿態(tài)最優(yōu)

為了使AUV以期望的航向姿態(tài)與水下移動(dòng)平臺(tái)完成對(duì)接，AUV與目標(biāo)的相對(duì)速度VAG沿垂直于水下移動(dòng)平臺(tái)移動(dòng)速度VG的分量應(yīng)該最大，即

設(shè)n為VT的 單 位 向 量 ， 為 了 使的 同一側(cè)，則n可表示為：

令

根據(jù)泰勒公式可知：

其 中 ， ?f=n表 示 函 數(shù) f(VAG) 的 梯 度 ，o(||ΔVAG||)表 示泰勒余項(xiàng)。

忽略高階項(xiàng)，以及為了與距離收斂目標(biāo)函數(shù)式（8）相對(duì)應(yīng)，式（11）最終為，

3.4 移動(dòng)對(duì)接目標(biāo)函數(shù)建模

在AUV與水下移動(dòng)平臺(tái)對(duì)接的第1階段，只需要考慮目標(biāo)點(diǎn)到達(dá)問題；在第2階段，不僅需要考慮目標(biāo)點(diǎn)達(dá)到問題，還要保證AUV是以期望方向到達(dá)目標(biāo)點(diǎn)。同時(shí)根據(jù)子目標(biāo)函數(shù)的分析，可得總的目標(biāo)函數(shù)：

其中，i=1,2,…M,表示維數(shù)，這里取M=2，表示二維平面 ， ωi,ωv1,ωv2≥0 為權(quán)值 ， φAG為VG與LAG之間 的 夾角，為開始進(jìn)行AUV與水下移動(dòng)平臺(tái)對(duì)接的第二階段的最小距離， φmin為AUV與水下移動(dòng)平臺(tái)最終對(duì)接時(shí)允許的最小姿態(tài)偏差。

4 算法流程

AUV與水下移動(dòng)對(duì)接平臺(tái)的精確對(duì)接系統(tǒng)的實(shí)時(shí)路徑規(guī)劃流程圖如圖4所示。具體的算法流程如下：

1）通過傳感器數(shù)據(jù)獲得水下移動(dòng)平臺(tái)和障礙物的位置信息和速度信息；

2）對(duì)傳感器約束和本體約束進(jìn)行線性化，獲得相應(yīng)模型；

3）根據(jù)規(guī)劃策略公式（14）設(shè)計(jì)相應(yīng)的目標(biāo)函數(shù)；

4）利用MILP規(guī)劃器實(shí)現(xiàn)對(duì)AUV加速度的優(yōu)化，并得到 期望的速度ud和 航 向ψd：

其中， (V+ΔVΔt)η， (V+ΔVΔt)ξ分別為速度矢量在η，ξ方向上的分項(xiàng)量；

5）將航行控制器作用于AUV的實(shí)際動(dòng)力學(xué)模型上，得到實(shí)際的速度u和 航向 ψ；

圖 4 算法流程圖Fig. 4 Flow diagram of algorithm

6）根據(jù)實(shí)際的速度u和航向 ψ ，得到下一時(shí)刻AUV的位置，即

7）判斷AUV是否到達(dá)水下移動(dòng)對(duì)接平臺(tái)位置，如果到達(dá)，則規(guī)劃算法結(jié)束，否則返回步驟2進(jìn)行下一周期規(guī)劃。

5 仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)在Matlab R2015b下進(jìn)行，采用PID控制器對(duì)某型AUV進(jìn)行航行控制。MILP的規(guī)劃周期Δt=1s，開始進(jìn)行AUV與水下移動(dòng)平臺(tái)對(duì)接的第二階段的最小距離dmin=50m， 允許的最小姿態(tài)偏差 φmin=15°，表1給出了仿真實(shí)驗(yàn)中AUV、障礙物和水下移動(dòng)對(duì)接平臺(tái)的位置信息和速度信息等信息。

表 1 仿真實(shí)驗(yàn)初始值Tab. 1 Initial value of simulation experiment

根據(jù)仿真結(jié)果可知，MILP單步規(guī)劃時(shí)間約為0.18 s，可以滿足AUV與水下移動(dòng)平臺(tái)之間路徑規(guī)劃的實(shí)時(shí)性需求。由圖5可知，AUV可以有效避開障礙物O1和障礙物O2，AUV在位置 (83.20,103.01)處進(jìn)入AUV與水下移動(dòng)平臺(tái)對(duì)接的第2階段，最終在(100.00,166.50)處完成對(duì)接任務(wù)。由圖6可知，AUV的縱向速度越來越大，在60 s左右達(dá)到Vmax，AUV在114 s左右進(jìn)入AUV與水下移動(dòng)平臺(tái)對(duì)接的第2階段，在160 s左右完成對(duì)接任務(wù)。由圖7可知，AUV最終以航向角偏差φAG≈3°的航向姿態(tài)與水下移動(dòng)平臺(tái)完成對(duì)接，滿足對(duì)接要求。

圖 5 實(shí)時(shí)規(guī)劃路徑圖Fig. 5 Real-time path planning map

圖 6 AUV速度曲線Fig. 6 The velocity curve of AUV

圖 7 AUV航向曲線Fig. 7 The heading curve of AUV

6 結(jié) 語

以往的大多數(shù)路徑規(guī)劃算法只考慮目標(biāo)點(diǎn)的到達(dá)問題，沒有考慮AUV到達(dá)目標(biāo)點(diǎn)的姿態(tài)需求。本文針對(duì)復(fù)雜動(dòng)態(tài)環(huán)境下AUV與水下移動(dòng)對(duì)接平臺(tái)的實(shí)時(shí)性和終端姿態(tài)需求，提出一種基于MILP的實(shí)時(shí)路徑規(guī)劃方法。充分考慮了障礙物約束、AUV本體約束，根據(jù)多個(gè)對(duì)接階段的需求設(shè)計(jì)了距離收斂、時(shí)間最優(yōu)和姿態(tài)最優(yōu)等不同的目標(biāo)優(yōu)化函數(shù)，建立了移動(dòng)對(duì)接目標(biāo)函數(shù)，形成相應(yīng)的多約束線性規(guī)劃模型，實(shí)現(xiàn)對(duì)AUV加速度優(yōu)化。仿真結(jié)果表明，該算法能滿足AUV與水下移動(dòng)對(duì)接平臺(tái)的實(shí)時(shí)性和終端姿態(tài)需求。

AUV的期望航向在路徑規(guī)劃的后半程存在劇烈變化，分析可知，這是由于AUV參數(shù)的設(shè)計(jì)不太恰當(dāng)導(dǎo)致的，需要進(jìn)一步改進(jìn)。下一步需要考慮海流對(duì)AUV和水下移動(dòng)對(duì)接平臺(tái)運(yùn)動(dòng)的影響，以便更好地滿足實(shí)際工程需要。