一种回收过程中UUV对运动母船的跟踪方法

张伟，张明臣，郭毅，陈涛，严浙平

（哈尔滨工程大学自动化学院，黑龙江哈尔滨150001）

一种回收过程中UUV对运动母船的跟踪方法

张伟，张明臣，郭毅，陈涛，严浙平

（哈尔滨工程大学自动化学院，黑龙江哈尔滨150001）

在无人水下航行器（UUV）对运动母船的跟踪过程中，跟踪方法需要适应母船的运动特性，为此该文提出了引入速度量的人工势场法。为了保证跟踪过程安全可靠，考虑到母船外部形状的复杂性，在母船周围划定四个禁航区。在运动规划过程中除了将UUV、母船和禁航区的位置信息作为规划要素外，加入了三者的速度信息，得到改进的势场法。基于改进的势场法得到UUV的速度和航向方程，并在母船以运动场方式运动的工况下进行了UUV跟踪母船的仿真实验，结果表明所设计的运动规划方法能够实现UUV水下自主回收作业中对运动母船的安全跟踪。

无人水下航行器；运动母船；跟踪；禁航区；自主回收

UUV在回收任务中回坞阶段需要跟踪运动母船并到达特定的对接地点［1］，因此使UUV安全地跟踪上运动母船具有很大的研究意义［2⁃4］。刘斌等针对高速UUV提出了基于UKF（unscented Kalman fil⁃ter，UKF）的动目标跟踪算法，仿真表明基于UKF的动目标跟踪算法有较好的优越性和实用性［5］。时晶晶基于卡尔曼滤波理论分别对非机动目标和机动目标进行了运动估计，并在此基础上实现了UUV的动目标跟踪控制［6］。边信黔等提出一种非线性输入状态反馈来跟踪轨迹，仿真显示UUV取得了较好的跟踪效果［7］。Z.H.Ismail等提出了一种新的基于自适应区域边界技术的AUV跟踪控制［8］。以上文献都是把UUV和运动目标看作质点，不考虑母船的外部形状，所以很可能造成UUV在跟踪上运动母船的同时和母船的外部特定结构发生碰撞，最终导致跟踪任务失败。针对现在船体外形结构的高度复杂：船体艉部有主推进器，将会造成很大的尾流；侧面距离船舶艏舵区域很近，会对UUV的航行造成不良影响；船体艏部有流线型前导结构［9⁃10］，在UUV接近母船的过程中必须考虑如何避开这些区域，因此在母船周围需要划定禁航区。

本文充分考虑到母船外形的复杂性，在母船艇体周围划定4个禁航区。通过对传统势场法改进，除了将UUV，母船和禁航区的位置信息作为规划要素外，加入了3者的速度信息，来解决UUV跟踪运动母船，同时避开母船艇体附近的禁航区的问题。

1 母船禁航区模型的建立

母船上的对接机构位于母船艇身的中前部。通过对母船回收UUV过程的分析，在母船艇体周围共设置了4个禁航区是UUV所不能进入的，如图1所示。

图1 禁航区划定示意图Fig.1 Sketch of delimited of the prohibited areas

禁航区1位于母船的艏部的一个圆形区域，圆心为母船艏部端点处，半径为r＝50 m，为了防止UUV与运动母船正面相撞，也可以保证回收成功，因为如果从这个区域接近对接机构，不利于UUV调整艏向以与对接机构的方向一致。禁航区2和禁航区3位于母船左右舷，并且靠近母船艏部的一个椭圆形区域，在母船艇体坐标系下，其中心的Y向坐标与对接机构中心坐标相同，椭圆的长半轴和短半轴分别为a＝100 m和b＝40 m，可以避免UUV进入母船的艏舵区域，同时，这个区域也不利于UUV调整艏向以与对接机构的方向一致。禁航区4位于母船的艉部的一个圆形区域，圆心为母船艉部端点处，半径也为r＝50 m，目的是防止UUV进入母船艉部推进器造成的扰流区域，保证UUV的运动安全。

2 UUV跟踪运动母船问题建模

对运动母船的跟踪属于平面跟踪，UUV需要进行运动规划以产生跟踪母船的速度和航向。UUV可以定时的通过USBL（超短基线声呐）采集母船的位置（母船端USBL单元安装在对接机构上），并且可以通过一定的预测和估计方法获得母船的速度和航向信息。首先对UUV跟踪运动母船的问题进行建模，如图2所示。

图2 UUV跟踪运动母船问题描述Fig.2 The description of UUV tracking the mother ship

下面的符号和标识用于问题的描述：XOY：固定坐标系；ptar∈R2：母船的位置；vtar∈R2：母船的速度；p∈R2：UUV的位置；v∈R2：UUV的速度；pobs_i∈R2：第i个禁航区的位置；vobs_i∈R2：第i个禁航区的速度；robs∈R2：圆形禁航区的半径；aobs、bobs∈R2：椭圆形禁航区的长、短半轴；prt＝ptar-p：UUV到母船的相对位置；pro_i＝pobs_i-p：UUV到第i个禁航区的相对位置；pot_i＝ptar-pobs_i：第i个禁航区到母船的相对位置；ψrt：prt的角度；ψro_i：pro_i的角度；ψot_i：pot_i的角度；ψobs_i：vobs_i的角度；ψtar：母船的航向角；ψ：UUV的航向角。

根据利用势场法求解问题的一般习惯，有以下假设：位置p、ptar、pobs_i以及速度v、vtar、vobs_i均是已知的；禁航区是凸的，本文考虑禁航区为圆或椭圆，该假设是满足的；UUV、母船和禁航区均看做质点，这里禁航区看做是以质点为中心的圆或椭圆，其运动仍然可以认为是质点的运动。

下面的假设是对UUV和母船的运动进行限制：UUV最大的线速度和角速度分别为vmax和rmax；母船的速度被限制为‖vtar‖≤vmax。

设prt＝［xrtyrt］T、pro_i＝［xro_iyro_i］，UUV和母船之间的相对运动可以描述为

本文首先考虑只有一个禁航区，所得出的结论很容易扩展到多个禁航区，对于多个禁航区将在最后讨论。

UUV和禁航区之间的相对运动，可以描述为

引力势场Uatt，斥力势场Urep及其合势场U定义为

式中：ρ为UUV和禁航区之间的最小距离；ρ0＞0称为禁航区的斥力作用距离；ξ1＞0、ξ2＞0是引力场和斥力场的权重因子。

在传统的势场法中，母船速度vtar一般是假设为0，且UUV的速度v是沿着相对于p的合势场U的负梯度方向的，即：

对于一个半径robs为圆形的禁航区，有

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

对于椭圆形的禁航区，根据式（7），也可以很容易的定义ρ。在式（8）中，UUV的速度v是prt和pro的函数。但是对于UUV跟踪运动母船的同时规避动禁航区，UUV的速度v除了与prt和pro有关外，还要考虑母船的速度vtar和禁航区的速度vobs。

3 UUV跟踪运动母船时的运动规划

首先研究UUV在禁航区斥力作用距离外（ρ＞ρ0）的运动规划，然后研究UUV在禁航区斥力作用距离内（ρ＜ρ0）的运动规划。最后，将研究结果扩展到多禁航区时的UUV运动规划。

3.1 禁航区不起作用时的UUV运动规划

当UUV在禁航区斥力作用距离外时，式（3）中只有引力场Uatt对UUV的速度规划起作用。UUV的速度v需要使p·rt在相对于prt的引力势场Uatt的负梯度方向：

由式（3）和上式有：v＝vtar+ξ1prt，相当于有下面的三角关系成立：

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

在ξ1＞0时，由式（11）和式（12）规划出的UUV速度可以使‖ptr‖→0且v→vtar。

母船速度在UUV速度范围之内时，UUV可以很好的跟踪运动母船。这个可以通过图3说明。当母船从UUV的方向运动，‖v‖应该比‖vtar‖大，这样UUV才能跟踪上母船；当母船从UUV的方向或π运动，比较小的‖v‖就可以使UUV跟踪上母船。

但利用式（11）和式（12）规划UUV的速度时，以下因素需要考虑：

1）‖prt‖→0时的速度收敛。求Uatt对时间t的导数，并利用式（9）可以进一步获得

显然，ξ1越大，Uatt和prt收敛到0的速度越快，同时ξ1也受限于UUV的最大线速度和最大角速度。

2）UUV的最大线速度。考虑式（11）和UUV的最大线速度vmax，有

规划角度ψ仍然由式（12）确定。如图4所示，由式（12）和式（15）确定的UUV的规划速度仍然可以使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），有

可知，权重因子ξ1直接影响和对的贡献。将ψrt＝arctan（yrt／xrt）对时间t求导，并结合式（1）得到

则

综合以上的讨论，可以得到以下的定理：

定理1 UUV跟踪速度为vtar的运动母船，UUV的航速和航向可以规划为

3.2 禁航区起作用时的UUV运动规划

禁航区对UUV起作用时，引力势场Uatt和斥力势场Urep都作用在UUV上，由式（15）可得UUV合势场为

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

式（24）中需要‖v‖≤‖vtar‖或者‖v‖＝0，而这不满足假设中的条件。因而，式（24）是不成立的，只能研究式（23）。考虑式（23），并注意到xrt＝‖prt‖cos ψrt，yrt＝‖prt‖sin ψrt，xro＝‖pro‖cos ψro，yro＝‖pro‖sin ψro，可以得到：

假设‖vtar‖≠0且‖v‖≠0，上式可以改写为

将式（21）中的U对时间t求导，得到：

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

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

由此可以看出，禁航区不起作用是禁航区起作用的特例，设置η＝0，就可以得到禁航区不起作用的情况。

上面的讨论结果可以总结为下面的定理：

定理2：UUV跟踪速度为vtar的运动母船，同时规避相对位置为pro速度为vro的禁航区，UUV的速度和航向可以规划为

式中：ξ2＞0为权重因子。

推论1 定理2可以很容易的扩展到多个作用距离为ρ0的禁航区。假设考虑到UUV在禁航区作用距离内时，UUV运动规划的势场为

式中：ρi＝proi-robsi，proi是UUV到第i个禁航区的距离，robsi是第i个禁航区的半径，对于椭圆形禁航区由长轴和短轴所共同起作用的距离。

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

进行和定理2一样的分析步骤，UUV的速度为

式中：‖vobsi‖和ψobsi是第i个禁航区的速度大小和航向。

4 仿真验证

仿真设计了实际回收UUV过程中母船以运动场方式运动的场景。UUV的最小速度设定为1 kn，最大速度设定为4 kn，最大偏转角速度10（°）／s，初始位置为（100，-300），母船的初始位置为（0，0），接近圆半径为10 m，母船运动轨迹中的航向和速度按以下2式变化：

图5给出了母船以运动场的方式运动的场景中，UUV对母船跟踪过程中不同控制节拍的情况。

图5 UUV对母船跟踪仿真案例Fig.5 Simulation case for UUV tracking the mother ship

图5 （a）和（b）给出了在T＝80，T＝110的仿真结果，在T＝80时，UUV探测到母船右舷的禁航区，并且开始做出规避动作。在T＝110时，UUV完成对母船禁航区的规避，并且开始基本按照母船的轨迹跟踪母船。图5（c）给出T＝400时的仿真跟踪结果。在T＝400时，母船处于转弯阶段时，UUV跟踪上母船，进入以母船上对接机构为中心的接近圆内，并始终保持对母船的跟踪。图5（d）给出整个仿真过程，显示UUV在跟踪上母船之后的过程中基本沿着母船的轨迹始终保持对母船的跟踪。

4 结论

针对UUV对运动母船跟踪问题，考虑了母船外部形状的复杂性，划定了4个艇体禁航区，并对传统势场法规划方法进行了改进，除了将UUV，母船和禁航区的位置信息作为规划要素外，加入了三者的速度信息，达到了UUV跟踪运动母船的同时规避运动的禁航区的目的。由于考虑了母船的外部形状，划定了禁航区，相比于传统的目标跟踪方法，大大地提高了跟踪过程的安全性。

［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］王宏健，陈子印，边信黔，等.欠驱动水下航行器三维直线航迹跟踪控制［J］.控制理论与应用，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主动目标跟踪算法［J］.系统仿真学报，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］时晶晶.小型自主水下航行器目标跟踪方法研究［M］.哈尔滨：哈尔滨工程大学，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］.大连：大连海事大学，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］刘可峰.船舶型线建模系统研究［M］.哈尔滨：哈尔滨工程大学，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.网络出版时间：2015⁃05⁃04.

国家自然科学基金资助项目（51309067／E091002）；中央高校基本科研业务费专项资金资助项目（HEUCFX41402）；中央高校基本科研业务费水下机器人重点实验室资助项目（HEUCF1002142）.

张伟（1978⁃）男，副教授，博士生导师；陈涛（1983⁃），男，讲师，博士.

陈涛，E⁃mail：chentao_7777＠163.com.