DYNAMIC COLLISION AVOIDANCE METHOD FOR UNMANNED SURFACE VESSEL BASED ON ROUTE REPLANNING

Abstract

Disclosed is a dynamic collision avoidance method for an unmanned surface vessel based on route replanning. The method comprises the following steps: acquiring navigation information and pose information of a neighboring ship of an unmanned vessel itself via a vessel-borne sensor; constructing a collision cone between the unmanned vessel and the neighboring ship; introducing a degree of uncertainty with respect to observing movement information of the neighboring ship and applying a layer of soft constraint to the collision cone; applying a speed and a heading limit range of the unmanned vessel; acquiring an ultimate candidate speed set; introducing a cost function to select an optimum collision avoidance speed; and performing an internal recycle of navigation simulation with the optimum collision avoidance speed to obtain a route replanning point for dynamic collision avoidance of the unmanned vessel. According to the present invention, a dynamic collision avoidance strategy of the unmanned surface vessel is output in form of route replanning to meet constraints of international regulations for preventing collisions at sea, and it is well adapted to manipulate and control the unmanned vessel itself, so that a dynamic collision avoidance requirement of the unmanned vessel is met.

Claims

1. A dynamic collision avoidance method for an unmanned surface vessel based on route replanning, the method comprising the following steps: S1: collecting real-time data of a vessel-borne sensor, and acquiring geometric dimension information, movement information and pose information of an unmanned surface vessel body of a neighboring navigating ship; S2: describing geometric information of the neighboring navigating ship and the unmanned surface vessel by using an oriented bounding box in combination with the geometric dimension information and movement information of the neighboring navigating ship and pose information of the unmanned surface vessel body, and establishing a speed set when the unmanned surface vessel collides with the neighboring navigating ship, i.e., constructing a collision cone between the unmanned surface vessel and the neighboring navigating ship; S3: introducing the uncertainty of observing the velocity information of ships sailing in the neighborhood, translating the collision cone outwards at a quantity of an upper bound value of the degree of uncertainty, i.e., applying a layer of soft constraint to the collision cone, and taking the speed set out of constraint as a collision-free speed set; S4: solving a speed and a heading limit range of the unmanned surface vessel in combination with a dynamic characteristic of the unmanned surface vessel itself, and performing an intersection operation with the collision-free speed set to obtain a feasible collision-free speed set of the unmanned surface vessel; S5: dividing the feasible collision-free speed set according to different encountering situations in combination with the constraint of the international regulations for preventing collisions at sea to obtain an ultimate candidate speed set; S6: performing discretization on the candidate speed set, introducing a cost function to evaluate each speed therein, and selecting the speed with the smallest cost as an optimum collision avoidance speed; S7: performing an internal recycle of navigation simulation at the optimum collision avoidance speed to obtain a route replanning point for dynamic collision avoidance of the unmanned surface vessel.

2. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 1, wherein the step S1 specifically comprises: acquiring the geometric dimension information and the movement information of the neighboring navigating ship by using an AIS system, acquiring longitude and latitude of the unmanned surface vessel by using GPS and acquiring a three-dimensional course angle of the unmanned surface vessel by using an electronic compass.

3. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 1, wherein the step S2 specifically comprises: generating a smallest rectangular bonding frame along a main component direction of the unmanned surface vessel and the neighboring navigating ship to describe shapes of the unmanned surface vessel and the neighboring navigating ship uniformly and correspondingly, respectively; superposing the geometric dimension of the unmanned surface vessel itself to the neighboring navigating ship along a navigation direction, specifically, superposing the rectangular bounding frame of the unmanned surface vessel by taking a point on a side line of the rectangular bonding frame of the neighboring navigating ship as a geometric center and regarding a most peripheral region after superposing as a barrier region so as to regard the unmanned surface vessel as a mass point; and started from the mass point, drawing two tangent lines of the barrier region, regarding a region surrounded by the two tangent lines as a speed barrier region, wherein the tangent line is a ray started from a geometric center P.sub.u of the unmanned surface vessel at a relative speed v.sub.r:
λ(P.sub.u, v.sub.r)={P.sub.u+v.sub.rt|t≥0}  (1), wherein, in the formula (1), t represents a time, v.sub.r is a speed v.sub.r=v.sub.u−v.sub.o of the unmanned surface vessel relative to the neighboring navigating ship, P.sub.u and v.sub.u respectively represent a position vector and a speed vector of the unmanned surface vessel, and v.sub.o represents a speed vector of the neighboring navigating ship; when the ray λ(P.sub.u, v.sub.r) is intersected with the speed barrier region, i.e., the ray falls between the two tangent lines of the speed barrier region, it is thought that the unmanned surface vessel must collide with the neighboring navigating ship at a certain moment; defining a relative speed v.sub.r set when the unmanned surface vessel collides with the neighboring navigating ship as the collision cone RCC.sub.UO under the relative speed set; and defining an absolute speed v.sub.u set of all unmanned surface vessels when the unmanned surface vessel collides with the neighboring navigating ship as the collision cone VO.sub.UO under the absolute speed set:
VO.sub.UO=RCC.sub.UO⊕v.sub.o   (2), wherein ⊕ in the formula (2) is Minkowski vector and operation with actual physical meaning of translating a relative collision region RCC.sub.UO at the speed v.sub.o of the neighboring navigating ship.

4. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 3, wherein the step S3 specifically comprises: introducing a degree of uncertainty δ.sub.O of the speed of the neighboring navigating ship after constructing the collision cone; categorizing all uncertain portions in a parameter of the speed of the neighboring navigating ship as a set {W.sub.O|δ.sub.O∈W.sub.O}, the set W.sub.O is a bounded set and an upper bound thereof is a constant; and translating the collision cone outwards a quantity of an upper bound value of the degree of uncertainty, wherein a collision cone region WVO.sub.UO when the degree of uncertainty δ.sub.O is the maximum can be constructed:
WVO.sub.UO=VO.sub.UO⊕W.sub.O   (3), the speed set except WVO.sub.UO is marked as the collision-free speed set.

5. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 4, wherein the step S4 specifically comprises: introducing a concept of a dynamic window to further constrain a feasible speed space of the unmanned surface vessel in combination with the dynamic characteristic of the unmanned surface vessel itself; giving a time window Δt; and calculating a speed window v.sub.d and an angular speed window ω.sub.d capable of being reached by the unmanned surface vessel within a time Δt, $\begin{array}{cc}{v}_d=\left\{v.Math.v\in \left[v_c-{\stackrel{g}{v}}_b\Delta t,v_c+{\stackrel{g}{v}}_a\Delta t\right]\right\},& \left(4\right)\end{array}$ $\begin{array}{cc}{\omega }_d=\left\{\omega .Math.\omega \in \left[{\omega }_c-{\stackrel{g}{\omega }}_b\Delta t,{\omega }_c+{\stackrel{g}{\omega }}_a\Delta t\right]\right\},& \left(5\right)\end{array}$ wherein v and ω in the formulas (4) and (5) are respectively a speed and an angular speed of the unmanned surface vessel, v.sub.c and ω.sub.c respectively represent the current speed and angular speed of the unmanned surface vessel, and are respectively a maximum acceleration and an angular acceleration under the condition that the unmanned surface vessel is accelerated, and and are respectively a maximum acceleration and an angular acceleration under the condition that the unmanned surface vessel is decelerated, wherein $\begin{array}{cc}{v}_d\le \sqrt{2R_T{\stackrel{g}{v}}_b,}& \left(6\right)\end{array}$ $\begin{array}{cc}{\omega }_d\le \sqrt{2R_T{\stackrel{g}{\omega }}_b},& \left(7\right)\end{array}$ wherein R.sub.T in the formulas (6) and (7) is a relative distance between the unmanned surface vessel and the neighboring navigating ship, so that the unmanned surface vessel can stop before colliding with the encountering ship; the unmanned surface vessel can reach a heading window θ.sub.d within a time Δt: $\begin{array}{cc}{\theta }_d=\left\{\theta .Math.\theta \in \left[{\theta }_c+{\omega }_c\Delta t-\frac{1}{2}{\stackrel{g}{\omega }}_b\Delta t^2,{\theta }_c+{\omega }_c\Delta t+\frac{1}{2}{\stackrel{g}{\omega }}_a\Delta t^2\right]\right\},& \left(8\right)\end{array}$ wherein, in the formula (8), θ represents a heading angle of the unmanned surface vessel, and θ.sub.c represents a current heading angle of the unmanned surface vessel; constructing a feasible speed set of the unmanned surface vessel via the speed window v.sub.d and the heading window θ.sub.d, and performing intersection operation with the collision-free speed set obtained in the step S3 to obtain the feasible collision-free speed set RAV of the unmanned surface vessel.

6. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 5, the step S5 specifically comprises: introducing segment lines to divide the absolute speed set, i.e., an absolute speed space, of the unmanned surface vessel into four regions which are respectively WVO.sub.UO, V.sub.1, V.sub.2 and V.sub.3, wherein the segment lines are perpendicular to angular bisectors of the two tangent lines in the speed barrier region and are tangential to a bottom end of WVO.sub.UO, wherein the speed of the region V.sub.1 represents that the unmanned surface vessel passes through the neighboring navigating ship from a larboard, and a mathematical description of the region V.sub.1 is as follows:
V.sub.1={v|[(P.sub.o−P.sub.u)×(v−v.sub.o)].sub.z<0, v.Math.WVO.sub.UO, v.Math.V.sub.3}  (9), wherein, in the formula (9), [ ].sub.z is used for extracting an z axis component of a vector, herein the geometric center of the unmanned surface vessel is a circle point, an exact front is an x axial positive direction, an exact right direction is a y axial positive direction, and an z axial positive direction points to a principal plane inwards; the speed of the region V.sub.2 represents that the unmanned surface vessel passes through the neighboring navigating ship from a starboard, and a mathematical description of the region V.sub.2 is as follows:
V.sub.2={v|[(P.sub.o−P.sub.u)×(v−v.sub.o)].sub.z>0, v.Math.WVO.sub.UO, v.Math.V.sub.3}  (10), wherein the speed of the region V.sub.3 represents that the unmanned surface vessel departs from the neighboring navigating ship, and a mathematical description of the region V.sub.3 is as follows:
V.sub.3={v|(P.sub.o−P.sub.u).Math.(v−v.sub.o)<0, v.Math.WVO.sub.UO}  (11); obtaining the ultimate candidate speed set CRAV meeting the international regulations for preventing collisions at sea in combination with the international regulations for preventing collisions at sea; constraining the speed of the unmanned surface vessel in the region of V.sub.1 and V.sub.3 when the unmanned surface vessel and the neighboring navigating ship are in an overtaking or a left-intersection situation,
CRAV={v|v∈RAV, v∈V.sub.1∪V.sub.3}  (12); constraining the speed of the unmanned surface vessel in the region of V.sub.2 and V.sub.3 when the unmanned surface vessel and the neighboring navigating ship are in an encountering or a right-intersection situation,
CRAV={v|v∈RAV, v∈V.sub.2∪V.sub.3}  (13), wherein, in the formula (9) to the formula (11), P.sub.o represents a position vector of the neighboring navigating ship and v represents an absolute speed of the unmanned surface vessel.

7. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 6, wherein the step S6 specifically comprises: performing discretized grid processing on speed v and heading direction θ in the candidate speed set CRAV, specifically, discretizing the speed v into M discrete speeds according to equally spaced grids, discretizing the heading direction θ into N discrete heading directions according to the equally spaced grids, each discrete speed and discrete heading direction forming an integral speed vector, and finally, discretizing the whole set into M×N speed vectors, wherein M and N are natural numbers,
M=INT((v.sub.max−v.sub.min)/Δv)+1   (14),
N=INT(2π/Δθ)+1   (15), wherein, in the formulas (14) and (15), INT is a rounding function, v.sub.max and v.sub.min are maximum and minimum speeds of the unmanned surface vessel, Δv is a minimum speed variation of the unmanned surface vessel, and Δθ is a minimum heading angle variation of the unmanned surface vessel; constructing a cost function to evaluate M×N speed vectors, wherein with respect to each discrete speed v.sub.i and discrete heading direction θ.sub.j, 1≤i≤M, 1≤j≤N, and a cost thereof can be calculated according to formulas below: $\begin{array}{cc}{C}_{\mathrm{ij}}={\omega }_1{.Math.\left[\begin{array}{c}{v}_i\cos {\theta }_j\\ v_i\sin {\theta }_j\end{array}\right]-v_g.Math.}_2+{\omega }_2{.Math.\left[\begin{array}{c}{v}_i\cos {\theta }_j\\ v_i\sin {\theta }_j\end{array}\right]-\left[\begin{array}{c}{v}_c\cos {\theta }_c\\ v_c\sin {\theta }_c\end{array}\right].Math.}_2,& \left(16\right)\end{array}$ $\begin{array}{cc}{v}_g=v_c\frac{P_g-P_u}{{.Math.P_g-P_u.Math.}_2},& \left(17\right)\end{array}$ wherein, in the formulas (16) and (17), C.sub.ij is a cost evaluation function, v.sub.g is a target speed vector of the unmanned surface vessel to a next local sub target point, v.sub.c is a a current speed of the unmanned surface vessel, θ.sub.c is a current heading angle of the unmanned surface vessel, P.sub.g is a position vector of the next local sub target point, P.sub.u is a position vector of the unmanned surface vessel, ω.sub.1 and ω.sub.2 are two weight coefficient, the cost of the M×N speed vectors is calculated, and the speed v.sub.u.sup.opt with the minimum cost is taken as an optimum collision avoidance speed of the unmanned surface vessel.

8. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim 7, wherein ω.sub.1>ω.sub.2.

9. The dynamic collision avoidance method for an unmanned surface vessel based on route replanning according to claim7, wherein the step S7 specifically comprises: adding an internal recycle that calculates a route replanning point, detecting whether the target speed vector v.sub.g of the unmanned surface vessel arriving the next local sub target point is in the set CRAV or not in a process simulating that the unmanned surface vessel navigates at the speed v.sub.u.sup.opt, and if it is not in the CRAV set, performing a simulation process of a next time period, and performing circulation successively till v.sub.g∈CRAV is met; and calculating the position vector P.sub.u of the route replanning point according to the position vector P.sub.r in an initial simulation stage of the unmanned surface vessel and a total time consumed by simulation:
P.sub.r=P.sub.u+v.sub.u.sup.optnΔt   (18), wherein, in the formula (18), n represents a number of circulation times of simulation, and Δt represents a time period of simulation.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0044] FIG. 1 is a flow diagram of a dynamic collision avoidance method for an unmanned surface vessel based on route replanning.

[0045] FIG. 2 is a schematic diagram of constructing a collision cone VO.sub.OU under an absolute speed set in an embodiment.

[0046] FIG. 3 is a schematic diagram of constructing a collision cone region WVO.sub.UO when the uncertainty δ.sub.O is maximum in an embodiment.

[0047] FIG. 4 is a schematic diagram of dividing an absolute speed spatial region of an unmanned surface vessel in an embodiment.

DETAILED DESCRIPTION

[0048] Further description of the present invention in detail will be made below in combination with specific embodiments and drawings, but implementation modes of the present invention are not limited thereto.

Embodiment

[0049] The embodiment provides a dynamic collision avoidance method for an unmanned surface vessel based on route replanning, as shown in FIG. 1, including the following steps:

[0050] S1: real-time data of a vessel-borne sensor is collected, and key information in a dynamic collision avoidance process is acquired, specifically, the geometric dimension information and the movement information of the neighboring navigating ship are acquired by using the AIS system, longitude and latitude of the unmanned surface vessel are acquired by using GPS and a three-dimensional course angle of the unmanned surface vessel is acquired by using an electronic compass;

[0051] S2: the unmanned surface vessel and the neighboring navigating ship are described with the direction bounding box when the collision cone is constructed according to the geometric information and the movement information of the neighboring navigating ship and the unmanned surface vessel, as shown in FIG. 2. An oblique line filling rectangular frame U represents the unmanned surface vessel, a filling-free rectangular frame O represents the neighboring navigating ship, P.sub.u and v.sub.u respectively represent a position vector and a speed vector of the unmanned surface vessel, and P.sub.o and v.sub.o respectively represent a position vector and a speed vector of the neighboring navigating ship. There will be two relative conversion processes when the collision cone is constructed: one is relative conversion of the dimension information, which specifically includes: the rectangular bounding box of the unmanned surface vessel is superposed continuously by taking a point on an edge line of the rectangular bounding box of the neighboring navigating ship as a geometric center, and the most periphery region after superposing is regarded as a barrier region, so that the unmanned surface vessel is regarded as a mass point. Started from the mass point, two tangent lines of the barrier region are drawn, and a region surrounded by the two tangent lines is regarded as a speed barrier region. Another one is relative conversion of the movement information, which specifically includes: the spee v.sub.r=v.sub.u−v.sub.o of the unmanned surface vessel relative to the neighboring navigating ship is calculated, so that the neighboring navigating ship can be regarded as being static.

[0052] A ray started from a geometric center P.sub.u of the unmanned surface vessel at a relative speed v.sub.r is defined as:

λ(P.sub.u, v.sub.r)={P.sub.u+v.sub.rt|t≥0}  (1),

[0053] wherein t in the formula (1) represents time. Therefore, the actual physical meaning of the ray λ(P.sub.u, v.sub.r) represents a change condition of relative positions of the two ships under the circumstance that the unmanned surface vessel and the neighboring navigating ship keep unchanged moving conditions. When the ray λ(P.sub.u, v.sub.r) is intersected with the enlarged neighboring navigating ship, i.e., the ray falls between the two peripheral lines of the speed barrier region, it can be thought that the unmanned surface vessel must collide with the neighboring navigating ship at a certain moment. An absolute speed v.sub.r set of all unmanned surface vessels causing that the unmanned surface vessel collides with the navigating neighboring ship is defined as the collision cone RCC.sub.UO under the absolute speed set. Similarly, an absolute speed v.sub.u set of all unmanned surface vessels causing that the unmanned surface vessel collides with the navigating neighboring ship is defined as the collision cone VO.sub.UO under the absolute speed set:

VO.sub.UO=RCC.sub.UO⊕v.sub.o   (2),

[0054] wherein ⊕ in the formula (2) is Minkowski vector and operation with actual physical meaning of translating a relative collision region RCC.sub.UO at the speed v.sub.o of the neighboring navigating ship.

[0055] S3: a degree of uncertainty δ.sub.O of the speed of the neighboring navigating ship is introduced after the collision cone is constructed; all uncertain portions in a parameter of the speed of the neighboring navigating ship are categorized as a set {W.sub.O|δ.sub.O∈W.sub.O}, wherein the set W.sub.O is a bounded set and an upper bound thereof is a constant; so that a collision cone region WVO.sub.UO under the worst circumstance can be constructed:

WVO.sub.UO=VO.sub.UO⊕W.sub.O   (3),

[0056] wherein a schematic diagram of WVO.sub.UO is shown in FIG. 3; as the upper bound of the set of the degree of uncertainty of speed of the dynamic neighboring navigating ship is a constant, a round region filled with grid lines can be used to represent W.sub.O, and the WVO.sub.UO set at the moment is equivalent to be enlarged by a circle based on the original one, and the soft constraint of the W.sub.O herein is equivalent to provide the safe buffer effect to local obstacle avoidance of the unmanned surface vessel. The speed set except WVO.sub.UO is marked as the collision-free speed set.

[0057] S4: a concept of a dynamic window is introduced to further constrain a feasible speed space of the unmanned surface vessel in combination with a dynamic characteristic of the unmanned surface vessel itself; a time window Δt given; and a speed window v.sub.d and an angular speed window ω.sub.d capable of being reached by the unmanned surface vessel within a time Δt are calculated,

[00005]$\begin{array}{cc}{v}_d=\left\{v.Math.v\in \left[v_c-{\stackrel{g}{v}}_b\Delta t,v_c+{\stackrel{g}{v}}_a\Delta t\right]\right\},& \left(4\right)\end{array}$$\begin{array}{cc}{\omega }_d=\left\{\omega .Math.\omega \in \left[{\omega }_c-{\stackrel{g}{\omega }}_b\Delta t,{\omega }_c+{\stackrel{g}{\omega }}_a\Delta t\right]\right\},& \left(5\right)\end{array}$

[0058] wherein v and co in the formulas (4) and (5) are respectively a speed and an angular speed of the unmanned surface vessel, v.sub.c and ω.sub.c respectively represent the current speed and angular speed of the unmanned surface vessel, and are respectively a maximum acceleration and an angular acceleration under the condition that the unmanned surface vessel is accelerated, and and are respectively a maximum acceleration and an angular acceleration under the condition that the unmanned surface vessel is decelerated,

[00006]$\begin{array}{cc}{v}_d\le \sqrt{2R_T{\stackrel{g}{v}}_b},& \left(6\right)\end{array}$$\begin{array}{cc}{\omega }_d\le \sqrt{2R_T{\stackrel{g}{\omega }}_b},& \left(7\right)\end{array}$

[0059] wherein R.sub.T in the formulas (6) and (7) is a relative distance between the unmanned surface vessel and the neighboring navigating ship, so that the unmanned surface vessel can stop before colliding with the encountering ship;

[0060] the unmanned surface vessel can reach a heading window θ.sub.d within a time Δt:

[00007]$\begin{array}{cc}{\theta }_d=\left\{\theta .Math.\theta \in \left[{\theta }_c+{\omega }_c\Delta t-\frac{1}{2}{\stackrel{g}{\omega }}_b\Delta t^2,{\theta }_c+{\omega }_c\Delta t+\frac{1}{2}{\stackrel{g}{\omega }}_a\Delta t^2\right]\right\},& \left(8\right)\end{array}$

[0061] wherein, in the formula (8), θ represents a heading angle of the unmanned surface vessel, and θ.sub.c represents a current heading angle of the unmanned surface vessel;

[0062] constructing a feasible speed set of the unmanned surface vessel via the speed window v.sub.d and the heading window θ.sub.d, and performing intersection operation with the collision-free speed set obtained in the step S3 to obtain the feasible collision-free speed set RAV of the unmanned surface vessel.

[0063] S5: as shown in FIG. 4, segment lines are introduced to divide the absolute speed set, i.e., an absolute speed space, of the unmanned surface vessel into four regions which are respectively WVO.sub.UO, V.sub.1, V.sub.2 and V.sub.3, wherein the region of WVO.sub.UO is the enlarged collision cone region, and the segment lines are perpendicular to angular bisectors of the two tangent lines in the speed barrier region and are tangential to a bottom end of WVO.sub.UO,

[0064] wherein the speed of the region V.sub.1 represents that the unmanned surface vessel passes through the neighboring navigating ship from a larboard, and a mathematical description of the region V.sub.1 is as follows:

V.sub.1={v|[(P.sub.o−P.sub.u)×(v−v.sub.o)].sub.z<0, v.Math.WVO.sub.UO, v.Math.V.sub.3}  (9);

[0065] wherein, in the formula (9), [ ].sub.z is used for extracting an z axis component of a vector, herein the geometric center of the unmanned surface vessel is a circle point, an exact front is an x axial positive direction, an exact right direction is a y axial positive direction, and the z axial positive direction is points to a principal plane inwards;

[0066] the speed of the region V.sub.2 represents that the unmanned surface vessel passes through the neighboring navigating ship from a starboard, and a mathematical description of the region V.sub.2 is as follows:

V.sub.2={v|[(P.sub.o−P.sub.u)×(v−v.sub.o)].sub.z>0, v.Math.WVO.sub.UO, v.Math.V.sub.3}  (10);

[0067] wherein the speed of the region V.sub.3 represents that the unmanned surface vessel departs from the neighboring navigating ship, and a mathematical description of the region V.sub.3 is as follows:

V.sub.3={v|(P.sub.o−P.sub.u).Math.(v−v.sub.o)<0, v.Math.WVO.sub.UO}  (11);

[0068] the ultimate candidate speed set CRAV meeting the international regulations for preventing collisions at sea is obtained in combination with the international regulations for preventing collisions at sea;

[0069] the speed of the unmanned surface vessel is constrained in the region of V.sub.1 and V.sub.3 when the unmanned surface vessel and the navigating neighboring ship are in an overtaking or a left-intersection situation,

CRAV={v|v∈RAV, v∈V.sub.1∪V.sub.3}  (12);

[0070] the speed of the unmanned surface vessel is constrained in the region of V.sub.2 and .sup.V.sub.3 when the unmanned surface vessel and the navigating neighboring ship are in an encountering or a right-intersection situation,

CRAV={v|v∈RAV, v∈V.sub.2∪V.sub.3}  (13),

[0071] wherein, in the formula (9) to the formula (11), P.sub.o represents a position vector of the neighboring navigating ship, and v represents an absolute speed of the unmanned surface vessel.

[0072] S6: discretized grid processing is performed on speed v and heading direction θ in the candidate speed set, specifically, the speed v is discretized into M discrete speeds according to equally spaced grids, the heading direction θ is discretized into N discrete heading directions according to the equally spaced grids, each discrete speed and discrete heading direction forming an integral speed vector, and finally, the whole set is discretized to M×N speed vectors, wherein M and N are natural numbers,

M=INT((v.sub.max−v.sub.min)/Δv)+1   (14),

N=INT(2π/Δθ)+1   (15),

[0073] wherein, in the formulas (14) and (15), INT is a rounding function, v.sub.max and v.sub.min are maximum and minimum speeds of the unmanned surface vessel, Δv is a minimum speed variation of the unmanned surface vessel, and Δθ is a minimum heading angle variation of the unmanned surface vessel;

[0074] a cost function is constructed to evaluate M×N speed vectors, wherein with respect to each discrete speed v.sub.i and discrete heading direction θ.sub.j, 1≤i≤M, 1≤j≤N, and a cost thereof can be calculated according to formulas below:

[00008]$\begin{array}{cc}{C}_{\mathrm{ij}}={\omega }_1{.Math.\left[\begin{array}{c}{v}_i\cos {\theta }_j\\ v_i\sin {\theta }_j\end{array}\right]-v_g.Math.}_2+{\omega }_2{.Math.\left[\begin{array}{c}{v}_i\cos {\theta }_j\\ v_i\sin {\theta }_j\end{array}\right]-\left[\begin{array}{c}{v}_c\cos {\theta }_c\\ v_c\sin {\theta }_c\end{array}\right].Math.}_2,& \left(16\right)\end{array}$$\begin{array}{cc}{v}_g=v_c\frac{P_g-P_u}{{.Math.P_g-P_u.Math.}_2},& \left(17\right)\end{array}$

[0075] wherein, in the formulas (16) and (17), v.sub.g is a target speed vector of the unmanned surface vessel to a next local sub target point, v.sub.c is a current speed of the unmanned surface vessel, θ.sub.c is a current heading angle of the unmanned surface vessel, P.sub.g is a position vector of the next local sub target point, P.sub.u is a position vector of the unmanned surface vessel, ω.sub.1 and ω.sub.2 are two weight coefficient, a cost evaluation function C.sub.ij synthesizes influence of two dimensionalities: target orientation and speed variation, the speed set which is deviated from the target point and has large speed variation has a larger cost, and the two weight coefficient shall meet ω.sub.1>ω.sub.2 in considering target orientation of a local obstacle avoidance algorithm. The cost of the M×N speed vectors is calculated, and the speed v.sub.u.sup.opt with the minimum cost is taken as an optimum collision avoidance speed of the unmanned surface vessel.

[0076] S7: an internal recycle that calculates a route replanning point is added, whether the target speed vector v.sub.g of the unmanned surface vessel arriving the next local sub target point is in the set CRAV or not is continuously detected in a process simulating that the unmanned surface vessel navigates at the speed v.sub.u.sup.opt, and if it is not in the CRAV set, a simulation process of a next time period is performed, and circulation is performed successively till v.sub.g∈CRAV is met. The position vector P.sub.r of the route replanning point is calculated according to the position vector P.sub.u in an initial simulation stage of the unmanned surface vessel and a total time consumed by simulation:

P.sub.r=P.sub.u+v.sub.u.sup.optnΔt   (18),

[0077] wherein, in the formula (18), n represents a number of circulation times of simulation, and Δt represents a time period of simulation.

[0078] The embodiments are preferred modes of execution of the present invention. The modes of execution of the present invention are not limited by the above-mentioned embodiments. Any other changes, modifications, substitutions, combinations and simplifications made without departing from the spirit and principle of the present invention shall be equivalent substitute modes and shall come within the protection scope of the present invention.