CN104020776B

CN104020776B - The tracking of the UUV motion lash ship to having multiple prohibited area

Info

Publication number: CN104020776B
Application number: CN201410264213.5A
Authority: CN
Inventors: 张伟; 张明臣; 徐达; 边信黔; 李本银; 周佳加
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2014-06-13
Filing date: 2014-06-13
Publication date: 2016-08-24
Anticipated expiration: 2034-06-13
Also published as: CN104020776A

Abstract

The invention discloses a UUV tracking method for a moving mother ship with multiple no-navigation areas. It includes the following steps: UUV uses communication sonar to detect the position _ptar and velocity v _tar of the moving mother ship, and transmits the preset mother ship no-navigation area information to the UUV to obtain the position p _{obs_i} and velocity v _{obs_i} of N no-navigation areas; Establish the relative motion of the UUV, the moving mother ship and the no-navigation area; plan the speed and heading of the UUV in the next step; the UUV executes the next speed and heading command, and judge whether the distance between the UUV and the moving mother ship is less than the threshold K, if not less than the threshold, repeat Step 1 to step 4, if it is less than the threshold, complete the UUV tracking of the moving mother ship. In addition to the location information of the UUV, the mother ship and the no-navigation area as planning elements, the present invention adds the speed information of the three to achieve the purpose of UUV tracking the moving mother ship while avoiding the moving no-navigation area, and has good control performance and real-time sex.

Description

UUV tracking method for moving mother ship with multiple no-navigation zones

技术领域technical field

本发明属于UUV对运动母船的跟踪方法，尤其涉及一种UUV对拥有多个禁航区的运动母船的跟踪方法。The invention belongs to a UUV tracking method for a moving mother ship, in particular to a UUV tracking method for a moving mother ship with multiple no-navigation areas.

背景技术Background technique

无人水下航行器UUV(Unmanned Underwater Vehicle)作为人类在海洋活动，特别是深海活动中的重要替代者和执行者，已被广泛应用于科学考察，深海作业，打捞救生等领域。无人水下航行器完成任务之后，需要返回母船进行数据回放，下载新的使命任务等工作，因此航行器的回收作业是不可缺少的过程。航行器回收作业中的回邬阶段是很重要的阶段，在该阶段中如何能快速安全地实现对带有禁航区的运动母船的跟踪是很重要的问题。Unmanned underwater vehicle UUV (Unmanned Underwater Vehicle), as an important substitute and executor for human beings in marine activities, especially in deep sea activities, has been widely used in scientific investigation, deep sea operations, salvage and lifesaving and other fields. After the unmanned underwater vehicle completes the mission, it needs to return to the mother ship for data playback, download new mission tasks, etc. Therefore, the recovery of the vehicle is an indispensable process. The recovery stage in the aircraft recovery operation is a very important stage. In this stage, how to quickly and safely realize the tracking of the moving mother ship with a no-navigation zone is a very important issue.

国内外众多学者对运动目标的跟踪问题进行了深入的研究，提出了许多有效的跟踪方法，比如UKF算法，粒子滤波方法，人工势场算法等，人工势场法，计算简单，实时性好，对传感器要求低，比如利用声呐传感器就可以完成对障碍的测量，今年得到了广泛的研究和应用，特别适合航行器在未知动态环境中的运动规划。但是传统的人工势场法仅仅是利用局部信息进行规划，不可避免的存在一些缺陷和漏洞：陷入局部最优解问题，即航行器在某些情况下不可达；窄道路径震荡问题，距离太近的障碍物无法得到有效路径的问题等，并且对于运动的物体很难实现避碰与跟踪。Many scholars at home and abroad have conducted in-depth research on the tracking of moving targets, and proposed many effective tracking methods, such as UKF algorithm, particle filter method, artificial potential field algorithm, etc. The artificial potential field method has simple calculation and good real-time performance. Low requirements for sensors, such as the use of sonar sensors to complete the measurement of obstacles, have been widely researched and applied this year, and are especially suitable for the motion planning of aircraft in unknown dynamic environments. However, the traditional artificial potential field method only uses local information for planning, and there are inevitably some defects and loopholes: it falls into the local optimal solution problem, that is, the aircraft is unreachable in some cases; the narrow path path oscillation problem, the distance is too large Near obstacles cannot obtain effective paths, etc., and it is difficult to achieve collision avoidance and tracking for moving objects.

发明内容Contents of the invention

本发明的目的是提供一种能够实现对运动母船跟踪的同时规避其周围禁航区的，UUV对拥有多个禁航区的运动母船的跟踪方法。The object of the present invention is to provide a tracking method for a UUV to a mobile mother ship with multiple no-navigation areas, which can realize the tracking of the mobile mother ship while avoiding the no-go zone around it.

本发明是通过以下技术方案实现的：The present invention is achieved through the following technical solutions:

UUV对拥有多个禁航区的运动母船的跟踪方法，包括以下几个步骤：The tracking method of the UUV to the mobile mother ship with multiple no-navigation areas includes the following steps:

步骤一：UUV利用通讯声纳探测运动母船的位置p_tar和速度v_tar，将预设的母船禁航区信息输送给UUV，得到N个禁航区的位置p_{obs_i}和速度v_{obs_i}，i＝1，2，···N；Step 1: The UUV uses communication sonar to detect the position p _tar and velocity v _tar of the moving mother ship, and transmits the preset mother ship no-navigation area information to the UUV to obtain the position p _{obs_i} and velocity v _{obs_i} of N no-navigation areas, i= 1, 2, ... N;

步骤二：建立UUV和运动母船以及禁航区的相对运动；Step 2: Establish the relative motion of UUV, moving mother ship and no-navigation zone;

步骤三：由UUV和运动母船以及禁航区的相对运动，规划UUV下一步的速度和航向；Step 3: Based on the relative motion of the UUV, the moving mother ship and the no-navigation zone, plan the next speed and heading of the UUV;

步骤四：UUV执行下一步的速度和航向指令，判断UUV到运动母船的距离是否小于阈值K，如果不小于阈值，重复步骤一～步骤四，如果小于阈值，完成UUV对运动母船的跟踪。Step 4: UUV executes the next speed and heading command, and judges whether the distance from UUV to the moving mother ship is less than the threshold K. If it is not less than the threshold, repeat steps 1 to 4. If it is less than the threshold, complete the UUV’s tracking of the moving mother ship.

本发明UUV对拥有多个禁航区的运动母船的跟踪方法还可以包括：The tracking method of the UUV of the present invention to the moving mother ship with multiple no-navigation areas can also include:

1、UUV和运动母船的相对运动为：1. The relative motion of UUV and moving mother ship is:

${\overset{\cdot &Center Dot;}{p p}}_{rt rt} = = {[\begin{matrix} {\overset{\cdot \cdot}{x x}}_{rt rt} & {\overset{\cdot &Center Dot;}{y the y}}_{rt rt} \end{matrix}]}^{T T}$

其中：in:

${\overset{\cdot \cdot}{x x}}_{rt rt} = = | | | | {v v}_{tar tar} | | | | {cos cos ψ ψ}_{tar tar} - - | | | | v v | | | | cos cos ψ ψ$

${\overset{\cdot &Center Dot;}{y the y}}_{rt rt} = = | | | | {v v}_{tar tar} | | | | {sin sin ψ ψ}_{tar tar} - - | | | | v v | | | | sin sin ψ ψ$

v为UUV当前的速度，ψ_tar为运动母船的航向角，ψ为UUV当前的航向角，v is the current speed of the UUV, ψ _tar is the heading angle of the moving mother ship, and ψ is the current heading angle of the UUV,

UUV和第i个禁航区的相对运动：Relative motion of UUV and i-th no-go zone:

${\overset{\cdot \cdot}{p p}}_{ro ro__i i} = = {[\begin{matrix} {\overset{\cdot &Center Dot;}{x x}}_{ro ro__i i} & {\overset{\cdot &Center Dot;}{y the y}}_{ro ro__i i} \end{matrix}]}^{T T}$

其中：in:

${\overset{\cdot &Center Dot;}{x x}}_{ro ro__i i} = = | | | | {v v}_{obs obs__i i} | | | | {cos cos ψ ψ}_{obs obs__i i} - - | | | | v v | | | | cos cos ψ ψ$

${\overset{\cdot &Center Dot;}{y the y}}_{ro ro__i i} = = | | | | {v v}_{obs obs__i i} | | | | {sin sin ψ ψ}_{obs obs__i i} - - | | | | v v | | | | sin sin ψ ψ$

ψ_{obs_i}为第i个禁航区的速度的角度。ψ _{obs_i} is the angle of the velocity of the i-th no-navigation zone.

2、规划UUV下一步的速度和航向的方法为：2. The method of planning the speed and heading of the UUV in the next step is:

(一)由UUV和运动母船以及禁航区的相对运动，建立UUV和运动母船的引力势场、UUV和禁航区的斥力势场；(1) Establish the gravitational potential field between the UUV and the moving mother ship and the repulsive potential field between the UUV and the moving mother ship and the no-navigation area;

UUV和运动母船的引力势场U_att：The gravitational potential field U _att of the UUV and the moving mother ship:

${U u}_{att att} = = \frac{11}{22} {ξ ξ}_{11} {p p}_{rt rt}^{T T} {p p}_{rt rt}$

UUV和第i个禁航区的斥力势场U_{rep_i}：UUV and the repulsion potential field U _{rep_i} of the ith no-navigation zone:

${U u}_{rep rep} = = \{\begin{matrix} \frac{11}{22} {ξ ξ}_{22} {(({ρ ρ}_{i i}^{- - 11} - - {ρ ρ}_{00}^{- - 11}))}^{22} & if if,, {ρ ρ}_{i i} \leq \leq {ρ ρ}_{00} \\ 00 & else else \end{matrix}$

ρ_i表示UUV和第i个禁航区之间的最小距离，ρ₀为每个禁航区的斥力作用距离，ξ₁和ξ₂分别是引力势场和斥力势场的权重因子；ρi represents the minimum distance between the UUV and the _i -th no-navigation zone, ρ0 is the repulsive distance of each no _- navigation zone, and _ξ1 and _ξ2 are the weight factors of the gravitational potential field and the repulsive potential field, respectively;

(二)如果UUV和每个禁航区的最小距离均大于或等于ρ₀，选择情况一规划UUV下一步的速度和航向，UUV和至少一个禁航区的最小距离小于ρ₀，选择情况二规划UUV下一步的速度和航向，(2) If the minimum distance between the UUV and each no-navigation area is greater than or equal to ρ ₀ , choose case 1 to plan the speed and heading of the UUV for the next step, and if the minimum distance between the UUV and at least one no-navigation area is less than ρ ₀ , choose case 2 Plan the speed and heading of the UUV for the next step,

情况一：只有引力势场U_att对规划UUV下一步的速度和航向起作用，Situation 1: only the gravitational potential field U _att plays a role in planning the speed and heading of the UUV in the next step,

UUV下一步的速度v′为：The speed v' of the UUV in the next step is:

$| | | | {v v}^{' '} | | | | = = min min {(((({| | | | {v v}_{tar tar} | | | |}^{22} + + {22 ξ ξ}_{11} | | | | {p p}_{rt rt} | | | | | | | | {v v}_{tar tar} | | | | \times \times cos cos (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt})))) + + {ξ ξ}_{11}^{22} {| | | | {p p}_{rt rt} | | | |}^{22}))}^{\frac{11}{22}},, {v v}_{max max}))$

其中，v_max为UUV的最大速度，ψ_rt为UUV和运动母船的相对运动p_rt的角度，Among them, v _max is the maximum velocity of UUV, ψ _rt is the angle of relative motion p _rt of UUV and moving mother ship,

UUV下一步的航向ψ′为：The heading ψ′ of the UUV in the next step is:

${ψ ψ}^{' '} = = {ψ ψ}_{rt rt} + + arcsin arcsin ((\frac{| | | | {v v}_{tar tar} | | | | sin sin (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt}))}{| | | | v v | | | |}))$

情况二：如果UUV和第i个禁航区的最小距离小于ρ₀，那么第i个禁航区为当前对UUV起斥力作用的禁航区，得到当前对UUV起斥力作用的禁航区的个数为j个，j＝(1，···N)，j个禁航区的斥力势场对规划UUV下一步的速度和航向起作用，Case 2: If the minimum distance between the UUV and the i-th no-navigation zone is less than ρ ₀ , then the i-th no-navigation zone is the no-navigation zone that currently acts as a repulsive force on the UUV, and the current value of the no-navigation zone that acts as a repulsive force on the UUV is obtained The number is j, j=(1,...N), and the repulsive potential fields of j no-navigation areas play a role in planning the speed and heading of the UUV in the next step,

UUV下一步的速度v′为：The speed v' of the UUV in the next step is:

$\begin{matrix} | | | | {v v}^{' '} | | | | = = [[((| | | | {v v}_{tar tar} | | | | cos cos (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt})) - - {Σ Σ}_{j j = = 11}^{n no} {β β}_{j j} | | | | {&upsi; &upsi;}_{obs obs__j j} | | | | cos cos (({ψ ψ}_{obs obs__j j} - - {ψ ψ}_{ro ro__j j})) \\ {+ + {{ξ ξ}_{11} | | | | {p p}_{rt rt} | | | |))}^{22} + + {| | | | {&upsi; &upsi;}_{tar tar} | | | |}^{22} {sin sin}^{22} (({ψ ψ}_{tar tar} - - {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt}))]]}^{\frac{11}{22}} \end{matrix}$

其中v_{obs_j}和ψ_{obs_j}为第j个对UUV起斥力作用的禁航区的速度和角度，ψ_{ro_j}为UUV和第j个禁航区的相对运动p_{ro_j}的角度，Among them, v _{obs_j} and ψ _{obs_j} are the speed and angle of the j-th no-navigation zone that repulses the UUV, and ψ _{ro_j} is the angle p _{ro_j} of the relative motion between the UUV and the j-th no-navigation zone,

${ψ ψ}^{' '} = = {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt} + + arcsin arcsin ((\frac{| | | | {v v}_{tar tar} | | | | sin sin (({ψ ψ}_{tar tar} - - {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt}))}{| | | | v v | | | |}))$

其中： ${\overset{&OverBar;}{ψ}}_{rt} = \arctan \frac{{\sin ψ}_{rt} - Σ_{j = 1}^{n} β_{j} \sin ψ_{ro_j}}{\cos ψ_{rt} - Σ_{j = 1}^{n} β_{j} \cos ψ_{ro_j}}, β_{j} = \frac{η_{j |} | | p_{ro_j} | |}{ξ_{1} | | p_{rt} | |},$ ||p_rt||≠0， $η_{j} = ξ_{2} ρ_{j}^{- 2} {| | p_{ro_j} | |}^{- 1} (ρ_{j}^{- 1} - ρ_{0}^{- 1}) .$ in: ${\overset{&OverBar;}{ψ}}_{rt} = \arctan \frac{{\sin ψ}_{rt} - Σ_{j = 1}^{no} β_{j} \sin ψ_{ro_j}}{\cos ψ_{rt} - Σ_{j = 1}^{no} β_{j} \cos ψ_{ro_j}}, β_{j} = \frac{η_{j |} | | p_{ro_j} | |}{ξ_{1} | | p_{rt} | |},$ ||p _rt ||≠0, $η_{j} = ξ_{2} ρ_{j}^{- 2} {| | p_{ro_j} | |}^{- 1} (ρ_{j}^{- 1} - ρ_{0}^{- 1}) .$

3、运动母船的禁航区有2个，分别位于船艏和船艉，第一禁航区和第二禁航区的中心坐标分别为：3. There are two no-navigation areas for the sports mothership, located at the bow and stern respectively. The center coordinates of the first no-navigation area and the second no-navigation area are:

q₁＝[x₀ y₀+L₁]^T q ₁ =[x ₀ y ₀ +L ₁ ] ^T

q₂＝[x₀ y₀+L₂]^T q ₂ ＝[x ₀ y ₀ +L ₂ ] ^T

其中，母船的长为L，宽为W，在母船坐标系下，母船的中心坐标为(x₀,y₀)，L₁为母船的中心到船艏的距离，L₂为母船的中心到船艉的距离。Among them, the length of the mother ship is L, and the width is W. In the mother ship coordinate system, the center coordinates of the mother ship are (x ₀ , y ₀ ), L ₁ is the distance from the center of the mother ship to the bow, and L ₂ is the distance from the center of the mother ship to The distance from the stern.

4、运动母船的禁航区有四个，设定运动母船的第一个禁航区、第二个禁航区、第三个禁航区、第四个禁航区中心的坐标分别为：4. There are four no-navigation areas for the moving mother ship. Set the coordinates of the center of the first no-navigation area, the second no-navigation area, the third no-navigation area and the fourth no-navigation area of the movement mother ship as follows:

q₁＝[x₀ y₀+L₁]^T q ₁ =[x ₀ y ₀ +L ₁ ] ^T

q₂＝[x₀ y₀+L₂]^T q ₂ ＝[x ₀ y ₀ +L ₂ ] ^T

q₃＝[x₀-b-W/2 y₀]^T q ₃ =[x ₀ -bW/2 y ₀ ] ^T

q₄＝[x₀+b+W/2 y₀]^T q ₄ ＝[x ₀ +b+W/2 y ₀ ] ^T

其中，母船的长为L，宽为W，在母船坐标系下，母船的中心坐标为(x₀,y₀)，L₁为母船的中心到船艏的距离，L₂为母船的中心到船艉的距离，b为第三个禁航区或第四个禁航区的中心到母船船体的距离。Among them, the length of the mother ship is L, and the width is W. In the mother ship coordinate system, the center coordinates of the mother ship are (x ₀ , y ₀ ), L ₁ is the distance from the center of the mother ship to the bow, and L ₂ is the distance from the center of the mother ship to The distance from the stern of the ship, b is the distance from the center of the third no-navigation zone or the fourth no-navigation zone to the hull of the mother ship.

5、UUV跟上运动母船的阈值K为8-15米。5. The threshold K for the UUV to keep up with the moving mother ship is 8-15 meters.

本发明的有益效果：Beneficial effects of the present invention:

本发明对传统势场法的规划方法进行了扩展，将UUV、母船和禁航区的位置信息作为规划要素外，加入了三者的速度信息，以达到UUV跟踪运动母船的同时规避运动的禁航区的目的，具有良好的控制性能和实时性。The present invention expands the planning method of the traditional potential field method. In addition to the position information of the UUV, the mother ship and the no-navigation area as the planning elements, the speed information of the three is added to achieve the UUV tracking the moving mother ship while avoiding the forbidden movement. The purpose of the navigation area has good control performance and real-time performance.

附图说明Description of drawings

图1本发明的流程图；Fig. 1 flow chart of the present invention;

图2-a运动母船禁航区示意图，图2-b运动母船禁航区中心位置示意图；Figure 2-a is a schematic diagram of the no-navigation area of the mobile mother ship, and Figure 2-b is a schematic diagram of the center position of the no-navigation area of the mobile mother ship;

图3 UUV跟踪运动母船的问题描述；Fig. 3 Problem description of UUV tracking moving mother ship;

图4仿真案例结果。Figure 4 Simulation case results.

具体实施方式detailed description

下面结合附图对本发明做进一步详细说明。The present invention will be described in further detail below in conjunction with the accompanying drawings.

首先设定运动母船的禁航区，禁航区的个数为N，设定UUV跟上运动母船的阈值K。First, set the no-navigation area of the moving mother ship, the number of no-navigation areas is N, and set the threshold K for the UUV to keep up with the moving mother ship.

实施例1：Example 1:

在母船艏艉两处分别设立一个禁航区，Set up a no-navigation zone at the bow and stern of the mother ship,

禁航区1：该禁航区是位于母船的艏部的一个圆形区域，圆心为母船艏部端点处，半径为r；No-navigation area 1: This no-navigation area is a circular area located at the bow of the mother ship, with the center of the circle at the end of the bow of the mother ship and a radius of r;

禁航区2：该禁航区是位于母船的艉部的一个圆形区域，圆心为母船艉部端点处，半径也为r。No-navigation area 2: This no-navigation area is a circular area located at the stern of the mother ship, the center of which is the endpoint of the stern of the mother ship, and the radius is also r.

实施例2：Example 2:

通过对母船回收UUV过程的分析，在母船艇体周围共设置了4个禁航区是UUV所不能进入的，如图2-a和图2-b所示。Through the analysis of the process of recovering UUV from the mother ship, a total of 4 no-navigation areas are set around the hull of the mother ship that UUV cannot enter, as shown in Figure 2-a and Figure 2-b.

禁航区2：该禁航区是位于母船的艉部的一个圆形区域，圆心为母船艉部端点处，半径也为r；No-navigation area 2: This no-navigation area is a circular area located at the stern of the mother ship, with the center of the circle at the end of the stern of the mother ship and a radius of r;

禁航区3：该禁航区是位于母船左舷，并且靠近母船艏部的一个椭圆形区域。在母船艇体坐标系下，其中心的Y向坐标与对接机构中心坐标相同，椭圆的长半轴和短半轴分别为a和b。No-navigation area 3: This no-navigation area is an oval area located on the port side of the mother ship and close to the bow of the mother ship. In the mother ship hull coordinate system, the Y-direction coordinates of its center are the same as the center coordinates of the docking mechanism, and the semi-major and semi-minor axes of the ellipse are a and b, respectively.

禁航区4：该禁航区是位于母船右舷，并且靠近母船艏部的一个椭圆形区域。在母船艇体坐标系下，其中心的Y向坐标也与对接机构中心坐标相同，椭圆的长半轴和短半轴也为a和b。No-navigation area 4: This no-navigation area is an oval area located on the starboard side of the mother ship and close to the bow of the mother ship. In the mother ship hull coordinate system, the Y-direction coordinates of its center are also the same as the center coordinates of the docking mechanism, and the semi-major and semi-minor axes of the ellipse are also a and b.

设母船的艇长为L，艇宽为W，在母船坐标系下，母船中心坐标的坐标为(x₀,y₀)，那么4个禁航区中心的坐标分别为：Suppose the length of the mother ship is L, the width of the ship is W, and in the mother ship coordinate system, the coordinates of the center coordinates of the mother ship are (x ₀ , y ₀ ), then the coordinates of the centers of the four no-navigation areas are:

q₁＝[x₀ y₀+L₁]^T q ₁ =[x ₀ y ₀ +L ₁ ] ^T

q₂＝[x₀ y₀+L₂]^T q ₂ ＝[x ₀ y ₀ +L ₂ ] ^T

q₃＝[x₀-b-W/2 y₀]^T q ₃ =[x ₀ -bW/2 y ₀ ] ^T

q₄＝[x₀+b+W/2 y₀]^T q ₄ ＝[x ₀ +b+W/2 y ₀ ] ^T

L₁为母船的中心到船艏的距离，L₂为母船的中心到船尾的距离，b为禁航区3或禁航区4的中心到母船船体的距离。L ₁ is the distance from the center of the mother ship to the bow, L ₂ is the distance from the center of the mother ship to the stern, b is the distance from the center of the no-navigation area 3 or 4 to the hull of the mother ship.

如图1所示本发明包括以下几个步骤：As shown in Figure 1 the present invention comprises the following steps:

步骤一：UUV利用声纳探测运动母船的位置p_tar和速度v_tar，同时得到N个禁航区的位置p_{obs_i}和速度v_{obs_i}，i＝(1，2，···N)。Step 1: UUV uses sonar to detect the position p _tar and velocity v _tar of the moving mother ship, and simultaneously obtains the positions p _{obs_i} and velocity v _{obs_i} of N no-navigation areas, i=(1, 2,...N).

步骤二：建立UUV和运动母船以及禁航区的相对运动。Step 2: Establish the relative motion of the UUV and the moving mother ship and the no-navigation zone.

设p_rt＝[x_rt y_rt]^T且p_{ro_i}＝[x_{ro_i} y_{ro_i}]，UUV和母船之间的相对运动可以描述为：Let p _rt =[x _rt y _rt ] ^T and p _{ro_i} =[x _{ro_i} y _{ro_i} ], the relative motion between UUV and mother ship can be described as:

${\overset{\cdot &Center Dot;}{p p}}_{rt rt} = = {[\begin{matrix} {\overset{\cdot &Center Dot;}{x x}}_{rt rt} & {\overset{\cdot &Center Dot;}{y the y}}_{rt rt} \end{matrix}]}^{T T} - - - - - - ((11))$

式中：In the formula:

${\overset{\cdot &Center Dot;}{x x}}_{rt rt} = = | | | | {v v}_{tar tar} | | | | {cos cos ψ ψ}_{tar tar} - - | | | | v v | | | | cos cos ψ ψ$

对于禁航区，首先考虑只有一个禁航区，所得出的结论同样很容易扩展到多个禁航区，For the no-fly zone, first consider that there is only one no-fly zone, and the conclusions drawn can also be easily extended to multiple no-fly zones,

UUV和禁航区之间的相对运动描述，适用于可以描述为：The description of the relative motion between the UUV and the no-go zone, suitable for use can be described as:

${\overset{\cdot \cdot}{p p}}_{ro ro} = = {[\begin{matrix} {\overset{\cdot \cdot}{x x}}_{ro ro} & {\overset{\cdot \cdot}{y the y}}_{ro ro} \end{matrix}]}^{T T} - - - - - - ((22))$

式中：In the formula:

${\overset{\cdot \cdot}{x x}}_{ro ro} = = | | | | {v v}_{obs obs} | | | | {cos cos ψ ψ}_{obs obs} - - | | | | v v | | | | cos cos ψ ψ$

${\overset{\cdot &Center Dot;}{y the y}}_{ro ro} = = | | | | {v v}_{obs obs} | | | | {sin sin ψ ψ}_{obs obs} - - | | | | v v | | | | sin sin ψ ψ$

如图3所示，v为UUV当前的速度，ψ_tar为运动母船的航向角，ψ为UUV当前的航向角，p_tar为母船的位置，v_tar为母船的速度，p为UUV的位置，v为UUV的速度，p_{obs_i}为第i个禁航区的位置，v_{obs_i}为第i个禁航区的速度，r_obs∈R²为圆形禁航区的半径，a_obs、b_obs∈R²为椭圆形禁航区的长半轴和短半轴，p_rt＝p_tar-p为UUV到母船的相对位置，p_{ro_i}＝p_{obs_i}-p为UUV到第i个禁航区的相对位置，p_{ot_i}＝p_tar-p_{obs_i}为第i个禁航区到母船的相对位置，ψ_rt为p_rt的角度，ψ_{ro_i}为p_{ro_i}的角度，ψ_{ot_i}为p_{ot_i}的角度，ψ_{obs_i}为v_{obs_i}的角度，ψ_tar为母船的航向角，ψ为UUV的航向角。As shown in Figure 3, v is the current speed of the UUV, ψ _tar is the heading angle of the moving mother ship, ψ is the current heading angle of the UUV, p _tar is the position of the mother ship, v _tar is the speed of the mother ship, and p is the position of the UUV. v is the speed of UUV, p _{obs_i} is the position of the i-th no-navigation zone, v _{obs_i} is the speed of the i-th no-navigation zone, r _obs ∈ R ² is the radius of the circular no-navigation zone, a _obs , b _obs ∈ R ² is the semi-major axis and semi-minor axis of the elliptical no-navigation zone, p _rt = p _tar -p is the relative position of the UUV to the mother ship, p _{ro_i} = p _{obs_i} -p is the relative position of the UUV to the ith no-navigation zone Position, p _{ot_i} = p _tar -p _{obs_i} is the relative position from the i-th no-navigation area to the mother ship, ψ _rt is the angle of p _rt , ψ _{ro_i} is the angle of p _{ro_i} , ψ _{ot_i} is the angle of p _{ot_i} , ψ _{obs_i} is v is the angle of _{obs_i} , ψ _tar is the heading angle of the mother ship, and ψ is the heading angle of the UUV.

步骤三：由UUV和运动母船以及禁航区的相对运动，规划UUV下一步的速度和航向。Step 3: From the relative motion of the UUV and the moving mother ship and the no-navigation zone, plan the next speed and heading of the UUV.

${U u}_{att att} = = \frac{11}{22} {ξ ξ}_{11} {p p}_{rt rt}^{T T} {p p}_{rt rt} - - - - - - ((33))$

${U u}_{rep rep} = = \{\begin{matrix} \frac{11}{22} {ξ ξ}_{22} {(({ρ ρ}_{i i}^{- - 11} - - {ρ ρ}_{00}^{- - 11}))}^{22} & if if,, {ρ ρ}_{i i} \leq \leq {ρ ρ}_{00} \\ 00 & else else \end{matrix} - - - - - - ((44))$

ρ表示UUV和第i个禁航区之间的最小距离，ρ₀为每个禁航区的斥力作用距离，ξ₁和ξ₂分别是引力势场和斥力势场的权重因子；ρ represents the minimum distance between the UUV and the i-th no-navigation zone, ρ0 is the repulsion action distance of each no _- navigation zone, and _ξ1 and _ξ2 are the weight factors of the gravitational potential field and the repulsive potential field, respectively;

首先研究UUV在禁航区斥力作用距离外(ρ>ρ₀)的运动规划，然后研究UUV在一个禁航区斥力作用距离内(ρ<ρ₀)的运动规划。最后，将研究结果扩展到多禁航区时的UUV运动规划。Firstly, the motion planning of UUV outside the repulsive distance of the no-flight zone (ρ>ρ ₀ ) is studied, and then the motion planning of the UUV within a repulsive distance of the no-navigation zone (ρ<ρ ₀ ) is studied. Finally, the findings are extended to UUV motion planning in multiple no-go zones.

3.1禁航区不起作用时的UUV运动规划3.1 UUV motion planning when no-fly zone does not work

当UUV在禁航区斥力作用距离外时，只有引力场U_att对UUV的速度规划起作用。UUV的速度v需要使在相对于p_rt的引力势场U_att的负梯度方向：When the UUV is outside the distance of the repulsive force in the no-flight zone, only the gravitational field U _att has an effect on the speed planning of the UUV. The velocity v of the UUV needs to be such that In the direction of the negative gradient of the gravitational potential field U _att with respect to p _rt :

${\overset{\cdot &Center Dot;}{p p}}_{rt rt} = = - - {&dtri; &dtri;}_{prt prt} {U u}_{att att} = = - - {((\frac{{&PartialD; &PartialD; U u}_{att att}}{{&PartialD; &PartialD; p p}_{rt rt}}))}^{T T} - - - - - - ((55))$

由式(3)和上式有：From formula (3) and the above formula, we have:

v＝v_tar+ξ₁p_rt (6)相当于有下面的三角关系成立：v=v _tar +ξ ₁ p _rt (6) is equivalent to the establishment of the following triangle relationship:

||v||sin(ψ-ψ_rt)＝||v_tar||sin(ψ_tar-ψ_rt) (7)||v||sin(ψ-ψ _rt )＝||v _tar ||sin(ψ _tar -ψ _rt ) (7)

$| | | | {v v}^{' '} | | | | = = min min {(((({| | | | {v v}_{tar tar} | | | |}^{22} + + {22 ξ ξ}_{11} | | | | {p p}_{rt rt} | | | | | | | | {v v}_{tar tar} | | | | \times \times cos cos (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt})))) + + {ξ ξ}_{11}^{22} {| | | | {p p}_{rt rt} | | | |}^{22}))}^{\frac{11}{22}},, {v v}_{max max})) - - - - - - ((88))$

上式中，假设v_tar≠0且||p_rt||≠0，v_max为UUV的最大速度。In the above formula, assuming that v _tar ≠0 and || _{pr rt} ||≠0, v _max is the maximum speed of UUV.

UUV的航向可以由式(7)和式(8)求得：The heading of UUV can be obtained by formula (7) and formula (8):

$ψ ψ = = {ψ ψ}_{rt rt} + + arcsin arcsin ((\frac{| | | | {v v}_{tar tar} | | | | sin sin (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt}))}{| | | | v v | | | |})) - - - - - - ((99))$

3.2一个禁航区起作用时的UUV运动规划3.2 UUV motion planning when a no-go zone is in effect

当一个禁航区对UUV起作用时，引力势场U_att和斥力势场U_rep同时作用在UUV上，由UUV的合势场为：When a no-go zone acts on the UUV, the gravitational potential field U _att and the repulsive potential field U _rep act on the UUV at the same time, and the resultant potential field of the UUV is:

$U u = = {U u}_{att att} + + {U u}_{rep rep} = = \frac{11}{22} {ξ ξ}_{11} {p p}_{rt rt}^{T T} {p p}_{rt rt} + + \frac{11}{22} {ξ ξ}_{22} {(({ρ ρ}^{- - 11} - - {ρ ρ}_{00}^{- - 11}))}^{22} - - - - - - ((1010))$

那么，UUV的速度将使指向U相对于p_rt的负方向。此时需要:Then, the speed of the UUV will make Points in the negative direction of U relative to p _rt . At this point you need:

$min min ((\frac{&PartialD; &PartialD; U u}{{&PartialD; &PartialD; p p}_{rt rt}} \frac{{\overset{\cdot &Center Dot;}{p p}}_{rt rt}}{| | | | {\overset{\cdot \cdot}{p p}}_{rt rt} | | | |})) - - - - - - ((1111))$

且满足：And satisfy:

$\frac{11}{{| | | | {\overset{\cdot &Center Dot;}{p p}}_{rt rt} | | | |}^{33}} ((\frac{&PartialD; &PartialD; U u}{{ax ax}_{rt rt}} {\overset{\cdot \cdot}{y the y}}_{rt rt} - - \frac{&PartialD; &PartialD; U u}{{&PartialD; &PartialD; y the y}_{rt rt}} {\overset{\cdot \cdot}{x x}}_{rt rt})) \times \times (({\overset{\cdot \cdot}{y the y}}_{rt rt} \frac{{&PartialD; &PartialD; \overset{\cdot &Center Dot;}{x x}}_{rt rt}}{&PartialD; &PartialD; ψ ψ} - - {\overset{\cdot &Center Dot;}{x x}}_{rt rt} \frac{{&PartialD; &PartialD; \overset{\cdot &Center Dot;}{y the y}}_{rt rt}}{&PartialD; &PartialD; ψ ψ})) = = 00,, | | | | {\overset{\cdot &Center Dot;}{p p}}_{rt rt} | | | | &NotEqual; &NotEqual; 00 - - - - - - ((1212))$

UUV跟踪速度为v_tar的运动母船，同时规避相对位置为p_ro速度为v_ro的禁航区，UUV的下一步速度和航向可以规划为：The UUV tracks the moving mother ship with a speed of v _tar , and at the same time avoids the no-navigation zone with a relative position of p _ro and a speed of v _ro . The next step speed and course of the UUV can be planned as:

$| | | | v v | | | | = = {[[{((| | | | {v v}_{tar tar} | | | | cos cos (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt})) - - β β | | | | {v v}_{obs obs} | | | | cos cos (({ψ ψ}_{obs obs} - - {ψ ψ}_{ro ro})) + + {ξ ξ}_{11} | | | | {p p}_{rt rt} | | | |))}^{22} + + {| | | | {v v}_{tar tar} | | | |}^{22} {sin sin}^{22} (({ψ ψ}_{tar tar} - - {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt}))]]}^{\frac{11}{22}} - - - - - - ((1313))$

$ψ ψ = = {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt} + + arcsin arcsin \frac{| | | | {v v}_{tar tar} | | | | sin sin (({ψ ψ}_{tar tar} - - {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt}))}{| | | | v v | | | |} - - - - - - ((1414))$

式中， ${\overset{&OverBar;}{ψ}}_{rt} = \arctan \frac{{\sin ψ}_{rt} - {β \sin ψ}_{ro}}{{\cos ψ}_{rt} - {β \cos ψ}_{ro}},$ ψ_rt≠ψ_ro， $β = \frac{η | | p_{ro} | |}{ξ_{1} | | p_{rt} | |},$ ||p_rt||≠0， $η = \{\begin{matrix} ξ_{2} (ρ^{- 1} - ρ_{0}^{- 1}) | | p_{ro} | | & if, ρ \leq ρ_{0} \\ 0 & else \end{matrix},$ ρ＝||p_ro||-r_obs，ξ₁>0,ξ₂>0为权重因子。In the formula, ${\overset{&OverBar;}{ψ}}_{rt} = \arctan \frac{{\sin ψ}_{rt} - {β \sin ψ}_{ro}}{{\cos ψ}_{rt} - {β \cos ψ}_{ro}},$ ψ _rt ≠ ψ _ro , $β = \frac{η | | p_{ro} | |}{ξ_{1} | | p_{rt} | |},$ ||p _rt ||≠0, $η = \{\begin{matrix} ξ_{2} (ρ^{- 1} - ρ_{0}^{- 1}) | | p_{ro} | | & if, ρ \leq ρ_{0} \\ 0 & else \end{matrix},$ ρ=||p _ro ||-r _obs , ξ ₁ >0, ξ ₂ >0 are weighting factors.

3.3多个禁航区起作用时的UUV运动规划3.3 UUV motion planning when multiple no-navigation zones are in effect

得到当前对UUV起斥力作用的禁航区为j个，j＝(1，···N)，j个禁航区的斥力势场对规划UUV下一步的速度和航向起作用，Obtaining that there are currently j no-go areas that act as repulsive forces on the UUV, j=(1,...N), and the repulsive force potential field of j no-go areas plays a role in planning the speed and course of the next step of the UUV,

UUV下一步的速度v′为：The speed v' of the UUV in the next step is:

$\begin{matrix} | | | | {v v}^{' '} | | | | = = [[((| | | | {v v}_{tar tar} | | | | cos cos (({ψ ψ}_{tar tar} - - {ψ ψ}_{rt rt})) - - {Σ Σ}_{j j = = 11}^{n no} {β β}_{j j} | | | | {&upsi; &upsi;}_{obs obs__j j} | | | | cos cos (({ψ ψ}_{obs obs__j j} - - {ψ ψ}_{ro ro__j j})) \\ {+ + {{ξ ξ}_{11} | | | | {p p}_{rt rt} | | | |))}^{22} + + {| | | | {&upsi; &upsi;}_{tar tar} | | | |}^{22} {sin sin}^{22} (({ψ ψ}_{tar tar} - - {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt}))]]}^{\frac{11}{22}} \end{matrix} - - - - - - ((1515))$

${ψ ψ}^{' '} = = {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt} + + arcsin arcsin ((\frac{| | | | {v v}_{tar tar} | | | | sin sin (({ψ ψ}_{tar tar} - - {\overset{&OverBar; &OverBar;}{ψ ψ}}_{rt rt}))}{| | | | v v | | | |})) - - - - - - ((1616))$

步骤四：UUV执行下一步的速度和航向指令，判断UUV到运动母船的距离是否小于阈值K，如果不小于阈值，重复步骤二～步骤五，如果小于阈值，完成UUV对运动母船的跟踪。设定UUV跟上运动母船的阈值K为10米。Step 4: UUV executes the next speed and heading command, and judges whether the distance from UUV to the moving mother ship is less than the threshold K. If it is not less than the threshold, repeat steps 2 to 5. If it is less than the threshold, the tracking of the moving mother ship by UUV is completed. Set the threshold K for the UUV to keep up with the moving mother ship to be 10 meters.

仿真案例：利用前面各节所设计的运动规划方法进行UUV对运动母船跟踪的仿真验证，如图4所示。仿真中设定母船四个禁航区的范围参数分别为：圆形禁航区1和4的半径为50m，椭圆形禁航区3和4的长轴为100m，短轴为40m。设定跟踪母船的接近圆半径为10m。UUV的最小速度设定为1节，最大速度设定为4节，最大偏转角速度10deg/s。设置UUV的初始位置为(50，-300)m，母船的初始位置为(0，0)m，接近圆半径设置为对接阶段所可接受的最大值：10m。母船运动轨迹中的航向和速度分别按以下两式变化：Simulation case: Use the motion planning method designed in the previous sections to carry out the simulation verification of UUV tracking the moving mother ship, as shown in Figure 4. In the simulation, the range parameters of the four no-navigation areas of the mother ship are set as follows: the radius of the circular no-navigation areas 1 and 4 is 50m, the major axis of the elliptical no-navigation areas 3 and 4 is 100m, and the short axis is 40m. Set the radius of the approach circle for tracking the mother ship to 10m. The minimum speed of the UUV is set to 1 knot, the maximum speed is set to 4 knots, and the maximum deflection angular velocity is 10deg/s. Set the initial position of the UUV to (50, -300) m, the initial position of the mother ship to (0, 0) m, and set the radius of the approach circle to the maximum value acceptable in the docking stage: 10 m. The course and speed of the mother ship's trajectory change according to the following two formulas:

${ψ ψ}_{t t} = = 1.7 1.7 - - \frac{t t}{200200} + + {((\frac{t t}{261500261500}))}^{33} + + 33 sin sin ((0.002 0.002 t t))$

V_t＝5(sin(0.0002t)+0.109)。V _t =5(sin(0.0002t)+0.109).

Claims

The tracking of the 1.UUV motion lash ship to having multiple prohibited area, it is characterised in that: include following step:

Step one: UUV utilizes the position p of communication sonar detection campaign lash ship_tarAnd speed v_tar, by default lash ship prohibited area letter Breath flows to UUV, obtains the position p in N number of prohibited area_{obs_i}And speed v_{obs_i}, i=1,2 ... N；

Step 2: set up UUV and motion lash ship and the relative motion in prohibited area；

Step 3: by UUV and motion lash ship and the relative motion in prohibited area, planning UUV next step speed and course；

Step 4: UUV performs next step speed and directional command, it is judged that whether the distance of UUV to motion lash ship is less than threshold value K, if not less than threshold value, repeats step one～step 4, if less than threshold value, completes the UUV tracking to motion lash ship.
The tracking of the UUV the most according to claim 1 motion lash ship to having multiple prohibited area, it is characterised in that: Described UUV and relative motion p of motion lash ship_rtFor:

${\overset{\cdot}{p}}_{r t} = {[\begin{matrix} {\overset{\cdot}{x}}_{r t} & {\overset{\cdot}{y}}_{r t} \end{matrix}]}^{T}$

Wherein:

${\overset{\cdot}{x}}_{r t} = | | v_{t a r} | | {cosψ}_{t a r} - | | v | | c o s ψ$

${\overset{\cdot}{y}}_{r t} = | | v_{t a r} | | {sinψ}_{t a r} - | | v | | s i n ψ$

V is the speed that UUV is current, ψ_tarFor the course angle of motion lash ship, ψ is the course angle that UUV is current,

UUV and relative motion p in i-th prohibited area_{ro_j}:

${\overset{\cdot}{p}}_{r o_i} = {[\begin{matrix} {\overset{\cdot}{x}}_{r o_i} & {\overset{\cdot}{y}}_{r o_i} \end{matrix}]}^{T}$

Wherein:

${\overset{\cdot}{x}}_{r o_i} = | | v_{o b s_i} | | {cosψ}_{o b s_i} - | | v | | c o s ψ$

${\overset{\cdot}{y}}_{r o_i} = | | v_{o b s_i} | | {sinψ}_{o b s_i} - | | v | | s i n ψ$

ψ_{obs_i}For the speed in the i-th prohibited area deflection under earth coordinates.
The tracking of the UUV the most according to claim 2 motion lash ship to having multiple prohibited area, it is characterised in that: Described planning UUV next step speed and the method in course be:

(1) by UUV and motion lash ship and the relative motion in prohibited area, set up the gravitation potential field of UUV and motion lash ship, UUV and The repulsion potential field in prohibited area；

Gravitation potential field U of UUV and motion lash ship_att:

$U_{a t t} = \frac{1}{2} ξ_{1} p_{r t}^{T} p_{r t}$

Repulsion potential field U in UUV and i-th prohibited area_{rep_i}:

$U_{r e p_i} = \{\begin{matrix} \frac{1}{2} ξ_{2} {({ρ_{i}}^{- 1} - ρ_{0}^{- 1})}^{2} & \begin{matrix} i f & ρ_{i} \leq ρ_{0} \end{matrix} \\ 0 & e l s e \end{matrix}$

ρ_iRepresent the minimum range between UUV and i-th prohibited area, ρ₀For the repulsion operating distance in each prohibited area, ξ₁And ξ₂Point It not gravitation potential field and the weight factor of repulsion potential field；

(2) if the minimum range in UUV and each prohibited area is all higher than or is equal to ρ₀, selection situation one plan UUV next step The minimum range in speed and course, UUV and at least one prohibited area is less than ρ₀, selection situation two plans next step speed of UUV And course,

Situation one: only gravitation potential field U_attTo planning UUV next step speed and course work,

Next step speed v ' of UUV is:

$| | v^{'} | | = \min {((| | v_{t a r} | |^{2} + 2 ξ_{1} | | p_{r t} | | | | v_{t a r} | | \times c o s (ψ_{t a r} - ψ_{r t})) + ξ_{1}^{2} | | p_{r t} | |^{2})}^{\frac{1}{2}}, v_{\max})$

Wherein, v_maxFor the maximal rate of UUV, ψ_rtFor UUV and relative motion p of motion lash ship_rtDirection under earth coordinates Angle,

Next step course ψ ' of UUV is:

$ψ^{'} = ψ_{r t} + a r c s i n (\frac{| | v_{t a r} | | s i n (ψ_{t a r} - ψ_{r t})}{| | v | |})$

Situation two: if the minimum range in UUV and i-th prohibited area is less than ρ₀, then i-th prohibited area is scolded for currently playing UUV The prohibited area of power effect, the number obtaining the current prohibited area that UUV plays repulsion effect is j, j=1, N, j taboo The repulsion potential field of navigating area to planning UUV next step speed and course work,

Next step speed v ' of UUV is:

$\begin{matrix} | | v^{'} | | = [(| | v_{t a r} | | \cos (ψ_{t a r} - ψ_{r t}) - Σ_{j = 1}^{N} β_{j} | | v_{o b s_j} | | \cos (ψ_{o b s_j} - ψ_{r o_j}) \\ + ξ_{1} | | p_{r t} | |)^{2} + | | v_{t a r} | |^{2} \sin^{2} (ψ_{t a r} - {\overset{&OverBar;}{ψ}}_{r t})]^{\frac{1}{2}} \end{matrix}$

Wherein v_{obs_j}And ψ_{obs_j}For jth, UUV is played speed and angle, the ψ in the prohibited area of repulsion effect_{ro_j}For UUV and jth Relative motion p in prohibited area_{ro_j}Deflection under earth coordinates,

Next step course ψ ' of UUV is:

$ψ^{'} = {\overset{&OverBar;}{ψ}}_{r t} + a r c s i n \frac{| | v_{t a r} | | s i n (ψ_{t a r} - {\overset{&OverBar;}{ψ}}_{r t})}{| | v | |}$

Wherein:
The tracking of the UUV the most according to claim 3 motion lash ship to having multiple prohibited area, it is characterised in that: The prohibited area of described motion lash ship has 2, lays respectively at stem and ship stern, and the center in the first prohibited area and the second prohibited area is sat Mark is respectively as follows:

q₁=[x₀ y₀+L₁]^T

q₂=[x₀ y₀+L₂]^T

Wherein, a length of L of lash ship, a width of W, under lash ship coordinate system, the centre coordinate of lash ship is (x₀,y₀), L₁For in lash ship The heart is to the distance of stem, L₂For the center of lash ship to the distance of ship stern.
The tracking of the UUV the most according to claim 3 motion lash ship to having multiple prohibited area, it is characterised in that: The prohibited area of described motion lash ship has four, sets first prohibited area of motion lash ship, second prohibited area, the 3rd taboo Navigating area, the coordinate at the 4th center, prohibited area are respectively as follows:

q₁=[x₀ y₀+L₁]^T

q₂=[x₀ y₀+L₂]^T

q₃=[x₀-b-W/2 y₀]^T

q₄=[x₀+b+W/2 y₀]^T

Wherein, a length of L of lash ship, a width of W, under lash ship coordinate system, the centre coordinate of lash ship is (x₀,y₀), L₁For in lash ship The heart is to the distance of stem, L₂For the center of lash ship to the distance of ship stern, b is the 3rd prohibited area or the center in the 4th prohibited area Distance to lash ship hull.
6., according to the tracking of the motion lash ship to having multiple prohibited area of the UUV described in claim 4 or 5, its feature exists In: it is 8-15 rice that UUV catches up with the threshold k of motion lash ship.