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

Abstract

The invention discloses the tracking of a kind of UUV motion lash ship to having multiple prohibited area.The position of communication sonar detection campaign lash ship is utilized including following step: UUV_ptarAnd speed v_tar, by default lash ship prohibited area information conveyance to UUV, obtain the position p in N number of prohibited area_{obs_i}And speed v_{obs_i}；Set up UUV and motion lash ship and the relative motion in prohibited area；Planning UUV next step speed and course；UUV performs next step speed and directional command, it is judged that whether the distance of UUV to motion lash ship, less than threshold k, if not less than threshold value, repeats step one～step 4, if less than threshold value, complete the UUV tracking to motion lash ship.The positional information in UUV, lash ship and prohibited area as outside program element, is added the velocity information of three, evades the purpose in the prohibited area of motion, have good control performance and real-time while reaching UUV pursuit movement lash ship by the present invention.

Description

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

Technical field

The invention belongs to the UUV tracking to motion lash ship, particularly relate to a kind of UUV fortune to having multiple prohibited area The tracking of dynamic lash ship.

Background technology

UAV navigation UUV (Unmanned Underwater Vehicle) as the mankind at Activities of Ocean, especially It is the important replacer in the activity of deep-sea and executor, has been widely used in scientific investigation, deep ocean work, has salvaged the necks such as lifesaving Territory.After UAV navigation completes task, need to return lash ship and carry out data readback, download new mission task dispatching work Making, therefore the recovery operation of aircraft is indispensable process.Wu that returns that aircraft reclaims in operation is critically important in the stage In the stage, the most how can realize quickly and safely the tracking of the motion lash ship with prohibited area being critically important asking Topic.

The tracking problem of moving target is conducted in-depth research by lot of domestic and foreign scholar, it is proposed that many effectively with Track method, such as UKF algorithm, particle filter method, Artificial Potential Field algorithm etc., Artificial Potential Field Method, calculate simple, real-time is good, Low to sensor requirements, such as utilize sonar sensor just can complete the measurement to obstacle, obtained this year studying widely And application, it is particularly suitable for aircraft motion planning in Unknown Dynamic Environment.But traditional Artificial Potential Field Method is only profit Plan with local message, inevitably there is some defects and leak: be absorbed in locally optimal solution problem, i.e. aircraft and exist The most unreachable；Arrow path path concussion problem, the nearest barrier of distance cannot obtain the problem etc. of active path, and And the object for motion is difficulty with collision prevention and tracking.

Summary of the invention

It is an object of the invention to provide a kind of be capable of motion lash ship is followed the tracks of while evade prohibited area about, The tracking of the UUV motion lash ship to having multiple prohibited area.

The present invention is achieved by the following technical solutions:

The tracking of the UUV motion lash ship to having multiple prohibited area, including following step:

Step one: UUV utilizes the position p of communication sonar detection campaign lash ship_tarAnd speed v_tar, by default lash ship navigation ban District's information conveyance, 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 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 of the present invention motion lash ship to having multiple prohibited area can also include:

1, the relative motion of UUV and motion lash ship is:

${\stackrel{\·}{p}}_{\mathrm{rt}}={\left[\begin{array}{cc}{\stackrel{\·}{x}}_{\mathrm{rt}}& {\stackrel{\·}{y}}_{\mathrm{rt}}\end{array}\right]}^T$

Wherein:

${\stackrel{\·}{x}}_{\mathrm{rt}}=\left|\right|v_{\mathrm{tar}}\left|\right|{\cos \mathrm{\ψ}}_{\mathrm{tar}}-\left|\right|v\left|\right|\cos \mathrm{\ψ}$

${\stackrel{\·}{y}}_{\mathrm{rt}}=\left|\right|v_{\mathrm{tar}}\left|\right|{\sin \mathrm{\ψ}}_{\mathrm{tar}}-\left|\right|v\left|\right|\sin \mathrm{\ψ}$

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 the relative motion in i-th prohibited area:

${\stackrel{\·}{p}}_{\mathrm{ro}\_i}={\left[\begin{array}{cc}{\stackrel{\·}{x}}_{\mathrm{ro}\_i}& {\stackrel{\·}{y}}_{\mathrm{ro}\_i}\end{array}\right]}^T$

Wherein:

${\stackrel{\·}{x}}_{\mathrm{ro}\_i}=\left|\right|v_{\mathrm{obs}\_i}\left|\right|{\cos \mathrm{\ψ}}_{\mathrm{obs}\_i}-\left|\right|v\left|\right|\cos \mathrm{\ψ}$

${\stackrel{\·}{y}}_{\mathrm{ro}\_i}=\left|\right|v_{\mathrm{obs}\_i}\left|\right|{\sin \mathrm{\ψ}}_{\mathrm{obs}\_i}-\left|\right|v\left|\right|\sin \mathrm{\ψ}$

ψ_{obs_i}Angle for the speed in i-th prohibited area.

2, 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 UUV and motion lash ship gravitation potential field, The repulsion potential field in UUV and prohibited area；

Gravitation potential field U of UUV and motion lash ship_att:

$U_{\mathrm{att}}=\frac{1}{2}{\mathrm{\ξ}}_1{p}_{\mathrm{rt}}^T{p}_{\mathrm{rt}}$

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

$U_{\mathrm{rep}}=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{\ξ}}_2{({\mathrm{\ρ}}_i^{-1}-{\mathrm{\ρ}}_0^{-1})}^2& \mathrm{if},{\mathrm{\ρ}}_i\≤{\mathrm{\ρ}}_0\\ 0& \mathrm{else}\end{array}\right.$

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

(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 The speed of step and the minimum range in course, UUV and at least one prohibited area are less than ρ₀, selection situation two plan UUV next step Speed 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:

$\left|\right|v^{\′}\left|\right|=\min {(({\left|\right|v_{\mathrm{tar}}\left|\right|}^2+{2\mathrm{\ξ}}_1|\left|p_{\mathrm{rt}}\right|\left|\right|\left|v_{\mathrm{tar}}\right||\×\cos ({\mathrm{\ψ}}_{\mathrm{tar}}-{\mathrm{\ψ}}_{\mathrm{rt}}))+{\mathrm{\ξ}}_1^2{\left|\right|p_{\mathrm{rt}}\left|\right|}^2)}^{\frac{1}{2}},v_{\max })$

Wherein, v_maxFor the maximal rate of UUV, ψ_rtFor UUV and relative motion p of motion lash ship_rtAngle,

Next step course ψ ' of UUV is:

${\mathrm{\ψ}}^{\′}={\mathrm{\ψ}}_{\mathrm{rt}}+\arcsin \left(\frac{\left|\right|v_{\mathrm{tar}}\left|\right|\sin ({\mathrm{\ψ}}_{\mathrm{tar}}-{\mathrm{\ψ}}_{\mathrm{rt}})}{\left|\right|v\left|\right|}\right)$

Situation two: if the minimum range in UUV and i-th prohibited area is less than ρ₀, then i-th prohibited area is the most right UUV plays the prohibited area of repulsion effect, and the number obtaining the current prohibited area that UUV plays repulsion effect is j, j=(1, N), the repulsion potential field in j prohibited area to planning UUV next step speed and course work,

Next step speed v ' of UUV is:

$\begin{array}{c}\left|\right|v^{\′}\left|\right|=[\left(\right|\left|v_{\mathrm{tar}}\right||\cos ({\mathrm{\ψ}}_{\mathrm{tar}}-{\mathrm{\ψ}}_{\mathrm{rt}})-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\β}}_j|\left|{\mathrm{\υ}}_{\mathrm{obs}\_j}\right||\cos ({\mathrm{\ψ}}_{\mathrm{obs}\_j}-{\mathrm{\ψ}}_{\mathrm{ro}\_j})\\ {+{{\mathrm{\ξ}}_1\left|\right|p_{\mathrm{rt}}\left|\right|)}^2+{\left|\right|{\mathrm{\υ}}_{\mathrm{tar}}\left|\right|}^2{\sin }^2({\mathrm{\ψ}}_{\mathrm{tar}}-{\stackrel{\‾}{\mathrm{\ψ}}}_{\mathrm{rt}})]}^{\frac{1}{2}}\end{array}$

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 Relative motion p in jth prohibited area_{ro_j}Angle,

Next step course ψ ' of UUV is:

${\mathrm{\ψ}}^{\′}={\stackrel{\‾}{\mathrm{\ψ}}}_{\mathrm{rt}}+\arcsin \left(\frac{\left|\right|v_{\mathrm{tar}}\left|\right|\sin ({\mathrm{\ψ}}_{\mathrm{tar}}-{\stackrel{\‾}{\mathrm{\ψ}}}_{\mathrm{rt}})}{\left|\right|v\left|\right|}\right)$

Wherein: ${\stackrel{\‾}{\mathrm{\ψ}}}_{\mathrm{rt}}=\arctan \frac{{\sin \mathrm{\ψ}}_{\mathrm{rt}}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\β}}_j\sin {\mathrm{\ψ}}_{\mathrm{ro}\_j}}{\cos {\mathrm{\ψ}}_{\mathrm{rt}}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\β}}_j\cos {\mathrm{\ψ}}_{\mathrm{ro}\_j}},{\mathrm{\β}}_j=\frac{{\mathrm{\η}}_{j|}\left|\right|p_{\mathrm{ro}\_j}\left|\right|}{{\mathrm{\ξ}}_1\left|\right|p_{\mathrm{rt}}\left|\right|},$ ||p_rt| | ≠ 0, ${\mathrm{\η}}_j={\mathrm{\ξ}}_2{\mathrm{\ρ}}_j^{-2}{\left|\right|p_{\mathrm{ro}\_j}\left|\right|}^{-1}({\mathrm{\ρ}}_j^{-1}-{\mathrm{\ρ}}_0^{-1}).$

3, the prohibited area of motion lash ship has 2, lays respectively in stem and ship stern, the first prohibited area and the second prohibited area Heart coordinate 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 lash ship Center to the distance of stem, L₂For the center of lash ship to the distance of ship stern.

4, the prohibited area of motion lash ship has four, set first prohibited area of motion lash ship, second prohibited area, the 3rd Individual prohibited 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 lash ship Center 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 4th prohibited area Center is to the distance of lash ship hull.

5, UUV catches up with the threshold k of motion lash ship is 8-15 rice.

Beneficial effects of the present invention:

The planing method of tradition potential field method is extended by the present invention, is made by the positional information in UUV, lash ship and prohibited area Outside for program element, add the velocity information of three, while reaching UUV pursuit movement lash ship, evade the prohibited area of motion Purpose, there is good control performance and real-time.

Accompanying drawing explanation

The flow chart of Fig. 1 present invention；

Fig. 2-a motion lash ship prohibited area schematic diagram, center, Fig. 2-b motion lash ship prohibited area schematic diagram；

The problem of Fig. 3 UUV pursuit movement lash ship describes；

Fig. 4 emulates case result.

Detailed description of the invention

Below in conjunction with the accompanying drawings the present invention is described in further details.

First setting the prohibited area of motion lash ship, the number in prohibited area is N, sets UUV and catches up with the threshold k of motion lash ship.

Embodiment 1:

A prohibited area is set up respectively at lash ship bow stern two,

Prohibited area 1: this prohibited area is in a border circular areas of the bow of lash ship, the center of circle is at lash ship bow end points, Radius is r；

Prohibited area 2: this prohibited area is in a border circular areas of the stern of lash ship, the center of circle is at lash ship stern end points, Radius is also r.

Embodiment 2:

By lash ship being reclaimed the analysis of UUV process, be provided with altogether around lash ship hull 4 prohibited areas be UUV institute not Can enter, as shown in Fig. 2-a and Fig. 2-b.

Prohibited area 1: this prohibited area is in a border circular areas of the bow of lash ship, the center of circle is at lash ship bow end points, Radius is r；

Prohibited area 2: this prohibited area is in a border circular areas of the stern of lash ship, the center of circle is at lash ship stern end points, Radius is also r；

Prohibited area 3: this prohibited area is in lash ship larboard, and near an elliptical region of lash ship bow.Mother Under boat body coordinate system, the Y-direction coordinate at its center is identical with docking mechanism centre coordinate, and oval major semiaxis and semi-minor axis are respectively For a and b.

Prohibited area 4: this prohibited area is in lash ship starboard, and near an elliptical region of lash ship bow.Mother Under boat body coordinate system, the Y-direction coordinate at its center is also identical with docking mechanism centre coordinate, and oval major semiaxis and semi-minor axis are also For a and b.

If the cox of lash ship is L, a width of W of ship, under lash ship coordinate system, the coordinate of lash ship centre coordinate is (x₀,y₀), that The coordinate at center, 4 prohibited areas is 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

L₁For the center of lash ship to the distance of stem, L₂For the center of lash ship to the distance of stern, b is prohibited area 3 or navigation ban The center in district 4 is to the distance of lash ship hull.

The present invention includes following step as shown in Figure 1:

Step one: UUV utilizes the position p of sonar detection campaign lash ship_tarAnd speed v_tar, obtain N number of prohibited area simultaneously Position p_{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.

If p_rt=[x_rt y_rt]^TAnd p_{ro_i}=[x_{ro_i} y_{ro_i}], the relative motion between UUV and lash ship can be described as:

${\stackrel{\·}{p}}_{\mathrm{rt}}={\left[\begin{array}{cc}{\stackrel{\·}{x}}_{\mathrm{rt}}& {\stackrel{\·}{y}}_{\mathrm{rt}}\end{array}\right]}^T---\left(1\right)$

In formula:

${\stackrel{\·}{x}}_{\mathrm{rt}}=\left|\right|v_{\mathrm{tar}}\left|\right|{\cos \mathrm{\ψ}}_{\mathrm{tar}}-\left|\right|v\left|\right|\cos \mathrm{\ψ}$

${\stackrel{\·}{y}}_{\mathrm{rt}}=\left|\right|v_{\mathrm{tar}}\left|\right|{\sin \mathrm{\ψ}}_{\mathrm{tar}}-\left|\right|v\left|\right|\sin \mathrm{\ψ}$

For prohibited area, first considering only one of which prohibited area, the conclusion drawn is easy to expand to multiple taboo equally Navigating area,

Relative motion between UUV and prohibited area describes, it is adaptable to can be described as:

${\stackrel{\·}{p}}_{\mathrm{ro}}={\left[\begin{array}{cc}{\stackrel{\·}{x}}_{\mathrm{ro}}& {\stackrel{\·}{y}}_{\mathrm{ro}}\end{array}\right]}^T---\left(2\right)$

In formula:

${\stackrel{\·}{x}}_{\mathrm{ro}}=\left|\right|v_{\mathrm{obs}}\left|\right|{\cos \mathrm{\ψ}}_{\mathrm{obs}}-\left|\right|v\left|\right|\cos \mathrm{\ψ}$

${\stackrel{\·}{y}}_{\mathrm{ro}}=\left|\right|v_{\mathrm{obs}}\left|\right|{\sin \mathrm{\ψ}}_{\mathrm{obs}}-\left|\right|v\left|\right|\sin \mathrm{\ψ}$

As it is shown on figure 3, v is the speed that UUV is current, ψ_tarFor the course angle of motion lash ship, ψ is the course angle that UUV is current, p_tarFor the position of lash ship, v_tarFor the speed of lash ship, p is the position of UUV, and v is the speed of UUV, p_{obs_i}For i-th prohibited area Position, v_{obs_i}For the speed in i-th prohibited area, r_obs∈R²For the radius in circular prohibited area, a_obs、b_obs∈R²Prohibit for ellipse The major semiaxis of navigating area and semi-minor axis, p_rt=p_tar-p is the UUV relative position to lash ship, p_{ro_i}=p_{obs_i}-p is that UUV arrives i-th The relative position in prohibited area, p_{ot_i}=p_tar-p_{obs_i}For the relative position in i-th prohibited area to lash ship, ψ_rtFor p_rtAngle, ψ_{ro_i}For p_{ro_i}Angle, ψ_{ot_i}For p_{ot_i}Angle, ψ_{obs_i}For v_{obs_i}Angle, ψ_tarFor the course angle of lash ship, ψ is UUV's Course angle.

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

Gravitation potential field U of UUV and motion lash ship_att:

$U_{\mathrm{att}}=\frac{1}{2}{\mathrm{\ξ}}_1{p}_{\mathrm{rt}}^T{p}_{\mathrm{rt}}---\left(3\right)$

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

$U_{\mathrm{rep}}=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{\ξ}}_2{({\mathrm{\ρ}}_i^{-1}-{\mathrm{\ρ}}_0^{-1})}^2& \mathrm{if},{\mathrm{\ρ}}_i\≤{\mathrm{\ρ}}_0\\ 0& \mathrm{else}\end{array}\right.---\left(4\right)$

ρ represents the minimum range between UUV and i-th prohibited area, ρ₀For the repulsion operating distance in each prohibited area, ξ₁With ξ₂It is gravitation potential field and the weight factor of repulsion potential field respectively；

First research UUV is at the outer (ρ > ρ of prohibited area repulsion operating distance₀) motion planning, then research UUV a taboo (ρ < ρ in the repulsion operating distance of navigating area₀) motion planning.Finally, result of study is expanded to UUV motion rule during many prohibited areas Draw.

UUV motion planning when 3.1 prohibited areas are inoperative

When UUV is when prohibited area repulsion operating distance is outer, only gravitational field U_attThe speed planning of UUV is worked.UUV Speed v need makeRelative to p_rtGravitation potential field U_attNegative gradient direction:

${\stackrel{\&CenterDot;}{p}}_{\mathrm{rt}}=-{\&dtri;}_{\mathrm{prt}}{U}_{\mathrm{att}}=-{\left(\frac{{\&PartialD;U}_{\mathrm{att}}}{{\&PartialD;p}_{\mathrm{rt}}}\right)}^T---\left(5\right)$

Had by formula (3) and above formula:

V=v_tar+ξ₁p_rt(6) be equivalent to have following triangle relation to set up:

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

$\left|\right|v^{\&prime;}\left|\right|=\min {(({\left|\right|v_{\mathrm{tar}}\left|\right|}^2+{2\mathrm{\&xi;}}_1|\left|p_{\mathrm{rt}}\right|\left|\right|\left|v_{\mathrm{tar}}\right||\&times;\cos ({\mathrm{\&psi;}}_{\mathrm{tar}}-{\mathrm{\&psi;}}_{\mathrm{rt}}))+{\mathrm{\&xi;}}_1^2{\left|\right|p_{\mathrm{rt}}\left|\right|}^2)}^{\frac{1}{2}},v_{\max })---\left(8\right)$

In above formula, it is assumed that v_tar≠ 0 and | | p_rt| | ≠ 0, v_maxMaximal rate for UUV.

The course of UUV can be tried to achieve by formula (7) and formula (8):

$\mathrm{\&psi;}={\mathrm{\&psi;}}_{\mathrm{rt}}+\arcsin \left(\frac{\left|\right|v_{\mathrm{tar}}\left|\right|\sin ({\mathrm{\&psi;}}_{\mathrm{tar}}-{\mathrm{\&psi;}}_{\mathrm{rt}})}{\left|\right|v\left|\right|}\right)---\left(9\right)$

UUV motion planning time working in 3.2 1 prohibited areas

When UUV is worked by a prohibited area, gravitation potential field U_attWith repulsion potential field U_repAct simultaneously on UUV, by The conjunction potential field of UUV is:

$U=U_{\mathrm{att}}+U_{\mathrm{rep}}=\frac{1}{2}{\mathrm{\&xi;}}_1{p}_{\mathrm{rt}}^T{p}_{\mathrm{rt}}+\frac{1}{2}{\mathrm{\&xi;}}_2{({\mathrm{\&rho;}}^{-1}-{\mathrm{\&rho;}}_0^{-1})}^2---\left(10\right)$

So, the speed of UUV will makePoint to U relative to p_rtNegative direction.Now need:

$\min \left(\frac{\&PartialD;U}{{\&PartialD;p}_{\mathrm{rt}}}\frac{{\stackrel{\&CenterDot;}{p}}_{\mathrm{rt}}}{\left|\right|{\stackrel{\&CenterDot;}{p}}_{\mathrm{rt}}\left|\right|}\right)---\left(11\right)$

And meet:

$\frac{1}{{\left|\right|{\stackrel{\&CenterDot;}{p}}_{\mathrm{rt}}\left|\right|}^3}(\frac{\&PartialD;U}{{\mathrm{ax}}_{\mathrm{rt}}}{\stackrel{\&CenterDot;}{y}}_{\mathrm{rt}}-\frac{\&PartialD;U}{{\&PartialD;y}_{\mathrm{rt}}}{\stackrel{\&CenterDot;}{x}}_{\mathrm{rt}})\&times;({\stackrel{\&CenterDot;}{y}}_{\mathrm{rt}}\frac{{\&PartialD;\stackrel{\&CenterDot;}{x}}_{\mathrm{rt}}}{\&PartialD;\mathrm{\&psi;}}-{\stackrel{\&CenterDot;}{x}}_{\mathrm{rt}}\frac{{\&PartialD;\stackrel{\&CenterDot;}{y}}_{\mathrm{rt}}}{\&PartialD;\mathrm{\&psi;}})=0,\left|\right|{\stackrel{\&CenterDot;}{p}}_{\mathrm{rt}}\left|\right|\&NotEqual;0---\left(12\right)$

UUV tracking velocity is v_tarMotion lash ship, evade relative position is p simultaneously_roSpeed is v_roProhibited area, UUV Next step speed and course can be planned to:

$\left|\right|v\left|\right|={[{\left(\right|\left|v_{\mathrm{tar}}\right||\cos ({\mathrm{\&psi;}}_{\mathrm{tar}}-{\mathrm{\&psi;}}_{\mathrm{rt}})-\mathrm{\&beta;}|\left|v_{\mathrm{obs}}\right||\cos ({\mathrm{\&psi;}}_{\mathrm{obs}}-{\mathrm{\&psi;}}_{\mathrm{ro}})+{\mathrm{\&xi;}}_1|\left|p_{\mathrm{rt}}\right|\left|\right)}^2+{\left|\right|v_{\mathrm{tar}}\left|\right|}^2{\sin }^2({\mathrm{\&psi;}}_{\mathrm{tar}}-{\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}})]}^{\frac{1}{2}}---\left(13\right)$

$\mathrm{\&psi;}={\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}}+\arcsin \frac{\left|\right|v_{\mathrm{tar}}\left|\right|\sin ({\mathrm{\&psi;}}_{\mathrm{tar}}-{\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}})}{\left|\right|v\left|\right|}---\left(14\right)$

In formula, ${\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}}=\arctan \frac{{\sin \mathrm{\&psi;}}_{\mathrm{rt}}-{\mathrm{\&beta;}\sin \mathrm{\&psi;}}_{\mathrm{ro}}}{{\cos \mathrm{\&psi;}}_{\mathrm{rt}}-{\mathrm{\&beta;}\cos \mathrm{\&psi;}}_{\mathrm{ro}}},$ ψ_rt≠ψ_ro, $\mathrm{\&beta;}=\frac{\mathrm{\&eta;}\left|\right|p_{\mathrm{ro}}\left|\right|}{{\mathrm{\&xi;}}_1\left|\right|p_{\mathrm{rt}}\left|\right|},$ ||p_rt| | ≠ 0, $\mathrm{\&eta;}=\left\{\begin{array}{cc}{\mathrm{\&xi;}}_2({\mathrm{\&rho;}}^{-1}-{\mathrm{\&rho;}}_0^{-1})\left|\right|p_{\mathrm{ro}}\left|\right|& \mathrm{if},\mathrm{\&rho;}\&le;{\mathrm{\&rho;}}_0\\ 0& \mathrm{else}\end{array}\right.,$ ρ=| | p_ro||-r_obs, ξ₁>0,ξ₂> 0 it is weight factor.

UUV motion planning time working in more than 3.3 prohibited area

Obtaining the current prohibited area that UUV plays repulsion effect is j, j=(1, N), the repulsion gesture in j prohibited area To planning UUV next step speed and course work,

Next step speed v ' of UUV is:

$\begin{array}{c}\left|\right|v^{\&prime;}\left|\right|=[\left(\right|\left|v_{\mathrm{tar}}\right||\cos ({\mathrm{\&psi;}}_{\mathrm{tar}}-{\mathrm{\&psi;}}_{\mathrm{rt}})-\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_j|\left|{\mathrm{\&upsi;}}_{\mathrm{obs}\_j}\right||\cos ({\mathrm{\&psi;}}_{\mathrm{obs}\_j}-{\mathrm{\&psi;}}_{\mathrm{ro}\_j})\\ {+{{\mathrm{\&xi;}}_1\left|\right|p_{\mathrm{rt}}\left|\right|)}^2+{\left|\right|{\mathrm{\&upsi;}}_{\mathrm{tar}}\left|\right|}^2{\sin }^2({\mathrm{\&psi;}}_{\mathrm{tar}}-{\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}})]}^{\frac{1}{2}}\end{array}---\left(15\right)$

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 Relative motion p in jth prohibited area_{ro_j}Angle,

Next step course ψ ' of UUV is:

${\mathrm{\&psi;}}^{\&prime;}={\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}}+\arcsin \left(\frac{\left|\right|v_{\mathrm{tar}}\left|\right|\sin ({\mathrm{\&psi;}}_{\mathrm{tar}}-{\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}})}{\left|\right|v\left|\right|}\right)---\left(16\right)$

Wherein: ${\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{\mathrm{rt}}=\arctan \frac{{\sin \mathrm{\&psi;}}_{\mathrm{rt}}-\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_j\sin {\mathrm{\&psi;}}_{\mathrm{ro}\_j}}{\cos {\mathrm{\&psi;}}_{\mathrm{rt}}-\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_j\cos {\mathrm{\&psi;}}_{\mathrm{ro}\_j}},{\mathrm{\&beta;}}_j=\frac{{\mathrm{\&eta;}}_{j|}\left|\right|p_{\mathrm{ro}\_j}\left|\right|}{{\mathrm{\&xi;}}_1\left|\right|p_{\mathrm{rt}}\left|\right|},$ ||p_rt| | ≠ 0, ${\mathrm{\&eta;}}_j={\mathrm{\&xi;}}_2{\mathrm{\&rho;}}_j^{-2}{\left|\right|p_{\mathrm{ro}\_j}\left|\right|}^{-1}({\mathrm{\&rho;}}_j^{-1}-{\mathrm{\&rho;}}_0^{-1}).$

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 k, if not less than threshold value, repeats step 2～step 5, if less than threshold value, completes the UUV tracking to motion lash ship. Set UUV and catch up with the threshold k of motion lash ship as 10 meters.

Emulation case: before utilization, the motion planning method designed by each joint carries out the emulation that motion lash ship is followed the tracks of by UUV Checking, as shown in Figure 4.The range parameter setting four prohibited areas of lash ship in emulation is respectively as follows: the radius in circular prohibited area 1 and 4 For 50m, the major axis in oval prohibited area 3 and 4 is 100m, and short axle is 40m.Set follow the tracks of lash ship close to radius of circle as 10m. The minimum speed of UUV is set as 1 joint, and maximal rate is set as 4 joints, maximum deflection angular velocity 10deg/s.The initial of UUV is set Position is (50 ,-300) m, and the initial position of lash ship is (0,0) m, is set to docking stage institute close to radius of circle acceptable Big value: 10m.Course and speed in lash ship movement locus are changed by following two formulas respectively:

${\mathrm{\&psi;}}_t=1.7-\frac{t}{200}+{\left(\frac{t}{261500}\right)}^3+3\sin \left(0.002t\right)$

V_t=5 (sin (0.0002t)+0.109).

Claims (6)

1. 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.
2. 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:

   ${\stackrel{\&CenterDot;}{p}}_{rt}={\left[\begin{array}{cc}{\stackrel{\&CenterDot;}{x}}_{rt}& {\stackrel{\&CenterDot;}{y}}_{rt}\end{array}\right]}^T$

   Wherein:

   ${\stackrel{\&CenterDot;}{x}}_{rt}=\left|\right|v_{tar}\left|\right|{\mathrm{cos\&psi;}}_{tar}-\left|\right|v\left|\right|cos\mathrm{\&psi;}$

   ${\stackrel{\&CenterDot;}{y}}_{rt}=\left|\right|v_{tar}\left|\right|{\mathrm{sin\&psi;}}_{tar}-\left|\right|v\left|\right|sin\mathrm{\&psi;}$

   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}:

   ${\stackrel{\&CenterDot;}{p}}_{ro\_i}={\left[\begin{array}{cc}{\stackrel{\&CenterDot;}{x}}_{ro\_i}& {\stackrel{\&CenterDot;}{y}}_{ro\_i}\end{array}\right]}^T$

   Wherein:

   ${\stackrel{\&CenterDot;}{x}}_{ro\_i}=\left|\right|v_{obs\_i}\left|\right|{\mathrm{cos\&psi;}}_{obs\_i}-\left|\right|v\left|\right|cos\mathrm{\&psi;}$

   ${\stackrel{\&CenterDot;}{y}}_{ro\_i}=\left|\right|v_{obs\_i}\left|\right|{\mathrm{sin\&psi;}}_{obs\_i}-\left|\right|v\left|\right|sin\mathrm{\&psi;}$

   ψ_{obs_i}For the speed in the i-th prohibited area deflection under earth coordinates.
3. 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_{att}=\frac{1}{2}{\mathrm{\&xi;}}_1{p}_{rt}^T{p}_{rt}$

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

   $U_{rep\_i}=\left\{\begin{array}{cc}\frac{1}{2}{\mathrm{\&xi;}}_2{({{\mathrm{\&rho;}}_i}^{-1}-{\mathrm{\&rho;}}_0^{-1})}^2& \begin{array}{cc}if& {\mathrm{\&rho;}}_i\&le;{\mathrm{\&rho;}}_0\end{array}\\ 0& else\end{array}\right.$

   ρ_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:

   $\left|\right|v^{\&prime;}\left|\right|=\min {\left(\left(\left|\right|v_{tar}|{|}^2+2{\mathrm{\&xi;}}_1|\left|p_{rt}\right|\left|\right|\left|v_{tar}\right||\&times;cos\left({\mathrm{\&psi;}}_{tar}-{\mathrm{\&psi;}}_{rt}\right)\right)+{\mathrm{\&xi;}}_1^2\left|\right|p_{rt}|{|}^2\right)}^{\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:

   ${\mathrm{\&psi;}}^{\&prime;}={\mathrm{\&psi;}}_{rt}+arcsin\left(\frac{\left|\right|v_{tar}\left|\right|sin({\mathrm{\&psi;}}_{tar}-{\mathrm{\&psi;}}_{rt})}{\left|\right|v\left|\right|}\right)$

   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{array}{c}\left|\right|v^{\&prime;}\left|\right|=\&lsqb;\left(\right|\left|v_{tar}\right|\left|\cos \right({\mathrm{\&psi;}}_{tar}-{\mathrm{\&psi;}}_{rt})-\underset{j=1}{\overset{N}{\&Sigma;}}{\mathrm{\&beta;}}_j\left|\right|v_{obs\_j}\left|\right|\cos ({\mathrm{\&psi;}}_{obs\_j}-{\mathrm{\&psi;}}_{ro\_j})\\ +{\mathrm{\&xi;}}_1\left|\right|p_{rt}\left|\right|{)}^2+\left|\right|v_{tar}|{|}^2{\sin }^2({\mathrm{\&psi;}}_{tar}-{\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{rt}){\&rsqb;}^{\frac{1}{2}}\end{array}$

   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:

   ${\mathrm{\&psi;}}^{\&prime;}={\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{rt}+arcsin\frac{\left|\right|v_{tar}\left|\right|sin({\mathrm{\&psi;}}_{tar}-{\stackrel{\&OverBar;}{\mathrm{\&psi;}}}_{rt})}{\left|\right|v\left|\right|}$

   Wherein:
4. 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.
5. 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. 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.