Path planning method, ship collision avoidance path reconstruction method based on virtual obstacle line

Abstract

The present invention relates to a path planning method and a ship collision avoidance path reconstruction method based on a virtual obstacle line. First, a multimodal sensor is used to obtain surrounding environment information, and potential collision risks are evaluated through the nearest encounter point; secondly, a decision support system is integrated to automatically parse the International Regulations for Preventing Collisions at Sea (COLREGs), and direct navigation or avoidance decisions are made according to these rules. If the other ship does not avoid according to the rules during direct navigation, our ship needs to execute an emergency avoidance decision; then, a virtual obstacle line reflecting the potential collision risk is generated based on the perception data, and the adjustment of the virtual obstacle line is based on the bow direction of our ship and the behavior of the other ship to prevent invalid path expansion; finally, a path planning method (dynamic Theta* algorithm) is used for real-time path planning, and the unmanned ship route is reconstructed to avoid all static and dynamic obstacles. The present invention significantly improves the autonomous collision avoidance capability and navigation safety of unmanned ships in complex and changeable marine environments.

Description

Technical Field

[0001] The invention relates to the technical field of automatic navigation and collision avoidance of ships, in particular to a method for realizing efficient path planning and collision avoidance through real-time dynamic environment feedback. Background Art

[0002] With the widespread application of unmanned ships in environmental monitoring, maritime search and rescue, scientific research and other fields, their safe and efficient navigation capabilities have become particularly important. At present, most unmanned ship collision avoidance systems mainly rely on local collision avoidance algorithms and partial sensor information, which faces many challenges in navigation environments with complex or high-density obstacles. Existing technologies can usually only obtain local avoidance directions, and cannot fully utilize the global data provided by sensors to reconstruct the collision avoidance path. They also ignore the situation where the other ship does not comply with the rules, and are prone to failure in emergency collision avoidance scenarios. Summary of the invention

[0003] The present invention proposes a dynamic Theta* algorithm for local path planning, which can avoid static obstacles and dynamic obstacles at the same time.

[0004] The present invention proposes a ship collision avoidance path reconstruction method based on virtual obstacle line and dynamic Theta* algorithm, which effectively improves the autonomous collision avoidance capability of the ship in complex sea areas, while ensuring compliance with the International Regulations for Preventing Collisions at Sea (COLREGs) and having an emergency avoidance strategy when the other ship does not comply with COLREGs.

[0005] The present invention will significantly improve the safety and efficiency of unmanned ship navigation, and can achieve dynamic collision avoidance and emergency avoidance in complex and changeable marine environments.

[0006] In a first aspect, a path planning method is provided. The method is: after determining the parent-child relationship between all nodes in a grid map, the shortest path is inferred in reverse. The parent-child relationship determination method between nodes includes: when the current node s is extended to the neighboring node s′, detecting whether there is a static obstacle on the line between the neighboring node s′ and the parent node parent(s) of the current node s, detecting whether the line between the neighboring node s′ and the parent node parent(s) of the current node s passes through the obstacle grid occupied by the movable obstacle during the estimated time (1). If there is neither a static obstacle nor the obstacle grid occupied by the movable obstacle at the corresponding time is passed, the neighboring node s′ and the current node s share a parent node. Otherwise, it is determined that the neighboring node s′ and the parent node parent(s) of the current node s are not visible.

[0007] Continue to check whether the line between the neighboring node s′ and the current node s passes through the obstacle grid occupied by the static obstacle or the movable obstacle at the estimated time (2). If there is neither a static obstacle nor does it pass through the obstacle grid occupied by the movable obstacle at the corresponding moment, the parent node of the neighboring node s′ is the current node s. Otherwise, it is determined that the connection between the neighboring node s′ and the current node s is not visible.

[0008] On the second aspect, a method for reconstructing a ship collision avoidance path based on a virtual obstacle line is provided, comprising: replacing the part of the original route where the collision occurs with a local collision avoidance path planned using the method described, and connecting it with the remaining part of the original route to complete the reconstruction of the entire route, wherein when the local collision avoidance path is planned using the method described, the movable obstacle is TS, and a dynamic predicted obstacle circle is modeled with the future position of TS as the center and the ship length as the radius.

[0009] In some examples, based on COLREGs, the heading of the OS, and the behavior of the TS, a t b and side d f Two virtual obstacle lines with lengths of d1 and d2 are generated at the locations as the boundaries of the grid map.

[0010] In some examples, if the OS needs to perform an avoidance maneuver, then the vehicle is positioned behind the current heading of the OS. b and left side d f Two virtual obstacle lines with lengths of d1 and d2 are generated at the left and the virtual obstacle line on the right is connected to the center of the predicted obstacle circle.

[0011] In some examples, if the TS should avoid the OS flying straight, but the TS does not take evasive action, the OS takes emergency avoidance measures and flies behind the OS on its current heading. b and right side d f Two virtual obstacle lines with lengths of d1 and d2 are generated at the right and the virtual obstacle line on the right is connected to the center of the predicted obstacle circle.

[0012] A third aspect provides a ship that uses the path planning method to plan a path.

[0013] A fourth aspect provides a ship that, when it is quantified that a collision with an obstacle is about to occur, applies the ship collision avoidance path reconstruction method based on a virtual obstacle line to reconstruct a route to avoid the obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is a flow chart of the unmanned ship collision avoidance path reconstruction method based on virtual obstacle lines.

[0015] Figure 2 It is a schematic diagram for detecting whether there is a collision between OS and TS.

[0016] Figure 3 It is a schematic diagram of collision avoidance path reconstruction for different encounter types. DETAILED DESCRIPTION

[0017] The present invention proposes a dynamic Theta* algorithm (path planning method). The traditional Theta* algorithm can only avoid static obstacles and belongs to a two-dimensional space search; the innovative dynamic Theta* algorithm adds a time dimension on the basis of two-dimensional space, and can avoid static and dynamic obstacles at the same time. On the basis of the traditional Theta* algorithm, the dynamic Theta* algorithm needs to determine whether the grid node to be expanded will be occupied by movable obstacles. If occupied, it will be modeled as an obstacle grid. Taking into account that the movable obstacles are dynamically changing, it is necessary to estimate the time from the starting point to the grid node to be expanded in order to calculate the future position of the movable obstacle. There are two situations for the calculation of the estimated time consumption:

[0018] Estimated time (1) = [g(parent(s)) + c(parent(s), s′)] ÷ movable obstacle speed;

[0019] Estimated time (2) = [g(s) + c(s, s′)] ÷ speed of movable obstacle.

[0020] Where s represents the current grid node (abbreviated as: current node); s′ represents the grid node to be expanded, that is, the neighboring node of the current node s; g(s) represents the path length from the starting point to the current node s, c(s, s′) represents the straight line length between the current node s and its neighboring node s′; g(parent(s) represents the path length from the starting point to the parent node parent(s) of the current node s, c(parent(s), s′) represents the straight line length between the parent node parent(s) of the current node s and the neighboring node s′ of the current node s.

[0021] When the current node s expands to the neighboring node s′, the dynamic Theta* algorithm not only detects whether there is a static obstacle on the line between the neighboring node s′ and the parent node parent(s) of the current node s, but also detects whether the line between the neighboring node s′ and the parent node parent(s) of the current node s passes through the obstacle grid occupied by the movable obstacle at the estimated time (1). If there is neither a static obstacle nor does it pass through the obstacle grid occupied by the movable obstacle at the corresponding time, the neighboring node s′ and the current node s share a parent node. Otherwise, it is determined that the connection between the neighboring node s′ and the parent node parent(s) of the current node s is not visible, and it is necessary to continue to detect whether the connection between the neighboring node s′ and the current node s passes through the static obstacle or the obstacle grid occupied by the movable obstacle at the estimated time (2). If there is neither a static obstacle nor does it pass through the obstacle grid occupied by the movable obstacle at the corresponding time, the parent node of the neighboring node s′ is the current node s. Otherwise, it is determined that the connection between the neighboring node s′ and the current node s is not visible. After determining the parent-child relationship between all nodes in the grid map, the shortest path can be inferred. The inversion method is the same as the traditional Theta* algorithm. After finding the target point (if the current node s is the target point), the algorithm stops iterating. Then, through the target point--find the parent node of the target point--the parent node of the parent node--......-the starting point. In this way, the path from the starting point to the target point can be connected.

[0022] In essence, the dynamic Theta* algorithm determines whether the grid nodes at the corresponding time and position will pose a threat to our ship based on the speed and direction of the movable obstacles, and finally obtains a local path from the starting point to the target point that can avoid both static and dynamic obstacles.

[0023] The dynamic Theta* algorithm can be applied to the automatic navigation of land robots, surface ships, etc. The following introduces an application of the dynamic Theta* algorithm in ship collision avoidance. The ship can be unmanned or manned. In this embodiment, the movable obstacle is the other ship (TS, Target Ship). The following is a detailed introduction to the ship collision avoidance path reconstruction method based on the dynamic Theta* algorithm and virtual obstacle lines. Figure 1 This is a flow chart of a ship collision avoidance path reconstruction method based on virtual obstacle lines.

[0024] Step 1, environmental perception and collision judgment: Use multimodal sensors (including radar, optical camera, sonar, etc.) to obtain more comprehensive surrounding environment information, and use environmental information for collision judgment.

[0025] Utilize the various sensors equipped on the ship to collect the detection range in real time rangeThe environmental data in the detection range includes static obstacle position, dynamic obstacle position, dynamic obstacle heading and dynamic obstacle speed. The collision avoidance decision support system is activated only when the ship determines that there will be a potential conflict with other ships in the future. The closest point of approach method (CPA) is used to quantify the risk of conflict. CPA evaluates the possibility of collision within the detection range by calculating the distance between the own ship (OS) and the other ship (TS) in multiple time steps in the future. CPA is the minimum distance between the future path points of TS and OS at constant heading and speed. CPA Indicates the time when TS and OS collide at a future path point. If TS is within the detection range and d CPA Less than the specified minimum collision distance d A , t CPA Less than the specified minimum collision time t A , it is considered that there is a risk of collision and the ship needs to avoid collision in accordance with the relevant provisions of COLREGs, such as Figure 2 shown.

[0026] Step 2, compliance mechanism: Integrate a decision support system to automatically parse COLREGs and make direct or avoid decisions based on these rules, including situations where the TS does not follow the collision avoidance rules.

[0027] COLREGs mainly defines three types of ship collisions: Overtaking (Rule 13), Head-on (Rule 14) and Crossing (Rule 15). The encounter type of the unmanned ship is determined by the relative position of the OS to the TS (Q ot ) and the relative position of TS to OS (Q to ) for detailed specifications see Table 1. The relative bearing is measured clockwise from the front of the bow, ranging from 0° to 360°.

[0028] If the decision support system determines that the TS should avoid the direct-flying OS, but the TS does not take corresponding avoidance actions according to the rules within m time steps, the OS should take emergency avoidance measures and make an active avoidance decision. m is a user-defined value.

[0029]

[0030] Step 3, virtual obstacle line generation technology: Based on the perception data, a virtual obstacle line reflecting the potential collision risk is generated based on the heading of the OS and the behavior of the TS.

[0031] A dynamic predicted obstacle circle is modeled with the future position of the TS as the center and the length of the ship as the radius. This TS / predicted obstacle circle is the movable obstacle. If the OS needs to perform an avoidance, it will be behind the current heading of the OS. b and left side d f Two line segments with lengths of d1 and d2 are generated at the left and the virtual obstacle line is connected to the center of the predicted obstacle circle. The virtual obstacle lines generated by different collision avoidance types are as follows: Figure 3 If it is determined that the TS should avoid the straight-flying OS, but the TS does not take evasive action, the OS needs to take emergency evasive measures and move d behind the current course. b and right side d f Two line segments with lengths of d1 and d2 are generated at the positions, and the virtual obstacle line on the right is connected to the center of the predicted obstacle circle, as shown in Figure 3 (d) as shown.

[0032] The main purpose of the side virtual obstacle line is to ensure that the local collision avoidance path planned by the dynamic Theta* algorithm meets the decision requirements (complying with the COLREGs rule); the rear virtual obstacle line is to prevent the grid nodes from invalidly expanding to the rear of the unmanned ship, thereby improving the calculation efficiency of the algorithm. b d f , d1 and d2 are provided by Captain L oa Determine as follows:

[0033] d b =d f =L oa / 1.5

[0034] d1=L oa *2.5

[0035] d2=L oa *3.5

[0036] Step 4, real-time path reconstruction system: Use the dynamic Theta* algorithm loaded with virtual obstacle lines for path planning and adjust the route of the unmanned ship in real time to avoid all static and dynamic obstacles.

[0037] Connect the side virtual obstacle line to the center of the predicted obstacle circle and load it into the dynamic Theta* algorithm. At the startup stage of the dynamic Theta* algorithm, the local map needs to be initialized, including the calibration of all known obstacle positions and the freely expanded node space. The starting point of the dynamic Theta* algorithm is set to the current position of the OS, and the target point is set to the original route position n time steps after the predicted obstacle circle, where n is a user-defined value.

[0038] In essence, the dynamic Theta* algorithm models the TS as a movable static obstacle, and determines whether the grid nodes at the corresponding time and position will pose a threat to the ship based on the TS speed and heading, and finally obtains a local path from the starting point to the target point that can avoid both static and dynamic obstacles.

[0039] The part of the original route where the collision occurred is replaced by the local collision avoidance path planned by the dynamic Theta* algorithm, and connected with the rest of the original route of the unmanned ship to complete the reconstruction of the entire route.

Claims

1. A path planning method, characterized in that: After determining the parent-child relationship between all nodes in the grid map, the shortest path is then inverted. The method for determining the parent-child relationship between nodes includes: When the current node s expands to the neighboring node s′, check whether there is a static obstacle on the line between the neighboring node s′ and the parent node parent(s) of the current node s, and check whether the line between the neighboring node s′ and the parent node parent(s) of the current node s will pass through the obstacle grid occupied by the movable obstacle at the estimated time (1). If there is neither a static obstacle nor does it pass through the obstacle grid occupied by the movable obstacle at the corresponding moment, then the neighboring node s′ and the current node s share a parent node. Otherwise, it is determined that the connection between the neighboring node s′ and the parent node parent(s) of the current node s is not visible. Continue to check whether the line between the neighboring node s′ and the current node s passes through the obstacle grid occupied by the static obstacle or the movable obstacle at the estimated time (2). If there is neither a static obstacle nor the obstacle grid occupied by the movable obstacle at the corresponding time, the parent node of the neighboring node s′ is the current node s. Otherwise, it is determined that the connection between the neighboring node s′ and the current node s is not visible. Estimated time (1) = [g(parent(s)) + c(parent(s), s′)] ÷ movable obstacle speed; Estimated time (2) = [g(s) + c(s, s′)] ÷ speed of movable obstacle; Where s represents the current node; s′ represents the node corresponding to the grid to be expanded, that is, the neighboring node of the current node s; g(s) represents the path length from the starting point to the current node s, c(s, s′) represents the straight line length between the current node s and its neighboring node s′; g(parent(s) represents the path length from the starting point to the parent node parent(s) of the current node s, and c(parent(s), s′) represents the straight line length between the parent node parent(s) of the current node s and the neighboring node s′ of the current node s.

2. A ship collision avoidance path reconstruction method based on virtual obstacle line, characterized in that: include: The part of the original route where the collision occurs is replaced by a local collision avoidance path planned using the method described in claim 1, and connected with the remaining part of the original route to complete the reconstruction of the entire route, wherein when the local collision avoidance path is planned using the method described in claim 1, the movable obstacle is the other ship TS.

3. The method for reconstructing a ship collision avoidance path based on a virtual obstacle line according to claim 2, characterized in that: The dynamic predicted obstacle circle is modeled with the future position of the other ship TS as the center and the ship length as the radius.

4. The method for reconstructing a ship collision avoidance path based on a virtual obstacle line according to claim 3, characterized in that: Based on COLREGs, the heading of our ship's OS and the behavior of the other ship's TS, d b and side d f Two virtual obstacle lines with lengths of d1 and d2 are generated at the locations as the boundaries of the grid map.

5. The method for reconstructing a ship collision avoidance path based on a virtual obstacle line according to claim 4, characterized in that: If our ship OS needs to perform avoidance, then it will be behind our ship OS current course d b and left side d f Two virtual obstacle lines with lengths of d1 and d2 are generated at the left and the virtual obstacle line on the left is connected to the center of the predicted obstacle circle.

6. The method for reconstructing a ship collision avoidance path based on a virtual obstacle line according to claim 4, characterized in that: If the other ship TS should avoid our ship OS which is heading straight, but the other ship TS does not make an evasive action, our ship OS will take emergency evasive measures and sail behind our ship OS in the current course. b and right side d f Two virtual obstacle lines with lengths of d1 and d2 are generated at the right and the virtual obstacle line on the right is connected to the center of the predicted obstacle circle.

7. The method for reconstructing a ship collision avoidance path based on a virtual obstacle line according to claim 5 or 6, characterized in that: d b d f , d1 and d2 and our ship OS Captain L oa The relationship is: d b =d f =L oa / 1.5 d1=L oa *2.5 d2=L oa *3.5。 8. A ship, characterized in that: The path planning method described in claim 1 is used to plan the path.

9. A ship, characterized in that: When it is quantified that a collision with an obstacle is about to occur, the ship collision avoidance path reconstruction method based on virtual obstacle lines as described in claim 2 is applied to reconstruct a route to avoid the obstacle.

10. The ship according to claim 9, characterized in that: Quantify the risk of collision with obstacles using the closest point of encounter method.

Images