Path planning method based on improved speed obstacle method

Abstract

The invention discloses a path planning method based on an improved speed obstacle method, and the method comprises the steps: S1, introducing a quaternary ship domain model and a collision threat distance into the calculation of the collision avoidance opportunity of the speed obstacle method, determining the extension domain of an obstacle, and determining the opportunity of the speed obstacle method for executing the avoidance; s2, the latest rudder applying distance is used as a critical value for transition from the collision danger situation to the urgent situation, and a urgent situation model is constructed; and S3, constructing an improved water surface unmanned ship collision avoidance model according to the determined obstacle extension field, the avoidance execution opportunity of the speed obstacle method and the urgent situation model, and obtaining a water surface unmanned ship collision avoidance path. According to the improved unmanned surface vehicle collision avoidance model, the collision avoidance path can be effectively shortened, and the collision avoidance time can be effectively shortened.

Description

Technical Field

[0001] The present invention relates to a path planning method based on an improved velocity obstacle method. Background Art

[0002] As a lightweight and intelligent water vehicle, an unmanned surface vehicle (USV) has advantages such as good maneuverability and diverse loading platforms, and is often used to perform tasks such as ocean surveys, maritime search and rescue, and environmental monitoring. Path planning, as the basis of motion control, is the core issue in the research of USV.

[0003] According to the knowledge of environmental information, path planning can be divided into global path planning and local path planning. The former can guide the USV to reach the destination in a known environment, and the latter can avoid collisions with unknown obstacles in real time. Local path planning mainly includes the dynamic window approach (DWA), artificial potential field (APF), and velocity obstacles (VO), etc. VO was first proposed by Fiorini in 1998 and has the advantages of simple principle and fast calculation speed. It can not only evaluate the collision risk but also provide a collision avoidance method, so it is widely used in the research of local path planning. However, currently, the VO algorithm is mostly used for unmanned vehicles and unmanned aerial vehicles and rarely used in the path planning of USV; moreover, most of the research ignores the constraints on left crossing encounters, which may lead to inconsistent avoidance and even pose a threat of collision. After sorting out and analyzing, it is found that the determination of the expansion domain of obstacles in the current improved VO is relatively subjective, often lacking consideration of factors such as the scale, speed, and maneuverability of the two ships, resulting in too long collision avoidance path length and collision avoidance time in local path planning. In addition, the VO algorithm based on COLREGs usually ignores the constraints on the responsibility of avoidance in close-quarter encounters.

[0004] Aiming at the above problems, a path planning method based on an improved velocity obstacle method is provided. Summary of the Invention

[0005] The purpose of the present invention is to provide a path planning method based on an improved velocity obstacle method to overcome the existing defects. The improved collision avoidance model of the unmanned surface vehicle can effectively shorten the collision avoidance path and time.

[0006] The technical solution for achieving the above purpose is:

[0007] A path planning method based on an improved velocity obstacle method, comprising:

[0008] Step S1: Introduce the four - element ship domain model and the collision threat distance into the calculation of the collision avoidance timing of the velocity obstacle method, and then determine the expansion domain of the obstacle and the timing for the velocity obstacle method to execute avoidance.

[0009] Step S2: Take the latest rudder - applying distance as the critical value for the transition from the collision - dangerous situation to the close - quarters situation, and construct a close - quarters situation model.

[0010] Step S3: Construct an improved collision avoidance model for the surface unmanned boat through the determined expansion domain of the obstacle, the timing for the velocity obstacle method to execute avoidance, and the close - quarters situation model, and obtain the collision avoidance path of the surface unmanned boat.

[0011] Preferably, in step S1, in local path planning, the velocity obstacle method regards the surface unmanned boat as a particle, and both the scale of the surface unmanned boat and the distance to be maintained between the two are extended to the target ship, and the extended radius formed on the target ship is called the safety distance.

[0012] If the surface unmanned boat detects the target ship within the collision threat distance, a conical obstacle interval is formed between the surface unmanned boat and the extended circle. When the relative velocity falls into the conical area, it is regarded that there is a collision conflict, and the ship with the responsibility to avoid should execute avoidance.

[0013] If the surface unmanned boat and the target ship are located at points O and T respectively, with velocities v o and v t , v r is the velocity of the target ship relative to the surface unmanned boat, l is the extension line of v r , d i is the safety distance, the circular area formed by the safety distance is P0, and RCC is the relative collision area. Then the mathematical expression of RCC is as follows:

[0014] RCC = {l ∩ P0 ≠ 0}.

[0015] Preferably, in step S1, the four - element ship domain model takes into account the ship's scale, speed, maneuverability, and encounter parameters, and can adjust the size of the domain according to the encounter situation at any time, and is then used to calculate the safety distance in the velocity obstacle method. For safety reasons, take the Minkowski sum of the maximum semi - axis in the four - element ship domain model and the target ship's length as the safety distance of the surface unmanned boat. Then the boundary equation of the four - element ship domain is:

[0016]

[0017] In the formula, R fore , R aft , R port and R starb are the elliptical radii of the bow, stern, port side, and starboard side of the surface unmanned boat respectively, and λ is the boundary value of the four - element ship domain model. is the angle between a point on the ellipse and the major axis of the ellipse, then the safe distance d between the surface unmanned boat and the target ship is safe From the following formula we can get:

[0018]

[0019] Where, is the Minkowski sum, l object is the length of the target ship.

[0020] Preferably, in step S1, in order to allow the two ships to pass within a safe distance, the surface unmanned boat should take appropriate avoidance action at a reasonable distance, which is called the collision threat distance; the surface unmanned boat collision threat distance is determined by the formula of the moving bound, which can be obtained by the following formula:

[0021] D arena =Df+k*v r ;

[0022]

[0023] Where D arena is the moving boundary value, k is the time parameter, Adv o is the ship's advance, which can be obtained through the turn experiment. t is the distance to the target ship, which can be obtained by the following empirical formula; the distance to the target ship Adv t The calculation formula is as follows:

[0024] Adv t =2.531l object +4.062b object +23.83;

[0025] Where b object is the width of the target ship.

[0026] Preferably, in step S2, if the unmanned surface boat and the target ship form a left intersection and are overtaken, the unmanned surface boat is a straight-line ship, and the unmanned surface boat should maintain its direction and speed in the initial stage of the collision and wait for the give-way ship to avoid;

[0027] However, if the target ship fails to make a timely avoidance and the distance between the two ships is less than the distance at the critical situation, a critical situation is formed. The straight-line ship can turn right to avoid the situation. The navigation industry uses the distance at the latest when the steering is applied as the critical value for the transition from a collision danger situation to a critical situation.

[0028] In order to facilitate the calculation of the critical value of the emergency situation, a northeast coordinate system is established, and the distance between the two ships when the surface unmanned boat turns 90° and can successfully avoid the straight-line ship violating the COLREGs at a safe distance is taken as the latest steering distance; the parameters involved are: d is the distance between the two ships in the emergency situation, d, is the distance between the two ships after the surface unmanned boat turns, and d safe is the safety distance, dcpa is the minimum passing distance between two ships, v o and v t are the speeds of the surface unmanned boat and the target ship, l is the latest steering point, n is the turning point, and the direction and true bearing of the target ship relative to the surface unmanned boat are θ r and B t , after the surface unmanned boat turns 90°, each parameter corresponds to θ' r and B' t , satisfying the following relationship:

[0029] d·sin(θ r ―B t ―π)=dcpa;

[0030] d'·|sin(θ' r ―B' t ―π)|=d safe ;

[0031]

[0032] B t =θ r ―arcsin(dcpa / d)―π;

[0033]

[0034] Assuming that the speed of the surface unmanned boat does not change when turning, t is the time it takes for the ship to turn 90 degrees, and the relative motion distance S between the two ships can be obtained by the following formula:

[0035] S=v r t;

[0036]

[0037] Where D t is the initial turning radius of the surface unmanned boat, and the relative speed v of the two ships r From the following formula we can get:

[0038]

[0039] Where, and The headings of the own ship and the target ship are respectively, and the above equations are combined and simplified to obtain the expression of the urgent situation d, which is as follows:

[0040]

[0041] Preferably, in step S3, in the improved collision avoidance model of the unmanned surface vehicle, (x1, y1) and (x2, y2) are two tangent points on the extended circle of the target ship, (x o , y o ), (x t , y t ) are the positions of the unmanned surface vehicle and the target ship respectively, D is the distance between the two ships, △θ is the circumferential angle of the obstacle cone, θ2 and θ1 are the angles between the left and right tangents and the x-axis respectively, θ is the angle between the line connecting the positions of the two ships and the x-axis, and θ r is the angle between the velocity direction of the own ship relative to the target ship and the x-axis; then:

[0042]

[0043] △θ / 2 = asin(d i / D);

[0044] If, θ r ∈(θ - △θ / 2, θ + △θ / 2), then the two ships will collide in the future, and the unmanned surface vehicle should turn to the right to make the relative motion line along the right tangent. The solution is as follows:

[0045]

[0046] (y - y t ) × (y - y o ) = -(x - x t ) × (x - x o );

[0047] Solving the above equation can obtain two sets of solutions for (x1, y1) and (x2, y2). If △θ satisfies the following equation:

[0048] △θ = θ2 - θ1, 0 < △θ < π or △θ < -π;

[0049]

[0050] Under the condition of knowing the speeds and headings of the unmanned surface vehicle and the target ship, if it is required to only change the heading of the unmanned surface vehicle so that the relative motion of the two ships can sail along the right tangent, the new heading that the unmanned surface vehicle needs to change can be obtained through the following equation:

[0051]

[0052] In the formula, is the new heading after the unmanned surface vehicle is adjusted to satisfy that the relative motion is along the right tangent;

[0053] In the next cycle, the positions of the surface unmanned boat and the target ship are updated as follows:

[0054]

[0055] Where Δt is the position update period;

[0056] The collision threat is considered to be eliminated when the surface unmanned boat moves to the right tangent point of the velocity collision cone of the two ships and the obstacle expansion circle. If the above formula is satisfied, the surface unmanned boat can change its course and sail towards the local target point, that is:

[0057] (x o ,y o )=(x1,y1).

[0058] Preferably, in step S3, the process of obtaining the collision avoidance path of the unmanned surface boat includes:

[0059] Step S31, the unmanned surface boat moves towards the target point;

[0060] Step S32, determining whether the target ship is within the threat range;

[0061] Step S33: If the vehicle is not within the threat distance, determine whether it has reached the destination. If so, terminate navigation. If not, continue driving towards the destination.

[0062] Step S34: If the vehicle is within the threat distance, then the encounter situation is determined. The encounter situation includes head-on encounter, right crossing, left crossing, and overtaking.

[0063] Step S35: determine whether the unmanned surface boat is a give-way boat. If it is, avoid it using the improved unmanned surface boat collision avoidance model algorithm. After avoiding it, determine whether it has reached the destination. If so, end navigation. If not, continue to sail towards the destination.

[0064] Step S36: If it is not a give-way vessel, determine whether it is an urgent and dangerous situation. If so, avoid it through the improved surface unmanned boat collision avoidance model algorithm. After avoiding, determine whether it has reached the destination. If so, end navigation. If not, continue to sail towards the target point.

[0065] Preferably, in step S34, when the two ships are in a head-on encounter, the two ships have equal avoidance responsibilities; when the two ships are in a right crossing or overtaking situation, the surface unmanned boat is the give-way boat and should give way to the target ship in a timely manner; when the two ships are in a left crossing or being overtaken, the surface unmanned boat is the straight-line boat and the target ship should give way to the surface unmanned boat; the calculation formula for the encounter situation based on the comprehensive azimuth and heading information is:

[0066]

[0067] Where, θ r is the true bearing of the target ship relative to the unmanned surface vehicle, and ΔC is the course difference between the two ships.

[0068] The beneficial effects of the present invention are as follows: by separately determining the safe distance and collision threat distance in the speed obstacle method through the four-element ship domain and the ship dynamic boundary, the present invention can shorten the length of the collision avoidance path and the collision avoidance time; the path planned by the improved unmanned surface vehicle collision avoidance model meets the avoidance requirements of Articles 8 and 13-17 of COLREGs while ensuring collision avoidance safety; when the unmanned surface vehicle is the give-way ship or a ship with the same avoidance responsibility, it can avoid the target ship in a timely manner; when the unmanned surface vehicle is the stand-on ship, it can maintain its speed and course and wait for the target ship to execute the avoidance action, and at the same time can perform an emergency avoidance when the two ships form a close-quarters situation; effectively shortening the collision avoidance path and time. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 is a flowchart of a path planning method based on an improved speed obstacle method of the present invention;

[0070] Figure 2 is a flowchart of obtaining the collision avoidance path of the unmanned surface vehicle in the present invention;

[0071] Figure 3 is a schematic diagram of threat collision of the traditional speed obstacle method;

[0072] Figure 4 is a schematic diagram of the encounter situation division between the unmanned surface vehicle and the target ship in the present invention;

[0073] Figure 5 is a schematic diagram of the latest rudder application point of the unmanned surface vehicle in the present invention;

[0074] Figure 6 is a schematic diagram of the collision cone formed between the unmanned surface vehicle and the target ship in the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0075] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. In the description of the present invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention. In addition, the terms "first", "second", "third" are only used for descriptive purposes and cannot be construed as indicating or implying relative importance.

[0076] The present invention will be further described below with reference to the accompanying drawings.

[0077] The traditional speed obstacle method for collision avoidance has the following steps: (1) Calculate whether the target ship is within the collision cone through the encounter parameters of the two ships: if it is not within the collision cone, there is no need for collision avoidance, otherwise the next step should be executed; (2) Change the speed magnitude or direction of the own ship so that the own ship can pass outside the collision cones of the two ships; (3) If the own ship safely passes the encountered ship, the own ship sails towards the target point. There is no definite collision avoidance path, and both the length and time of the collision avoidance path will be relatively long.

[0078] As Figure 1 shown, a path planning method based on an improved speed obstacle method includes:

[0079] Step S1, introduce the four-element ship domain model and the collision threat distance into the calculation of the collision avoidance opportunity of the speed obstacle method, and then determine the extended domain of the obstacle and the opportunity to execute collision avoidance by the speed obstacle method.

[0080] In the embodiment, in local path planning, the speed obstacle method regards the unmanned surface vehicle as a particle, and both the scale size of the unmanned surface vehicle and the distance to be maintained between them are extended to the target ship, and the extended radius formed on the target ship is called the safety distance;

[0081] If the unmanned surface vehicle detects the target ship within the collision threat distance, the unmanned surface vehicle and the extended circle form a conical obstacle interval. When the relative speed falls into the conical area, it is regarded as a collision conflict, and the ship with the responsibility of collision avoidance should perform collision avoidance. The speed obstacle cone is as Figure 3 shown;

[0082] If the unmanned surface vehicle and the target ship are located at points O and T respectively, with speeds of v o and v t respectively, v r is the relative speed of the target ship with respect to the unmanned surface vehicle, l is the extension line of v r , d i is the safety distance, and the circular area formed by the safety distance is P0, and RCC is the relative collision area. Then the mathematical expression of RCC is as follows:

[0083] RCC = {l ∩ P0 ≠ 0}.

[0084] In the embodiment, the four-element ship domain model takes into account the ship scale, speed, maneuverability, and encounter parameters, and can adjust the size of the domain at any time according to the encounter situation, and is then used to calculate the safety distance in the speed obstacle method. For safety reasons, take the Minkowski sum of the maximum semi-axis in the four-element ship domain model and the target ship length as the safety distance of the unmanned surface vehicle. Then the boundary equation of the four-element ship domain is:

[0085]

[0086] In the formula, R fore、R aft 、R port and R starb are the ellipse radii of the bow, stern, port and starboard of the surface unmanned vehicle, λ is the boundary value of the quaternary ship domain model, is the angle between a point on the ellipse and the major axis of the ellipse, then the safe distance d between the surface unmanned boat and the target ship is safe From the following formula we can get:

[0087]

[0088] Where, is the Minkowski sum, l object is the length of the target ship.

[0089] In this embodiment, in order to allow the two ships to pass within a safe distance, the surface unmanned boat should take appropriate avoidance action at a reasonable distance. This distance is called the collision threat distance. The surface unmanned boat collision threat distance is determined by the formula of the moving boundary, which can be obtained by the following formula:

[0090] D arena =Df+k*v r ;

[0091]

[0092] Where D arena is the moving boundary value, k is the time parameter, Adv o is the ship's advance, which can be obtained through the turn experiment. t is the distance to the target ship, which can be obtained by the following empirical formula; the distance to the target ship Adv t The calculation formula is as follows:

[0093] Adv t =2.531l object +4.062b object +23.83;

[0094] Where b object is the width of the target ship.

[0095] Step S2: Using the latest steering distance as the critical value for transitioning from a collision risk situation to an emergency situation, an emergency situation model is constructed.

[0096] In the embodiment, if the unmanned surface boat and the target ship form a left intersection and are overtaken, the unmanned surface boat is a straight-line ship. In the initial stage of the collision, the unmanned surface boat should maintain its direction and speed and wait for the give-way ship to avoid it.

[0097] However, when the target ship fails to avoid in time and the distance between the two ships is less than the distance at the critical situation, a critical situation is formed. The stand-on ship can turn right to avoid. The nautical community takes the distance at the latest helm application as the critical value for the transition from the collision risk situation to the critical situation;

[0098] To facilitate the calculation of the critical value of the critical situation, a northeast coordinate system is established. As Figure 5 shown, the distance between the two ships when the unmanned surface vehicle can just successfully avoid the stand-on ship violating the COLREGs at a safe distance after turning 90° is taken as the latest helm application distance. Among them, the parameters involved are: d is the distance between the two ships at the critical situation, d' is the distance between the two ships after the unmanned surface vehicle turns, d safe is the safe distance, dcpa is the minimum passing distance between the two ships, v o and v t are the speeds of the unmanned surface vehicle and the target ship respectively, l is the latest helm application point, n is the turning point, and the direction and true bearing of the target ship relative to the unmanned surface vehicle are θ r and B t respectively. After the unmanned surface vehicle turns 90°, the parameters correspond to θ' r and B' t respectively, and satisfy the following relational expressions:

[0099] d·sin(θ r −B t −π) = dcpa;

[0100] d'·|sin(θ' r −B' t −π)| = d safe ;

[0101]

[0102] B t = θ r −arcsin(dcpa / d)−π;

[0103]

[0104] Assume that the speed of the unmanned surface vehicle does not change when it turns. t is the time taken for the ship to turn 90°. The relative movement distance S between the two ships can be obtained from the following formula:

[0105] S = v r ·t;

[0106]

[0107] In the formula, D t is the initial turning diameter of the unmanned surface vehicle. The relative speed v r between the two ships can be obtained from the following formula:

[0108]

[0109] Wherein, and are the headings of the own ship and the target ship respectively. Then, by combining and simplifying the above equations, the expression of the close-quarter situation d can be obtained as follows:

[0110]

[0111] Step S3: Construct an improved collision avoidance model for the surface unmanned boat by means of the extended domain of the determined obstacle, the timing of executing avoidance by the velocity obstacle method, and the close-quarter situation model, so as to obtain the collision avoidance path of the surface unmanned boat.

[0112] In the embodiment, in the improved collision avoidance model of the surface unmanned boat, (x1, y1) and (x2, y2) are two tangent points on the extended circle of the target ship, (x o , y o ), (x t , y t ) are the positions of the surface unmanned boat and the target ship respectively, D is the distance between the two ships, △θ is the circumferential angle of the obstacle cone, θ2 and θ1 are the angles between the left and right tangent lines and the x-axis respectively, θ is the angle between the line connecting the positions of the two ships and the x-axis, θ r is the angle between the velocity direction of the own ship relative to the target ship and the x-axis, as shown in Figure 6 ; then:

[0113]

[0114] △θ / 2 = asin(d i / D);

[0115] If, θ r ∈(θ - △θ / 2, θ + △θ / 2), then the two ships will collide in the future, and the surface unmanned boat should turn to the right to make the relative motion line along the right tangent line. The solution is as follows:

[0116]

[0117] (y - y t ) × (y - y o ) = -(x - x t ) × (x - x o );

[0118] By solving the above equation, two sets of solutions of (x1, y1) and (x2, y2) can be obtained. If △θ satisfies the following equation:

[0119] △θ = θ2 - θ1, 0 < △θ < π or △θ < -π;

[0120]

[0121] Under the condition that the speed and heading of the surface unmanned boat and the target ship are known, if it is required to change the heading of the surface unmanned boat so that the relative motion of the two ships can sail along the right tangent line, the new heading that the surface unmanned boat needs to change can be calculated by the following formula:

[0122]

[0123]

[0124] Where, In order to satisfy the new course of the surface unmanned boat after the relative motion along the right tangent line;

[0125] In the next cycle, the positions of the surface unmanned boat and the target ship are updated as follows:

[0126]

[0127] Where Δt is the position update period;

[0128] The collision threat is considered to be eliminated when the surface unmanned boat moves to the right tangent point of the velocity collision cone of the two ships and the obstacle expansion circle. If the above formula is satisfied, the surface unmanned boat can change its course and sail towards the local target point, that is:

[0129] (x o ,y o )=(x1,y1).

[0130] like Figure 2 As shown in FIG, the process of obtaining the collision avoidance path of the surface unmanned boat includes:

[0131] Step S31: The unmanned surface boat moves towards the target point.

[0132] Step S32: Determine whether the target ship is within the threat range.

[0133] Step S33: If the vehicle is not within the threat distance, determine whether it has reached the destination. If so, end navigation; if not, continue driving towards the destination.

[0134] Step S34: If the vehicle is within the threat distance, then a meeting situation judgment is performed. The meeting situations include head-on meeting, right crossing, left crossing, and overtaking.

[0135] In the embodiment, Figure 4 As shown in the figure, when two ships are in a head-on encounter, both ships have equal avoidance responsibilities; when two ships are crossing to the right or overtaking the other ship, the surface unmanned boat is the give-way boat and should give way to the target ship in a timely manner; when two ships are crossing to the left or being overtaken, the surface unmanned boat is the straight-line boat and the target ship should give way to the surface unmanned boat; the calculation formula for the encounter situation based on the comprehensive azimuth and heading information is:

[0136]

[0137] where θ r is the true bearing of the target ship relative to the surface unmanned boat, and ΔC is the course difference between the two ships.

[0138] Step S35, and determine whether the surface unmanned boat is the give-way vessel. If it is the give-way vessel, avoid it through the improved collision avoidance model algorithm for the surface unmanned boat. After avoidance, determine whether the destination has been reached. If so, end the navigation. If not, continue to sail towards the target point.

[0139] Step S36, if it is not the give-way vessel, then determine whether it is a situation of close-quarters danger. If so, avoid it through the improved collision avoidance model algorithm for the surface unmanned boat. After avoidance, determine whether the destination has been reached. If so, end the navigation. If not, continue to sail towards the target point.

[0140] Let Scenario 1: head-on; Scenario 2: starboard crossing; Scenario 3: port crossing; Scenario 4: overtaking.

[0141] In the local path planning before improvement, the safety threshold could not be set according to the actual situation of the ship, and the surface unmanned boat avoided the target ship earlier, resulting in a longer collision avoidance distance and time. After improvement, the safety distance and collision threat distance were determined considering the ship's size, speed, and maneuverability, etc. During the avoidance process in the 4 scenarios, the distance between the two ships was always not less than the required safety distance of the own ship. Under the condition of ensuring safe avoidance, the timing of avoidance was postponed, making the collision avoidance path and time shorter. The improved algorithm shortened the average path length by 38.88% and reduced the average planning time by 44.26% compared with that before improvement. It is verified that the improved algorithm of the present invention can effectively shorten the collision avoidance path and time.

[0142] The speed obstacle method before improvement did not consider the avoidance responsibility when the unmanned surface vehicle is a stand-on vessel; for example, when the unmanned surface vehicle in Scenario 3 and the target ship form a left-crossing encounter situation, the USV did not recognize its identity as a stand-on vessel and still carried out collision avoidance in the way of active avoidance, and executed the avoidance action when a collision danger was formed; this collision avoidance model violated the requirements of COLREGs for the collision avoidance action of a stand-on vessel; the improved speed obstacle method determined the close-quarters situation through the latest rudder application distance; a close-quarters situation was formed during the collision avoidance process in Scenario 3. When the unmanned surface vehicle maintained its course and speed at the initial stage of the collision danger until a close-quarters situation was formed between the two vessels, the unmanned surface vehicle began to execute the avoidance action, meeting the provisions of Article 17 of COLREGs; in addition, during the encounters in Scenarios 1, 2, and 4, the unmanned surface vehicle was the give-way vessel in all cases and actively carried out collision avoidance in accordance with the provisions of Articles 13-15 of COLREGs. At the same time, the collision avoidance actions conformed to the principles of "early", "large", "wide", and "clear", meeting the requirements of Articles 8 and 16 of COLREGs; therefore, it is proved that the path planned by the improved speed obstacle method meets the avoidance requirements of Articles 8 and 13-17 of COLREGs while ensuring collision avoidance safety.

[0143] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method based on an improved velocity obstacle method, characterized in that, Including: Step S1: Introduce the four - element ship domain model and the collision threat distance into the calculation of the collision avoidance timing of the velocity obstacle method, thereby determining the expansion domain of the obstacle and the timing for the velocity obstacle method to execute avoidance; Step S2: Take the latest rudder - applying distance as the critical value for the transition from the collision - dangerous situation to the close - quarters situation, and construct a close - quarters situation model; Step S3: Construct an improved collision avoidance model for the surface unmanned boat through the determined expansion domain of the obstacle, the timing for the velocity obstacle method to execute avoidance, and the close - quarters situation model, and obtain the collision avoidance path of the surface unmanned boat.

2. The path planning method based on the improved speed obstacle method according to claim 1, characterized in that In the said step S1, in local path planning, the velocity obstacle method regards the surface unmanned boat as a particle, and both the scale of the surface unmanned boat and the distance to be maintained between them are extended to the target ship, and the extended radius formed on the target ship is called the safety distance; If the surface unmanned boat detects the target ship within the collision threat distance, a conical obstacle interval is formed between the surface unmanned boat and the extended circle. When the relative velocity falls into the conical area, it is regarded that there is a collision conflict, and the ship with the responsibility of avoidance should carry out avoidance; If the unmanned surface vehicle and the target ship are located at points O and T respectively, with speeds of v o and v t , v r is the speed of the target ship relative to the unmanned surface vehicle, l is the extension line of v r , d i is the safety distance, the circular area formed by the safety distance is P0, and the RCC is the relative collision area. Then the mathematical expression of the RCC is as follows: RCC = {l∩P0≠0}.

3. The path planning method based on the improved speed obstacle method according to claim 2, characterized in that, In the said step S1, the four - element ship domain model takes into account the ship's scale, speed, maneuverability, and encounter parameters, and can adjust the size of the domain at any time according to the encounter situation, and is thus used to calculate the safety distance in the velocity obstacle method. For safety reasons, take the Minkowski sum of the maximum semi - axis in the four - element ship domain model and the target ship's length as the safety distance of the surface unmanned boat. Then the boundary equation of the four - element ship domain is: where R fore , R aft , R port and R starb are the elliptical radii of the bow, stern, port side and starboard side of the unmanned surface vehicle respectively, λ is the boundary value of the four-element ship domain model, is the angle between a point on the ellipse and the major axis of the ellipse, then the safe distance d safe between the unmanned surface vehicle and the target ship can be obtained from the following formula: In the formula, is the Minkowski sum, and l object is the length of the target ship.

4. The path planning method based on the improved velocity obstacle method according to claim 3, characterized in that, In the said step S1, in order to enable the two ships to pass outside the safety distance, the surface unmanned boat should take appropriate avoidance actions at a certain reasonable distance, and this distance is called the collision threat distance; the collision threat distance of the surface unmanned boat is determined by the formula of the moving boundary, and can be obtained from the following formula: D arena = Df + k*v r ; where D arena is the dynamic boundary value, k is the time parameter, and Adv o is the advance of the own ship, which can be obtained through a turning circle experiment. Adv t is the advance of the target ship, which can be obtained through the following empirical formula; the advance Adv t of the target ship is calculated as follows: Adv t = 2.531l object + 4.062b object + 23.83; Where b object is the width of the target ship.

5. A path planning method based on an improved speed obstacle method according to claim 4, characterized in that, In the said step S2, when the surface unmanned boat and the target ship form a left - crossing and overtaken situation, the surface unmanned boat is a through - ship. The surface unmanned boat should maintain its course and speed and wait for the give - way ship to avoid in the initial stage of the collision; However, when the target ship fails to avoid in time and the distance between the two ships is less than the distance in the close - quarters situation, a close - quarters situation is formed. The through - ship can make a right - turn avoidance. The nautical community takes the distance at the latest rudder - applying time as the critical value for the transition from the collision - dangerous situation to the close - quarters situation; To facilitate the calculation of the critical value of the close-quarters situation, a northeast coordinate system is established. The distance between the two vessels when the unmanned surface vehicle can just successfully avoid the oncoming vessel violating the COLREGs at a safe distance after turning 90° is taken as the latest steering distance. Among them, the parameters involved are: d is the distance between the two vessels in the close-quarters situation, d, is the distance between the two vessels after the unmanned surface vehicle turns, d safe is the safe distance, dcpa is the minimum passing distance between the two vessels, v o and v t are the speeds of the unmanned surface vehicle and the target vessel respectively, l is the latest steering point, n is the turning point, the direction and true bearing of the target vessel relative to the unmanned surface vehicle are θ r and B t , after the unmanned surface vehicle turns 90°, the parameters respectively correspond to θ' r and B' t , and satisfy the following relationship: d·sin(θ r ―B t ―π) = dcpa; d'·|sin(θ' r ―B' t ―π)|=d safe ; B t = θ r ― arcsin(dcpa / d) ― π; Assume that the speed of the surface unmanned boat does not change when turning, t is the time taken for the ship to turn 90°, and the relative movement distance S between the two ships can be obtained from the following formula: S = v r ·t; where D t is the initial turning diameter of the unmanned surface vehicle, and the relative speed v r of the two ships can be obtained from the following formula: Wherein, and are the headings of the own ship and the target ship respectively. Then, by combining and simplifying the above equations, the expression for the close-quarters situation d can be obtained as follows:

6. The path planning method based on the improved speed obstacle method according to claim 5, characterized in that, In step S3, in the improved collision avoidance model of the unmanned surface vehicle, (x1, y1) and (x2, y2) are two tangent points on the extended circle of the target ship, (x o , y o ), (x t , y t ) are the positions of the unmanned surface vehicle and the target ship respectively, D is the distance between the two ships, △θ is the circumferential angle of the obstacle cone, θ2 and θ1 are the angles between the left and right tangents and the x-axis respectively, θ is the angle between the line connecting the positions of the two ships and the x-axis, θ r is the angle between the velocity direction of the own ship relative to the target ship and the x-axis; then: △θ / 2 = asin(d i / D); If, θ r ∈(θ - △θ / 2, θ + △θ / 2), then the two ships will collide in the future. The unmanned surface vehicle should turn right to make the relative motion line along the tangent on the right side. The solution is as follows: (y - y t ) × (y - y o ) = -(x - x t ) × (x - x o ); Solving the above formula can obtain two sets of solutions (x1, y1), (x2, y2). If △θ satisfies the following formula: △θ = θ2―θ1, 0 < △θ < π or △θ < ―π; Under the condition of knowing the speeds and headings of the surface unmanned boat and the target ship, if it is required to only change the heading of the surface unmanned boat so that the relative movement of the two ships can sail along the right - hand tangent, the new heading that the surface unmanned boat needs to change can be obtained from the following formula: In the formula, is the adjusted new course of the unmanned surface vehicle when the relative motion satisfies the right tangent and there is no one on the water surface; In the next cycle, the positions of the surface unmanned boat and the target ship are updated as follows: In the formula, Δt is the position update cycle; Regarding the surface unmanned boat moving to the right - hand tangent point of the velocity collision cone of the two ships and the obstacle extended circle as the release of the collision threat, then if the above formula is satisfied, the surface unmanned boat can change its heading and sail towards the local target point, that is: (x o , y o ) = (x1, y1).

7. A path planning method based on an improved speed obstacle method according to claim 6, characterized in that, In the said step S3, the process of obtaining the collision avoidance path of the unmanned surface vessel includes: Step S31, the unmanned surface vessel sails towards the target point; Step S32, determine whether the target ship is within the threat distance; Step S33, if it is not within the threat distance, determine whether the destination has been reached. If so, end the navigation. If not, continue to sail towards the target point; Step S34, if it is within the threat distance, then conduct a meeting situation judgment. The meeting situations include head-on encounter, starboard crossing, port crossing, and overtaking; Step S35, and determine whether the unmanned surface vessel is the give-way vessel. If it is the give-way vessel, avoid it through the improved collision avoidance model algorithm of the unmanned surface vessel. After avoidance, determine whether the destination has been reached. If so, end the navigation. If not, continue to sail towards the target point; Step S36, if it is not the give-way vessel, then determine whether it is an imminent danger situation. If so, avoid it through the improved collision avoidance model algorithm of the unmanned surface vessel. After avoidance, determine whether the destination has been reached. If so, end the navigation. If not, continue to sail towards the target point.

8. A path planning method based on an improved speed obstacle method according to claim 7, characterized in that In the said step S34, when the two vessels form a head-on encounter situation, the two vessels have equal responsibilities for avoidance; when the two vessels form a starboard crossing or an overtaking situation of another vessel, the unmanned surface vessel as the give-way vessel should avoid the target ship in a timely manner; when the two vessels form a port crossing or a situation of being overtaken, the unmanned surface vessel is the stand-on vessel and the target ship should give way to the unmanned surface vessel; the calculation formula for the meeting situation adopted by integrating the azimuth and course information is: where θ r is the true bearing of the target ship relative to the surface unmanned boat, and ΔC is the course difference between the two ships.

Images