CN112650232B - Reverse speed obstacle method dynamic obstacle avoidance method combined with COLRGES - Google Patents

Abstract

The invention provides a dynamic obstacle avoidance method combining with a COLRGES (COLRGES) reverse speed obstacle method, which comprises the steps of realizing local dynamic obstacle avoidance of an unmanned ship based on the reverse speed obstacle method; based on an inverse speed obstacle method, a conflict detection function is introduced to detect whether collision danger exists between the unmanned ship and the obstacle in real time; based on the marine obstacle avoidance rules, an obstacle avoidance strategy is added when the ship is in a dangerous early warning area, and the decision of the ship after the ship is out of the marine rules is defined. The invention applies the reverse speed obstacle method to the local dynamic obstacle avoidance of the unmanned ship, so that the unmanned ship is prevented from error interference caused by self-state detection, and the local obstacle avoidance capability of the unmanned ship is improved; the collision detection function of the unmanned ship enhances the efficiency of the unmanned ship in obstacle avoidance decision, and improves the capability of local obstacle avoidance; the ship can quickly take more efficient countermeasures to the obstacle avoidance in the dangerous early warning area. The reverse speed obstacle method is combined with the improved maritime rule, so that the high efficiency of dynamic obstacle avoidance of the unmanned ship in a local environment is improved, and the safety of the unmanned ship during navigation is ensured.

Description

A Dynamic Obstacle Avoidance Method Based on Inverse Velocity Obstacle Method Combined with COLRGES

technical field

The present invention relates to the technical field of ship application, in particular to a dynamic obstacle avoidance method combined with COLRGES inverse speed obstacle method.

Background technique

The speed obstacle method is a method applied to the dynamic collision avoidance of surface unmanned vehicles. Its principle is to establish an unmanned vehicle-obstacle UO environment model after expanding the obstacles and unmanned vehicles to determine the collision area. As shown in Figures 3 and 4, the UO coordinate system is established. At time t, the unmanned boat emits rays L ₁ and L ₂ tangentially to both sides of the obstacle to form a velocity collision cone; the speed of the unmanned boat A and obstacle B are V _A and V _B respectively, and the relative velocity of the two is V _AB = _VA -V _B , if the relative velocity of the two is within the velocity collision cone, it is considered that the two will collide at some point in the future, so we need Change the relative velocity of the two so that V _AB breaks away from the velocity collision cone.

When the speed obstacle method is running, it will always detect the speed and relative position of the unmanned boat and the dynamic obstacle itself, which leads to the fact that when the unmanned boat is performing self-state detection, it will be affected by the interference and detection of the device itself. The error will lead to errors in the detected speed and position information of the unmanned vehicle, which will cause errors in the selection of the relative speed, resulting in collisions between the unmanned vehicle and dynamic obstacles in the near future.

Contents of the invention

According to the above-mentioned technical problems, a dynamic obstacle avoidance method of inverse speed obstacle method combined with COLRGES is provided.

The technical means adopted in the present invention are as follows:

A dynamic obstacle avoidance method of inverse speed obstacle method combined with COLRGES, including:

S1. Based on the inverse speed obstacle method, realize the local dynamic obstacle avoidance of the unmanned ship;

S2. Based on the inverse speed obstacle method, the collision detection function f(*) is introduced to detect whether there is a collision risk between the unmanned ship and the obstacle in real time;

S3. Based on the maritime obstacle avoidance rules, add the obstacle avoidance strategy when the ship is in the danger warning area, and clarify the decision-making of the ship after breaking away from the maritime rules.

Further, the step S1 specifically includes:

S11. Assuming that the unmanned ship is stationary, establish a framework centered on the unmanned ship;

S12. Turn the obstacle into points, denoted by A, expand the unmanned ship, and expand the unmanned ship into a disc with a radius of R, denoted by B, where R is the radius of the obstacle and the unmanned the sum of the radii of the ship;

S13. Two rays are emitted from point A and are tangent to disk B, forming a velocity collision cone;

S14. During the driving process of the unmanned ship, if there is an obstacle in the conventional obstacle avoidance area of the unmanned ship, assuming that at the instant t, the speed of the obstacle is V _A , and the unmanned ship is stationary at this time, that is, V _B = 0, then the relative speed V AB of the unmanned ship and the obstacle V _AB = V _A -V _B = V _A ;

S15. If V _AB is located in the velocity collision cone, it is considered that the unmanned ship will collide with obstacles, and artificially give the unmanned ship a speed control u, so that V _AB = V _A + u, so that the relative speed is out of the speed collision cone , the unmanned ship realizes collision avoidance.

Further, the step S2 specifically includes:

S21. Using the direction guidance function f(/), the unmanned ship is always moving towards the direction of the target point at an optimal speed;

S22. During the driving process, use the conflict detection function f(*) to check whether the unmanned ship will collide with the obstacle. When f(*)≤0, it means that the unmanned ship will collide with the obstacle;

S23. When it is detected that there is a risk of collision between the unmanned ship and the obstacle, use the conflict resolution function f( ) to guide the relative speed of the unmanned ship and the obstacle out of the speed collision cone, so that the unmanned ship and the obstacle avoid collision .

Further, the direction guidance function f(/) in the step S21 is specifically:

In the above formula, f(/) represents the direction guidance function, V _desired represents the optimal speed of the unmanned ship heading to the target point, V _r represents the current speed of the unmanned ship, λ represents the smoothing factor for adjusting the speed transformation of the unmanned ship, λ||u|| ² means that in order to control the smoothness of the speed change of the unmanned ship, make the speed change smoother; where:

In the above formula, g ^r represents the relative position of the obstacle centered on the unmanned ship under the frame; |g ^r | represents the module length of the relative position, Indicates the direction of the speed, and the result is +1 or -1, +1 means that the optimal speed of the unmanned ship should point to the positive direction of the y-axis, -1 means that the optimal speed of the unmanned ship should point to the negative direction of the y-axis; v _max Indicates the maximum speed of the unmanned ship, expressed as a scalar;

In the above formula, u represents the instantaneous speed that is artificially input to the unmanned ship under the frame; r represents the position of the obstacle seen from the frame of the agent at time t, and the agent is located at the origin in the frame and at momentarily at rest, and /> Respectively represent the position of the obstacle under the frame relative to the unmanned ship on the x-axis and y-axis; v represents the relative velocity of the obstacle with the unmanned ship as the coordinate origin, /> and /> Respectively represent the speed of the obstacle under the frame relative to the unmanned ship on the x-axis and y-axis.

Further, the conflict detection function f(*) in the step S22 is specifically:

In the above formula, f(*) represents the function of detecting whether the unmanned ship will collide with obstacles, r represents the distance between the unmanned ship and the obstacle at time t; The radius of the circle, V _r represents the projection of the relative speed of the unmanned ship and the obstacle on the AB axis, and V _θ represents the projection of the relative speed of the unmanned ship and the obstacle on the r ^⊥ axis; where:

In the above formula, Indicates the relative speed of the unmanned ship and the obstacle, represented by a vector, /> represents a unit vector on the AB axis, /> is the unit vector on the r ^⊥ axis; project the relative velocity of the unmanned ship and the obstacle onto the AB axis and the r ^⊥ axis to generate their respective relative velocity components.

Further, the conflict resolution function f(·) in the step S23 is specifically:

In the above formula, f(·) represents the conflict resolution function that helps the unmanned ship avoid collision with obstacles. The manned ship removes the risk of collision with the obstacle, r represents the position of the obstacle seen from the frame of the agent at time t, r ^T is the transpose of r, ||r|| represents the distance between the unmanned ship and the obstacle distance, v represents the relative velocity of the obstacle under the frame centered on the unmanned ship, ||v|| The radius of the circle.

Further, the step S2 also includes the reasoning steps of the conflict detection function f(*) and the conflict resolution function f(·):

Step 1: Deduce the outlier detection function f(*);

According to the speed collision cone formed by the unmanned ship and the obstacle, it can be known that:

right processed to get:

A known Simplifies the listing to:

because get:

Substitute the above into the formula , get:

when When , that is, the relative speed of the unmanned ship and the obstacle is within the speed collision cone, it means that the two will collide, so there is /> That is, when /> , the unmanned ship will collide with the obstacle;

Define f(*): As the conflict detection condition, when , the unmanned ship collides with the obstacle;

Step 2: Deduce the conflict resolution function f(·);

The triangle formed by the unmanned ship and the obstacle shows that:

Processing the above formula, we get:

also known get:

F Substituting it into the above formula, we get:

Then there are:

when , the unmanned ship and obstacles will avoid collisions;

Define f(·): is the condition for conflict resolution, when , the unmanned ship and the obstacle are out of the danger of collision.

Further, the step S3 specifically includes:

Combined with the ship area division and encounter situation of the unmanned ship, the obstacle avoidance situation is divided into six situations, which are the encounter situation, cross encounter situation, overtaking situation in the conventional obstacle avoidance area, and the encounter situation, intersection situation and crossover situation in the danger warning area. encounter situation, overtaking situation;

The encounter situation in the conventional obstacle avoidance area specifically includes:

θ∈(0°,15°), there is an obstacle on the right side of the USV, and the USV gives way;

θ∈(0°,-15°), there is an obstacle on the left side of the USV, and the USV goes straight;

The COLREGS Convention stipulates that in the conventional obstacle avoidance area, the ship on the right side of the ship has the priority to pass; that is, when the ship appears on the right side of the ship, the ship acts as a give-way ship first, and when the ship appears on the left side of the ship, the ship acts as a straight-going ship Priority pass.

The intersection encounter situation in the conventional obstacle avoidance area specifically includes:

When the obstacle is on the right side of the USV, there are:

V _USV >V ₀ , θ∈(15°,90°), USV goes straight;

V _USV >V ₀ , θ∈(90°,135°), USV goes straight;

V _USV <V ₀ , θ∈(15°,90°), USV avoidance;

V _USV <V ₀ , θ∈(90°,135°), USV goes straight;

When the obstacle is on the left side of the USV, there are:

When there is no danger, the USV will go straight and avoid obstacles;

The situation of overtaking in the conventional obstacle avoidance area specifically includes:

The USV does not need to avoid, but the overtaking ship must avoid the USV;

The encounter situation in the danger warning area specifically includes:

θ∈(-15°,15°), there is an obstacle in front of the USV, and the USV decelerates and drives to the right first;

The cross-encounter situation in the danger warning area specifically includes:

When the obstacle is on the right side of the USV, there are:

V _USV >V ₀ , θ∈(15°,90°), the USV accelerates and drives to the left;

V _USV >V ₀ , θ∈(90°,135°), USV accelerates and goes straight;

V _USV <V ₀ , θ∈(15°,90°), USV decelerates and drives to the right;

V _USV <V ₀ , θ∈(90°,135°), the USV decelerates and drives to the left;

When the obstacle is on the left side of the USV, there are:

V _USV >V ₀ , θ∈(-15°,-90°), USV accelerates to the right;

V _USV >V ₀ , θ∈(-90°,-135°), USV accelerates and goes straight;

V _USV <V ₀ , θ∈(-15°,-90°), USV decelerates and drives to the left;

V _USV <V ₀ , θ∈(-90°,-135°), the USV decelerates and travels to the right.

Compared with the prior art, the present invention has the following advantages:

1. The inverse speed obstacle method dynamic obstacle avoidance method combined with COLRGES provided by the present invention uses the inverse speed obstacle method for the local dynamic obstacle avoidance of unmanned ships. Compared with the speed obstacle method, its advantage is that it can make unmanned ships avoid The self-state detection is subject to error interference, which greatly improves the local obstacle avoidance ability of the unmanned ship.

2. The inverse speed obstacle method dynamic obstacle avoidance method combined with COLRGES provided by the present invention improves the original inverse speed obstacle method, and proposes a conflict detection function on the basis of the original formula, which enhances the obstacle avoidance ability of unmanned ships. The efficiency of decision-making improves the ability of local obstacle avoidance.

3. The inverse velocity obstacle method dynamic obstacle avoidance method combined with COLRGES provided by the present invention, on the basis of the original maritime obstacle avoidance rules, increases the obstacle avoidance strategy when the ship is in the danger warning area, and clarifies that the ship will The decision-making enables the ship to quickly take more efficient countermeasures for obstacle avoidance in the danger warning area.

4. The inverse speed obstacle method dynamic obstacle avoidance method combined with COLRGES provided by the present invention combines the inverse speed obstacle method with the improved maritime rules, which greatly improves the efficiency of dynamic obstacle avoidance of unmanned ships in local environments Sex, which further guarantees the safety of unmanned ships when sailing.

Based on the above reasons, the present invention can be widely promoted in the fields of ship application and the like.

Description of drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description These are some embodiments of the present invention. For those skilled in the art, other drawings can also be obtained according to these drawings without any creative effort.

Fig. 1 is a flow chart of the method of the present invention.

Fig. 2 is a model diagram of the inverse speed obstacle method provided by the embodiment of the present invention.

Fig. 3 is a model diagram of the speed obstacle method provided by the embodiment.

Fig. 4 is a schematic diagram of the speed obstacle method provided by the embodiment.

Fig. 5 is a schematic diagram of the reverse velocity obstacle method provided by the embodiment of the present invention.

Fig. 6 is a schematic diagram of the conflict judgment and conflict resolution of the inverse speed obstacle method provided by the embodiment of the present invention.

Fig. 7 is a schematic diagram of the direction guidance of the reverse speed obstacle method provided by the embodiment of the present invention.

Fig. 8 is the judgment of ship area and encounter situation provided by the embodiment of the present invention.

Fig. 9 is a simulation result diagram of the inverse velocity obstacle method combined with the COLREGS convention provided by the embodiment of the present invention.

Detailed ways

In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only It is an embodiment of a part of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts shall fall within the protection scope of the present invention.

It should be noted that the terms "first" and "second" in the description and claims of the present invention and the above drawings are used to distinguish similar objects, but not necessarily used to describe a specific sequence or sequence. It is to be understood that the data so used are interchangeable under appropriate circumstances such that the embodiments of the invention described herein can be practiced in sequences other than those illustrated or described herein. Furthermore, the terms "comprising" and "having", as well as any variations thereof, are intended to cover a non-exclusive inclusion, for example, a process, method, system, product or device comprising a sequence of steps or elements is not necessarily limited to the expressly listed instead, may include other steps or elements not explicitly listed or inherent to the process, method, product or apparatus.

As shown in Figure 1, the present invention provides a dynamic obstacle avoidance method combined with COLRGES inverse speed obstacle method, including:

S1, as shown in Figure 2, is a model diagram of the inverse velocity obstacle method, based on the inverse velocity obstacle method, the local dynamic obstacle avoidance of the unmanned ship is realized;

During specific implementation, as a preferred embodiment of the present invention, as shown in Figure 5, it is a schematic diagram of the reverse velocity obstacle method, and the step S1 specifically includes:

S11. Assuming that the unmanned ship is stationary, establish a framework centered on the unmanned ship;

S12. As shown in FIG. 5 , treat the obstacle as a point, denoted by A, expand the unmanned ship, and expand the unmanned ship into a disc with a radius of R, denoted by B, where R is The sum of the obstacle radius and the radius of the unmanned ship;

S13. Two rays are emitted from point A and are tangent to disk B, forming a velocity collision cone;

S14. During the driving process of the unmanned ship, if there is an obstacle in the conventional obstacle avoidance area of the unmanned ship, assuming that at the instant t, the speed of the obstacle is V _A , and the unmanned ship is stationary at this time, that is, V _B = 0, then the relative speed V AB of the unmanned ship and the obstacle V _AB = V _A -V _B = V _A ;

S15. If V _AB is located in the velocity collision cone, it is considered that the unmanned ship will collide with obstacles, and artificially give the unmanned ship a speed control u, so that V _AB = V _A + u, so that the relative speed is out of the speed collision cone , the unmanned ship realizes collision avoidance.

S2. Based on the inverse speed obstacle method, the collision detection function f(*) is introduced to detect whether there is a collision risk between the unmanned ship and the obstacle in real time;

During specific implementation, as a preferred embodiment of the present invention, the step S2 specifically includes:

S21. As shown in FIG. 7 , it is a principle diagram of the direction guidance of the inverse speed obstacle method, and the unmanned ship is always moving towards the direction of the target point at an optimal speed by using the direction guidance function f(/);

The direction guide function f(/) in the step S21 is specifically:

In the above formula, f(/) represents the direction guidance function, V _desired represents the optimal speed of the unmanned ship heading to the target point, V _r represents the current speed of the unmanned ship, λ represents the smoothing factor for adjusting the speed transformation of the unmanned ship, λ||u|| ² means that in order to control the smoothness of the speed change of the unmanned ship, make the speed change smoother; where:

In the above formula, g ^r represents the relative position of the obstacle centered on the unmanned ship under the frame; |g ^r | represents the module length of the relative position, Indicates the direction of the speed, and the result is +1 or -1, +1 means that the optimal speed of the unmanned ship should point to the positive direction of the y-axis, -1 means that the optimal speed of the unmanned ship should point to the negative direction of the y-axis; v _max Indicates the maximum speed of the unmanned ship, expressed as a scalar;

In the above formula, u represents the instantaneous speed that is artificially input to the unmanned ship under the frame; r represents the position of the obstacle seen from the frame of the agent at time t, and the agent is located at the origin in the frame and at momentarily at rest, and /> respectively represent the position of the obstacle under the frame relative to the unmanned ship on the x-axis and y-axis; v represents the relative velocity of the obstacle with the unmanned ship as the coordinate origin, because in the frame centered on the unmanned By default, the unmanned ship is stationary at instant t, so the speed of the unmanned ship is 0 at time t. In order to avoid the risk of collision of the unmanned ship, it is necessary to manually input the speed control u at time t to make the relative speed out of Velocity collision outside the cone. Therefore, the relative speed is represented by the speed of the obstacle minus the speed of the human input. where /> and /> Respectively represent the speed of the obstacle under the frame relative to the unmanned ship on the x-axis and y-axis.

S22. As shown in FIG. 6, it is a schematic diagram of the conflict judgment and conflict resolution of the inverse speed obstacle method. During the driving process, use the conflict detection function f(*) to check whether the unmanned ship will collide with the obstacle. When f(* )≤0, it means that the unmanned ship will collide with obstacles;

The conflict detection function f(*) in the step S22 is specifically:

In the above formula, f(*) represents the function of detecting whether the unmanned ship will collide with obstacles, r represents the distance between the unmanned ship and the obstacle at time t; The radius of the circle, V _r represents the projection of the relative speed of the unmanned ship and the obstacle on the AB axis, and V _θ represents the projection of the relative speed of the unmanned ship and the obstacle on the r ^⊥ axis; where:

In the above formula, Indicates the relative speed of the unmanned ship and the obstacle, represented by a vector, /> represents a unit vector on the AB axis, /> is the unit vector on the r ^⊥ axis; project the relative velocity of the unmanned ship and the obstacle onto the AB axis and the r ^⊥ axis to generate their respective relative velocity components.

S23. When it is detected that there is a risk of collision between the unmanned ship and the obstacle, use the conflict resolution function f( ) to guide the relative speed of the unmanned ship and the obstacle out of the speed collision cone, so that the unmanned ship and the obstacle avoid collision .

The conflict resolution function f(·) in step S23 is specifically:

In the above formula, f(·) represents the conflict resolution function that helps the unmanned ship avoid collision with obstacles. The manned ship removes the risk of collision with the obstacle, r represents the position of the obstacle seen from the frame of the agent at time t, r ^T is the transpose of r, ||r|| represents the distance between the unmanned ship and the obstacle distance, v represents the relative velocity of the obstacle under the frame centered on the unmanned ship, ||v|| The radius of the circle.

During specific implementation, as a preferred embodiment of the present invention, the step S2 also includes a reasoning step of a conflict detection function f(*) and a conflict resolution function f( ):

Step 1: Deduce the outlier detection function f(*);

As shown in Figure 6, according to the speed collision cone formed by the unmanned ship and the obstacle, it can be known that:

right processed to get:

A known Simplifies the listing to:

because get:

Substitute the above into the formula , get:

when When , that is, the relative speed of the unmanned ship and the obstacle is within the speed collision cone, it means that the two will collide, so there is /> That is, when /> , the unmanned ship will collide with the obstacle;

Define f(*): As the conflict detection condition, when , the unmanned ship collides with the obstacle;

Step 2: Deduce the conflict resolution function f(·);

As shown in Figure 6, the triangle formed by the unmanned ship and obstacles shows that:

Processing the above formula, we get:

also known get:

F Substituting it into the above formula, we get:

Then there are:

when , the unmanned ship and obstacles will avoid collisions;

Define f(·): is the condition for conflict resolution, when , the unmanned ship and the obstacle are out of the danger of collision.

S3. Based on the maritime obstacle avoidance rules, add the obstacle avoidance strategy when the ship is in the danger warning area, and clarify the decision-making of the ship after breaking away from the maritime rules.

The original International Maritime Collision Prevention Regulations stipulated:

1. Overtaking: When any ship is overtaking another ship, it should give way to the overtaken ship.

2. Confrontation situation: When two ships meet on the opposite or nearly opposite course and there is a risk of collision, each should turn to starboard so as to pass the other ship's port side.

3. Crossing encounter situation: When ships crossing and encountering pose a danger, the ship with another ship on the starboard side of the own ship should give way to the other ship, and when the environment permits, it should also avoid crossing the front of the other ship.

On the basis of the original maritime rules, for the collision avoidance behavior of unmanned ships in the dangerous area of ships, a detailed description is made according to the relative speed of unmanned ships and obstacles.

During specific implementation, as a preferred embodiment of the present invention, the step S3 specifically includes:

As shown in Figure 8, combined with the division of the unmanned ship's ship area and the encounter situation, the obstacle avoidance situation is divided into six situations, which are the encounter situation in the conventional obstacle avoidance area, the cross encounter situation, the overtaking situation, and the danger warning area. encounter situations, cross-encounter situations, and overtake situations;

When an obstacle enters the routine obstacle avoidance area of the unmanned ship, the unmanned ship starts to take obstacle avoidance measures. If the obstacle enters the danger warning area of the unmanned ship, it only relies on the routine obstacle avoidance of the unmanned ship and the COLREGS convention It is impossible to get out of danger. At this time, it is necessary to break away from the requirements of the COLREGS convention and perform autonomous obstacle avoidance according to the actual encounter situation. specific:

The encounter situation in the conventional obstacle avoidance area specifically includes:

θ∈(0°,15°), there is an obstacle on the right side of the USV, and the USV gives way;

θ∈(0°,-15°), there is an obstacle on the left side of the USV, and the USV goes straight;

The COLREGS Convention stipulates that in the conventional obstacle avoidance area, the ship on the right side of the ship has the priority to pass; that is, when the ship appears on the right side of the ship, the ship acts as a give-way ship first, and when the ship appears on the left side of the ship, the ship acts as a straight-going ship Priority pass.

The intersection encounter situation in the conventional obstacle avoidance area specifically includes:

When the obstacle is on the right side of the USV, there are:

V _USV >V ₀ , θ∈(15°,90°), USV goes straight;

V _USV >V ₀ , θ∈(90°,135°), USV goes straight;

V _USV <V ₀ , θ∈(15°,90°), USV avoidance;

V _USV <V ₀ , θ∈(90°,135°), USV goes straight;

In the conventional obstacle avoidance area, when two ships cross and meet, the unmanned ship decides whether to avoid according to the relationship between the orientation and relative speed of the two ships; when there is an obstacle on the right side of the unmanned ship, and the When the angle θ∈(15°, 90°) formed by the ship, the relationship between the speed of the two ships needs to be considered; when the speed of the unmanned ship is greater than the speed of the obstacle, the unmanned ship will pass first; when the speed of the unmanned ship is less than the speed of the obstacle When the speed of the object is high, the unmanned ship gives priority to avoiding obstacles; when the angle θ∈(90°, 135°) formed by the obstacle and the unmanned ship does not need to consider the relationship between the two speeds, the unmanned ship has priority. through power;

When the obstacle is on the left side of the USV, there are:

When there is no danger, the USV will go straight and avoid obstacles;

The situation of overtaking in the conventional obstacle avoidance area specifically includes:

The USV does not need to avoid, but the overtaking ship must avoid the USV;

The encounter situation in the danger warning area specifically includes:

θ∈(-15°,15°), there is an obstacle in front of the USV, and the USV decelerates and drives to the right first;

The cross-encounter situation in the danger warning area specifically includes:

When the obstacle is on the right side of the USV, there are:

V _USV >V ₀ , θ∈(15°,90°), the USV accelerates and drives to the left;

V _USV >V ₀ , θ∈(90°,135°), USV accelerates and goes straight;

V _USV <V ₀ , θ∈(15°,90°), USV decelerates and drives to the right;

V _USV <V ₀ , θ∈(90°,135°), the USV decelerates and drives to the left;

In the danger warning area, if there is an obstacle in the θ∈(15°, 90°) area on the right side of the unmanned ship, when the speed of the unmanned ship is greater than the speed of the obstacle, the unmanned ship will accelerate to the left and pass quickly; When the speed of the manned ship is lower than the speed of the obstacle, the unmanned ship will slow down and drive to the right to let the obstacle pass first; if there is an obstacle in the θ∈(90°,135°) area on the right side of the unmanned ship, when the speed of the unmanned ship is greater than When the speed of the obstacle is high, the unmanned ship can accelerate to go straight and pass quickly; when the speed of the unmanned ship is lower than the speed of the obstacle, the unmanned ship needs to slow down and drive to the left to let the obstacle pass first;

When the obstacle is on the left side of the USV, there are:

V _USV >V ₀ , θ∈(-15°,-90°), USV accelerates to the right;

V _USV >V ₀ , θ∈(-90°,-135°), USV accelerates and goes straight;

V _USV <V ₀ , θ∈(-15°,-90°), USV decelerates and drives to the left;

V _USV <V ₀ , θ∈(-90°,-135°), the USV decelerates and travels to the right.

In the danger warning area, there is an obstacle in the θ∈(-15°, -90°) area on the left side of the unmanned ship. When the speed of the unmanned ship is greater than the speed of the obstacle, the unmanned ship accelerates to the right and passes quickly; When the speed of the unmanned ship is less than the speed of the obstacle, the unmanned ship slows down and drives to the left, allowing the obstacle to pass first; When the speed of the manned ship is greater than the speed of the obstacle, the unmanned ship accelerates to pass straight ahead; when the speed of the unmanned ship is lower than the speed of the obstacle, the unmanned ship slows down and drives to the right to let the obstacle pass first.

In order to verify the effectiveness of the method of the present invention, a simulation experiment was carried out, and the result of the simulation experiment is shown in FIG. 9 .

Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than limiting them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: It is still possible to modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the various embodiments of the present invention. scope.

Claims (3)

1. The reverse speed obstacle avoidance method combined with the COLRGES is characterized by comprising the following steps of:

s1, realizing local dynamic obstacle avoidance of an unmanned ship based on a reverse speed obstacle method;

the step S1 specifically includes:

s11, assuming that the unmanned ship is stationary, establishing a frame taking the unmanned ship as a center;

s12, performing dotting treatment on the barrier substance, which is denoted by A, performing puffing treatment on the unmanned ship, puffing the unmanned ship into a disc with a radius of R, and denoted by B, wherein R is the sum of the radius of the barrier and the radius of the unmanned ship;

s13, two rays from the point A are tangent to the disk B to form a speed collision cone;

s14, in the running process of the unmanned ship, if an obstacle appears in a conventional obstacle avoidance area of the unmanned ship, the speed of the obstacle is assumed to be V at the instant t _A At this time, the unmanned ship is stationary, i.e. V _B =0, then the relative speed V of the unmanned ship and the obstacle _AB ＝V _A -V _B ＝V _A ；

S15, if V _AB If the speed collision cone is positioned in the speed collision cone, the unmanned ship is considered to collide with the obstacle, and the speed control u is manually controlled for the unmanned ship to enable V to be _AB ＝V _A +u, make the relative speed break away from the speed collision awl, the unmanned ship realizes the collision avoidance;

s2, based on an inverse speed obstacle method, introducing a conflict detection function f (x), and detecting whether collision danger exists between the unmanned ship and the obstacle in real time;

the step S2 specifically includes:

s21, utilizing a direction guiding function f (/) to enable the unmanned ship to always advance towards the direction of the target point at an optimal speed;

the direction guide function f (/) in step S21 is specifically:

in the above formula, f (/) represents a direction guide function, V _desired Representing the optimal speed of the unmanned ship towards the target point, V _r Representing the current speed of the unmanned ship, lambda represents a smoothing factor that adjusts the speed transformation of the unmanned ship, lambda u ² To control the smoothness of the unmanned ship speed change, the speed change is smoother; wherein:

in the above, g ^r Representing the relative position of an obstacle centered on the unmanned ship under said frame; g I ^r The i indicates the modular length of the relative position,indicating the direction of the speed, the result is +1 or-1, +1 indicates that the optimal speed of the unmanned ship should be directed in the positive y-axis direction, and-1 indicates that the optimal speed of the unmanned ship should be directed in the negative y-axis direction; v _max Representing the maximum speed of the unmanned ship, and representing the maximum speed by a scalar;

in the above formula, u represents the instantaneous speed of human input to the unmanned ship under the frame; r denotes the position of the obstacle seen from the framework of the agent, at time t, which is located at the origin in said framework and is at rest at the instant,and->Representing the position of the obstacle under the frame in the x-axis and the y-axis, respectively, relative to the unmanned ship; v represents the relative speed of the obstacle with the unmanned ship as origin of coordinates, +.>And->Representing the speed of the obstacle under the frame in the x-axis and the y-axis, respectively, relative to the unmanned ship;

s22, in the driving process, checking whether the unmanned ship collides with the obstacle or not by using a collision detection function f (x), and when f (x) is less than or equal to 0, indicating that the unmanned ship collides with the obstacle;

the collision detection function f (x) in step S22 is specifically:

f(*)≤0:

in the above formula, f (x) represents a function for detecting whether the unmanned ship collides with an obstacle, and r represents a distance between the unmanned ship and the obstacle at time t; r represents the radius of a circle formed after the unmanned ship is puffed, V _r Representing the projection of the relative speed of the unmanned ship and the obstacle on the AB-axis, V _θ Indicating that the relative speed of the unmanned ship and the obstacle is r ^⊥ Projection on the axis; wherein:

in the above-mentioned method, the step of,representing the relative speed of unmanned ship and obstacle by vectorQuantity representation, ->Representing the unit vector on the AB axis,r is ^⊥ A unit vector on the axis; projecting the relative velocity of the unmanned ship to the obstacle to the AB-axis and r ^⊥ On the shaft, generating respective relative velocity components;

s23, when detecting that the unmanned ship and the obstacle are in collision danger, guiding the relative speed of the unmanned ship and the obstacle to be separated from a speed collision cone by using a collision resolution function f (·) so that the unmanned ship and the obstacle are prevented from collision;

the conflict resolution function f (·) in step S23 is specifically:

f(·)≤0:

in the above formula, f (-) represents a collision resolution function for helping the unmanned ship avoid collision with the obstacle, when f (-) is less than or equal to 0, the relative speed of the unmanned ship and the obstacle is shown to be separated from the speed collision cone, the collision danger of the unmanned ship and the obstacle is relieved, r represents the position of the obstacle seen from the framework of the agent at the moment t, and r is shown as follows ^T For the transposition of R, R represents the distance between the unmanned ship and the obstacle, v represents the relative speed of the obstacle under the frame taking the unmanned ship as the center, v represents the relative speed of the unmanned ship and the obstacle, and R is the radius of a circle formed after the unmanned ship is puffed;

and S3, based on maritime obstacle avoidance rules, an obstacle avoidance strategy is added when the ship is in a dangerous early warning area, and a decision of the ship after the ship is separated from the maritime rules is made clearly.

2. The reverse speed obstacle avoidance method according to claim 1, wherein said step S2 further comprises the step of reasoning about a collision detection function f (), and a collision resolution function f ():

step one: reasoning the process detection function f ();

from the speed collision cone formed by the unmanned ship and the obstacle, it can be known that:

for a pair ofAnd (3) processing to obtain:

is known to beThe marketing is simplified as follows:

because ofThe method comprises the following steps:

substituting the above formula into the formulaIn (1), the following steps are obtained:

when (when)When the relative speed of the unmanned ship and the obstacle is in the speed collision cone, the collision of the unmanned ship and the obstacle is indicated, so the unmanned ship and the obstacle are in the collision, and the unmanned ship is in the collision>I.e. when +.>When the unmanned ship collides with an obstacle;

definition f ():for collision detection conditions, when f (+.0):when the unmanned ship collides with the obstacle;

step two: reasoning about a conflict resolution function f ();

the triangle formed by the unmanned ship and the obstacle can be known as:

processing the formula to obtain:

and is also knownThe method comprises the following steps:

so there isSubstituting it into the above formula gives:

then there are:

when (when)When the unmanned ship collides with the obstacle, the unmanned ship and the obstacle are prevented from collision;

definition f (·):for conflict resolution conditions, when f (+.) is less than or equal to 0:and when the unmanned ship is separated from the obstacle, the collision risk is avoided.

3. The reverse speed obstacle avoidance method combined with colreges according to claim 1, wherein the step S3 specifically comprises:

dividing the obstacle avoidance situation into 6 situations by combining the ship domain division and meeting situation of the unmanned ship, wherein the situations are the meeting situation, the cross meeting situation, the overtaking situation of the obstacle avoidance area, the meeting situation, the cross meeting situation and the overtaking situation of the dangerous early warning area;

the meeting situation of the conventional obstacle avoidance area specifically comprises the following steps:

θ∈ (0 °,15 °), an obstacle appears on the right side of USV, USV yields;

θ∈ (0 °, -15 °), an obstacle appears on the left side of USV, and USV moves straight;

the COLREGS convention states that in a conventional obstacle avoidance area, vessels on the right side of the vessel have priority to pass; when the ship appears on the right side of the ship, the ship is used as a yielding ship to give way preferentially, and when the ship appears on the left side of the ship, the ship is used as a straight ship to pass preferentially;

the cross-meeting situation of the conventional obstacle avoidance area specifically comprises the following steps:

when the obstacle is on the right side of the USV, then there are:

V _USV ＞V ₀ θ∈ (15 °,90 °), USV travels straight;

V _USV ＞V ₀ θ∈ (90 °,135 °), USV travels straight;

V _USV ＜V ₀ θ∈ (15 °,90 °), USV dodges;

V _USV ＜V ₀ θ∈ (90 °,135 °), USV travels straight;

when the obstacle is on the left side of the USV, then there are:

when no dangerous condition exists, USV moves straight, and obstacles are avoided;

the following situation of the conventional obstacle avoidance area specifically comprises the following steps:

USV does not need to be avoided, and the overtaking ship needs to avoid the USV;

the meeting situation of the dangerous early warning area specifically comprises the following steps:

θ ε (-15 °,15 °), obstacles appear in front of USV, and USV is decelerated and runs right preferentially; the crossing situation of the danger early warning area specifically comprises the following steps:

when the obstacle is on the right side of the USV, then there are:

V _USV ＞V ₀ θ∈ (15 °,90 °), USV accelerates to the left;

V _USV ＞V ₀ θ∈ (90 °,135 °), USV accelerates straight;

V _USV ＜V ₀ θ∈ (15 °,90 °), USV decelerates to the right;

V _USV ＜V ₀ θ∈ (90 °,135 °), USV decelerates to the left;

when the obstacle is on the left side of the USV, then there are:

V _USV ＞V ₀ θ ε (-15 °, -90 °), USV accelerates to the right;

V _USV ＞V ₀ θ ε (-90 °, -135 °), USV accelerates straight;

V _USV ＜V ₀ θ ε (-15 °, -90 °), USV decelerates to travel to the left;

V _USV ＜V ₀ θ ε (-90 °, -135 °), USV decelerates to the right.