Unmanned ship dynamic obstacle avoidance method based on safe distance

Abstract

The invention relates to the technical field of unmanned ship obstacle avoidance, in particular to an unmanned ship dynamic obstacle avoidance method based on a safe distance, which comprises the following steps: step 1, checking whether an unmanned ship reaches a target point or not, and if not, entering step 2; 2, acquiring obstacle information, and detecting whether the unmanned ship is collided or not based on the safety circle range of the obstacle and the filtering circle range of the unmanned ship; 3, when it is judged that obstacle avoidance is needed, obstacle avoidance path points are obtained based on the tangent points according to a real-time tracking proportional guidance method, and the unmanned ship moves towards the obstacle avoidance path points; and step 4, checking whether the unmanned ship intersects with the obstacle again after a time interval delta t, if not, enabling the unmanned ship to continuously move along the obstacle avoidance path point, and otherwise, repeating the step 3 and the step 4. According to the invention, the corresponding obstacle avoidance path points can be adjusted in real time, and the problems of overlarge energy consumption and overlarge compound flight angle of fixed course tracking are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ship obstacle avoidance, and in particular to an unmanned ship dynamic obstacle avoidance method based on a safe distance. Background Art

[0002] When an unmanned ship autonomously navigates along a known path, it may encounter dynamic obstacles, such as swimming fish, floating objects, and ships. In order to solve this problem, an optimization algorithm for avoiding dynamic obstacles has been developed in the prior art. The input of the algorithm is the surrounding environment information detected by sonar, as well as the size of the unmanned ship, the speed of the unmanned ship, the heading angle, and other unmanned ship models themselves. That is, the data of the automatic identification system (AIS). AIS is an automatic tracking system used by ships and vessel traffic services (VTS). It identifies and locates ships by exchanging electronic data with other nearby ships. Generally, the AIS system on unmanned ships can receive information from other ships within a range of at least 10 miles. General systems use local obstacle avoidance algorithms, such as the artificial potential field method, which may obtain a constant angle of obstacle avoidance based on DCPA (closest approach distance) and TCPA (closest approach time). Although the effectiveness of dynamic obstacle avoidance is maintained, the real-time obstacle avoidance effect cannot be maintained based on the constant obstacle avoidance angle, resulting in the moment of obstacle avoidance completion, the navigation distance based on the specific obstacle avoidance angle is too long, increasing the energy consumption of the unmanned ship for collision avoidance, and the re-navigation angle is too large, resulting in increased mechanical losses in the turning angle of the unmanned ship. Summary of the invention

[0003] In view of this, the purpose of the present invention is to propose an obstacle avoidance method for unmanned ships with dynamic obstacles based on a safe distance, so as to solve the problems that the existing obstacle avoidance method based on a constant obstacle avoidance angle cannot maintain a real-time obstacle avoidance effect, increases the energy consumption of the unmanned ship for collision avoidance, and the re-entry angle is too large, resulting in increased mechanical loss of the unmanned ship's turning angle.

[0004] Based on the above purpose, the present invention provides an obstacle avoidance method for unmanned ship dynamic obstacles based on a safe distance, comprising: step 1: checking whether the unmanned ship has reached the target point, if it has reached, the unmanned ship stops moving, if it has not reached, entering step 2;

[0005] Step 2: Obtain obstacle information, set the safety radius of the obstacle, set the filter circle range of the unmanned ship, and detect whether the unmanned ship will collide based on the safety circle range of the obstacle and the filter circle range of the unmanned ship:

[0006] When no collision is detected, the status flag is 0, and the unmanned ship continues to navigate along the waypoint;

[0007] When the filter circle range of the unmanned ship intersects with the safety area of ​​the intruding ship, the state flag is 1, and the unmanned ship continues to sail along the waypoint;

[0008] When the obstacle is within the filter circle, the status flag is 2. If the status flag is 2, the collision detection algorithm is triggered to check whether the unmanned ship will collide with the obstacle. If there is no collision, the unmanned ship continues to navigate along the path point. If there is a collision, the status flag is 3;

[0009] Step 3: When the status flag is 3, it is determined whether the unmanned ship needs to take action to avoid obstacles. When it is determined that the unmanned ship needs to avoid obstacles, the tangent point of the safety circle range of the obstacle that the unmanned ship needs to avoid is obtained, and based on the tangent point, an obstacle avoidance path point is obtained according to the real-time tracking proportional guidance method, and the unmanned ship moves to the obstacle avoidance path point;

[0010] Step 4: After a time interval Δt, check again whether the unmanned boat crosses the obstacle. If not, the unmanned boat continues to move along the obstacle avoidance path point, otherwise repeat steps 3 and 4.

[0011] Optionally, the step of checking whether the unmanned ship has reached the target point includes:

[0012] Check whether the unmanned ship has reached the target point by the following formula:

[0013]

[0014] p ux ,p uy is the position coordinate of the unmanned ship, p fx ,p fy is the final target point coordinate, r d The distance the unmanned boat needs to reach the target point;

[0015] If f>r d , which means that the unmanned ship has not reached the target point, otherwise the unmanned ship reaches the target point.

[0016] Optionally, the detecting whether the unmanned ship will collide with the obstacle-based safety circle range and the unmanned ship filtering circle range includes:

[0017] The distance between the unmanned boat and the obstacle is calculated by the following formula:

[0018]

[0019] where p ux ,p uy is the position coordinate of the unmanned ship, p obsx ,p obsy are the coordinates of the obstacle;

[0020] s r is the filtering circle range of the unmanned ship, and r is the obstacle safety circle range;

[0021] If g>s r +r, means no collision was detected,

[0022] If s r <g≤s r +r, indicating that the filtering circle of the unmanned ship intersects with the safety circle of the intruding ship;

[0023] If r<g≤s r , indicating that the obstacle is only within the filter circle.

[0024] Optionally, the collision detection algorithm includes detecting whether the unmanned ship will collide by the following method:

[0025] The speed vux,v of the unmanned ship on the x-axis and y-axis is calculated by the following formula (3): uy ;

[0026] v ux =V u ×cos(θ u )

[0027] v uy =V u ×sin(θ u ) (3)

[0028] Where V u represents the speed of the unmanned ship, θ u Indicates the heading angle of the unmanned ship;

[0029] The speed v of the obstacle decomposed on the x-axis and y-axis is calculated by the following formula (4): obsx ,v obsy

[0030] v obsx =V obs ×cos(θ obs )

[0031] v obsy =V obs ×sin(θ obs ) (4)

[0032] The relative speed V between the unmanned ship and the obstacle is calculated by the following formula (5): r Speed ​​v decomposed into x and y axes rx, v ry

[0033] v rx =v ux -v obsx

[0034] v ry =vyx -v obsy (5)

[0035] The relative velocity modulus and relative velocity angle are calculated in the vector domain by formula 6 as follows:

[0036]

[0037] θ r =atan(v ry / v rx ) (6)

[0038] The relative velocity angle is adjusted to the range of 0 to 2π by:

[0039] if v rx ≥0&v ry >0

[0040] θ r =θ r

[0041] if v rx ≥0&v ry <0

[0042] θ r =θ r +2π

[0043] if v rx <0&v ry ≥0

[0044] θ r =θ r +π

[0045] if v rx <0&v ry ≤0

[0046] θ r =θ r +π

[0047] The distance between the unmanned ship and the obstacle is represented by r d It is expressed as follows, formula (7):

[0048]

[0049] Among them, Δθ represents the angle range between the left tangent and the right tangent of the obstacle circle of the unmanned ship and the dynamic obstacle;

[0050] The heading vector from the unmanned ship to the obstacle is expressed as (p obsx -p ux ,p obsy -puy ), the angle θ between the unmanned ship and the center of the dynamic obstacle is calculated by the following formula (8):

[0051]

[0052] when When , it means that the relative speed angle of the unmanned ship is within the angle range of the left and right tangents of the obstacle ship, a collision will occur, otherwise no collision will occur.

[0053] Optionally, the determining whether the unmanned ship needs to take action to avoid obstacles includes:

[0054] Establish a cross rectangular coordinate system along the direction of the unmanned ship, and set the upper left quadrant as quadrant 1

[0055] When θ satisfies the following equation, the obstacle is located in quadrant 1 of the unmanned ship:

[0056]

[0057]

[0058] Only when the status flag is equal to 3 and θ satisfies one of the above two if conditions, the unmanned ship needs to take action to avoid obstacles.

[0059] Optionally, the step of obtaining the tangent point of the safety circle range of obstacles that the unmanned ship needs to avoid includes:

[0060] There are two tangent points from the position of the unmanned ship to the obstacle safety circle. The tangent point position is set to (x, y) and (x, y) is solved by the following equation (9).

[0061] (xp obsx ) 2 +(yp obsy ) 2 =r 2

[0062] (yp obsy )×(yp uy )=-(xp obsx )×(xp ux ) (9)

[0063] Get two tangent points (x1, y1) and (x2, y2), and determine which tangent point is the starboard tangent point relative to the unmanned ship by the following method:

[0064] The sight angles between the two dynamic obstacle avoidance points and the unmanned ship are set to be θ1 and θ2 respectively;

[0065] Δθ=θ2-θ1

[0066] If Δθ satisfies the conditions in formula (21), then (x1, y1) is the starboard tangent point. Otherwise, the starboard tangent point is (x2, y2).

[0067] Δθ represents the angle range between the left tangent and the right tangent of the obstacle circle of the unmanned ship and the dynamic obstacle;

[0068] 0<Δθ<π or Δθ<-π (21)

[0069] Optionally, obtaining the obstacle avoidance path point based on the tangent point according to the real-time tracking proportional guidance method includes:

[0070] set up is the speed vector of the unmanned ship, is the obstacle velocity vector, which is equal to the tangent point velocity vector Phase same; θ r is the line of sight angle between the unmanned ship and the tangent point, that is, the relative speed angle, is the rate of change of sight angle, P is the tangent point between the unmanned ship and the obstacle circle of the dynamic obstacle;

[0071] The rotational angular velocity of the unmanned ship velocity vector is calculated by the following formula (10);

[0072]

[0073] Among them, N is the proportional constant, which needs to be adjusted during implementation. In the simulation, N is set to 6;

[0074] From this we can get the unmanned ship (θ u_new ) is as follows:

[0075]

[0076] where Δt is θ u Update the time interval and use the new heading angle as the obstacle avoidance path point for the unmanned ship.

[0077] When performing obstacle avoidance using this method, first check whether the unmanned ship has reached the target point. If it has arrived, the unmanned ship stops moving. If it has not arrived, obtain obstacle information, set the safety radius of the obstacle, set the filter circle range of the unmanned ship, and detect whether the unmanned ship will collide based on the safety circle range of the obstacle and the filter circle range of the unmanned ship: when no collision is detected, the status flag is 0, and the unmanned ship is controlled to continue sailing along the path point; when the filter circle range of the unmanned ship intersects with the safety area of ​​the invading ship, the status flag is 1, and the unmanned ship continues to sail along the path point; when the obstacle is within the filter circle range, the status flag is 2. If the status flag is 2, the collision detection algorithm is triggered, and the The triggering collision detection algorithm checks whether the unmanned ship will collide with the obstacle. If there is no collision, the unmanned ship continues to navigate along the path point. If there is a collision, the status flag is 3. When the status flag is 3, it is determined whether the unmanned ship needs to take action to avoid the obstacle. When it is determined that the obstacle needs to be avoided, the tangent point of the safety circle range of the obstacle that the unmanned ship needs to avoid is obtained, and based on the tangent point, the obstacle avoidance path point is obtained according to the real-time tracking proportional guidance method. The unmanned ship moves to the obstacle avoidance path point, and then checks again after a time interval Δt whether the unmanned ship and the obstacle intersect. If there is no intersection, the unmanned ship continues to move along the obstacle avoidance path point, otherwise it returns to determine whether the unmanned ship needs to take action to avoid the obstacle and continues to execute.

[0078] From the above, it can be seen that the method can detect in real time whether the current obstacle avoidance path point meets the avoidance requirements, and can adjust the corresponding obstacle avoidance path point, solving the problems of excessive energy consumption and excessive return angle of fixed heading tracking, and can provide a real-time strategy for tracking the optimal obstacle avoidance point, adjust the heading angle of the unmanned ship in real time, and after completing the obstacle avoidance task, can effectively reduce the obstacle avoidance return angle of the unmanned ship, while effectively reducing the energy consumption of the unmanned ship. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings in the following description are only for the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0080] Figure 1 This is a schematic diagram of a scene where a ship crosses and encounters an obstacle according to an embodiment of the present invention;

[0081] Figure 2 A geometric diagram of a collision scene according to an embodiment of the present invention;

[0082] Figure 3 This is a schematic diagram of the Δθ geometry of an embodiment of the present invention;

[0083] Figure 4A schematic diagram of a collision scenario in a coordinate system of an embodiment of the present invention;

[0084] Figure 5 This is a schematic diagram of the tangent points of the obstacle safety circle range according to an embodiment of the present invention;

[0085] Figure 6 A schematic diagram of a real-time tracking proportional guidance method for an unmanned ship according to an embodiment of the present invention;

[0086] Figure 7 This is a diagram showing the consistency between the velocity vector of a dynamic obstacle and the velocity vector of the obstacle avoidance point in an embodiment of the present invention. DETAILED DESCRIPTION

[0087] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments.

[0088] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0089] like Figure 1 As shown, a method for avoiding dynamic obstacles of an unmanned ship based on a safe distance includes:

[0090] Step 1: Check whether the unmanned boat has reached the target point. If it has, the unmanned boat stops moving. If it has not, go to step 2.

[0091] Step 2: Obtain obstacle information, set the safety radius of the obstacle, set the filter circle range of the unmanned ship, and detect whether the unmanned ship will collide based on the safety circle range of the obstacle and the filter circle range of the unmanned ship:

[0092] When no collision is detected, the status flag is 0, and the unmanned ship continues to navigate along the waypoint;

[0093] When the filter circle range of the unmanned ship intersects with the safety area of ​​the intruding ship, the state flag is 1, and the unmanned ship continues to sail along the waypoint;

[0094] When the obstacle is within the filter circle, the status flag is 2. If the status flag is 2, the collision detection algorithm is triggered to check whether the unmanned ship will collide with the obstacle. If there is no collision, the unmanned ship continues to navigate along the path point. If there is a collision, the status flag is 3;

[0095] Step 3: When the status flag is 3, it is determined whether the unmanned ship needs to take action to avoid obstacles. When it is determined that the unmanned ship needs to avoid obstacles, the tangent point of the safety circle range of the obstacle that the unmanned ship needs to avoid is obtained, and based on the tangent point, an obstacle avoidance path point is obtained according to the real-time tracking proportional guidance method, and the unmanned ship moves to the obstacle avoidance path point;

[0096] Step 4: After a time interval Δt, check again whether the unmanned boat crosses the obstacle. If not, the unmanned boat continues to move along the obstacle avoidance path point, otherwise repeat steps 3 and 4.

[0097] When performing obstacle avoidance using this method, first check whether the unmanned ship has reached the target point. If it has arrived, the unmanned ship stops moving. If it has not arrived, obtain obstacle information, set the safety radius of the obstacle, set the filter circle range of the unmanned ship, and detect whether the unmanned ship will collide based on the safety circle range of the obstacle and the filter circle range of the unmanned ship: when no collision is detected, the status flag is 0, and the unmanned ship is controlled to continue sailing along the path point; when the filter circle range of the unmanned ship intersects with the safety area of ​​the invading ship, the status flag is 1, and the unmanned ship continues to sail along the path point; when the obstacle is within the filter circle range, the status flag is 2. If the status flag is 2, the collision detection algorithm is triggered, and the The triggering collision detection algorithm checks whether the unmanned ship will collide with the obstacle. If there is no collision, the unmanned ship continues to navigate along the path point. If there is a collision, the status flag is 3. When the status flag is 3, it is determined whether the unmanned ship needs to take action to avoid the obstacle. When it is determined that the obstacle needs to be avoided, the tangent point of the safety circle range of the obstacle that the unmanned ship needs to avoid is obtained, and based on the tangent point, the obstacle avoidance path point is obtained according to the real-time tracking proportional guidance method. The unmanned ship moves to the obstacle avoidance path point, and then checks again after a time interval Δt whether the unmanned ship and the obstacle intersect. If there is no intersection, the unmanned ship continues to move along the obstacle avoidance path point, otherwise it returns to determine whether the unmanned ship needs to take action to avoid the obstacle and continues to execute.

[0098] From the above, it can be seen that the method can detect in real time whether the current obstacle avoidance path point meets the avoidance requirements, and can adjust the corresponding obstacle avoidance path point, solving the problems of excessive energy consumption and excessive return angle of fixed heading tracking, and can provide a real-time strategy for tracking the optimal obstacle avoidance point, adjust the heading angle of the unmanned ship in real time, and after completing the obstacle avoidance task, can effectively reduce the obstacle avoidance return angle of the unmanned ship, while effectively reducing the energy consumption of the unmanned ship.

[0099] In some embodiments, checking whether the unmanned ship has reached the target point includes:

[0100] Check whether the unmanned ship has reached the target point by the following formula:

[0101]

[0102] p ux ,p uy is the position coordinate of the unmanned ship, p fx ,p fy is the final target point coordinate, r d The distance the unmanned boat needs to reach the target point;

[0103] If f>r d , which means that the unmanned ship has not reached the target point, otherwise the unmanned ship reaches the target point.

[0104] like Figure 2-Figure 3 As shown, in some embodiments, the detecting whether the unmanned ship will collide based on the safety circle range of the obstacle and the filtering circle range of the unmanned ship includes:

[0105] The distance between the unmanned boat and the obstacle is calculated by the following formula:

[0106]

[0107] where p ux ,p uy is the position coordinate of the unmanned ship, p obsx ,p obsy are the coordinates of the obstacle;

[0108] s r is the filtering circle range of the unmanned ship, and r is the obstacle safety circle range;

[0109] If g>s r +r, means no collision was detected,

[0110] If s r <g≤s r +r, indicating that the filtering circle of the unmanned ship intersects with the safety circle of the intruding ship;

[0111] If r<g≤s r , indicating that the obstacle is only within the filter circle.

[0112] In some embodiments, the speed of the unmanned vessel is set to It can be described as (v ux ,v uy ). The velocity vector of the obstacle is It can be described as (v obsx ,vobsy ). The relative speed of the unmanned ship relative to the obstacle is The collision detection algorithm includes detecting whether the unmanned ship will collide by the following method:

[0113] The speed v of the unmanned ship decomposed on the x-axis and y-axis is calculated by the following formula (3): ux ,v uy ;

[0114] v ux =V u ×cos(θ u )

[0115] v uy =V u ×sin(θ u ) (3)

[0116] Where V u represents the speed of the unmanned ship, θ u Indicates the heading angle of the unmanned ship;

[0117] The speed v of the obstacle decomposed on the x-axis and y-axis is calculated by the following formula (4): obsx ,v obsy

[0118] v obsx =V obs ×cos(θ obs )

[0119] v obsy =V obs ×sin(θ obs ) (4)

[0120] The relative speed V between the unmanned ship and the obstacle is calculated by the following formula (5): r Speed ​​v decomposed into x and y axes rx ,v ry

[0121] v rx =v ux -v obsx

[0122] v ry =v yx -v obsy (5)

[0123] The relative velocity modulus and relative velocity angle are calculated in the vector domain by formula 6 as follows:

[0124]

[0125] θ r =atan(vry / v rx ) (6)

[0126] The relative velocity angle is adjusted to the range of 0 to 2π by:

[0127] if v rx ≥0&v ry >0

[0128] θ r =θ r

[0129] if v rx ≥0&v ry <0

[0130] θ r =θ r +2π

[0131] if v rx <0&v ry ≥0

[0132] θ r =θ r +π

[0133] if v rx <0&v ry ≤0

[0134] θ r =θ r +π

[0135] The distance between the unmanned ship and the obstacle is represented by r d It is expressed as follows, formula (7):

[0136]

[0137] Among them, Δθ represents the angle range between the left tangent and the right tangent of the obstacle circle of the unmanned ship and the dynamic obstacle;

[0138] The heading vector from the unmanned ship to the obstacle is expressed as (p obsx -p ux ,p obsy -p uy ), the angle θ between the unmanned ship and the center of the dynamic obstacle is calculated by the following formula (8):

[0139]

[0140] when When , it means that the relative speed angle of the unmanned ship is within the angle range of the left and right tangents of the obstacle ship, a collision will occur, otherwise no collision will occur.

[0141] like Figure 4 As shown, in some embodiments, according to the International Regulations for Preventing Collisions at Sea (COLREGS), when two powered vessels cross, the other vessel on the starboard side must give way to avoid crossing in front of it. When two powered vessels meet head-on, both must change course to starboard in order to pass on the other's port side. The overtaking vessel must avoid the overtaken vessel.

[0142] When the status flag is 3, it means there is a potential risk of collision. However, this does not mean that the unmanned ship owner will take action to avoid the obstacle. Only when the obstacle is in the first quadrant of the unmanned ship, that is, the starboard side of the ship, and the unmanned ship is in a crossing or head-on situation, it needs to turn to the starboard side.

[0143] The method for checking whether an obstacle is in quadrant 1 is as follows. The method for determining whether the unmanned ship needs to take action to avoid an obstacle includes:

[0144] Establish a cross rectangular coordinate system along the direction of the unmanned ship, and set the upper left quadrant as quadrant 1

[0145] When θ satisfies the following equation, the obstacle is located in quadrant 1 of the unmanned ship:

[0146]

[0147]

[0148] Only when the state flag is equal to 3 and θ satisfies one of the above two if conditions, the unmanned ship needs to take action to avoid obstacles. The situation in quadrant 2 indicates that the intruder is on the port side of the unmanned ship, and quadrants 3 and 4 indicate that the intruder is in the overtaking state, at which time the unmanned ship does not need to take any action.

[0149] like Figure 5 As shown, in some embodiments, obtaining the tangent point of the safety circle range of obstacles that the unmanned ship needs to avoid includes:

[0150] There are two tangent points from the position of the unmanned ship to the obstacle safety circle. The tangent point position is set to (x, y) and (x, y) is solved by the following equation (9).

[0151] (xp obsx ) 2 +(yp obsy ) 2 =r 2

[0152] (yp obsy )×(yp uy )=-(xp obsx )×(xp ux ) (9)

[0153] Get two tangent points (x1, y1) and (x2, y2), and determine which tangent point is the starboard tangent point relative to the unmanned ship by the following method:

[0154] The sight angles between the two dynamic obstacle avoidance points and the unmanned ship are set to be θ1 and θ2 respectively;

[0155] Δθ=θ2-θ1

[0156] If Δθ satisfies the conditions in formula (21), then (x1, y1) is the starboard tangent point. Otherwise, the starboard tangent point is (x2, y2).

[0157] Δθ represents the angle range between the left tangent and the right tangent of the obstacle circle of the unmanned ship and the dynamic obstacle;

[0158] 0<Δθ<π or Δθ<-π (21)

[0159] like Figure 6 As shown, in some embodiments, obtaining the obstacle avoidance path point based on the tangent point according to the real-time tracking proportional guidance method includes:

[0160] set up is the speed vector of the unmanned ship, is the obstacle velocity vector, which is equal to the tangent point velocity vector Same (the points on the obstacle circle of the dynamic obstacle have the same movement direction and speed as the dynamic obstacle, see Figure 7 θ r is the line of sight angle between the unmanned ship and the tangent point, that is, the relative speed angle (the relative speed angle is consistent with the line of sight angle between the unmanned ship and the tangent point); is the rate of change of sight angle, P is the tangent point between the unmanned ship and the obstacle circle of the dynamic obstacle;

[0161] The rotational angular velocity of the unmanned ship velocity vector is calculated by the following formula (10);

[0162]

[0163] Among them, N is the proportional constant, which needs to be adjusted during implementation. In the simulation, N is set to 6;

[0164] From this we can get the unmanned ship (θ u_new ) is as follows:

[0165]

[0166] where Δt is θ u Update the time interval and use the new heading angle as the obstacle avoidance path point for the unmanned ship.

[0167] By using the above method, in the time interval of Δt, when the obstacle avoidance path point needs to be adjusted, the new heading angle can be obtained in real time as the obstacle avoidance path point for the unmanned ship to navigate, and the corresponding obstacle avoidance path point can be adjusted in real time.

[0168] It should be understood by those skilled in the art that the discussion of any of the above embodiments is merely illustrative and is not intended to imply that the scope of the present invention (including the claims) is limited to these examples. Within the scope of the present invention, the technical features in the above embodiments or in different embodiments may be combined, the steps may be implemented in any order, and there are many other variations of the above aspects of the present invention, which are not provided in detail for the sake of simplicity.

[0169] The present invention is intended to cover all such substitutions, modifications and variations that fall within the broad scope of the appended claims. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for avoiding dynamic obstacles of an unmanned ship based on a safe distance, characterized in that: include: Step 1: Check whether the unmanned boat has reached the target point. If it has, the unmanned boat stops moving. If it has not, go to step 2. Step 2: Obtain obstacle information, set the safety radius of the obstacle, set the filter circle range of the unmanned ship, and detect whether the unmanned ship will collide based on the safety circle range of the obstacle and the filter circle range of the unmanned ship: When no collision is detected, the status flag is 0, and the unmanned ship continues to navigate along the waypoint; When the filter circle range of the unmanned ship intersects with the safety area of ​​the intruding ship, the state flag is 1, and the unmanned ship continues to sail along the waypoint; When the obstacle is within the filter circle, the status flag is 2. If the status flag is 2, the collision detection algorithm is triggered to check whether the unmanned ship will collide with the obstacle. If there is no collision, the unmanned ship continues to navigate along the path point. If there is a collision, the status flag is 3; Step 3: When the status flag is 3, it is determined whether the unmanned ship needs to take action to avoid obstacles. When it is determined that the unmanned ship needs to avoid obstacles, the tangent point of the safety circle range of the obstacle that the unmanned ship needs to avoid is obtained, and based on the tangent point, an obstacle avoidance path point is obtained according to the real-time tracking proportional guidance method, and the unmanned ship moves to the obstacle avoidance path point; Step 4: After a time interval Δt, check again whether the unmanned boat crosses the obstacle. If not, the unmanned boat continues to move along the obstacle avoidance path point, otherwise repeat steps 3 and 4.

2. The method for avoiding dynamic obstacles of an unmanned ship based on a safe distance according to claim 1, characterized in that: The checking whether the unmanned ship has reached the target point includes: Check whether the unmanned ship has reached the target point by the following formula: p ux ,p uy is the position coordinate of the unmanned ship, p fx ,p fy is the final target point coordinate, r d The distance the unmanned boat needs to reach the target point; If f>r d , which means that the unmanned ship has not reached the target point, otherwise the unmanned ship reaches the target point.

3. The method for avoiding dynamic obstacles of an unmanned ship based on a safe distance according to claim 1, characterized in that: The detection of whether the unmanned ship will collide based on the safety circle range of obstacles and the unmanned ship filtering circle range includes: The distance between the unmanned boat and the obstacle is calculated by the following formula: where p ux ,p uy is the position coordinate of the unmanned ship, p obsx ,p obsy is the coordinate of the obstacle; s r is the filtering circle range of the unmanned ship, and r is the obstacle safety circle range; If g>s r +r, means no collision was detected, If s r <g≤s r +r, indicating that the filtering circle of the unmanned ship intersects with the safety circle of the intruding ship; If r<g≤s r , indicating that the obstacle is only within the filter circle.

4. The method for avoiding dynamic obstacles of an unmanned ship based on a safe distance according to claim 1, characterized in that: The collision detection algorithm includes detecting whether the unmanned ship will collide by the following method: The speed v of the unmanned ship decomposed on the x-axis and y-axis is calculated by the following formula (3): ux ,v uy ; v ux =V u ×cos(θ u ) v uy =V u ×sin(θ u ) (3) Where V u represents the speed of the unmanned ship, θ u Indicates the heading angle of the unmanned ship; The speed v of the obstacle decomposed on the x-axis and y-axis is calculated by the following formula (4): obsx ,v obsy v obsx =V obs ×cos(θ obs ) v obsy =V obs ×sin(θ obs ) (4) The relative speed V between the unmanned ship and the obstacle is calculated by the following formula (5): r Speed ​​v decomposed into x and y axes rx ,v ry v rx =v ux -v obsx v ry =v yx -v obsy (5) The relative velocity modulus and relative velocity angle are calculated in the vector domain by formula 6 as follows: θ r =atan(v ry / v rx ) (6) The relative velocity angle is adjusted to the range of 0 to 2π by: if v rx ≥0&v ry >0 i r =θ r if v rx ≥0&v ry <0 i r =θ r +2p if v rx <0&v ry ≥0 i r =θ r +p if v rx <0&v ry ≤0 i r =θ r +p The distance between the unmanned ship and the obstacle is represented by r d It is expressed as follows, formula (7): Among them, Δθ represents the angle range between the left tangent and the right tangent of the obstacle circle of the unmanned ship and the dynamic obstacle; The heading vector from the unmanned ship to the obstacle is expressed as (p obsx -p ux ,p obsy -p uy ), the angle θ between the unmanned ship and the center of the dynamic obstacle is calculated by the following formula (8): when When , it means that the relative speed angle of the unmanned ship is within the angle range of the left and right tangents of the obstacle ship, a collision will occur, otherwise no collision will occur.

5. The method for avoiding dynamic obstacles of an unmanned ship based on a safe distance according to claim 1, characterized in that: The determination of whether the unmanned ship needs to take action to avoid obstacles includes: Establish a cross rectangular coordinate system along the direction of the unmanned ship, and set the upper left quadrant as quadrant 1 When θ satisfies the following equation, the obstacle is located in quadrant 1 of the unmanned ship: Only when the status flag is equal to 3 and θ satisfies one of the above two if conditions, the unmanned ship needs to take action to avoid obstacles.

6. The method for avoiding dynamic obstacles of an unmanned ship based on a safe distance according to claim 1, characterized in that: The tangent points of the safety circle range of obstacles that the unmanned ship needs to avoid include: There are two tangent points from the position of the unmanned ship to the obstacle safety circle. The tangent point position is set to (x, y) and the following equation (9) is used to solve (x, y): (x-p obsx ) 2 +(y-p obsy ) 2 =r 2 (y-p obsy )×(y-p uy )=-(x-p obsx )×(x-p ux ) (9) Get two tangent points (x1, y1) and (x2, y2), and determine which tangent point is the starboard tangent point relative to the unmanned ship by the following method: The sight angles between the two dynamic obstacle avoidance points and the unmanned ship are set to be θ1 and θ2 respectively; Δθ=θ2-θ1 If Δθ satisfies the conditions in formula (21), then (x1, y1) is the starboard tangent point. Otherwise, the starboard tangent point is (x2, y2). Δθ represents the angle range between the left tangent and the right tangent of the obstacle circle of the unmanned ship and the dynamic obstacle; 0<Δθ<π or Δθ<-π (21).

7. The method for avoiding dynamic obstacles of an unmanned ship based on a safe distance according to claim 6, characterized in that: The method of obtaining the obstacle avoidance path point based on the tangent point according to the real-time tracking proportional guidance method includes: set up is the speed vector of the unmanned ship, is the obstacle velocity vector, which is equal to the tangent point velocity vector Same; θ r is the line of sight angle between the unmanned ship and the tangent point, that is, the relative speed angle, is the rate of change of sight angle, P is the tangent point between the unmanned ship and the obstacle circle of the dynamic obstacle; The rotational angular velocity of the unmanned ship velocity vector is calculated by the following formula (10); Among them, N is the proportional constant, which needs to be adjusted during implementation; From this we can get the unmanned ship (θ u_new ) is as follows: where Δt is θ u Update the time interval and use the new heading angle as the obstacle avoidance path point for the unmanned ship.

Images