一種回收過程中UUV對運動母船的跟蹤方法

張偉，張明臣，郭毅，陳濤，嚴(yán)浙平

（哈爾濱工程大學(xué)自動化學(xué)院，黑龍江哈爾濱150001）

一種回收過程中UUV對運動母船的跟蹤方法

張偉，張明臣，郭毅，陳濤，嚴(yán)浙平

（哈爾濱工程大學(xué)自動化學(xué)院，黑龍江哈爾濱150001）

在無人水下航行器（UUV）對運動母船的跟蹤過程中，跟蹤方法需要適應(yīng)母船的運動特性，為此該文提出了引入速度量的人工勢場法。為了保證跟蹤過程安全可靠，考慮到母船外部形狀的復(fù)雜性，在母船周圍劃定四個禁航區(qū)。在運動規(guī)劃過程中除了將UUV、母船和禁航區(qū)的位置信息作為規(guī)劃要素外，加入了三者的速度信息，得到改進的勢場法?；诟倪M的勢場法得到UUV的速度和航向方程，并在母船以運動場方式運動的工況下進行了UUV跟蹤母船的仿真實驗，結(jié)果表明所設(shè)計的運動規(guī)劃方法能夠?qū)崿F(xiàn)UUV水下自主回收作業(yè)中對運動母船的安全跟蹤。

無人水下航行器；運動母船；跟蹤；禁航區(qū)；自主回收

UUV在回收任務(wù)中回塢階段需要跟蹤運動母船并到達特定的對接地點［1］，因此使UUV安全地跟蹤上運動母船具有很大的研究意義［2?4］。劉斌等針對高速UUV提出了基于UKF（unscented Kalman fil?ter，UKF）的動目標(biāo)跟蹤算法，仿真表明基于UKF的動目標(biāo)跟蹤算法有較好的優(yōu)越性和實用性［5］。時晶晶基于卡爾曼濾波理論分別對非機動目標(biāo)和機動目標(biāo)進行了運動估計，并在此基礎(chǔ)上實現(xiàn)了UUV的動目標(biāo)跟蹤控制［6］。邊信黔等提出一種非線性輸入狀態(tài)反饋來跟蹤軌跡，仿真顯示UUV取得了較好的跟蹤效果［7］。Z.H.Ismail等提出了一種新的基于自適應(yīng)區(qū)域邊界技術(shù)的AUV跟蹤控制［8］。以上文獻都是把UUV和運動目標(biāo)看作質(zhì)點，不考慮母船的外部形狀，所以很可能造成UUV在跟蹤上運動母船的同時和母船的外部特定結(jié)構(gòu)發(fā)生碰撞，最終導(dǎo)致跟蹤任務(wù)失敗。針對現(xiàn)在船體外形結(jié)構(gòu)的高度復(fù)雜：船體艉部有主推進器，將會造成很大的尾流；側(cè)面距離船舶艏舵區(qū)域很近，會對UUV的航行造成不良影響；船體艏部有流線型前導(dǎo)結(jié)構(gòu)［9?10］，在UUV接近母船的過程中必須考慮如何避開這些區(qū)域，因此在母船周圍需要劃定禁航區(qū)。

本文充分考慮到母船外形的復(fù)雜性，在母船艇體周圍劃定4個禁航區(qū)。通過對傳統(tǒng)勢場法改進，除了將UUV，母船和禁航區(qū)的位置信息作為規(guī)劃要素外，加入了3者的速度信息，來解決UUV跟蹤運動母船，同時避開母船艇體附近的禁航區(qū)的問題。

1 母船禁航區(qū)模型的建立

母船上的對接機構(gòu)位于母船艇身的中前部。通過對母船回收UUV過程的分析，在母船艇體周圍共設(shè)置了4個禁航區(qū)是UUV所不能進入的，如圖1所示。

圖1 禁航區(qū)劃定示意圖Fig.1 Sketch of delimited of the prohibited areas

禁航區(qū)1位于母船的艏部的一個圓形區(qū)域，圓心為母船艏部端點處，半徑為r＝50 m，為了防止UUV與運動母船正面相撞，也可以保證回收成功，因為如果從這個區(qū)域接近對接機構(gòu)，不利于UUV調(diào)整艏向以與對接機構(gòu)的方向一致。禁航區(qū)2和禁航區(qū)3位于母船左右舷，并且靠近母船艏部的一個橢圓形區(qū)域，在母船艇體坐標(biāo)系下，其中心的Y向坐標(biāo)與對接機構(gòu)中心坐標(biāo)相同，橢圓的長半軸和短半軸分別為a＝100 m和b＝40 m，可以避免UUV進入母船的艏舵區(qū)域，同時，這個區(qū)域也不利于UUV調(diào)整艏向以與對接機構(gòu)的方向一致。禁航區(qū)4位于母船的艉部的一個圓形區(qū)域，圓心為母船艉部端點處，半徑也為r＝50 m，目的是防止UUV進入母船艉部推進器造成的擾流區(qū)域，保證UUV的運動安全。

2 UUV跟蹤運動母船問題建模

對運動母船的跟蹤屬于平面跟蹤，UUV需要進行運動規(guī)劃以產(chǎn)生跟蹤母船的速度和航向。UUV可以定時的通過USBL（超短基線聲吶）采集母船的位置（母船端USBL單元安裝在對接機構(gòu)上），并且可以通過一定的預(yù)測和估計方法獲得母船的速度和航向信息。首先對UUV跟蹤運動母船的問題進行建模，如圖2所示。

圖2 UUV跟蹤運動母船問題描述Fig.2 The description of UUV tracking the mother ship

下面的符號和標(biāo)識用于問題的描述：XOY：固定坐標(biāo)系；ptar∈R2：母船的位置；vtar∈R2：母船的速度；p∈R2：UUV的位置；v∈R2：UUV的速度；pobs_i∈R2：第i個禁航區(qū)的位置；vobs_i∈R2：第i個禁航區(qū)的速度；robs∈R2：圓形禁航區(qū)的半徑；aobs、bobs∈R2：橢圓形禁航區(qū)的長、短半軸；prt＝ptar-p：UUV到母船的相對位置；pro_i＝pobs_i-p：UUV到第i個禁航區(qū)的相對位置；pot_i＝ptar-pobs_i：第i個禁航區(qū)到母船的相對位置；ψrt：prt的角度；ψro_i：pro_i的角度；ψot_i：pot_i的角度；ψobs_i：vobs_i的角度；ψtar：母船的航向角；ψ：UUV的航向角。

根據(jù)利用勢場法求解問題的一般習(xí)慣，有以下假設(shè)：位置p、ptar、pobs_i以及速度v、vtar、vobs_i均是已知的；禁航區(qū)是凸的，本文考慮禁航區(qū)為圓或橢圓，該假設(shè)是滿足的；UUV、母船和禁航區(qū)均看做質(zhì)點，這里禁航區(qū)看做是以質(zhì)點為中心的圓或橢圓，其運動仍然可以認(rèn)為是質(zhì)點的運動。

下面的假設(shè)是對UUV和母船的運動進行限制：UUV最大的線速度和角速度分別為vmax和rmax；母船的速度被限制為‖vtar‖≤vmax。

設(shè)prt＝［xrtyrt］T、pro_i＝［xro_iyro_i］，UUV和母船之間的相對運動可以描述為

本文首先考慮只有一個禁航區(qū)，所得出的結(jié)論很容易擴展到多個禁航區(qū)，對于多個禁航區(qū)將在最后討論。

UUV和禁航區(qū)之間的相對運動，可以描述為

引力勢場Uatt，斥力勢場Urep及其合勢場U定義為

式中：ρ為UUV和禁航區(qū)之間的最小距離；ρ0＞0稱為禁航區(qū)的斥力作用距離；ξ1＞0、ξ2＞0是引力場和斥力場的權(quán)重因子。

在傳統(tǒng)的勢場法中，母船速度vtar一般是假設(shè)為0，且UUV的速度v是沿著相對于p的合勢場U的負(fù)梯度方向的，即：

對于一個半徑robs為圓形的禁航區(qū)，有

式中：η＝ξ2（ρ-1-ρ-10）‖pro‖。

對于橢圓形的禁航區(qū)，根據(jù)式（7），也可以很容易的定義ρ。在式（8）中，UUV的速度v是prt和pro的函數(shù)。但是對于UUV跟蹤運動母船的同時規(guī)避動禁航區(qū)，UUV的速度v除了與prt和pro有關(guān)外，還要考慮母船的速度vtar和禁航區(qū)的速度vobs。

3 UUV跟蹤運動母船時的運動規(guī)劃

首先研究UUV在禁航區(qū)斥力作用距離外（ρ＞ρ0）的運動規(guī)劃，然后研究UUV在禁航區(qū)斥力作用距離內(nèi)（ρ＜ρ0）的運動規(guī)劃。最后，將研究結(jié)果擴展到多禁航區(qū)時的UUV運動規(guī)劃。

3.1 禁航區(qū)不起作用時的UUV運動規(guī)劃

當(dāng)UUV在禁航區(qū)斥力作用距離外時，式（3）中只有引力場Uatt對UUV的速度規(guī)劃起作用。UUV的速度v需要使p·rt在相對于prt的引力勢場Uatt的負(fù)梯度方向：

由式（3）和上式有：v＝vtar+ξ1prt，相當(dāng)于有下面的三角關(guān)系成立：

假設(shè)vtar≠0且‖prt‖≠0。UUV的航向可以由式（10）、（11）求得：

在ξ1＞0時，由式（11）和式（12）規(guī)劃出的UUV速度可以使‖ptr‖→0且v→vtar。

母船速度在UUV速度范圍之內(nèi)時，UUV可以很好的跟蹤運動母船。這個可以通過圖3說明。當(dāng)母船從UUV的方向運動，‖v‖應(yīng)該比‖vtar‖大，這樣UUV才能跟蹤上母船；當(dāng)母船從UUV的方向或π運動，比較小的‖v‖就可以使UUV跟蹤上母船。

但利用式（11）和式（12）規(guī)劃UUV的速度時，以下因素需要考慮：

1）‖prt‖→0時的速度收斂。求Uatt對時間t的導(dǎo)數(shù)，并利用式（9）可以進一步獲得

顯然，ξ1越大，Uatt和prt收斂到0的速度越快，同時ξ1也受限于UUV的最大線速度和最大角速度。

2）UUV的最大線速度?？紤]式（11）和UUV的最大線速度vmax，有

規(guī)劃角度ψ仍然由式（12）確定。如圖4所示，由式（12）和式（15）確定的UUV的規(guī)劃速度仍然可以使p·rt指向prt的反方向。prt收斂于0仍然可以保證，只是比不限值的速度‖v‖的速度要小。

圖3 UUV和母船相對方向及對v的影響Fig.3 The relative direction between UUV and mother ship and their effects on v

圖4 UUV最大線速度下的速度約束Fig.4 The constraint of velocity under the highest line?ar velocity of UUV

3）UUV最大角速度。將式（12）擴展為

考慮式（11）中的‖v‖，并且由式（16），有

可知，權(quán)重因子ξ1直接影響和對的貢獻。將ψrt＝arctan（yrt／xrt）對時間t求導(dǎo)，并結(jié)合式（1）得到

則

綜合以上的討論，可以得到以下的定理：

定理1 UUV跟蹤速度為vtar的運動母船，UUV的航速和航向可以規(guī)劃為

3.2 禁航區(qū)起作用時的UUV運動規(guī)劃

禁航區(qū)對UUV起作用時，引力勢場Uatt和斥力勢場Urep都作用在UUV上，由式（15）可得UUV合勢場為

由式（22），并且注意到ρ＝‖pro‖-robs＝‖prtpot‖-robs和p·rt≠0，可以得到以下任意一式：

式（24）中需要‖v‖≤‖vtar‖或者‖v‖＝0，而這不滿足假設(shè)中的條件。因而，式（24）是不成立的，只能研究式（23）?？紤]式（23），并注意到xrt＝‖prt‖cos ψrt，yrt＝‖prt‖sin ψrt，xro＝‖pro‖cos ψro，yro＝‖pro‖sin ψro，可以得到：

假設(shè)‖vtar‖≠0且‖v‖≠0，上式可以改寫為

將式（21）中的U對時間t求導(dǎo)，得到：

由式（27）～（29），可以得到：

設(shè)‖v‖＝［（‖vtar‖cos（ψtar-ψrt）-β‖vobs‖cos（ψobs-ψro）+ξ1‖prt‖）2+，并代入式（29），可以得到：＝-2ξ1Uatt＜0，由于U≥0且＜0，所以U是有界的。注意到只有在‖prt‖＝0時，＝0，因而當(dāng)t→∞時，‖prt‖→0。

由此可以看出，禁航區(qū)不起作用是禁航區(qū)起作用的特例，設(shè)置η＝0，就可以得到禁航區(qū)不起作用的情況。

上面的討論結(jié)果可以總結(jié)為下面的定理：

定理2：UUV跟蹤速度為vtar的運動母船，同時規(guī)避相對位置為pro速度為vro的禁航區(qū)，UUV的速度和航向可以規(guī)劃為

式中：ξ2＞0為權(quán)重因子。

推論1 定理2可以很容易的擴展到多個作用距離為ρ0的禁航區(qū)。假設(shè)考慮到UUV在禁航區(qū)作用距離內(nèi)時，UUV運動規(guī)劃的勢場為

式中：ρi＝proi-robsi，proi是UUV到第i個禁航區(qū)的距離，robsi是第i個禁航區(qū)的半徑，對于橢圓形禁航區(qū)由長軸和短軸所共同起作用的距離。

將上式代入式（22），可以得到：

進行和定理2一樣的分析步驟，UUV的速度為

式中：‖vobsi‖和ψobsi是第i個禁航區(qū)的速度大小和航向。

4 仿真驗證

仿真設(shè)計了實際回收UUV過程中母船以運動場方式運動的場景。UUV的最小速度設(shè)定為1 kn，最大速度設(shè)定為4 kn，最大偏轉(zhuǎn)角速度10（°）／s，初始位置為（100，-300），母船的初始位置為（0，0），接近圓半徑為10 m，母船運動軌跡中的航向和速度按以下2式變化：

圖5給出了母船以運動場的方式運動的場景中，UUV對母船跟蹤過程中不同控制節(jié)拍的情況。

圖5 UUV對母船跟蹤仿真案例Fig.5 Simulation case for UUV tracking the mother ship

圖5 （a）和（b）給出了在T＝80，T＝110的仿真結(jié)果，在T＝80時，UUV探測到母船右舷的禁航區(qū)，并且開始做出規(guī)避動作。在T＝110時，UUV完成對母船禁航區(qū)的規(guī)避，并且開始基本按照母船的軌跡跟蹤母船。圖5（c）給出T＝400時的仿真跟蹤結(jié)果。在T＝400時，母船處于轉(zhuǎn)彎階段時，UUV跟蹤上母船，進入以母船上對接機構(gòu)為中心的接近圓內(nèi)，并始終保持對母船的跟蹤。圖5（d）給出整個仿真過程，顯示UUV在跟蹤上母船之后的過程中基本沿著母船的軌跡始終保持對母船的跟蹤。

4 結(jié)論

針對UUV對運動母船跟蹤問題，考慮了母船外部形狀的復(fù)雜性，劃定了4個艇體禁航區(qū)，并對傳統(tǒng)勢場法規(guī)劃方法進行了改進，除了將UUV，母船和禁航區(qū)的位置信息作為規(guī)劃要素外，加入了三者的速度信息，達到了UUV跟蹤運動母船的同時規(guī)避運動的禁航區(qū)的目的。由于考慮了母船的外部形狀，劃定了禁航區(qū)，相比于傳統(tǒng)的目標(biāo)跟蹤方法，大大地提高了跟蹤過程的安全性。

［1］張偉，徐達，王南南，等.基于對線控位策略的UUV回收運動控制研［J］.船舶工程，2013，35（5）：64?67.ZHANG Wei，XU Da，WANG Nannan，et al.Research on recovery motion control of UUV based on dynamic positio?ning to line method［J］.Ship Engineering，2013，35（5）：64?67.

［2］王宏健，陳子印，邊信黔，等.欠驅(qū)動水下航行器三維直線航跡跟蹤控制［J］.控制理論與應(yīng)用，2013，4：443?453.WANG Hongjian，CHEN Ziyin，BIAN Xinqian，et al.Three dimensional straight line path tracking control for under actu?ated underwater vehicle［J］.Control Theory and Applica?tions，2013，4：443?453.

［3］YAN Zheping，YU Haomiao，LI Benyin，et al.Sliding mode trajectory tracking of underactuated UUV on dive plane［C］／／Chinese Control Conference.［S.l.］，2014：7909?7914.

［4］SUJIT P B，HEALEY A J，SOUSA J B.AUV docking on a moving submarine using a K?R navigation function［C］／／2011 IEEE／RSJ International Conference on Intelligent Ro?bots and Systems.［S.l.］，2011：3154?3159.

［5］劉斌，馬曉川，候朝煥.針對高速自治水下航行器的UKF主動目標(biāo)跟蹤算法［J］.系統(tǒng)仿真學(xué)報，2008，20（4）：947?955.LIU Bin，MA Xiaochuan，HOU Chaohuan.A UKF?based active target tracking algorithm for highspeed autonomous un?derwater vehicles［J］.Journal of System Simulation，2008，20（4）：947?955.

［6］時晶晶.小型自主水下航行器目標(biāo)跟蹤方法研究［M］.哈爾濱：哈爾濱工程大學(xué)，2012：56?70.SHI Jingjing.Research on target tracking control method of micro autonomous underwater vehicle［M］.Harbin：Harbin Engineering University，2012：56?70.

［7］BIAN Xinqian，QU Ying，YAN Zheping.Nonlinear feedback control for trajectory tracking of an unmanned underwater ve?hicle［C］／／IEEE International Conference on Information and Automation.Harbin，China，2010：1387?1392.

［8］ISMAIL Z H，MOKHAR B M，DUNNIGAN M W.Tracking control for an autonomous underwater vehicke based on mul?tiplicative potential energy function［J］.Oceans，2012，39：289?296.

［9］賈傳熒.船舶碰撞危險度及碰撞決策模型的研究［M］.大連：大連海事大學(xué)，2000：89?100.JIA Chuanying.Study on ship's collision risk index and colli?sion avoidance modle［M］.Dalian Maritime University，2000：89?100.

［10］劉可峰.船舶型線建模系統(tǒng)研究［M］.哈爾濱：哈爾濱工程大學(xué)，2005：103?110.LIU Kefeng.Research of hull lines modeling system［M］.Harbin：Harbin Engineering University，2005：103?110.

An approach for UUV tracking the moving mothership in the recovery stage

ZHANG Wei，ZHANG Mingchen，GUO Yi，CHEN Tao，YAN Zheping

（College of Automation，Harbin Engineering University，Harbin 150001，China）

To make an unmanned underwater vehicle（UUV）better track the moving mother ship，an artificial po?tential field approach with inputted speed which is adapt to the movement characteristic of mother ship was pro?posed.Considering the complexity of mother ship's outside shape，four prohibited areas were delimited around mothership to guarantee the safety of tracking process.During the motion planning process，an improved potential field approach was achieved by treating the position and speed information of UUV，mothership and prohibited areas as the planning element.Utilizing the improved potential field approach，the speed and direction equations of UUV were obtained.Finally，the simulation experiment of UUV tracking mothership has been done on the working condi?tion that the mothership moves in the motion field way.The simulation results show that the motion planning method can make the UUV track the moving mothership safely in the process of underwater autonomous recovery.

unmanned underwater vehicle；moving mother ship；tracking；prohibited areas；autonomous recovery

10.3969／j.issn.1006?7043.201406062

TP274

：A

：1006?7043（2015）06?0795?06

http：／／www.cnki.net／kcms／detail／23.1390.u.20150504.1034.001.html

2014?06?24.網(wǎng)絡(luò)出版時間：2015?05?04.

國家自然科學(xué)基金資助項目（51309067／E091002）；中央高校基本科研業(yè)務(wù)費專項資金資助項目（HEUCFX41402）；中央高?；究蒲袠I(yè)務(wù)費水下機器人重點實驗室資助項目（HEUCF1002142）.

張偉（1978?）男，副教授，博士生導(dǎo)師；陳濤（1983?），男，講師，博士.

陳濤，E?mail：chentao_7777＠163.com.