A collision avoidance decision-making method for safe navigation of unmanned boats in narrow waterways

Abstract

The present invention discloses a collision avoidance strategy method for safe navigation of unmanned boats in narrow waterways. The collision avoidance strategy method includes the setting of waterway boundaries based on the unmanned boat tracking route, the division of obstacle encounter situations and ship safety areas based on the direction of the waterway, the analysis and processing of dynamic and static obstacles, the calculation of obstacle avoidance heading and speed, the detection of the distance between the unmanned boat and the waterway boundary and the heading adjustment, etc., and organizes each part of the content according to a reasonable processing flow to realize collision avoidance calculation in narrow waterways. The present invention solves the collision avoidance problem of unmanned boats in narrow waterways and realizes the safe navigation of unmanned boats in narrow waterways.

Description

Technical Field

[0001] The invention relates to the technical field of motion control of surface unmanned boats, and in particular to a collision avoidance decision method for safe navigation of unmanned boats in narrow waterways. Background Art

[0002] Unmanned surface boats are widely used in countries around the world because of their small size, high speed, good stealth, and high intelligence. With the continuous improvement of their application value, unmanned surface boats have become the focus of research at home and abroad. As one of the core technologies of unmanned surface boats, collision avoidance technology is an important prerequisite for the safe navigation and normal execution of tasks of unmanned boats. In the research field of local collision avoidance of unmanned boats, the relevant local reaction obstacle avoidance methods include speed obstacle method, virtual force field and Voronoi diagram. The obstacle avoidance methods through local route planning include RRT and A* path planning methods, as well as dynamic window method and optimization calculation of local optimal route using PSO.

[0003] At present, the research on unmanned boat collision avoidance mainly focuses on the collision avoidance problem of unmanned boats in open water environments, such as the Chinese patent with publication number CN111158361A, entitled "Surface unmanned boat collision avoidance method based on ship domain model". There are few studies specifically on the safe navigation of unmanned boats in narrow waterways, and only some studies on the safe navigation of manned ships in inland waterways, such as the Chinese patent with publication number CN109491386A, entitled "Narrow waterway ship collision avoidance method based on path planning tracking control", which is applicable to manned ships. The collision avoidance strategy in narrow waterways is of great significance to improving the mission capability of unmanned boats and ensuring the safe navigation of unmanned boats in restricted waters such as ports and narrow waterways. Summary of the invention

[0004] In view of the deficiencies in existing research on obstacle avoidance of unmanned boats, the present invention provides a collision avoidance strategy method for unmanned boats to safely navigate in narrow waterways, aiming to solve the problem of reasonable avoidance of static and dynamic obstacles when the unmanned boat is navigating in narrow waterways, while maintaining a safe distance from the shore.

[0005] The technical solution to achieve the purpose of the present invention is:

[0006] A collision avoidance strategy method for an unmanned boat to safely navigate in a narrow waterway comprises the following steps:

[0007] Setting of waterway boundaries based on unmanned boat tracking routes;

[0008] Classify obstacle encounter situations and ship safety areas based on the direction of the channel;

[0009] Analyze and process dynamic and static obstacles to determine obstacle conflict range and navigable range;

[0010] Obstacle avoidance heading and speed calculation;

[0011] Detection of the distance between the unmanned boat and the channel boundary, course adjustment, and collision avoidance strategy;

[0012] Obtain obstacle avoidance heading and speed or stop instruction or return instruction.

[0013] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention is based on the setting of the channel boundary of the unmanned boat tracking route, the division of obstacle encounter situations and ship safety areas based on the direction of the channel, the analysis and processing of dynamic and static obstacles, the calculation of obstacle avoidance heading and speed, the detection of the distance between the unmanned boat and the channel boundary and the heading adjustment to avoid collision, and realize the collision avoidance calculation in narrow waterways. The present invention solves the collision avoidance problem of unmanned boats in narrow waterways, and can realize the unmanned boats to avoid obstacles in narrow waterways while maintaining a safe distance from the boundaries, so as to realize the safe navigation of unmanned boats in narrow waterways. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 Schematic diagram of the channel boundary setting for unmanned boats in narrow waterways.

[0015] Figure 2 Schematic diagram of situations that unmanned boats may encounter and the division of safety areas.

[0016] Figure 3 Calculate the schematic diagram of obstacle conflict range and navigable range.

[0017] Figure 4 This is a flow chart of the collision avoidance strategy for unmanned boats in narrow waterways.

[0018] Figure 5 This is a simulation diagram of an unmanned boat avoiding obstacles in a narrow waterway. Figure 5 (a) is the beginning of obstacle avoidance simulation. Figure 5 (b) To complete the obstacle avoidance simulation diagram, Figure 5 (c) is the simulation diagram of the regression route. DETAILED DESCRIPTION

[0019] The specific technical solutions of the present invention are further described below to help those skilled in the art further understand the present invention without limiting the rights thereof.

[0020] The narrow waterway obstacle avoidance method designed by the present invention sets the boundary of the narrow waterway according to the route. The boundary is a relative boundary rather than an absolute boundary. This increases the flexibility of the algorithm and can be applied in many scenarios. The narrow waterway is defined relative to the size of the boat. The narrow waterway described in the present invention refers to a waterway that can generally accommodate only about two unmanned boats passing through in parallel.

[0021] The present invention provides a collision avoidance strategy method for an unmanned boat to navigate safely in a narrow waterway, which is based on the setting of a waterway boundary of the route, the construction of a ship safety field based on the direction of the waterway, the obstacle avoidance calculation of dynamic and static obstacles, the obstacle avoidance heading and speed calculation, and the detection of the waterway boundary distance. Then, based on the above contents, a reasonable processing flow is used to organize the method so as to obtain a better obstacle avoidance effect.

[0022] Regarding the use of the closest encounter point in the obstacle avoidance decision of the unmanned boat in narrow waterways to evaluate the collision risk between the unmanned boat and the obstacle:

[0023] The position coordinates of the unmanned boat in the present invention are Velocity Vector Location of obstacles Velocity Vector The closest distance is d CPA , and the shortest encounter time t CPA , the calculation formula is as follows:

[0024]

[0025]

[0026] Regarding the obstacle encounter situation judgment and ship safety area division method mentioned in the collision avoidance strategy process of unmanned boats in narrow waterways:

[0027] 1) Location area division and security area setting

[0028] In order to adapt to the characteristics of narrow waterways, the division of encounter situations is slightly different from that of open waters. The schematic diagram of the division of encounter situations and the ship safety area diagram are as follows: Figure 2 ;

[0029] Assume that the detection radius of the ship is R, the width of the ship's safety area is W, the length is L, and the width of the transverse area is also set to W. The ship's detection range is divided into 9 position areas to describe the positional relationship between the obstacle and the ship, laying the foundation for subsequent processing. They are directly in front, left front, right front, transverse left, transverse right, directly behind, left rear, right rear, and danger zone.

[0030] The front position area plus the danger area is set as the safety area of ​​the unmanned boat. The safety area of ​​the ship is set as a long rectangle along the direction of the channel. The width is set to be small to reduce the impact of ships sailing on both sides on the unmanned boat. In the division of the position area, the left and right areas mainly play a transitional role from front to back. Because the position measurement value of the obstacle has a certain degree of oscillation, if only the horizontal y-axis is used to distinguish the front and back, when the obstacle is in the area near the y-axis, the front and back oscillation of the position area determination is prone to occur. If there is a difference in the obstacle handling in the front and rear position areas, it will inevitably cause oscillation of the obstacle avoidance decision.

[0031] 2) You will encounter a situation judgment

[0032] In the encounter situation judgment, for static obstacles, it is only necessary to judge the location area, and for dynamic obstacles in the ship's safety area (i.e. the area directly in front of the ship), it is necessary to further judge the crossing type according to its heading. The crossing types of dynamic obstacles include encounter, overtaking, left crossing, and right crossing.

[0033] 2.1) Dynamic obstacles in the front position area will be judged

[0034] Taking the channel direction as 0°, according to the relative heading of the dynamic obstacle The types of encounters are as follows:

[0035] Encounter situation:

[0036] Left Crossing Situation:

[0037] Right Cross Position:

[0038] Overtaking situation:

[0039] 2.2) Dynamic obstacles in the right front position area will be judged

[0040] The main encounter situations in narrow waterways are encounter and overtaking, but it is possible that another ship may pass in front of the ship by sailing sideways from the right or left front. Based on the principle of sailing to the right, it is necessary to pay attention to the dynamic obstacles in the right front area. The encounter situation of obstacles located in the right front also needs to be considered, mainly considering the right crossing encounter situation.

[0041] The conditions for determining the right cross situation are as follows:

[0042]

[0043] b)d CPA <R O +R USV &&0<t CPA <t CPA_min

[0044]

[0045] When the above three conditions are met, a right cross situation is formed, in which d CPA is the closest encounter distance, t CPA is the shortest encounter time, t CPA_min is the minimum value of the shortest encounter time, is the obstacle position, is the position of the unmanned boat, R USV is the length of the unmanned boat, R O is the obstacle radius, R safe For a safe distance.

[0046] Combination Figure 4 The technical problem to be solved by the present invention is achieved through the following technical solutions: The collision avoidance strategy process of unmanned boats in narrow waterways:

[0047] Step 1: Setting of waterway boundaries

[0048] Combination Figure 1 , set the left and right boundaries and length of the channel according to the current tracking route of the unmanned boat. The distance from the left and right boundaries to the route can be set to different values ​​to meet the needs of different scenarios. The length of the channel is slightly larger than the current tracking route length to ensure that obstacles near the target waypoint can also be taken into account and will not be filtered out;

[0049] The second step is obstacle screening.

[0050] Screen obstacles detected by the unmanned boat’s perception system and filter out obstacles outside the channel boundary;

[0051] The third step is obstacle data processing.

[0052] All obstacles within the waterway shall be dealt with as follows:

[0053] 1) Limit the radius of obstacles according to the maximum size of ships that can pass through narrow waterways. Avoid the entire waterway being completely blocked and impassable due to the obstacle size given by sensor detection errors being too large;

[0054] 2) Calculate the distance between the boat and the obstacle. If the target enters the danger zone, it is considered that a collision has occurred and the boat needs to stop. Go directly to step 10 and output a stop command. If the distance is greater than the sum of the distance between the unmanned boat and the obstacle, continue to execute;

[0055] 3) According to the size of the unmanned boat and taking into account a certain safety margin, a safe area for the ship is established along the waterway from the center of the unmanned boat, and the encounter situations of obstacles are divided;

[0056] 4) Calculate the direct conflict range and navigable range of each obstacle based on the type of target and the encounter situation; Figure 3 , the specific calculation steps of the conflict range and the navigable range are:

[0057] ① Calculate the two tangent lines from the boat to the obstacle;

[0058] ②Transfer the obstacle speed to the center of the unmanned boat and reverse it;

[0059] ③ Draw a circle with the expected speed of the unmanned boat as the radius and the opposite obstacle speed as the center;

[0060] ④ If the circle intersects with the tangent line, the orientation area between the two tangent points is the absolute conflict zone. If there is no intersection, the obstacle is a safe obstacle.

[0061] 5) Determine whether the obstacle covers the target waypoint. If it does, and the distance between the unmanned boat and the target waypoint is less than the diameter of the covered obstacle, update the waypoint and go to the first step;

[0062] Step 4: Extract dangerous obstacles.

[0063] According to the obstacle encounter situation, obstacles within the ship's safety area and obstacles in the right front area that meet the right crossing requirements are selected and added to the dangerous obstacle list, and sorted from near to far according to the distance. If the dangerous obstacle list is empty, go directly to step 8;

[0064] Step 5: Obstacle avoidance considers obstacle extraction.

[0065] The first obstacle in the dangerous obstacle list is added to the obstacle avoidance consideration list, and the distance value between the subsequent obstacles in the dangerous list and the boundary of the first obstacle is calculated. If the boundary distance value is less than the distance that allows the unmanned boat to pass safely, the obstacle is also added to the obstacle avoidance consideration list;

[0066] Step 6: Obstacle avoidance heading calculation.

[0067] First, according to the navigable range of obstacles in the obstacle avoidance list, the direction with the smallest deviation from the channel direction is selected as the pre-selected obstacle avoidance direction; then the distance between the obstacle to be avoided and the left and right boundaries of the channel is detected. If the distance from the target to one boundary is not enough for the unmanned boat to pass safely, and the pre-selected direction given above is toward this side of the target, the heading needs to be adjusted to the other side. Finally, it is corrected according to the maximum allowable deviation from the channel direction angle;

[0068] Step 7: Calculate obstacle avoidance speed.

[0069] According to the minimum encounter distance d of the first obstacle in the obstacle avoidance list CPA and the shortest encounter time t CPA Calculate the obstacle avoidance speed; the obstacle avoidance speed is calculated according to the following conditions:

[0070] 1) Minimum encounter distance d CPA The calculated shortest encounter time is greater than the required stopping time and less than the minimum safe encounter time, that is, t CPA_stop <t CPA <t CPA_min When the obstacle avoidance speed is reduced, t CPA =tCPA_min ;

[0071] 2) Minimum encounter distance d CPA Less than the sum of the radius of the obstacle and half the width of the ship, and the shortest encounter time is less than the time required to stop the ship, that is, t CPA <t CPA_stop When the ship stops,

[0072] 3) In other cases, the obstacle avoidance speed is equal to the set speed of the tracking route.

[0073] Step 8. Strategy for handling when the list of dangerous obstacles is empty.

[0074] If the list of dangerous obstacles is empty and there are obstacles within the safety radius of the unmanned boat, the direction of the channel is used as the obstacle avoidance direction, the speed is set as the obstacle avoidance speed, and the projection distance of the unmanned boat on the route is detected. If the projection length is greater than the route length, the waypoint needs to be updated; if there are no obstacles within the safety radius of the unmanned boat, go to step 10 and hand over the control to the navigation control to return to the route control;

[0075] Step 9: Detect the distance between the unmanned boat and the channel boundary.

[0076] Detect the distance between the unmanned boat and the channel boundary. If the distance is less than the safety threshold, adjust the course toward the inside of the channel.

[0077] Step 10: Output obstacle avoidance heading and speed or stop command or return command;

[0078] Step 11. The flight control system executes the instructions given by the obstacle avoidance system.

[0079] Example 1

[0080] An unmanned surface boat, whose obstacle avoidance system is equipped with the narrow waterway collision avoidance strategy method designed by the present invention, sails in a harbor or enters and leaves a harbor.

[0081] For most ports, there is generally a narrow channel connecting the harbor to the open sea. The width of this channel is small and only allows a small number of ships to enter and exit at the same time. When the unmanned boat passes through this waterway, it can avoid static obstacles or slow-moving ships in front and keep a safe distance from the shore. For obstacles outside the safety range of the unmanned boat, no matter how it navigates, it will not affect the boat.

[0082] Example 2

[0083] An unmanned surface boat, whose obstacle avoidance system is equipped with the narrow waterway collision avoidance strategy method designed by the present invention, sails along the coastline in nearshore waters.

[0084] When sailing along the coastline in the nearshore waters, the shallow water depth near the coast needs to be considered when avoiding obstacles. Therefore, the width of the channel near the coast can be set smaller, and the width of the channel near the open sea can be set larger. The left and right boundaries of the channel in the present invention can be set differently as needed. In this way, the risk of running aground can be avoided when sailing in the nearshore waters.

[0085] Example 3

[0086] An unmanned surface boat, whose obstacle avoidance system is equipped with the narrow waterway collision avoidance strategy method designed by the present invention, navigates in a continuous broken line narrow channel:

[0087] The simulation effect diagram is shown in Figure 5 (a) to (c), in a continuous zigzag narrow channel, obstacle avoidance can be achieved by setting corresponding waypoints at the turning points of the zigzag narrow channel. Because the channel considered in the present invention is set based on the currently tracked route segment. Therefore, obstacles on other route segments will not affect the unmanned boat.

Claims

1. A collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway, characterized in that: Includes steps: Setting of waterway boundaries based on unmanned boat tracking routes; Classify obstacle encounter situations and unmanned boat safety areas based on the direction of the waterway; Analyze and process dynamic and static obstacles to determine obstacle conflict range and navigable range; Obstacle avoidance heading and speed calculation; Detection of the distance between the unmanned boat and the channel boundary, course adjustment, and collision avoidance strategy; Obtain obstacle avoidance heading and speed or stop instruction or return instruction; The obstacle encounter situation division and unmanned boat safety area division based on the waterway direction specifically include: dividing the unmanned boat detection range into 9 position areas to describe the position relationship between the obstacle and the boat, namely, front, left front, right front, left side, right side, rear, left rear, right rear and danger zone, wherein the front position area and the danger zone are set as the safety area of ​​the unmanned boat; the safety area of ​​the unmanned boat is set as a long rectangle along the waterway direction; The obstacle avoidance heading and speed calculation specifically includes: Dangerous obstacle extraction: According to the encounter situation of obstacles, obstacles within the safety area of ​​the unmanned boat and obstacles that meet the right crossing requirements in the right front position area are selected and added to the dangerous obstacle list, and sorted from near to far according to the distance. If the dangerous obstacle list is empty, the collision avoidance strategy is directly executed; Obstacle avoidance consideration extraction: add the first obstacle in the dangerous obstacle list to the obstacle avoidance consideration list, and calculate the distance value between the subsequent obstacles in the dangerous obstacle list and the boundary of the first obstacle. If the boundary distance value is less than the distance that allows the unmanned boat to pass safely, then add the obstacle to the obstacle avoidance consideration list; Obstacle avoidance heading calculation: First, according to the navigable range of obstacles in the obstacle avoidance consideration list, the direction with the smallest deviation from the same channel direction is selected as the pre-selected obstacle avoidance heading; then the distance between the obstacle to be avoided and the left and right boundaries of the channel is detected. If the distance from the obstacle to one boundary does not meet the safe passage of the unmanned boat, and the pre-selected heading given above is toward this side of the obstacle, the heading needs to be adjusted to the other side; finally, it is corrected according to the maximum allowable deviation from the channel direction angle; Calculate obstacle avoidance speed; The collision avoidance strategy is a space-time processing strategy. If the list of dangerous obstacles is empty and there are obstacles within the safety radius of the unmanned boat, the direction of the channel is used as the obstacle avoidance direction, the speed is set as the obstacle avoidance speed, and the projection distance of the unmanned boat on the route is detected. If the projection length is greater than the route length, the waypoint needs to be updated; if there are no obstacles within the safety radius of the unmanned boat, the control is handed over to the navigation control to perform return route control.

2. The collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway according to claim 1 is characterized in that: The channel boundary setting based on the unmanned boat tracking route is specifically as follows: the left and right boundaries and length of the channel are set according to the current tracking route segment of the unmanned boat, the distances from the left and right boundaries to the route can be set to different values, the length of the channel is greater than the current tracking route length, and the obstacles around the target waypoint are included.

3. The collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway according to claim 1 is characterized in that: The encounter situations in the front area include the encounter situation, left crossing situation, right crossing situation and overtaking situation. The direction of the channel is taken as the 0° reference, and the relative heading of the dynamic obstacle is taken as the reference. The types of encounters are as follows: Encounter situation: Left Crossing Situation: Right Cross Position: Overtaking situation:

4. The collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway according to claim 3 is characterized in that: The situation encountered in the right front area is a dynamic target right crossing situation, which meets the following three conditions: 1) 2)d CPA <R O +R USV &&0<t CPA <t CPA_min 3) Among them, d CPA is the closest encounter distance, t CPA is the shortest encounter time, t CPA_min The minimum meeting time for safety, is the obstacle position, is the position of the unmanned boat, R USV is the length of the unmanned boat, R O is the obstacle radius, R safe For a safe distance.

5. The collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway according to claim 1 is characterized in that: The analysis and processing of dynamic and static obstacles to determine the obstacle conflict range and the navigable range specifically includes: ① Calculate the two tangent lines from the boat to the obstacle; ②Transfer the obstacle speed to the center of the unmanned boat and reverse it; ③ Draw a circle with the expected speed of the unmanned boat as the radius and the opposite obstacle speed as the center; ④ If the circle intersects with the tangent, the azimuth area between the two intersections is the dynamic obstacle conflict range, and the angle range between the tangents is the static obstacle conflict range. If there is no intersection, the obstacle is a safe obstacle.

6. The collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway according to claim 1 is characterized in that: The calculation of obstacle avoidance speed specifically includes: According to the minimum encounter distance d of the first obstacle in the obstacle avoidance list CPA and the shortest encounter time t CPA Calculate the obstacle avoidance speed; the obstacle avoidance speed is calculated according to the following conditions: 1) Minimum encounter distance d CPA The calculated shortest encounter time is greater than the required stopping time and less than the minimum safe encounter time, that is, t CPA_stop <t CPA <t CPA_min When the obstacle avoidance speed is reduced, t CPA =t CPA_min ; 2) Minimum encounter distance d CPA Less than the sum of the radius of the obstacle and half the width of the ship, and the shortest encounter time is less than the time required to stop the ship, that is, t CPA <t CPA_stop When the ship stops, 3) In other cases, the obstacle avoidance speed is equal to the set speed of the tracking route.

7. The collision avoidance strategy method for safe navigation of an unmanned boat in a narrow waterway according to claim 1 is characterized in that: The detection of the distance between the unmanned boat and the channel boundary includes: detecting the distance between the unmanned boat and the channel boundary, and if the distance is less than a safety threshold, adjusting the heading toward the inside of the channel.

Images