Path planning method based on ship intelligent collision avoidance decision-making system and fused with improved DWA and A* algorithms

Abstract

The invention discloses an improved DWA and A * algorithm fused path planning method based on a ship intelligent collision avoidance decision-making system, and relates to the field of path planning. Constructing a rasterized map according to the electronic chart data and the starting point and the ending point of path planning, and determining physical information of ships on the water surface; an improved DWA algorithm is adopted for path planning, when the distance between the water surface ship and the obstacle is smaller than the safe distance, an improved A * algorithm is called for global path planning from the current position to the terminal point, and a path point is returned; extracting a path key point as a temporary end point of the improved DWA algorithm, and continuing to perform dynamic path planning until the surface ship reaches the end point; and importing the DWA algorithm and the A * algorithm after fusion improvement into a ship intelligent collision avoidance decision-making system, and generating an optimal obstacle avoidance path in combination with the marine environment data and the target fusion data. The method is suitable for a complex sea area dynamic environment, and the feasibility and safety of water surface ship path planning can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of surface vessel path planning, and more specifically to a fusion-improved DWA and A based on a ship intelligent collision avoidance decision system. * Algorithmic path planning method. Background Art

[0002] The ship intelligent collision avoidance decision system is a comprehensive navigation safety assistance system that aims to provide ships with all-weather, all-round collision avoidance decision support. By real-time monitoring and analysis of the ship's surrounding environment and maritime traffic conditions, it automatically identifies and diagnoses potential collision risks and makes collision avoidance navigation recommendations, thereby improving the navigation safety and efficiency of ships in complex maritime environments.

[0003] The DWA (dynamic window approach) algorithm is a local path planning algorithm that is suitable for dynamic environments and has good real-time performance. Its basic idea is to search for feasible speed instructions in a local range based on the current state of the surface vessel and environmental information. When the target point is behind an obstacle, the DWA algorithm may be limited by the local search range, resulting in the inability to generate an effective path to the target. That is, when the target point is behind an obstacle, the surface vessel may need to move in the opposite direction to bypass the obstacle and reach the target point, but the speed window may not cover the speed range of the reverse movement, resulting in the inability to generate a suitable speed instruction. In addition, in complex or restrictive water environments, the DWA algorithm is prone to fall into a local optimal solution and cannot find a global optimal solution, which is manifested as the surface vessel repeatedly circling on the same path and unable to jump out.

[0004] A* (A-star) algorithm is a widely used search algorithm for finding the shortest path from a starting point to a destination in graphs and grids. The core of A* algorithm is its heuristic function h(n), which estimates the cost from the current node to the destination node. By changing the heuristic function, it can be adjusted from a strict shortest path algorithm to a search algorithm that gives priority to other factors (such as cost, time, safety, etc.). A* algorithm occupies the core of path planning and other search problems because of its efficiency, accuracy and reliability.

[0005] Therefore, how to overcome the defect of the DWA algorithm that it is easy to fall into the local optimal solution when performing local path planning and improve the feasibility and safety of surface vessel path planning is a technical problem that technical personnel in this field urgently need to solve. Summary of the invention

[0006] In view of this, the present invention provides a fusion-improved DWA and A based on a ship intelligent collision avoidance decision system. * The path planning method of the algorithm solves the problems existing in the background technology.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] A fusion-improved DWA and A based on ship intelligent collision avoidance decision system * The path planning method of the algorithm includes the following steps:

[0009] S1. Construct a raster map based on the electronic chart data and the starting and ending points of the path planning to determine the physical information of the surface vessel;

[0010] S2, according to the starting point and end point of the path planning and the physical information of the surface ship, the improved DWA algorithm is used for path planning to obtain the best combination of speed and angular velocity;

[0011] S3. When the distance between the surface vessel and the obstacle is less than the safe distance, call the improved A * The algorithm performs global path planning from the current position to the end point and returns the path point;

[0012] S4, extracting the key points of the path as the temporary end point of the improved DWA algorithm, and continuing the dynamic path planning until the surface vessel reaches the end point;

[0013] S5, integrate the improved DWA algorithm and A * The algorithm is imported into the ship intelligent collision avoidance decision system, and the optimal obstacle avoidance path is generated by combining the ocean environment data and target fusion data.

[0014] Optionally, in S1, the specific steps are as follows:

[0015] According to the position coordinates of the starting point and the end point, the minimum rectangular area containing the starting point and the end point is calculated;

[0016] Based on the minimum rectangular area, the area boundary is expanded to construct a rasterized map.

[0017] Optionally, in S1, the physical information of the surface vessel includes: maximum speed, maximum rotation angle, maximum acceleration, rotation acceleration, speed resolution, and rotation speed resolution.

[0018] Optionally, in S2, the specific steps are as follows:

[0019] S21, based on the current state and motion model of the surface vessel, determine the speed and angular velocity range at the next moment, generate multiple predicted trajectories in the dynamic window, and score them according to the target point and obstacle information;

[0020] S22. Based on the scoring results, an optimal combination of speed and angular velocity is selected to guide the surface vessel to move toward the target point.

[0021] Optionally, in S21, the specific steps are as follows:

[0022] Based on multiple predicted trajectories, the evaluation score of the predicted trajectory endpoint is calculated, including the heading score, distance score, speed score and braking distance;

[0023] Determine whether the predicted trajectory is feasible based on the braking distance and obstacle information, and store the feasible predicted trajectory and its evaluation score in the database;

[0024] Each evaluation score is regularized, and the final score of each predicted trajectory is calculated based on the weight of the evaluation score.

[0025] Optional, in S3, improved A * The heuristic function of the algorithm is * The obstacle distance penalty term is introduced based on the algorithm heuristic function, namely:

[0026] f(n)=g(n)+h(n)

[0027] h(n)=euclidean_cost(n)+obstacle_cost(n)

[0028] Where: g(n) represents the actual distance cost from the current node n to the starting node s; h(n) represents the predicted distance cost from the current node n to the target node g; euclidean_cost(n) represents the Euclidean distance heuristic from the current node n to the target node g; obstacle_cost(n) represents the cost generated based on the distance from the current node n to the obstacle.

[0029] Optionally, in S4, for the improved A * The algorithm returns the path points, extracts the key points of the path as the temporary target points of the improved DWA algorithm, and does not process other path points.

[0030] Optionally, in S5, the software of the ship intelligent collision avoidance decision system consists of an intelligent recognition model, an intelligent fusion model and an intelligent collision avoidance system, and is accompanied by small tools for zooming in, out and calculating charts, as well as a comprehensive situation display interface.

[0031] It can be seen from the above technical solutions that compared with the prior art, the present invention discloses a fusion-improved DWA and A based on a ship intelligent collision avoidance decision system. * The algorithm's path planning method has the following beneficial effects:

[0032] (1) The present invention constructs a grid map using electronic chart data and the latitude and longitude coordinates of the starting point and the end point. Compared with constructing a grid map of the entire electronic chart, the present invention has many advantages, such as reducing the amount of calculation, improving real-time performance, concentrating resources on important areas, flexibility and scalability, and improving path planning accuracy. This method can perform path planning more efficiently and accurately, and is particularly suitable for dynamic environments with high real-time requirements.

[0033] (2) The present invention uses the improved DWA algorithm to perform path planning based on the physical information of the surface vessel. The improvement is mainly to optimize the trajectory evaluation function. The calculation of the evaluation function score can generate a more comprehensive path evaluation result by comprehensively considering factors such as heading, distance and speed. Based on the advantages of weight adjustment, fast calculation, scalability and balance between safety and efficiency, the quality and adaptability of path planning are significantly improved;

[0034] (3) When the distance between the surface vessel and the obstacle is less than the safe distance, the improved A * The algorithm performs global path planning from the current position to the destination of the surface vessel, which can avoid the problem that the surface vessel repeatedly circles on the same path and cannot jump out due to the speed window limitation of the DWA algorithm and the local optimal solution when the destination is directly behind an obstacle;

[0035] (4) The improved DWA algorithm and A * The algorithm is introduced into the ship intelligent collision avoidance decision system. Combining rich multi-source marine environment data and target fusion data can generate the optimal collision avoidance path in complex water environments, ensuring that the ship avoids dynamic obstacles and static obstacles in complex sea areas and reaches the target point safely and efficiently. This method is suitable for complex sea dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0037] Figure 1 The present invention provides a fusion-improved DWA and A based on the ship intelligent collision avoidance decision system. * Flowchart of the algorithm’s path planning method;

[0038] FIG. 2( a ) is a diagram of the present invention using the optimized A * Path planning diagram of the algorithm in the grid map;

[0039] FIG. 2( b ) shows the optimized A * Path planning diagram of the algorithm in the grid map;

[0040] FIG3( a ) is a path planning diagram in a grid map using only the improved DWA algorithm in a complex sea area provided by the present invention;

[0041] FIG3( b ) is a partial enlarged diagram of the path planning in the grid map using only the improved DWA algorithm in the complex sea area provided by the present invention;

[0042] Figure 4 The improved DWA algorithm and the improved A are integrated in the complex sea area provided by the present invention. * Path planning diagram of the algorithm in the grid map;

[0043] Figure 5 A real-scene perception interface diagram of the ship intelligent collision avoidance decision-making system provided by the present invention;

[0044] Figure 6 This is a diagram of the comprehensive situation display interface of the ship intelligent collision avoidance decision-making system provided by the present invention. DETAILED DESCRIPTION

[0045] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0046] In order to solve the problem that surface ships repeatedly circle on the same path and cannot jump out, and the problem that the planned path is too close to obstacles, and to improve the feasibility and safety of surface ship path planning, the embodiment of the present invention discloses a fusion-improved DWA and A based on a ship intelligent collision avoidance decision system. * Algorithmic path planning methods, such as Figure 1 As shown, the following steps are included:

[0047] S1. Construct a raster map based on the electronic chart data and the starting and ending points of the path planning to determine the physical information of the surface vessel;

[0048] S2, according to the starting point and end point of the path planning and the physical information of the surface ship, the improved DWA algorithm is used for path planning to obtain the best combination of speed and angular velocity;

[0049] S3. When the distance between the surface vessel and the obstacle is less than the safe distance, call the improved A *The algorithm performs global path planning from the current position to the end point and returns the path point;

[0050] S4, extracting the key points of the path as the temporary end point of the improved DWA algorithm, and continuing the dynamic path planning until the surface vessel reaches the end point;

[0051] S5, integrate the improved DWA algorithm and A * The algorithm is imported into the ship intelligent collision avoidance decision system, and the optimal obstacle avoidance path is generated by combining the ocean environment data and target fusion data.

[0052] Furthermore, in S1, the specific steps are as follows:

[0053] According to the position coordinates of the starting point and the end point, the minimum rectangular area containing the starting point and the end point is calculated; based on the minimum rectangular area, the area boundary is expanded to construct a rasterized map. Specifically, assuming that the starting point and the end point are both in the sea area of ​​the northern hemisphere and the eastern hemisphere, the longitude of the left boundary of the minimum rectangular area is reduced by 0.5 degrees, and the longitude of the right boundary is increased by 0.5 degrees; the latitude of the upper boundary of the minimum rectangular area is increased by 0.5 degrees, and the latitude of the lower boundary is reduced by 0.5 degrees. Subsequent path planning is carried out in the expanded rectangular area.

[0054] Furthermore, in S1, the physical information of the surface vessel includes: maximum speed, maximum rotation angle, maximum acceleration, rotation acceleration, speed resolution, and rotation speed resolution.

[0055] Furthermore, in S2, the specific steps are as follows:

[0056] S21, based on the current state and motion model of the surface vessel, determine the speed and angular velocity range at the next moment, generate multiple predicted trajectories in the dynamic window, and score them according to the target point and obstacle information;

[0057] S22. Based on the scoring results, an optimal combination of speed and angular velocity is selected to guide the surface vessel to move toward the target point.

[0058] Furthermore, in S21, the specific steps are as follows:

[0059] Based on multiple predicted trajectories, the evaluation score of the predicted trajectory endpoint (at a fixed time step) is calculated, including heading score, distance score, speed score, and braking distance;

[0060] Determine whether the predicted trajectory is feasible based on the braking distance and obstacle information, and store the feasible predicted trajectory and its evaluation score in the database;

[0061] Each evaluation score is regularized to convert its value to the same magnitude for easy comparison. The final score of each predicted trajectory is calculated based on the weight of the evaluation score.

[0062] Specifically, the heading score weight is multiplied by the i-th speed and angular velocity combination to obtain the heading score, the distance score weight is multiplied by the i-th speed and angular velocity combination to obtain the distance score, and the speed score weight is multiplied by the i-th speed to obtain the speed score; these three scores are added together to obtain the final score of the i-th speed and angular velocity. The parameter combination with the highest score in all trajectories is selected as the optimal speed and angular velocity.

[0063] Furthermore, in S3, the improved A * The heuristic function of the algorithm is * The obstacle distance penalty term is introduced based on the algorithm heuristic function, namely:

[0064] f(n)=g(n)+h(n)

[0065] h(n)=euclidean_cost(n)+obstacle_cost(n)

[0066] Where: g(n) represents the actual distance cost from the current node n to the starting node s; h(n) represents the predicted distance cost from the current node n to the target node g; euclidean_cost(n) represents the Euclidean distance heuristic from the current node n to the target node g; obstacle_cost(n) represents the cost generated based on the distance from the current node n to the obstacle.

[0067] For the obstacle(n) item, initialize obstacle_cost(n) to 0. Traverse all obstacles and calculate the Euclidean distance between the current node n and each obstacle. If this distance is less than safety_margin, calculate the additional distance within the safety margin (safety_margin minus the actual distance), multiply it by the penalty factor and add it to obstacle_cost. The value of safety_margin is equivalent to the influence range of the obstacle. The purpose of this is to impose a high penalty on the position close to the obstacle in the cost function, so as to encourage path planning to avoid obstacles.

[0068] Referring to the path planning diagrams shown in Figure 2(a) and Figure 2(b), the latitude and longitude coordinates of the starting point are [121.77, 31.26], and the latitude and longitude coordinates of the target point are [121.95, 31.28]. The coordinates of the rasterized map are [50, 78] for the starting point and [74, 83] for the end point. * The heuristic function of the algorithm is improved. * Adding the obstacle distance cost term to the heuristic function of the algorithm can make it possible to *The algorithm maintains a certain distance from obstacles when performing global path planning from the current position to the target point.

[0069] Specifically, the improved A * The algorithm flow is as follows: (1) Put the starting point in the Openlist; (2) Repeat the following process: First, determine whether the target point has been reached or there is no path. If the end point has been added to the Openlist, the path has been found (the starting point is the target point at this time, and there is no need to find it again), the Openlist is empty, and there is no path; then sort according to the third column (cost function F) in the Openlist, find the node with the smallest F value, and move this node with the smallest F value to the Closelist as the current node. If the 8 adjacent nodes around the current node are unreachable or in the Closelist, ignore it. If it is not in the Openlist, add it to the Openlist and set the current node as its parent node, record the F value of the node, if it is already in the Openlist, check whether it is better to reach there via the current node (judged by G or F value). If it is better, set the current node as its parent node and update the F and G values. If it is not good, do not process it; (3) Save the path.

[0070] Furthermore, in S4, for the improved A * The algorithm returns the path points, extracts the key points of the path as the temporary target points of the improved DWA algorithm, and does not process other path points.

[0071] In this embodiment, when the position coordinates of the current vessel reach the outer expansion circle of the position coordinates of the target point, it is considered that the end point has been reached, and the diameter of the outer expansion circle is set to 1 grid of the rasterized map.

[0072] As shown in Figure 3(a) and Figure 3(b), the red dots represent obstacles, the blue curves represent the ship routes, the latitude and longitude coordinates of the starting point are [121.506833, 31.453333], and the latitude and longitude coordinates of the target point are [121.703333, 31.465500]. The coordinates of the rasterized map are [120, 120] for the starting point and [167.16, 122.9201] for the end point. In complex water environments, only the improved DWA algorithm is used for path planning. When the target point appears directly behind an obstacle, it is easy for the surface ship to repeatedly circle on the same path and be unable to jump out. The main reasons for this phenomenon are: when the target point is behind an obstacle, the DWA algorithm may be limited by the local search range, resulting in the inability to generate an effective path to the target; the DWA algorithm uses a dynamic window to limit the speed range that the robot can choose within a given time interval. When the target point is behind an obstacle, the robot may need to move in the opposite direction to bypass the obstacle and reach the target point, but the speed window may not cover the speed range of the reverse movement, resulting in the inability to generate a suitable speed instruction. However, this embodiment uses A * The key path points returned by the algorithm are set as temporary target points of the DWA algorithm, which can make the current ship jump out of the loop and thus solve the above problem.

[0073] like Figure 4 As shown in the figure, the red dots represent obstacles, the blue curve is the route of the ship, and the yellow curve is the trigger for calling the improved A when the ship enters the safe distance range of the obstacle. * The path planned by the algorithm, the green dot is the target point, and the blue dot is the current ship. At this time, the heading weight coefficient, distance weight coefficient, and speed weight coefficient are respectively: 1, 0.4, and 0.1.

[0074] Furthermore, in S5, the software of the ship intelligent collision avoidance decision system consists of an intelligent recognition model, an intelligent fusion model and an intelligent collision avoidance system, mainly including: navigation data access module, radar information, AIS information, visible light (infrared + high-definition camera) information access module, target recognition module, target solution module, target fusion module, risk assessment module, collision avoidance decision module. The ship intelligent collision avoidance system integrates multiple functions such as CPA solution, CPA alarm, target fusion, target recognition and collision avoidance decision, and comes with small tools for enlarging, reducing and calculating nautical charts and comprehensive situation display functions, so that the nautical charts are integrated with the actual navigation perspective, and the output of collision avoidance decisions can be understood more intuitively and accurately.

[0075] The above improved DWA algorithm and improved A *The algorithm is integrated and imported into the collision avoidance decision module of the ship intelligent collision avoidance decision system, combining rich and multi-source marine environment data and target fusion data to generate the optimal collision avoidance path. The collision avoidance decision takes into account factors such as the ship's navigation performance, marine environment, and traffic rules to ensure the rationality and feasibility of the decision.

[0076] In this embodiment, the ship intelligent collision avoidance decision-making system is mainly divided into three interfaces: a global situation interface, a real-scene perception interface, and a comprehensive situation display.

[0077] like Figure 5 As shown, the real-scene perception interface includes target recognition, route collision avoidance path display, etc. The AIS target list on the left is often displayed in three interfaces. The top shows the ship's speed, heading and collision avoidance algorithm output heading, the upper right corner shows a chart thumbnail, and the right side is the switch between infrared and real-scene output.

[0078] like Figure 6 As shown in the figure, the comprehensive situation integrates the global situation interface and the real-scene perception interface into one scene, making the collision avoidance output more intuitive.

[0079] In summary, in this embodiment, the improvement of the DWA algorithm is mainly reflected in the trajectory evaluation function, A * The improvement of the algorithm is mainly reflected in the heuristic function. By integrating the improved DWA algorithm and the improved A * The algorithm is combined with rich and multi-source marine environment data and target fusion data, and the ship intelligent collision avoidance decision system can generate the optimal collision avoidance path in a complex water environment. The simulation experimental results show that the method proposed in this invention can effectively plan a safe and efficient path, and is superior to the traditional path planning method in terms of performance and efficiency.

[0080] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0081] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A fusion-improved DWA and A based on ship intelligent collision avoidance decision system * The path planning method of the algorithm is characterized by: The following steps are involved: S1. Construct a raster map based on the electronic chart data and the starting and ending points of the path planning to determine the physical information of the surface vessel; S2, according to the starting point and end point of the path planning and the physical information of the surface ship, the improved DWA algorithm is used for path planning to obtain the best combination of speed and angular velocity; S3. When the distance between the surface vessel and the obstacle is less than the safe distance, call the improved A * The algorithm performs global path planning from the current position to the end point and returns the path point; S4, extracting the key points of the path as the temporary end point of the improved DWA algorithm, and continuing the dynamic path planning until the surface vessel reaches the end point; S5, integrate the improved DWA algorithm and A * The algorithm is imported into the ship intelligent collision avoidance decision system, and the optimal obstacle avoidance path is generated by combining the ocean environment data and target fusion data.

2. The path planning method based on the ship intelligent collision avoidance decision system according to claim 1, characterized in that: In S1, the specific steps are as follows: According to the position coordinates of the starting point and the end point, the minimum rectangular area containing the starting point and the end point is calculated; Based on the minimum rectangular area, the area boundary is expanded to construct a rasterized map.

3. The improved DWA and A fusion based on the ship intelligent collision avoidance decision system according to claim 1 * The path planning method of the algorithm is characterized by: In S1, the physical information of the surface vessel includes: maximum speed, maximum rotation angle, maximum acceleration, rotation acceleration, speed resolution, and rotation speed resolution.

4. The improved DWA and A fusion based on the ship intelligent collision avoidance decision system according to claim 1 * The path planning method of the algorithm is characterized by: In S2, the specific steps are as follows: S21, based on the current state and motion model of the surface vessel, determine the speed and angular velocity range at the next moment, generate multiple predicted trajectories in the dynamic window, and score them according to the target point and obstacle information; S22. Based on the scoring results, an optimal combination of speed and angular velocity is selected to guide the surface vessel to move toward the target point.

5. The improved DWA and A fusion based on the ship intelligent collision avoidance decision system according to claim 4 * The path planning method of the algorithm is characterized by: In S21, the specific steps are as follows: Based on multiple predicted trajectories, the evaluation score of the predicted trajectory endpoint is calculated, including the heading score, distance score, speed score and braking distance; Determine whether the predicted trajectory is feasible based on the braking distance and obstacle information, and store the feasible predicted trajectory and its evaluation score in the database; Each evaluation score is regularized, and the final score of each predicted trajectory is calculated based on the weight of the evaluation score.

6. The improved DWA and A fusion based on the ship intelligent collision avoidance decision system according to claim 1 * The path planning method of the algorithm is characterized by: In S3, the improved A * The heuristic function of the algorithm is * The obstacle distance penalty term is introduced based on the algorithm heuristic function, namely: f(n)=g(n)+h(n) h(n)=euclidean_cost(n)+obstacle_cost(n) Where: g(n) represents the actual distance cost from the current node n to the starting node s; h(n) represents the predicted distance cost from the current node n to the target node g; euclidean_cost(n) represents the Euclidean distance heuristic from the current node n to the target node g; obstacle_cost(n) represents the cost generated based on the distance from the current node n to the obstacle.

7. The improved DWA and A fusion based on the ship intelligent collision avoidance decision system according to claim 1 * The path planning method of the algorithm is characterized by:In S4, for the improved A * The algorithm returns the path points, extracts the key points of the path as the temporary target points of the improved DWA algorithm, and does not process other path points.

8. The improved DWA and A fusion based on the ship intelligent collision avoidance decision system according to claim 1 * The path planning method of the algorithm is characterized by: In S5, the software of the ship intelligent collision avoidance decision system consists of an intelligent recognition model, an intelligent fusion model and an intelligent collision avoidance system, and is equipped with small tools for zooming in, out and calculating nautical charts, as well as a comprehensive situation display interface.

Images