An unmanned ship local path planning method based on a three-stage obstacle avoidance strategy

Abstract

The application discloses an unmanned ship local path planning method based on a three-stage obstacle avoidance strategy and belongs to the technical field of unmanned ship path planning. Stage 0 adopts a dynamic obstacle avoidance control range and a dynamic target point, solves inherent problems of a dynamic window method, and introduces the influence of an environment in the selection process of the dynamic target point to reduce energy consumption of the unmanned ship in a static obstacle avoidance process. Stage 1 combines a speed obstacle method and the International Regulations for Preventing Collisions at Sea, adopts speed obstacle limitation, and reduces collision risks and obstacle avoidance difficulties of the unmanned ship in the obstacle avoidance process. Stage 2 combines the speed obstacle method and the International Regulations for Preventing Collisions at Sea, adopts a left side limitation and a right side scoring mechanism, and realizes obstacle avoidance of the unmanned ship based on the International Regulations for Preventing Collisions at Sea. Meanwhile, through a double-threshold method based on a collision risk value, a shaking problem in the switching process of the obstacle avoidance strategies of stages 1 and 2 is effectively reduced. The application is suitable for fields such as sea transportation and improves the safety of the obstacle avoidance process of the unmanned ship.

Description

Technical Field

[0001] The present invention relates to a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy, and belongs to the technical field of unmanned ship path planning. Background Art

[0002] With the advent of smart ships, unmanned vessels are finding widespread application in rescue, maritime operations, and ocean exploration. Consequently, the requirements for autonomous navigation performance are becoming increasingly stringent, and route planning technology is a crucial component of autonomous navigation. Autonomous navigation typically follows a pre-planned global path, but actual navigation presents several challenges: obstacles that were not anticipated during global path planning; the complex situation of static obstacles and dynamic vessels; inherent issues with the dynamic window method; the impact of complex wind, wave, and current information on the unmanned vessel's obstacle avoidance process; the need for obstacle avoidance between dynamic vessels to comply with the International Regulations for Preventing Collisions at Sea; and the jitter associated with stage switching during the phased obstacle avoidance process.

[0003] The global path planning of unmanned vessels usually obtains environmental information based on electronic nautical charts. Therefore, the accuracy of environmental information in global path planning is limited by the accuracy of electronic nautical charts. During the operation of unmanned vessels, static obstacles that have not been foreseen in advance may appear, such as small islands and reefs. In addition, since electronic nautical charts cannot obtain specific information about dynamic ships, and the information about surrounding ships during the operation of unmanned vessels is real-time, obstacle avoidance of dynamic ships cannot be considered in global path planning.

[0004] Most existing local path planning methods consider collision avoidance between dynamic ships in an open sea area. However, in the actual operation of unmanned ships, especially in offshore areas, there may be scenarios where static obstacles and dynamic ships coexist, which increases the difficulty of local path planning for unmanned ships.

[0005] The dynamic window method is a common technique in local path planning, but it has drawbacks: It requires calculating the distance term between the unmanned vessel and the obstacle, using this term as a metric in the evaluation function. This function calculates the optimal speed, but it doesn't specify the calculation range for the obstacle distance term. This can cause the unmanned vessel to over-avoid obstacles, meaning it's too far from the obstacle, resulting in higher energy consumption. During actual obstacle avoidance, the unmanned vessel may become trapped in a local optimal position, stopping near an obstacle before reaching the target point, making effective obstacle avoidance impossible.

[0006] During obstacle avoidance in real-world waters, unmanned vessels are subject to interference from wind, waves, and currents. When this interference is within the vessel's tolerance, the energy required for the avoidance process is affected. Ignoring these factors can increase energy consumption, hindering efficient obstacle avoidance.

[0007] Obstacle avoidance maneuvers between unmanned vessels and dynamic vessels must comply with the International Regulations for Preventing Collisions at Sea (hereinafter referred to as the "Regulations"). The Regulations are maritime traffic rules established by the International Maritime Organization to prevent collisions between vessels at sea and ensure safe and efficient navigation. Because the Regulations define different encounter situations and prescribe collision avoidance maneuvers for each, this presents challenges for local path planning for unmanned vessels.

[0008] When switching between phases of a phased obstacle avoidance strategy, the collision risk of the dynamic vessel must be considered. If only a high risk threshold is introduced as the switching condition between phases, and the collision risk of the dynamic vessel fluctuates around the high risk threshold, the phase switching process of the obstacle avoidance strategy will fluctuate, and the unmanned vessel's obstacle avoidance behavior will also fluctuate, which is not conducive to the unmanned vessel's safe obstacle avoidance. Summary of the Invention

[0009] The existing local path planning technology does not take into account the complex situation that unmanned ships need to avoid both static obstacles and dynamic ships during navigation, and the obstacle avoidance between dynamic ships needs to comply with the International Regulations for Preventing Collisions at Sea. The main purpose of this invention is to propose a local path planning method for unmanned ships based on a three-stage obstacle avoidance strategy, which can achieve obstacle avoidance in this complex situation and improve the safety of the unmanned ship's obstacle avoidance process.

[0010] The object of the present invention is achieved through the following technical solutions:

[0011] The present invention discloses a local path planning method for an unmanned vessel based on a three-stage obstacle avoidance strategy. The three stages correspond to the following situations: a dynamic ship is at a long distance; a dynamic ship is at a close distance but does not meet the risk conditions; and a dynamic ship is at a close distance and meets the risk conditions. The obstacle avoidance strategy in stage 0 uses a dynamic obstacle avoidance maneuvering range and a dynamic target point to address the inherent problems of the dynamic window method. Environmental influences are also introduced during the selection of the dynamic target point, reducing the energy consumption of the unmanned vessel during the static obstacle avoidance process. The obstacle avoidance strategy in stage 1 combines the speed barrier method with the International Regulations for Preventing Collisions at Sea, employing speed barrier restrictions (VO restrictions) to reduce the collision risk and difficulty of obstacle avoidance during the obstacle avoidance process in stage 1. The obstacle avoidance strategy in stage 2 combines the speed barrier method with the International Regulations for Preventing Collisions at Sea, employing a port restriction and starboard scoring mechanism to implement obstacle avoidance for the unmanned vessel based on the International Regulations for Preventing Collisions at Sea. Furthermore, a dual-threshold method based on collision risk value is used to implement the switching process between the obstacle avoidance strategies in stages 1 and 2, effectively reducing jitter during the switching process and improving the safety of the unmanned vessel's obstacle avoidance process.

[0012] The present invention discloses a method for local path planning of an unmanned ship based on a three-stage obstacle avoidance strategy, comprising the following steps:

[0013] Step 1: Simplify the kinematic model of the unmanned ship to three degrees of freedom. The calculation formula is as follows:

[0014]

[0015] Among them, [uvr] T is the speed of the unmanned ship in the carrier coordinate system, is the position of the UAV in the north-east coordinate system, and J(η) is the transformation matrix between the two coordinate systems.

[0016] Step 2: Build a raster map based on the perceived static obstacles, and obtain the environmental vector field based on the interference force of wind, waves, and current information on the unmanned vessel.

[0017] Step 3: Use fuzzy comprehensive evaluation method to calculate the collision risk value of dynamic ships.

[0018] Step 3.1: Calculate relevant information about the perceived obstacle.

[0019] According to the known perception radius R perception , determine the position and size of static obstacles within the sensing range, as well as the position, speed and other information of dynamic ships. If there is a dynamic ship within the sensing range, the collision risk value between the ships is calculated, otherwise there is no need to calculate the collision risk value. The factors affecting the collision risk value include the minimum encounter distance DCPA, the shortest encounter time TCPA, the distance between the two ships D, the relative direction of the obstacle ship B and the ship speed ratio K. The calculation formula is as follows,

[0020]

[0021] K=v T / v0 (7)

[0022] Among them, (x0,y0), v0 and They represent the position, speed and heading of the unmanned ship in the north-east coordinate system, (x T ,y T ) and v T They represent the position and velocity of the dynamic ship in the north-east coordinate system, v R and They represent the speed and heading of the dynamic ship relative to the unmanned ship, and θ represents the azimuth of the dynamic ship relative to the unmanned ship.

[0023] Step 3.2: Calculate the membership function of each influencing factor.

[0024] Calculate the membership functions of the minimum encounter distance, the shortest encounter time, the distance between the two ships, the relative orientation of the obstacle ship and the ship speed ratio, which are expressed as U DCPA 、U TCPA 、U D 、U B and U K , the calculation formula is as follows,

[0025]

[0026]

[0027] Where d1 represents the minimum safe range between the unmanned vessel and the dynamic vessel during navigation, and d2 represents the safe passing distance for the unmanned vessel. t1 represents the sailing time from the dynamic vessel's latest avoidance action position to the closest encounter point, and t2 represents the sailing time from the dynamic vessel's current position to the closest encounter point. D1 is related to the size of the unmanned vessel, and D2 represents the safe distance between the unmanned vessel and the dynamic vessel. C represents the heading angle between the unmanned vessel and the dynamic vessel.

[0028] Step 3.3: Use the weighted summation method to calculate the collision risk value. The calculation formula is as follows:

[0029] CRI=[ω DCPA ,ω TCPA ,ω D ,ω B ,ω K ]*[U DCPA ,U TCPA ,U D ,U B ,U K ] T (13)

[0030] Among them, ω DCPA 、ω TCPA 、ω D 、ω B and ω K The calculation weights corresponding to the five influencing factors respectively.

[0031] Step 4: Define the corresponding ranges and decision conditions of the three stages in the three-stage obstacle avoidance strategy. The unmanned ship selects the obstacle avoidance strategy of the corresponding stage according to the corresponding ranges and decision conditions.

[0032] Step 4.1: Define the corresponding ranges of the three stages in the three-stage obstacle avoidance strategy.

[0033] Three semicircular areas with radii of R, 2R, and 4R, which are symmetrical with respect to the bow direction of the unmanned ship, are defined in the bow direction of the unmanned ship. The area is used to judge the distance and direction between the dynamic ship and the unmanned ship. The size of R is constrained by the motion characteristics of the unmanned ship itself and the dynamic obstacle avoidance control domain. The semicircular area with a radius of 4R should be within the perception range of the unmanned ship, that is, R perception >4R,R perception Indicates the maximum radius of the unmanned vessel's sensing range. Phase 0 corresponds to a range where the dynamic vessel is outside a semicircular area with a radius of 4R in the direction of the unmanned vessel's bow. Phase 1 corresponds to a range where the dynamic vessel is within a semicircular area with a radius of 2R and 4R in the direction of the unmanned vessel's bow. Phase 2 corresponds to a range where the dynamic vessel is within a semicircular area with a radius of 2R in the direction of the unmanned vessel's bow.

[0034] Step 4.2: Define the decision conditions of the three stages of the three-stage obstacle avoidance strategy, as shown in Table 1. Where distance represents the distance between the unmanned ship and the dynamic ship, CRI represents the collision risk value between the dynamic ship and the unmanned ship, and CRI high CRI is a high collision risk threshold. low is the high collision risk threshold. When there are no dynamic vessels within the unmanned vessel's perception range, the unmanned vessel always adopts the obstacle avoidance strategy of stage 0. When there are dynamic vessels within the unmanned vessel's perception range, if the dynamic vessel is within the corresponding range of stage 0, the obstacle avoidance strategy of stage 0 is adopted; if the dynamic vessel is within the corresponding range of stage 1, the obstacle avoidance strategy of stage 1 is adopted; if the dynamic vessel is within the corresponding range of stage 2, the obstacle avoidance strategy is selected based on the collision risk value of the dynamic vessel: if the collision risk value is higher than the high risk threshold, the obstacle avoidance strategy of stage 2 is adopted. The collision risk value gradually decreases during the obstacle avoidance process until the collision risk value is lower than the low risk threshold, at which point the obstacle avoidance strategy of stage 1 is adopted.

[0035] Table 1 Determining conditions for the three-stage obstacle avoidance strategy

[0036]

[0037] Step 4.3: Since there is a switching problem between the obstacle avoidance strategies of stage 1 and stage 2 within the corresponding range of stage 2, a dual threshold method is used in the switching condition between the obstacle avoidance strategies of stage 1 and stage 2 in combination with the collision risk value. The formula is as follows:

[0038]

[0039] Among them, Stage represents the stage number, Flag stage2 Indicates whether the obstacle avoidance strategy of stage 2 is currently adopted. A value of 1 indicates that the obstacle avoidance strategy of stage 2 is adopted, and a value of 0 indicates that the obstacle avoidance strategy of stage 2 is not adopted. When the dynamic ship is within the corresponding range of stage 2, if the collision risk value is always lower than the low risk threshold CRI low , then the obstacle avoidance strategy of stage 1 is adopted; if the collision risk value is higher than the high risk threshold CRI high , the obstacle avoidance strategy of stage 2 is adopted until the collision risk value is lower than the low risk threshold, and then the obstacle avoidance strategy of stage 1 is switched.

[0040] Step 5: It is stipulated that when the unmanned ship and the dynamic ship are in a head-on, right crossing or left crossing situation, they shall actively turn to the starboard side to avoid; when the unmanned ship and the dynamic ship are in a pursuit situation, the unmanned ship as the rear ship shall overtake the dynamic ship from the port or starboard side, so that the obstacle avoidance behavior of the unmanned ship complies with the "Rules".

[0041] Step 6: Improve the dynamic window method through dynamic obstacle avoidance control domain and dynamic target point to achieve collision avoidance in the case of only static obstacles or no obstacles within the perception range of the unmanned ship, that is, stage 0 collision avoidance.

[0042] Step 6.1: Determine the parameters related to the dynamic window method, including the fixed time Δt of the dynamic speed window and the calculation weight α of the evaluation function d , β d and γ d .

[0043] Step 6.2: Set the dynamic obstacle avoidance maneuvering area of ​​the unmanned vessel.

[0044] The dynamic obstacle avoidance maneuvering domain is an elliptical area surrounding the unmanned ship. The unmanned ship is located at the focus of the elliptical area slightly behind the unmanned ship. The major axis of the ellipse is consistent with the bow direction of the unmanned ship. When the speed of the unmanned ship increases, the range of the dynamic obstacle avoidance maneuvering domain increases, and vice versa. The calculation formula of the ellipse parameters of the dynamic obstacle avoidance maneuvering domain is as follows:

[0045]

[0046]

[0047] Where a and b are the major and minor axes of the ellipse, respectively, and R min Indicates the minimum major axis value, R self and R obs Represent the expansion radius of the unmanned ship and the obstacle, R stop Indicates the minimum braking distance corresponding to the current speed, and requires that the range corresponding to R is larger than the maximum range of the elliptical area. max is the maximum speed of the unmanned ship, and u(t) is the current speed of the unmanned ship.

[0048] Step 6.3: Define the candidate point set, candidate point constraints and evaluation function of the dynamic target point, and use the dynamic target point as the target point in the dynamic window method.

[0049] A semicircle with a radius of R is obtained with the unmanned ship as the center in the bow direction of the unmanned ship. The candidate point set is a series of points located in the semicircle. The candidate point constraint in the obstacle avoidance strategy of stage 0 is mainly the obstacle restriction. Before selecting the dynamic target point, first exclude the candidate points where there are obstacles within a certain width range from the unmanned ship to the candidate point, that is, the obstacle constraint. Calculate the evaluation function, and the candidate point corresponding to its maximum value is the dynamic target point. The calculation formula of the evaluation function G0 of the dynamic target point is as follows:

[0050] G0=[dist ob ,dist goal ,g w ]·ω0 T (18)

[0051] Where ω0 is the calculation weight. dist ob Indicates the minimum distance between the candidate point and the static obstacle. dist goal It is negatively correlated with the distance between the candidate point and the target point. w It represents the amount of work done by the environmental vector field on the unmanned ship when the unmanned ship travels a fixed distance Δx along the straight line direction of the candidate point.

[0052] dist goal and g w The calculation formula is as follows,

[0053]

[0054] g w =g val ·Δx·cos(θ cg ) (20)

[0055] Among them, (x c ,y c ), (x goal ,y goal) and (x0, y0) represent the position coordinates of the candidate point, target point and unmanned ship respectively, g val represents the size of the environmental vector field, θ cg Indicates the angle between the UAV heading and the current environment vector direction.

[0056] Step 6.4: Dynamic selection of dynamic target points and dynamic change of evaluation function.

[0057] The dynamic window method, improved by using dynamic target points, suffers from the problem of unreachable targets. To address this, the true target point is used as the dynamic target point when it enters the dynamic obstacle avoidance maneuvering domain. Simultaneously, the weight parameters of the evaluation function of the current dynamic window method are adjusted, increasing the corresponding weight of the target distance term and decreasing the corresponding weights of the speed and obstacle terms.

[0058] Step 7: Based on the improved dynamic window method, a new candidate point constraint is introduced into the obstacle avoidance strategy of Phase 1, namely, a VO restriction based on speed barriers. The strategy prioritizes collision avoidance actions that comply with the regulations, with this priority determined by a predefined threshold for starboard candidate points. This strategy achieves collision avoidance for both static obstacles and dynamic vessels.

[0059] Step 7.1: The candidate point constraints in stage 1 include obstacle restrictions and VO restrictions. The obstacle restrictions are the same as those in stage 0, and the VO restrictions are obtained by the speed barrier method. According to the principle of the speed barrier method, the relative collision cone between the dynamic ship and the unmanned ship is first generated, hereinafter referred to as RCC. When the speed of the unmanned ship relative to the dynamic ship is inside the RCC, the two ships will collide at a future time. The RCC is translated along the speed direction of the obstacle ship to obtain the speed barrier area, hereinafter referred to as VO. When the speed of the unmanned ship is inside the VO, it means that the two ships will collide in the future. With the current speed of the unmanned ship as the radius, a semicircular arc with the position of the unmanned ship as the center is obtained. There are two intersection points between the arc and VO. Two straight lines are obtained by connecting the two intersection points from the position point of the unmanned ship. If the candidate points on the boundary of the semicircular arc with a radius of R are within the range of these two straight lines, they are unsafe candidate points and are discarded. To ensure sufficient obstacle avoidance space, a semicircular boundary with a radius of R is formed with the UAV as the center. This semicircular boundary intersects the VO at two additional points, and two additional lines are connected from the UAV's position to form the final VO restricted area. Based on these four lines, the leftmost and rightmost lines are selected to determine the final VO restricted area. If a candidate point on the semicircular boundary with a radius of R falls within the VO restricted area, the candidate point is discarded. This VO restricted area is larger than the original area, providing sufficient space for the UAV to avoid obstacles.

[0060] Step 7.2: Define the evaluation function of the dynamic target point in phase 1. The obstacle avoidance strategy in phase 1 requires that the obstacle avoidance behavior of the unmanned ship conform to the "Rules" as much as possible. For the three encounter situations of head-on encounter, left crossing, and right crossing, if the number of candidate points on the starboard side is greater than the set threshold, the dynamic target point is selected from the candidate points on the starboard side; if the threshold condition is not met, the dynamic target point is selected from the candidate points on the port side of the unmanned ship. The calculation formula of the evaluation function G1 of the dynamic target point in phase 1 is as follows,

[0061] G1=[dist ob ,dist goal ,g w ]·ω1 T (twenty one)

[0062] Among them, ω1 represents the calculation weight.

[0063] Step 8: Based on the improved dynamic window method, the obstacle avoidance strategy in Phase 2 introduces new candidate point constraints and evaluation function terms, namely, the port restriction and starboard scoring mechanism based on the Rules, while avoiding static obstacles and dynamic ships, and strictly following the Rules.

[0064] Step 8.1: The candidate point constraints for Phase 2 include obstacle restrictions, VO restrictions, and port restrictions. The obstacle restrictions are consistent with those for Phase 0, the VO restrictions are consistent with those for Phase 1, and the port restrictions are defined in accordance with the Rules. During Phase 2 obstacle avoidance, the UAV turns to starboard for encounter, left crossing, and right crossing situations. Therefore, a port restriction is introduced for these three situations, eliminating candidate points located to the UAV's port side. By combining the obstacle restrictions and VO restrictions, a feasible candidate point set is obtained.

[0065] Step 8.2: Since the main task of phase 2 is obstacle avoidance, which has a higher priority than the energy consumption target of the unmanned vessel, the influence of the environmental vector field is removed from the evaluation function, and a starboard scoring mechanism is introduced. The calculation formula of the evaluation function G2 of the dynamic target point in phase 2 is as follows:

[0066] G2=[dist ob ,dist goal ,right score ]·ω2 T (twenty two)

[0067]

[0068] Among them, ω2 is the calculation weight, right score is the starboard score of all candidate points, θ left and θ c They represent the positive port direction of the unmanned ship and the angle from the center of gravity of the unmanned ship to the candidate point in the northeast coordinate system. overtakingIndicates whether the unmanned ship is in an overtaking situation. A value of 1 indicates an overtaking situation, and a value of 0 indicates an encounter, left crossing, or right crossing situation.

[0069] Step 9: Apply the three-stage obstacle avoidance strategy of steps 6, 7, and 8 to the local path planning of the unmanned boat to improve the safety of the unmanned boat's obstacle avoidance process.

[0070] Beneficial effects:

[0071] 1. The present invention discloses a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy. In view of the complex situation in which unknown static obstacles and dynamic ships appear simultaneously during the driving process of the unmanned ship, the three-stage obstacle avoidance strategy is used to achieve collision avoidance of the unmanned ship against both static obstacles and dynamic ships at the same time, and collision avoidance of dynamic ships is achieved in accordance with the "Rules".

[0072] 2. The present invention discloses a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy. The method adopts an elliptical dynamic obstacle avoidance control domain to determine the position range of the obstacle distance term in the evaluation function of the dynamic window method. The parameters of the dynamic obstacle avoidance control domain change dynamically with the current motion parameters of the unmanned ship, realizing adaptive parameter adjustment, effectively solving the problem of excessive obstacle avoidance of the dynamic window method, and reducing the energy consumption of the unmanned ship during the obstacle avoidance process.

[0073] 3. The present invention discloses a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy. A semicircular predicted trajectory area with the unmanned ship as the center and a radius of R is obtained from the boundary of the area. The best dynamic target point is selected from the candidate points according to the defined evaluation function. The dynamic target point is used as the target point in the dynamic window method to guide the unmanned ship to avoid obstacles in real time, effectively solving the local optimal problem of the dynamic window method.

[0074] 4. The present invention discloses a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy. The method introduces the related terms of the work done by environmental forces on the unmanned ship into the evaluation function of the dynamic target point, thereby reducing the energy consumption of the unmanned ship in the obstacle avoidance process in stage 0 when the unmanned ship is disturbed by wind, waves and currents at sea, and improving the adaptability of the unmanned ship's obstacle avoidance process to the external environment.

[0075] 5. The present invention discloses a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy. In the obstacle avoidance strategy of stage 1, the candidate point set that complies with the "Rules" is given priority. Based on the improved dynamic window method in stage 0, two candidate point constraints are proposed in combination with the speed obstacle method, including VO restriction and obstacle restriction, which effectively reduce the collision risk and obstacle avoidance difficulty of the unmanned ship in the obstacle avoidance process of stage 2.

[0076] 6. This invention discloses a local path planning method for an unmanned vessel based on a three-stage obstacle avoidance strategy. In the second stage of the obstacle avoidance strategy, an improved dynamic window method is used, combined with the speed barrier method and the "Regulations," to propose three candidate point constraints: VO restriction, obstacle restriction, and port restriction. A starboard scoring mechanism is introduced into the evaluation function for dynamic target points, ensuring that the unmanned vessel strictly adheres to the "Regulations" for collision avoidance of dynamic vessels.

[0077] 7. The present invention discloses a local path planning method for an unmanned ship based on a three-stage obstacle avoidance strategy. The method adopts a dual-threshold method based on the collision risk value. According to the current dynamic ship position and the collision risk value, the start and end times of the obstacle avoidance in stage 2 are calculated to avoid excessive obstacle avoidance operations of the unmanned ship on the dynamic ship when the dynamic ship is within the corresponding range of stage 2 but its collision risk is extremely low. The method effectively reduces the jitter of the unmanned ship in the switching process between the obstacle avoidance strategies in stage 1 and stage 2, making obstacle avoidance safer and more reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 This is a flowchart of the local path planning of the unmanned ship based on the three-stage obstacle avoidance strategy;

[0079] Figure 2 It is the North-East coordinate system and the carrier coordinate system;

[0080] Figure 3 are raster maps and environmental vector fields;

[0081] Figure 4 is the corresponding range of each stage in the three-stage obstacle avoidance strategy;

[0082] Figure 5 These are the four conditions for judging the situations you will encounter;

[0083] Figure 6 It is the dynamic target point and dynamic obstacle avoidance control domain;

[0084] Figure 7 It is a method for determining dynamic target points;

[0085] Figure 8 It is a local optimal problem of the traditional dynamic window method;

[0086] Figure 9 is the result of the static obstacle avoidance experiment;

[0087] Among them, Figure (a) shows the experimental results when the weight is ω0 = [0.4, 0.5, 0], and Figure (b) shows the experimental results when the weight is ω0 = [0.4, 0.5, 0.1];

[0088] Figure 10 This is the experimental result of unmanned boats in encounter situations;

[0089] Figure (a) shows the obstacle avoidance process in stage 1, Figure (b) shows the obstacle avoidance process in stage 2, Figure (c) shows the end of obstacle avoidance, and Figure (d) shows the distance between the two ships.

[0090] Figure 11 This is the experimental result of the unmanned boat in the left crossing situation;

[0091] Among them, Figure (a) shows the obstacle avoidance process in stage 2, Figure (b) shows the obstacle avoidance process in stage 0, Figure (c) shows the end of obstacle avoidance, and Figure (d) shows the distance between the two ships. DETAILED DESCRIPTION

[0092] The present invention will be described in detail below with reference to the accompanying drawings and embodiments. The technical problems solved by the technical solution of the present invention and the beneficial effects thereof are also described. It should be noted that the described embodiments are only intended to facilitate understanding of the present invention and do not serve to limit the present invention in any way.

[0093] This embodiment discloses a flowchart of the local path planning of the unmanned ship based on the three-stage obstacle avoidance strategy proposed by the present invention. Figure 1 The following implementation is implemented in MATLAB to enable the unmanned ship to avoid both static and dynamic obstacles based on the International Regulations for Preventing Collisions at Sea.

[0094] Step 1: North-East coordinate system and carrier coordinate system as shown in the attached Figure 2 As shown, [uvr] T is the speed of the unmanned ship in the carrier coordinate system, [xyψ] T is the position of the unmanned ship in the north-east coordinate system. Initialize the relevant parameters of the unmanned ship. The initial position vector of the unmanned ship is The velocity vector is [0,0,0] T .

[0095] Step 2: Create a 40x31 grid map in MATLAB with static obstacles, assuming the entire area is within the UAV's sensing range. Obtain marine meteorological data from the International Ocean Data Center. After processing the data, calculate the interference forces of wind, waves, and currents on the UAV. Combine these three interference forces to create an environmental vector field.

[0096] When the unmanned boat is traveling on the water, the interference forces of the sea breeze on the three degrees of freedom of the unmanned boat are X wind , Y wind and N wind , the calculation formula is as follows,

[0097]

[0098] Among them, ρ a is the air density, Af and A s They are the orthographic projection area and lateral projection area above the waterline of the unmanned ship, L oa The maximum length of the unmanned ship, U R is the relative average wind speed, α R is the windward angle, C WX (α R ), C WY (α R ) and C WN (α R ) is the wind pressure coefficient, calculated according to the Isherwood formula.

[0099] When the unmanned boat is traveling on the water, the influence of the second-order wave force on the movement of the unmanned boat is analyzed. The interference forces of the waves on the three degrees of freedom of the unmanned boat are X wave , Y wave and N wave , the calculation formula is as follows,

[0100]

[0101] Among them, ρ is the density of seawater, L is the length of the unmanned ship, and a wave is the average wave amplitude, λ is the wavelength of the corresponding wave, χ is the encounter angle of the unmanned ship, C DX (λ), C DY (λ), C DN (λ) is the wave drift force and moment coefficient, which can be calculated by the following formula

[0102]

[0103] Among them, L is the length of the unmanned ship, and λ is the wavelength of the corresponding wave.

[0104] The ocean current will cause the unmanned ship to have an axial moment N c , which changes the position and speed of the unmanned ship. The calculation formula is as follows:

[0105]

[0106] Among them, V c is the ocean current velocity, L s is the waterline length of the unmanned ship, β is the drift angle, C Nc (β) is the coefficient of the current force around the oz axis. Considering that ocean currents change slowly over time, the current velocity can be considered constant over a certain period of time.

[0107] The interference force of wind, waves and currents on the unmanned ship is weighted and calculated to obtain the size of the environmental vector field. Only the interference force in the sea level direction is considered in the calculation process. The calculation formula is as follows:

[0108]

[0109] Among them, u env is the magnitude of the environmental vector field in the north direction, v env is the size of the environmental vector field in the east direction. cur ,u wind and u wave are the north-direction components of the interference forces exerted on the unmanned ship by ocean currents, sea breezes, and waves. cur , v wind and v wave They are the east-direction components of the interference forces exerted on the unmanned ship by ocean currents, sea breezes, and waves.

[0110] The grid map and environment vector field are as follows Figure 3 In the figure, the black area represents static obstacles, the white area represents obstacle-free areas at sea, and the green arrows represent the direction and magnitude of the environmental vector field.

[0111] Step 3: Calculate the collision risk value of dynamic ships based on the fuzzy comprehensive evaluation method.

[0112] Step 3.1: Calculate the information about static obstacles and dynamic ships. Assume that the position and size of the perceived static obstacles are as follows: Figure 3 The black area in the figure is shown. The unmanned vessel can obtain the dynamic ship's position and speed in real time. Using the formula in step 3.1 of the technical solution, the five factors influencing the collision risk are calculated: minimum encounter distance DCPA, shortest encounter time TCPA, distance between the two ships D, relative bearing of the obstacle ship B, and ship speed ratio K.

[0113] Step 3.2: Calculate the membership function U of each influencing factor DCPA 、U TCPA 、U D 、U B and U K Calculate the membership function U that sets the minimum encounter distance in this embodiment DCPA The calculation process is related to the position of the dynamic ship relative to the unmanned ship. According to the Rules, different angle ranges are defined with the unmanned ship as the center, and four different encounter situations are obtained, as shown in the attached figure. Figure 5 As shown. The calculation formulas for d1 and d2 are as follows,

[0114]

[0115] d2=2d1 (30)

[0116] Where B represents the relative position of the obstacle ship, θ T Indicates the angle of the dynamic ship relative to the unmanned ship, corresponding to the Figure 5 .

[0117] This embodiment sets the membership function U of the minimum encounter time TCPA The calculation formulas for the relevant parameters t1 and t2 are as follows:

[0118]

[0119] Among them, v R Indicates the speed of the dynamic ship relative to the unmanned ship.

[0120] In this embodiment, the membership function U of the distance between the two ships is set as D The calculation formulas of the relevant parameters D1 and D2 are as follows.

[0121] D1=2 (33)

[0122]

[0123] Step 3.3: Calculate the collision risk value using the weighted summation method. Choose ω based on experience. DCPA 、ω TCPA 、ω D 、ω B and ω K The value of is calculated as follows:

[0124] CRI=[0.4,0.367,0.167,0.033,0.033]*[U DCPA ,U TCPA ,U D ,U B ,U K ] T (35)

[0125] Step 4: In this embodiment, the corresponding ranges and decision conditions of the three stages in the three-stage obstacle avoidance strategy are given by steps 4.1 and 4.2.

[0126] Step 4.1: The corresponding ranges of the three stages in the three-stage obstacle avoidance strategy are as shown in the attached figure. Figure 4 The value of R is determined by the ship's own motion characteristics and dynamic obstacle avoidance maneuvering domain. The boundary value 4R of stage 0 and stage 1 should be within the ship's perception range, that is, R perception >4R, where R perception Is the size of the perception range radius. Set R = 8, the perception range radius R perception>48. Corresponding range for Phase 0: The dynamic ship is outside the semicircular area with a radius of 48 in the bow direction of the unmanned vessel. Corresponding range for Phase 1: The dynamic ship is within the semicircular area with an inner radius of 24 and an outer radius of 48 in the bow direction of the unmanned vessel. Corresponding range for Phase 2: The dynamic ship is within the semicircular area with a radius of 24 in the bow direction of the unmanned vessel.

[0127] Step 4.2: The decision conditions of the three stages in the three-stage obstacle avoidance strategy are the main and high risk thresholds (CRI). high and low risk threshold CRI low Related. Set CRI high =0.48, CRI low =0.05. If the dynamic ship is within the range corresponding to stage 0, the unmanned vessel adopts the obstacle avoidance strategy of stage 0; if the dynamic ship is within the range corresponding to stage 1, the unmanned vessel adopts the obstacle avoidance strategy of stage 1; if the dynamic ship is within the range corresponding to stage 2, the collision risk value between the dynamic ship and the unmanned vessel is calculated, and the obstacle avoidance strategy of stage 1 or stage 2 is determined based on the risk threshold condition.

[0128] Step 4.3: Within the corresponding range of stage 2, there is a switching process between the obstacle avoidance strategies of stage 1 and stage 2. According to the high risk threshold CRI proposed in step 4.2 high and low risk threshold CRI low This constitutes a dual-threshold judgment condition. Within the corresponding range of Phase 2, if the collision risk value of the dynamic ship remains below 0.48, the obstacle avoidance strategy of Phase 1 is adopted. If the collision risk value of the dynamic ship exceeds 0.48, the obstacle avoidance strategy of Phase 2 is adopted. This continues until the collision risk value falls below 0.05, at which point the obstacle avoidance process of Phase 2 ends and switches to the obstacle avoidance strategy of Phase 1.

[0129] Step 5: Specify obstacle avoidance behaviors for unmanned vessels in accordance with the Rules. When the unmanned vessel and a dynamic vessel are in a head-on, starboard, or left-crossing situation, the unmanned vessel will actively turn to starboard to avoid the dynamic vessel. When the unmanned vessel and a dynamic vessel are in a pursuit situation, the unmanned vessel, as the rear vessel, may overtake the dynamic vessel from either port or starboard.

[0130] Step 6: The Phase 0 collision avoidance algorithm addresses situations where only static obstacles or no obstacles are within the UAV's perception range. This phase addresses the inherent issues of the dynamic window method and implements Phase 0 collision avoidance based on an improved dynamic window method. Step 6.1 describes the traditional dynamic window method, and the specific improvements to the dynamic window method are described in Steps 6.2, 6.3, and 6.4.

[0131] Step 6.1: Determine the relevant parameters in the traditional dynamic window method.

[0132] First, calculate the dynamic speed window and set the maximum speed u of the unmanned shipmax =1, maximum angular velocity r max =40°, fixed time Δt=3s, the calculation formula is as follows:

[0133] V s (u,r)={(u,r)|0≤u≤1,0≤r≤40°} (36)

[0134]

[0135] V f (u,r)=V s (u,r)∩V d (u,r) (38)

[0136] Among them, V f (u, r) represents the constraints of the maximum speed and maximum bow angular velocity of the unmanned ship. u and v represent the speed and angular velocity of the unmanned ship obtained by discretizing the dynamic speed window. u max and r max Represent the maximum speed and maximum angular velocity of the unmanned ship respectively. d (u, r) represents the finite speed that the unmanned ship can reach within a fixed time Δt, u(t) and r(t) represent the speed and angular velocity of the unmanned ship at the current time t, respectively. f (u,r) represents the final velocity window.

[0137] Then determine the relevant parameters of the evaluation function. Set the specific value of the calculation weight of the evaluation function and the maximum distance of the obstacle distance item. The calculation formula is as follows:

[0138] G(u,r)=0.55H(u,r)+0.35D(u,r)+0.1v(u,r) (39)

[0139]

[0140] Among them, H(u,r) is used to evaluate the angle between the end of the predicted path and the target point and the angle difference between the unmanned ship's navigation, D(u,r) is used to evaluate the distance between the end of the predicted path and the nearest obstacle, and v(u,r) is used to evaluate the speed value of the unmanned ship at the end of the predicted path. Indicates the azimuth of the target point relative to the unmanned ship, Indicates the heading angle of the unmanned ship. dist t Indicates the actual distance between the unmanned vessel and the obstacle within the maximum distance.

[0141] Step 6.2: Set the dynamic obstacle avoidance maneuvering area of ​​the unmanned boat, as shown in the attached Figure 6As shown. The dynamic obstacle avoidance maneuvering domain is elliptical, and the unmanned ship is located at the focus of the elliptical area slightly behind. The long axis of the ellipse is consistent with the bow direction of the unmanned ship, and its parameters change with the speed of the unmanned ship. Set the maximum acceleration of the unmanned ship Calculate the minimum major axis value R min =1.67, set the expansion radius R of the unmanned ship self = 0.2, the expansion radius of the obstacle R obs =0.2. The parameter calculation formula of the dynamic obstacle avoidance control range is as follows:

[0142]

[0143] R stop =u(t) 2 / 0.6 (43)

[0144] Step 6.3: Define the candidate point set, candidate point constraints and evaluation function of the dynamic target point, and use the dynamic target point as the target point in the dynamic window method. With the unmanned ship as the center, obtain the semicircular arc boundary with a radius of R in the bow direction of the unmanned ship, as shown in the attached figure. Figure 6 As shown in Figure 1, candidate points are a series of points located on the boundary. The candidate point constraints in the obstacle avoidance strategy of stage 0 are mainly obstacle restrictions. Before selecting the dynamic target point, first exclude candidate points with obstacles within a certain width from the unmanned ship to the candidate point, i.e., obstacle constraints, as shown in the following figure. Figure 6 As shown in the long rectangle in the figure. Calculate the evaluation function, and the candidate point corresponding to its maximum value is the dynamic target point. Set the calculation weight ω0 = [0.4, 0.5, 0.1], and the calculation formula of the evaluation function G0 of the dynamic target point is as follows:

[0145] G0=[dist ob ,dist goal ,g w ]·[0.4,0.5,0.1] T (44)

[0146] Among them, dist ob Indicates the minimum distance between the candidate point and the static obstacle. dist goal It is negatively correlated with the distance between the candidate point and the target point. w It represents the amount of work done by the environmental vector field on the unmanned ship when the unmanned ship travels a fixed distance Δx along the straight line direction of the candidate point.

[0147] dist goal and g w The calculation formula is as follows,

[0148]

[0149] gw =0.5g val ·u(t)·cos(θ cg ) (46)

[0150] Among them, (x c ,y c ), (x goal ,y goal ) and (x0, y0) represent the position coordinates of the candidate point, target point and unmanned ship respectively, g val represents the size of the environmental vector field, θ cg It represents the angle between the heading of the unmanned ship and the current environment vector direction, and is set to Δx = 0.5u(t).

[0151] Step 7: The collision avoidance algorithm in Phase 1 of the Regulations simultaneously avoids both static obstacles and dynamic vessels. The Phase 1 obstacle avoidance strategy prioritizes collision avoidance behaviors that comply with the Regulations. This priority is determined by a predefined threshold for starboard candidate points. The threshold for starboard candidate points is set to 10. The constraints and evaluation function for the dynamic target points in Phase 1 are detailed in Steps 7.1 and 7.2.

[0152] Step 7.1: The candidate point constraints in stage 1 include obstacle constraints and VO constraints. The obstacle constraints are the same as those in stage 0. The VO constraints are obtained by the speed obstacle method, as shown in the following figure. Figure 7 As shown. According to the principle of the speed barrier method, the relative collision cone between the dynamic ship and the unmanned ship is first generated, hereinafter referred to as RCC. When the speed of the unmanned ship relative to the dynamic ship is inside the RCC, the two ships will collide at a certain moment in the future. The speed barrier area can be obtained by translating the RCC along the speed direction of the obstacle ship, hereinafter referred to as VO. If the speed of the unmanned ship is inside VO, the two ships will collide in the future. With the current speed of the unmanned ship as the radius, a semicircular arc with the position of the unmanned ship as the center is obtained. There are two intersection points between the arc and VO. Two straight lines are obtained by connecting the two intersection points from the position point of the unmanned ship. If the candidate points on the boundary of the semicircular arc with a radius of R are within the range of these two straight lines, they are unsafe candidate points and are discarded. In order to ensure sufficient obstacle avoidance space, a semicircular arc boundary with a radius of R is obtained with the unmanned ship as the center. The semicircular arc and VO have another two intersection points. Two other straight lines are obtained by connecting the two intersection points from the position point of the unmanned ship. Based on these four lines, the leftmost and rightmost lines are selected to determine the final VO restricted area. If a candidate point on the semicircular arc boundary with a radius of R is within the VO restricted area, the candidate point is discarded. This VO restricted area is larger than the original area, which helps provide sufficient obstacle avoidance space for the unmanned vessel.

[0153] Step 7.2: Determine the evaluation function of the dynamic target point in phase 1. The obstacle avoidance strategy in phase 1 requires that the obstacle avoidance behavior of the unmanned ship conform to the "Rules" as much as possible. For the three encounter situations of head-on encounter, left crossing, and right crossing, if the number of candidate points on the starboard side is greater than 10, the dynamic target point is selected from the candidate points on the starboard side; if the threshold condition is not met, the dynamic target point is selected from the candidate points on the port side of the unmanned ship. Set ω1 = [0.45, 0.5, 0.05], and the calculation formula of the evaluation function G1 of the dynamic target point in phase 1 is as follows,

[0154] G1=[dist ob ,dist goal ,g w ]·[0.45,0.5,0.05] T (47)

[0155] Step 8: The Phase 2 collision avoidance algorithm avoids both static obstacles and dynamic vessels, strictly adhering to the Regulations. Based on the improved dynamic window method, the Phase 2 obstacle avoidance strategy introduces new candidate point constraints and evaluation function terms, namely, the port restriction and starboard scoring mechanism based on the Regulations. The Phase 2 candidate point constraints and evaluation function are detailed in Steps 8.1 and 8.2.

[0156] Step 8.1: The candidate point constraints for Phase 2 include obstacle restrictions, VO restrictions, and port restrictions. The obstacle restrictions are consistent with those for Phase 0, the VO restrictions are consistent with those for Phase 1, and the port restrictions are defined in accordance with the Rules. During Phase 2 obstacle avoidance, the UAV turns to starboard for encounter, left crossing, and right crossing situations. Therefore, a port restriction is introduced for these three situations, eliminating candidate points located to the UAV's port side. By combining the obstacle restrictions and VO restrictions, a feasible candidate point set is obtained.

[0157] Step 8.2: Determine the evaluation function G2 for the dynamic target point in phase 2. Since the main task of phase 2 is obstacle avoidance, which has a higher priority than the energy consumption target of the unmanned vessel, the influence of the environmental vector field is removed from the evaluation function, and a starboard scoring mechanism is introduced. Set ω2 = [0.5, 0.3, 0.2], and the calculation formula for the evaluation function G2 for the dynamic target point in phase 2 is as follows:

[0158] G2=[dist ob ,dist goal ,right score ]·[0.5,0.3,0.2] T (48)

[0159]

[0160] Among them, right score is the starboard score of all candidate points, θleft and θ c They represent the positive port direction of the unmanned ship and the angle from the center of gravity of the unmanned ship to the candidate point in the northeast coordinate system. overtaking Indicates whether the unmanned ship is in an overtaking situation. A value of 1 indicates an overtaking situation, and a value of 0 indicates an encounter, left crossing, or right crossing situation.

[0161] Step 9: Based on Steps 1 to 8, the experimental results obtained in MATLAB for this example are given in Steps 9.1, 9.2, and 9.3, respectively. Step 9.1 verifies the effectiveness of the Stage 0 obstacle avoidance algorithm and the improved dynamic window method. Steps 9.2 and 9.3 incorporate a dynamic ship to verify the effectiveness of the complete obstacle avoidance strategy and algorithm proposed in this invention. During the experiment, the safety radius of the dynamic ship was set to 2.

[0162] Step 9.1: The traditional dynamic window method is prone to fall into local optimality, as shown in the following figure. Figure 8 As shown in the figure, the unmanned boat cannot avoid collision effectively and fails to reach the target point. Figure 9 As shown. Figure 9 (a) and (b) both show that the unmanned boat can effectively avoid the local optimal position and achieve obstacle avoidance, and safely reach the target point. Figure 9 (a) Without considering the impact on the environment, Figure 9 (b) The environmental impact is introduced, so the attached Figure 9 The obstacle avoidance path in (b) is more consistent with the direction of the environmental vector field. Assuming the net energy consumption of the unmanned boat = the energy consumption of the unmanned boat in the still water state - the work done by the environmental vector field on the unmanned boat, the unmanned boat in the surrounding Figure 9 The net energy consumption in (b) is 110.2203, and the unmanned ship is nearby. Figure 9 The net energy consumption in (a) is 112.6559. The results show that the energy consumption of the unmanned boat's obstacle avoidance can be effectively reduced after introducing environmental influences.

[0163] Step 9.2: Attach Figure 10 The experimental results of the unmanned ship using the three-stage obstacle avoidance strategy proposed in this invention in the encounter situation. The dynamic ship starts from (5, 0). Figure 10 (a) Using the obstacle avoidance strategy of stage 1, the candidate point on the starboard side of the unmanned ship is selected as the dynamic target point. Figure 10 (b) Using the obstacle avoidance strategy of stage 2, the unmanned ship turns to starboard in the encounter situation. Figure 10 (c) indicates that the unmanned boat has completed obstacle avoidance and reached the target point safely. Figure 10 (d) shows that the minimum distance between the two ships during the obstacle avoidance process is 2.2518, which is larger than the safety radius of the dynamic ship, and safe obstacle avoidance can be achieved.

[0164] Step 9.3: Attach Figure 11 The experimental results of the unmanned ship using the three-stage obstacle avoidance strategy proposed in this invention in the left crossing situation. The dynamic ship starts from (39,17). Figure 11 (a) Using the obstacle avoidance strategy of stage 2, the unmanned ship turns to starboard to avoid obstacles in the left crossing situation. Figure 11 In (b), the dynamic ship is located within the corresponding range of stage 0 and the obstacle avoidance strategy of stage 0 is adopted. Figure 11 (c) indicates that the unmanned boat has completed obstacle avoidance and reached the target point safely. Figure 11 (d) shows that the minimum distance between the two ships during the obstacle avoidance process is 3.0446, which is larger than the safety radius of the dynamic ship, and safe obstacle avoidance can be achieved.

[0165] The above specific description further illustrates the purpose, technical solutions and beneficial effects of the invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any 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 local path planning method for an unmanned vessel based on a three-stage obstacle avoidance strategy, characterized by: The following steps are included: Step 1: Simplify the kinematic model of the unmanned ship to three degrees of freedom; The implementation method of step one is: Among them, [uvr] T is the speed of the unmanned ship in the carrier coordinate system, is the position of the UAV in the north-east coordinate system, J(η) is the transformation matrix between the two coordinate systems; Step 2: Build a raster map based on the perceived static obstacles, and obtain the environmental vector field based on the interference force of wind, waves, and currents acting on the unmanned vessel. The implementation method of step 2 is: A 40x31 grid map with static obstacles was constructed in MATLAB, assuming that the entire area was within the sensing range of the unmanned vessel. Marine meteorological data was obtained from the International Ocean Data Center. After processing the data, the interference forces of wind, waves, and currents on the unmanned vessel were calculated, and the environmental vector field was established by combining these three interference forces. When the unmanned boat is traveling on the water, the interference forces of the sea breeze on the three degrees of freedom of the unmanned boat are X wind , Y wind and N wind , the calculation formula is as follows, Among them, ρ a is the air density, A f and A s They are the orthographic projection area and lateral projection area above the waterline of the unmanned ship, L oa The maximum length of the unmanned ship, U R is the relative average wind speed, α R is the windward angle, C WX (α R ), C WY (α R ) and C WN (α R ) is the wind pressure coefficient, calculated according to the Isherwood formula; When the unmanned boat is traveling on the water, the influence of the second-order wave force on the movement of the unmanned boat is analyzed. The interference forces of the waves on the three degrees of freedom of the unmanned boat are X wave , Y wave and N wave , the calculation formula is as follows, Among them, ρ is the density of seawater, L is the length of the unmanned ship, and a wave is the average wave amplitude, λ is the wavelength of the corresponding wave, χ is the encounter angle of the unmanned ship, C DX (λ), C DY (λ), C DN (λ) is the wave drift force and moment coefficient, which can be calculated by the following formula Where L is the length of the unmanned ship, and λ is the wavelength of the corresponding wave; The ocean current will cause the unmanned ship to have an axial moment N c , which changes the position and speed of the unmanned ship. The calculation formula is as follows: Among them, V c is the ocean current velocity, L s is the waterline length of the unmanned ship, β is the drift angle, C Nc (β) is the flow force coefficient around the oz axis. Considering that the ocean current changes slowly over time, the flow velocity can be considered constant within a certain period of time. The interference force of wind, waves and currents on the unmanned ship is weighted and calculated to obtain the size of the environmental vector field. Only the interference force in the sea level direction is considered in the calculation process. The calculation formula is as follows: Among them, u env is the magnitude of the environmental vector field in the north direction, v env is the size of the environmental vector field in the east direction; u cur ,u wind and u wave are the north-direction components of the interference forces exerted on the unmanned ship by ocean currents, sea breezes, and waves; v cur , v wind and v wave are the east-direction components of the interference forces exerted on the unmanned vessel by ocean currents, sea breezes, and waves, respectively; Step 3: Calculate the collision risk value of dynamic ships using fuzzy comprehensive evaluation method; Step 4: Define the corresponding ranges and decision conditions of the three stages of the three-stage obstacle avoidance strategy. The unmanned ship selects the obstacle avoidance strategy of the corresponding stage according to the corresponding ranges and decision conditions. The implementation method of step four is: Step 4.1: Define the corresponding ranges of the three stages of the three-stage obstacle avoidance strategy; Three semicircular areas with radii of R, 2R, and 4R, which are symmetrical with respect to the bow direction of the unmanned ship, are defined in the bow direction of the unmanned ship. The area is used to judge the distance and direction between the dynamic ship and the unmanned ship. The size of R is constrained by the motion characteristics of the unmanned ship itself and the dynamic obstacle avoidance control domain. The semicircular area with a radius of 4R should be within the perception range of the unmanned ship, that is, R perception >4R,R perception Indicates the maximum radius of the unmanned ship's perception range; the corresponding range of stage 0: the dynamic ship is outside the semicircular area with a radius of 4R in the direction of the unmanned ship's bow; the corresponding range of stage 1: the dynamic ship is within the semicircular area with a radius of 2R and 4R in the direction of the unmanned ship's bow; the corresponding range of stage 2: the dynamic ship is within the semicircular area with a radius of 2R in the direction of the unmanned ship's bow; Step 4.2: Define the decision conditions for the three stages of the three-stage obstacle avoidance strategy; When there is no dynamic ship within the perception range of the unmanned ship, the unmanned ship always adopts the obstacle avoidance strategy of stage 0; when there is a dynamic ship within the perception range of the unmanned ship, if the dynamic ship is within the corresponding range of stage 0, the obstacle avoidance strategy of stage 0 is adopted; if the dynamic ship is within the corresponding range of stage 1, the obstacle avoidance strategy of stage 1 is adopted; if the dynamic ship is within the corresponding range of stage 2, the obstacle avoidance strategy is selected according to the collision risk value of the dynamic ship: if the collision risk value is higher than the high risk threshold, the obstacle avoidance strategy of stage 2 is adopted, and the collision risk value gradually decreases during the obstacle avoidance process until the collision risk value is lower than the low risk threshold, and the obstacle avoidance strategy of stage 1 is adopted; Step 4.3: Since there is a switching problem between the obstacle avoidance strategies of stage 1 and stage 2 within the corresponding range of stage 2, a dual threshold method is used in the switching condition between the obstacle avoidance strategies of stage 1 and stage 2 in combination with the collision risk value. The formula is as follows: Among them, Stage represents the stage number, Flag stage2 Indicates whether the obstacle avoidance strategy of stage 2 is currently adopted. A value of 1 indicates that the obstacle avoidance strategy of stage 2 is adopted, and a value of 0 indicates that the obstacle avoidance strategy of stage 2 is not adopted. When the dynamic ship is within the corresponding range of stage 2, if the collision risk value is always lower than the low risk threshold CRI low , then the obstacle avoidance strategy of stage 1 is adopted; if the collision risk value is higher than the high risk threshold CRI high , then the obstacle avoidance strategy of stage 2 is adopted until the collision risk value falls below the low risk threshold, and then the obstacle avoidance strategy of stage 1 is switched; Step 5: It is stipulated that when the unmanned vessel and the dynamic vessel are in a head-on, starboard crossing, or left crossing situation, they shall actively turn to starboard to avoid; when the unmanned vessel and the dynamic vessel are in a pursuit situation, the unmanned vessel, as the rear vessel, shall overtake the dynamic vessel from the port or starboard side, so that the obstacle avoidance behavior of the unmanned vessel complies with the International Regulations for Preventing Collisions at Sea; Step 6: Improve the dynamic window method by using the dynamic obstacle avoidance maneuvering domain and dynamic target point to achieve collision avoidance in the case where there are only static obstacles or no obstacles within the perception range of the unmanned ship, that is, collision avoidance in stage 0; The implementation method of step six is: Step 6.1: Determine the parameters related to the dynamic window method, including the fixed time Δt of the dynamic speed window and the calculation weight α of the evaluation function d , β d and γ d ; Step 6.2: Set the dynamic obstacle avoidance maneuvering area of ​​the unmanned vessel; The dynamic obstacle avoidance maneuvering domain is an elliptical area surrounding the unmanned ship. The unmanned ship is located at the focus of the elliptical area slightly behind the unmanned ship. The major axis of the ellipse is consistent with the bow direction of the unmanned ship. When the speed of the unmanned ship increases, the range of the dynamic obstacle avoidance maneuvering domain increases, and vice versa. The calculation formula of the ellipse parameters of the dynamic obstacle avoidance maneuvering domain is as follows: Where a and b are the major and minor axes of the ellipse, respectively, and R min Indicates the minimum major axis value, R self and R obs Represent the expansion radius of the unmanned ship and the obstacle, R stop Indicates the minimum braking distance corresponding to the current speed, and requires that the corresponding range of R is larger than the maximum range of the elliptical area; u max is the maximum speed of the unmanned ship, u(t) is the current speed of the unmanned ship; Step 6.3: Define the candidate point set, candidate point constraints and evaluation function of the dynamic target point, and use the dynamic target point as the target point in the dynamic window method; A semicircle with a radius of R is obtained with the unmanned ship as the center in the bow direction of the unmanned ship. The candidate point set is a series of points located in the semicircle. The candidate point constraint in the obstacle avoidance strategy of stage 0 is mainly the obstacle restriction. Before selecting the dynamic target point, the candidate points with obstacles within a certain width from the unmanned ship to the candidate point are first excluded, that is, the obstacle constraint. The evaluation function is calculated, and the candidate point corresponding to its maximum value is the dynamic target point. The calculation formula of the evaluation function G0 of the dynamic target point is as follows: G0=[dist ob ,dist goal ,g w ]·ω0 T (18) Among them, ω0 is the calculation weight; dist ob Indicates the minimum distance between the candidate point and the static obstacle; dist goal It is negatively correlated with the distance between the candidate point and the target point; g w It represents the amount of work done by the environmental vector field on the unmanned ship when the unmanned ship travels a fixed distance Δx along the straight line of the candidate point; dist goal and g w The calculation formula is as follows, g w =g val ·Δx·cos(θ cg ) (20) Among them, (x c ,y c ), (x goal ,y goal ) and (x0, y0) represent the position coordinates of the candidate point, target point and unmanned ship respectively, g val represents the size of the environmental vector field, θ cg Indicates the angle between the UAV heading and the current environment vector direction; Step 6.4: Dynamic selection of dynamic target points and dynamic change of evaluation function; The dynamic window method improved by the dynamic target point has the problem of unreachable target. To solve this problem, when the real target point enters the dynamic obstacle avoidance control domain, the real target point is used as the dynamic target point. At the same time, the weight parameters of the evaluation function of the current dynamic window method are adjusted, increasing the corresponding weight of the target distance term and reducing the corresponding weights of the speed term and obstacle term. Step 7: Based on the improved dynamic window method, a new candidate point constraint is introduced into the obstacle avoidance strategy of stage 1, namely, the VO restriction based on speed barriers. The collision avoidance strategy that complies with the International Regulations for Preventing Collisions at Sea is prioritized. This priority is determined by a predefined threshold for the starboard candidate point. Collision avoidance is achieved simultaneously with static obstacles and dynamic ships. The implementation method of step seven is: Step 7.1: The candidate point constraints in stage 1 include obstacle restrictions and VO restrictions; the obstacle restrictions are the same as those in stage 0, and the VO restrictions are obtained by the speed obstacle method; according to the principle of the speed obstacle method, first generate the relative collision cone between the dynamic ship and the unmanned ship, hereinafter referred to as RCC; when the speed of the unmanned ship relative to the dynamic ship is inside the RCC, the two ships will collide at a future time; translate the RCC along the speed direction of the obstacle ship to obtain the speed obstacle area, hereinafter referred to as VO; when the speed of the unmanned ship is inside the VO, it means that the two ships will collide in the future; use the current speed of the unmanned ship as the radius to obtain a semicircular arc with the position of the unmanned ship as the center, and the arc has two intersections with VO, respectively. Connect the two intersection points from the position of the unmanned ship to obtain two straight lines. If the candidate point on the semicircular boundary with a radius of R is within the range of these two straight lines, it is an unsafe candidate point and is abandoned. In order to ensure sufficient obstacle avoidance space, a semicircular boundary with a radius of R is obtained with the unmanned ship as the center. The semicircular boundary and VO obtain two other intersection points, and two other straight lines are connected from the position of the unmanned ship to obtain these two intersection points. Based on these four straight lines, the leftmost and rightmost straight lines are selected to determine the final VO restricted area. If the candidate point on the semicircular boundary with a radius of R is within the VO restricted area, the candidate point is abandoned. The VO restricted area is larger than the original area, which is conducive to providing sufficient obstacle avoidance operation space for the unmanned ship. Step 7.2: Define the evaluation function of the dynamic target point in phase 1. The obstacle avoidance strategy in phase 1 requires that the obstacle avoidance behavior of the unmanned vessel comply with the International Regulations for Preventing Collisions at Sea as much as possible. For the three encounter situations of head-on encounter, left crossing, and right crossing, if the number of candidate points on the starboard side is greater than the set threshold, the dynamic target point is selected from the candidate points on the starboard side. If the threshold condition is not met, the dynamic target point is selected from the candidate points on the port side of the unmanned vessel. The calculation formula of the evaluation function G1 of the dynamic target point in phase 1 is as follows: G1=[dist ob ,dist goal ,g w ]·ω1 T (21) Among them, ω1 represents the calculation weight; Step 8: Based on the improved dynamic window method, the obstacle avoidance strategy in stage 2 introduces new candidate point constraints and evaluation function terms, namely, the port restriction and starboard scoring mechanism based on the International Regulations for Preventing Collisions at Sea. It avoids both static obstacles and dynamic vessels, and strictly adheres to the International Regulations for Preventing Collisions at Sea. The implementation method of step eight is: Step 8.1: The candidate point constraints in Phase 2 include obstacle restrictions, VO restrictions, and port restrictions. The obstacle restrictions are consistent with those in Phase 0, the VO restrictions are consistent with those in Phase 1, and the port restrictions are defined according to the International Regulations for Preventing Collisions at Sea. During the obstacle avoidance process in Phase 2, the UAV turns to starboard for encounter, left crossing, and right crossing situations. Therefore, a port restriction is introduced for these three situations, eliminating candidate points located on the UAV's port side. The obstacle restrictions and VO restrictions are combined to obtain a feasible candidate point set. Step 8.2: Since the main task of stage 2 is obstacle avoidance, which has a higher priority than the energy consumption target of the unmanned ship, the influence of the environmental vector field is deleted from the evaluation function, and the starboard scoring mechanism is introduced. The calculation formula of the evaluation function G2 of the dynamic target point in stage 2 is as follows: G2=[dist ob ,dist goal ,right score ]·ω2 T (22) Among them, ω2 is the calculation weight, right score is the starboard score of all candidate points, θ left and θ c Flag represents the positive port direction of the unmanned ship and the angle from the center of gravity of the unmanned ship to the candidate point in the northeast coordinate system; overtaking Indicates whether the unmanned ship is in an overtaking situation. A value of 1 indicates an overtaking situation, and a value of 0 indicates an encounter, left crossing, or right crossing situation. Step 9: Apply the three-stage obstacle avoidance strategy of steps 6, 7, and 8 to the local path planning of the unmanned boat to improve the safety of the unmanned boat's obstacle avoidance process.

2. The method for local path planning of an unmanned vessel based on a three-stage obstacle avoidance strategy according to claim 1, characterized in that: The implementation method of step three is: Step 3.1: Calculate relevant information about perceived obstacles; According to the known perception radius R perception , determine the position and size of static obstacles within the sensing range, as well as the position, speed and other information of dynamic ships; if there is a dynamic ship within the sensing range, calculate the collision risk value between the ships, otherwise there is no need to calculate the collision risk value; the factors affecting the collision risk value include the minimum encounter distance DCPA, the shortest encounter time TCPA, the distance between the two ships D, the relative direction of the obstacle ship B and the ship speed ratio K. The calculation formula is as follows, K=v T / v0 (7) Among them, (x0,y0), v0 and They represent the position, speed and heading of the unmanned ship in the north-east coordinate system, (x T ,y T ) and v T They represent the position and velocity of the dynamic ship in the north-east coordinate system, v R and They represent the speed and heading of the dynamic ship relative to the unmanned ship, and θ represents the azimuth of the dynamic ship relative to the unmanned ship; Step 3.2: Calculate the membership function of each influencing factor; Calculate the membership functions of the minimum encounter distance, the shortest encounter time, the distance between the two ships, the relative orientation of the obstacle ship and the ship speed ratio, which are expressed as U DCPA 、U TCPA 、U D 、U B and U K , the calculation formula is as follows, Where d1 represents the minimum safe range between the unmanned ship and the dynamic ship during navigation, d2 represents the safe passing distance of the unmanned ship; t1 represents the sailing time of the dynamic ship from the latest avoidance action position to the closest encounter point, and t2 represents the sailing time of the dynamic ship from the current position to the closest encounter point; D1 is related to the size of the unmanned ship, D2 represents the safe distance between the unmanned ship and the dynamic ship; C represents the heading angle between the unmanned ship and the dynamic ship; Step 3.3: Use the weighted summation method to calculate the collision risk value. The calculation formula is as follows: CRI=[ω DCPA ,ω TCPA ,ω D ,ω B ,ω K ]*[IN DCPA ,IN TCPA ,IN D ,IN B ,IN K ] T (13) Among them, ω DCPA 、ω TCPA 、ω D 、ω B and ω K The calculation weights corresponding to the five influencing factors respectively.

Images