A narrow waterway unmanned ship collision avoidance local path planning method

Abstract

The application provides a narrow waterway unmanned ship collision avoidance local path planning method, and has the characteristics that the method comprises the following steps: S1) after entering the narrow waterway, path planning is carried out based on terrain perception information, and a local path is established; S2) an identification channel is established, the identification channel is a dynamic area defined based on the unmanned ship position and the channel width, and is used for judging the interference relationship with obstacles to make a collision avoidance decision; S3) the unmanned ship is navigated according to the local path, and whether there is a collision avoidance target in the identification channel is judged; S4) if it is a collision avoidance target, the collision avoidance target is dilated; 5) whether the unmanned ship is located in the narrow waterway is judged, and if yes, the step S3) is jumped to; S6) the unmanned ship is navigated according to the global path. The collision avoidance local path planning method of the application adopts simple and effective collision avoidance rules, dynamically plans the navigation path by perceiving the environment, and solves the problem that the existing path planning algorithm is difficult to meet the real-time requirement and may have no solution and may not converge.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned ship navigation control, and particularly relates to a narrow waterway unmanned ship collision avoidance local path planning method. BACKGROUND

[0002] A narrow waterway refers to a navigable water area with a narrow width and limited ship maneuvering. In current specifications, whether a waterway belongs to a narrow waterway is determined according to the conventional navigation method and the opinion of a navigation expert, so as to determine whether the narrow waterway clause is applicable. When an unmanned ship navigates in a narrow waterway, there are many difficulties. First, the actual traffic environment of a narrow waterway is complex, with various ship types and a wide speed distribution. In order to ensure safe navigation, the speed of the unmanned ship is usually low, and the rudder effect of the unmanned ship is poor under low speed conditions, making it difficult to avoid high-speed other ships. Second, some small civilian ships often do not comply with the International Regulations for Preventing Collisions at Sea when passing through a narrow waterway, and the conventional algorithm usually considers the constraints of maritime rules. The difference between the two leads to the fact that the use of the conventional algorithm for collision avoidance is prone to incoordination between the unmanned ship and other ships, and thus dangerous situations occur.

[0003] A narrow waterway unmanned platform autonomous navigation route planning method and device (Chinese Patent Application No. CN202011493165.9) proposes a route planning method for an unmanned platform in a narrow waterway, which uses a grid map and a search algorithm to avoid collision and dynamically adjust the route. However, a narrow waterway is generally constrained by a shore wall, the collision avoidance space of the unmanned ship is narrow, and the real-time requirement of collision avoidance is high. Therefore, the conventional algorithm may fail when dealing with the problem of collision avoidance in a narrow waterway. The conventional graph search algorithm and heuristic intelligent search algorithm are difficult to meet the real-time requirement, the speed obstacle method is prone to no solution when encountering a meeting situation with a faster other ship, and the path planning algorithm based on random sampling converges slowly in a narrow and obstacle-laden area, and may not give a steering instruction when the unmanned ship and the collision avoidance target are about to collide, which brings great safety risks to the unmanned ship navigating in a narrow waterway. SUMMARY

[0004] The purpose of the present application is to provide a narrow waterway unmanned ship collision avoidance local path planning method, which uses a simple and effective collision avoidance rule for an unmanned ship navigating in a narrow waterway, and dynamically plans a navigation path by perceiving the environment, so as to solve the problem that the existing path planning algorithm is difficult to meet the real-time requirement and may have no solution or non-convergence.

[0005] The specific technical solution of the present application is a narrow waterway unmanned ship collision avoidance local path planning method, characterized by comprising the following steps:

[0006] S1) After entering the narrow waterway, a navigation route is planned based on terrain perception information, and a local navigation route is established;

[0007] S2) establishing an identification channel, which is a dynamic area based on the position of the unmanned ship and the width of the channel, and is used to determine the interference relationship with the obstacle to make collision avoidance decision;

[0008] S3) sailing according to the local route, and determining whether there is a collision avoidance target in the identification channel, if the obstacle is located outside the identification channel or the speed direction of the obstacle is the same as that of the unmanned ship and the speed is greater than that of the unmanned ship, the obstacle is determined as a non-collision avoidance target, otherwise, it is determined as a collision avoidance target;

[0009] S4) if it is a collision avoidance target, the collision avoidance target is inflated, and the relative position relationship between the collision avoidance target in the identification channel and the unmanned ship and the relative speed relationship between them are determined, and the collision avoidance target in the identification channel is avoided, and after the collision avoidance is completed, the original local route sailing is returned;

[0010] S5) determining whether the unmanned ship is located in the narrow waterway, if yes, jumping to step S3);

[0011] S6) returning to global path sailing.

[0012] Further, the local route establishment method in step S1 is that the unmanned ship establishes a local route parallel to the right side wall and two ship widths away from the right side wall as the local route of the unmanned ship sailing in the narrow waterway based on the perceived size information of the narrow waterway.

[0013] Further, the method of establishing the identification channel in step S2 is that the identification channel is a rectangular identification channel, the center line of the channel is taken as the left boundary of the rectangular identification channel to ensure that the unmanned ship sails on the right side of the channel, the position two ship widths away from the channel bank is taken as the right boundary of the identification channel, the front boundary is at the maximum detection position of the unmanned ship, the left and right boundaries are extended to the front boundary, and the rear boundary is a vertical line passing through the center of gravity of the unmanned ship and perpendicular to the left and right boundaries, and the local path is taken as the center line of the rectangular identification channel.

[0014] Further, the method of inflating the collision avoidance target in step S4 is that, according to the sensing result of the sensing radar, the position, speed and size information of the obstacle are obtained, the center of the position of the collision avoidance target is taken as the center, the sum of the longest projection length of the collision avoidance target and half of the longest projection length of the ship is taken as the radius to make an inflation circle, and the position, speed and size information of the inflated obstacle are stored as the collision avoidance obstacle information.

[0015] Further, the specific collision avoidance method in step S4 is as follows:

[0016] S41) judging according to the speed relationship between the collision avoidance target and the unmanned ship, if the speed of the collision avoidance target is 0, the collision avoidance is performed according to the position relationship between the collision avoidance target and the unmanned ship;

[0017] S42) If the target speed is not 0 and the direction is the same as the unmanned ship, the unmanned ship is not allowed to overtake due to the narrow waterway, so the unmanned ship is slowed down to the same speed as the target for following;

[0018] S43) If the target speed is not 0 and the direction is opposite to the unmanned ship, it is a meeting situation, so the unmanned ship is slowed down to 1 kn and avoids collision according to the position relationship between the target and the unmanned ship;

[0019] S44) When the parallel line between the position of the unmanned ship and the center line of the identified channel is outside the expansion circle of the target, and the perpendicular line between the position of the unmanned ship and the center line of the identified channel crosses the expansion circle, it is judged that the collision avoidance is completed, if the position of the unmanned ship is inside the expansion circle, the unmanned ship is slowed down and stopped until the perpendicular line between the position of the unmanned ship and the center line of the identified channel crosses the expansion circle, and it is judged that the collision avoidance is completed.

[0020] Further, the specific method of collision avoidance in the collision avoidance method according to the position relationship between the target and the unmanned ship is: if the target position is on the right side of the center line of the identified channel, the unmanned ship turns left to avoid left, if the target is on the left side of the center line of the identified channel, the unmanned ship turns right to avoid right, if there are multiple targets occupying both sides of the channel, the unmanned ship is slowed down and stopped until there is a passable condition.

[0021] Further, the route planning method of the global path in step 6 is to grid the unmanned ship navigation chart, based on the given starting point and target point, an initial global path is searched in the chart by using A* algorithm, the unmanned ship starts to move along the global path with fixed step length, and RRT algorithm is used for dynamic collision avoidance path planning during movement.

[0022] The beneficial effects of this invention are: 1) The local path planning method for collision avoidance of unmanned surface vessels (USVs) in narrow waterways adopts a simple method of setting up identification channels and collision avoidance judgment rules to correct the navigation path planned by the search algorithm, solving the problems of difficulty in meeting real-time requirements, lack of solutions, and non-convergence in existing path planning algorithms, and effectively reducing the complexity of USV collision avoidance decisions; 2) The use of rectangular identification channels can easily apply various collision avoidance rules and perform corresponding USV operations on various types of collision avoidance targets. When making planning decisions, the narrow waterway collision avoidance rules exclude right-crossing and right-chasing encounters, simplifying the collision avoidance decision-making process; 3) The method uses the phase relationship between USVs... Collision avoidance is performed using simple collision avoidance rules based on positional relationships and relative speed relationships with the unmanned surface vessel (USV). When it is difficult to determine which collision avoidance strategy to adopt, the USV is slowed down to 0 to avoid affecting the judgment of the collision avoidance target; 4) The collision avoidance target is expanded by drawing an expansion circle with the radius of the sum of the longest projected length of the collision avoidance target and half of the longest projected length of the ship. This expansion method takes into account that the collision avoidance target of the USV is relatively small compared to the general size, making the collision avoidance rule algorithm simple and feasible; 5) When the USV is navigating normally, the A* search algorithm and the RRT algorithm are used for local route planning to ensure that the navigation path planning of the USV in narrow waterways is consistent with the global path planning.

[0023] The collision avoidance local path planning method for unmanned surface vessels (USVs) in narrow waterways of the present invention ensures that USVs can perform reasonable collision avoidance path planning when navigating in narrow waterways by establishing an identification channel and combining it with a collision avoidance strategy. This ensures that USVs meet the navigation requirements of the International Regulations for Preventing Collisions at Sea (ICP-1) and guarantee the navigation safety and collision avoidance efficiency of USVs in narrow waterways. Attached Figure Description

[0024] Figure 1 This is a flowchart of the local path planning method for collision avoidance of unmanned surface vessels in narrow waterways according to the present invention;

[0025] Figure 2 This is a schematic diagram of the identification channel in one embodiment of the local path planning method for collision avoidance of unmanned surface vessels in narrow waterways according to the present invention. Detailed Implementation

[0026] The following structural specification and accompanying drawings further describe the specific technical solution of the present invention.

[0027] As attached Figure 1 As shown, the present invention provides a local path planning method for collision avoidance of unmanned surface vessels in narrow waterways, characterized by comprising the following steps:

[0028] S1) After entering the narrow waterway, a local route is established and route planning is carried out based on terrain perception information.

[0029] Terrain perception information is based on the shape of the shoreline, the location of the shipping route, and the width of the shoreline, which are obtained from perception radar identification.

[0030] The route planning method of the local route is that the unmanned ship establishes a local route parallel to the right bank wall and at a distance of twice the ship width from the right bank wall as the local route of the unmanned ship in the narrow waterway based on the perceived size information of the narrow waterway.

[0031] S2) Establish an identification channel, which is a dynamic area including the unmanned ship, for judging whether the obstacle interferes with the dynamic area. The identification channel is a rectangular identification channel, taking the center line of the channel to the left as the left boundary of the rectangular identification channel to ensure that the unmanned ship keeps sailing on the right side of the channel, taking the position of twice the ship width from the channel bank wall to the right as the right boundary of the identification channel, the front boundary is at the maximum detection position of the unmanned ship, the left and right boundaries extend to the front boundary, and the rear boundary is a vertical line passing through the center of gravity of the unmanned ship and perpendicular to the left and right boundaries, and taking the local route as the center line of the rectangular identification channel;

[0032] S3) Sail according to the local route, and judge whether there is an obstacle avoidance target in the identification channel. If the obstacle is located outside the identification channel or the speed direction of the obstacle is the same as that of the ship and the speed is greater than that of the ship, the obstacle is determined as a non-obstacle avoidance target, otherwise it is determined as an obstacle avoidance target;

[0033] S4) If it is an obstacle avoidance target, the obstacle avoidance target is processed by inflation. The method of processing the obstacle avoidance target by inflation is to make an inflation circle with the position of the obstacle avoidance target as the center and the sum of half of the longest projection length of the obstacle avoidance target and the longest projection length of the ship as the radius, and to use the area divided by the inflation circle as the area where the obstacle avoidance target is located for obstacle avoidance. This can leave a certain margin for the control and perception deviation of the unmanned ship, and ensure the safety of the unmanned ship sailing.

[0034] Judge the relative position relationship between the obstacle avoidance target in the identification channel and the unmanned ship, and the relative speed relationship between them, and perform obstacle avoidance on the obstacle avoidance target in the identification channel. After the obstacle avoidance is completed, return to the original sailing route according to the local route. Make a circle with the position of the unmanned ship as the center and twice the ship length as the radius, and take the intersection point of the circle and the original route as the target point to return to the original route.

[0035] The specific obstacle avoidance method is as follows:

[0036] S41) According to the speed relationship between the obstacle avoidance target and the unmanned ship, if the speed of the obstacle avoidance target is 0, the obstacle avoidance is performed according to the position relationship between the obstacle avoidance target and the unmanned ship.

[0037] S42) If the speed of the obstacle avoidance target is not 0 and the direction is the same as that of the ship, the unmanned ship is not allowed to overtake in the narrow waterway, so the unmanned ship is slowed down to the same speed as the obstacle avoidance target for following.

[0038] S43) If the target speed of collision avoidance is not 0 and the direction is opposite to the unmanned vehicle, it is a meeting situation, then the unmanned vehicle is decelerated to 1 kn and the collision avoidance is performed according to the position relationship between the target of collision avoidance and the unmanned vehicle.

[0039] S44) When the parallel line between the position of the unmanned vehicle and the center line of the identified channel is outside the expansion circle of the target of collision avoidance, and the perpendicular line between the position of the unmanned vehicle and the center line of the identified channel crosses the expansion circle, it is judged that the collision avoidance is completed, if the position of the unmanned vehicle is inside the expansion circle, the unmanned vehicle is decelerated and stopped until the perpendicular line between the position of the unmanned vehicle and the center line of the identified channel crosses the expansion circle, and it is judged that the collision avoidance is completed.

[0040] The specific method of collision avoidance according to the position relationship between the target of collision avoidance and the unmanned vehicle is that if the position of the target of collision avoidance is on the right side of the center line of the identified channel, the unmanned vehicle turns left to avoid leftward, if the target of collision avoidance is on the left side of the center line of the identified channel, the unmanned vehicle turns right to avoid rightward, if there are multiple targets occupying both sides of the channel, the unmanned vehicle is decelerated and stopped until there is a passable condition. Of course, corresponding logic can be added, that is, if the unmanned vehicle is allowed to pass through the middle of the channel safely, that is, when the unmanned vehicle passes through the middle of the channel, the position of the unmanned vehicle is outside the expansion circle of the target of collision avoidance, if not, the unmanned vehicle is decelerated and stopped;

[0041] S5) It is judged whether the unmanned vehicle is located in a narrow waterway, if yes, it is jumped to step S3);

[0042] S6) The route planning is performed according to the global path and the unmanned vehicle is navigated.

[0043] The route planning method of the global path is that after the sea chart of the unmanned vehicle is rasterized, based on the given starting point and target point, an initial global path is searched in the sea chart by using A* algorithm, the unmanned vehicle starts to move along the global path at a fixed step, and RRT algorithm is used for dynamic collision avoidance path planning in the movement process.

[0044] As shown in FIG. 4, the global path is planned according to the global path planning method. Figure 2As shown, in one specific embodiment of the present application, when the unmanned ship is sailing along the path of the global path planning, it is determined that the unmanned ship enters the narrow waterway and starts to execute the planning method of the present application. First, according to the channel terrain sensing result of the perception radar, the identification channel 1 and the local route 2 are established, and the unmanned ship starts to sail along the local route. In the sailing process, the ship ④, the ship ⑤ and the ship ⑥ are located outside the identification channel, so they are set as non-collision targets, the ship ①, the ship ②, the ship ③ and the ship ⑦ are located inside the identification channel, and they are set as collision avoidance targets and are inflated according to the obstacle information. Then, according to the obstacle information, the ship ① is set as the obstacle target for collision avoidance decision because the distance between the ship ① and the unmanned ship is the smallest. Since the ship ① has the same sailing direction as the unmanned ship and the speed is greater than the speed of the unmanned ship, the unmanned ship will continue to sail along the original local route at the original speed. After a period of time, the ship ② becomes the obstacle with the smallest distance to the unmanned ship, and the ship ② is set as the obstacle target. Since the ship ② has the same sailing direction as the unmanned ship and the speed is less than the speed of the unmanned ship, the unmanned ship will slow down to the same speed as the ship ② and sail along the original local route. After a period of time, the unmanned ship meets the ship ⑦. Since the ship ⑦ has the opposite sailing direction to the unmanned ship and is located on the left side of the identification channel, the unmanned ship will avoid to the right. After the avoidance is completed, the unmanned ship returns to the original local route to continue sailing until it sails out of the narrow waterway and completes the collision avoidance in the narrow waterway. Then the unmanned ship returns to the original global route for sailing.

[0045] Although the present application has been disclosed with the preferred embodiments as above, the embodiments are not intended to limit the present application. Any equivalent changes or modifications made without departing from the spirit and scope of the present application shall also fall within the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the content defined by the claims of the present application.

Claims

1. A local path planning method for collision avoidance of unmanned surface vessels in narrow waterways, characterized in that, Includes the following steps: S1) After entering the narrow waterway, route planning is carried out based on terrain perception information to establish a local route; S2) Establish an identification channel, which is a dynamic area defined based on the unmanned surface vessel's position and channel width, used to determine the interference relationship with obstacles in order to make collision avoidance decisions; S3) Navigate according to the local route and simultaneously determine whether there is a collision avoidance target in the identification channel. If the obstacle is located outside the identification channel or the obstacle's speed direction is the same as the vessel's and its speed is greater than the vessel's, the obstacle is determined to be a non-collision avoidance target; otherwise, it is determined to be a collision avoidance target. S4) If it is a collision avoidance target, the collision avoidance target is expanded, and the relative positional relationship between the collision avoidance target and the unmanned surface vessel and the relative speed relationship between the collision avoidance target and the unmanned surface vessel in the identification channel are determined. The collision avoidance target in the identification channel is avoided, and after the collision avoidance is completed, the original local route is returned to navigation. S5) Determine whether the unmanned surface vessel is located in a narrow waterway. If so, proceed to step S3). S6) Return to the global path navigation; The method for establishing the identification channel in step S2 is as follows: the identification channel is a rectangular identification channel. The left boundary of the rectangular identification channel is taken as the centerline of the channel to the left to ensure that the unmanned surface vessel (USV) stays on the right side of the channel. The right boundary of the identification channel is taken as the position twice the width of the ship from the channel bank. The front boundary is at the maximum detection position of the USV. The left and right boundaries extend forward to the front boundary. The rear boundary is a vertical line passing through the center of gravity of the USV and perpendicular to the left and right boundaries. The local path is used as the centerline of the rectangular identification channel.

2. The method for local path planning for collision avoidance of unmanned surface vessels in narrow waterways according to claim 1, characterized in that, The method for establishing a local route in step S1 is as follows: based on the perceived size information of the narrow waterway, the unmanned surface vessel (USV) establishes a route that is parallel to the right bank and twice the width of the vessel from the right bank as a local route for the USV to navigate in the narrow waterway.

3. The method for local path planning for collision avoidance of unmanned surface vessels in narrow waterways according to claim 1, characterized in that, The method for expanding the collision avoidance target in step S4 is as follows: after obtaining the obstacle's position, velocity, and size information based on the sensing results from the sensing radar, an expansion circle is drawn with the location of the collision avoidance target as the center and the sum of the longest projected length of the collision avoidance target and half of the longest projected length of the ship as the radius. The expanded obstacle's position, velocity, and size information are then stored as collision avoidance obstacle information.

4. The local path planning method for collision avoidance of unmanned surface vessels in narrow waterways according to claim 3, characterized in that, The specific collision avoidance method in step S4 is as follows: S41) Make a judgment based on the speed relationship between the collision avoidance target and the unmanned surface vessel. If the speed of the collision avoidance target is 0, then avoid the collision based on the positional relationship between the collision avoidance target and the unmanned surface vessel. S42) If the speed of the target being avoided is not 0 and its direction is the same as that of the vessel, the unmanned surface vessel shall decelerate to the same speed as the target being avoided and follow it, since overtaking is not allowed in narrow waterways. S43) If the speed of the target to be avoided is not 0 and the direction is opposite to that of the unmanned vessel, it is a head-on situation. The unmanned vessel shall decelerate to 1 knot and avoid collision according to the positional relationship between the target and the vessel. S44) When the line parallel to the centerline of the identification channel at the location of the unmanned surface vessel is outside the expansion circle of the collision avoidance target, and the perpendicular line between the location of the unmanned surface vessel and the centerline of the identification channel crosses the expansion circle, the collision avoidance is considered complete. If the location of the unmanned surface vessel is inside the expansion circle, the vessel is decelerated and stopped until the perpendicular line between the location of the unmanned surface vessel and the centerline of the identification channel crosses the expansion circle, the collision avoidance is considered complete.

5. The method for local path planning for collision avoidance of unmanned surface vessels in narrow waterways according to claim 4, characterized in that, The specific method for avoiding collisions based on the positional relationship between the target and the unmanned surface vessel (USV) in the aforementioned collision avoidance method is as follows: if the target is located to the right of the centerline of the identification channel, the USV steers to the left to avoid a collision; if the target is located to the left of the centerline of the identification channel, the USV steers to the right to avoid a collision; if multiple targets occupy both sides of the channel simultaneously, the USV slows down and stops until there are conditions for passage.

6. The method for local path planning for collision avoidance of unmanned surface vessels in narrow waterways according to claim 1, characterized in that, The route planning method for the global path in step 6 is as follows: after the unmanned surface vessel navigation chart is rasterized, an initial global path is searched in the chart based on the given starting point and target point using the A* algorithm. The unmanned surface vessel starts to move along the global path with a fixed step size, and dynamic collision avoidance path planning is performed using the RRT algorithm during the movement.

Images