Ship collision avoidance path planning method integrating improved satellite A and improved dynamic window algorithm

Abstract

The invention provides a ship collision avoidance path planning method fusing an improved satellite A and an improved dynamic window algorithm, and the method comprises the following steps: S1, adding node evaluation and child node extension optimization in an original satellite A algorithm, and obtaining an improved satellite A algorithm; s2, smoothing the global path by adopting a Floyd algorithm so as to remove redundant inflection points on the global path, reduce the ship steering frequency and improve the continuity of the global path; s3, based on a quaternary ship domain improved speed obstacle method, establishing a ship collision avoidance risk prediction model, and monitoring the collision risk of the ship in the process of sailing along the global path in real time; s4, adding a path yaw evaluation function and a threat cone evaluation function into an original dynamic window algorithm to obtain an improved dynamic window algorithm; and performing local collision avoidance path planning by using the improved dynamic window algorithm. According to the method, ship collision avoidance path planning in a dynamic unknown environment is realized, and ship collision avoidance rules and marine navigation requirements are met.

Description

Technical Field

[0001] The present invention relates to the field of marine ship technology, and in particular to a ship collision avoidance path planning method integrating an improved A-star and an improved dynamic window algorithm. Background Art

[0002] In recent years, with the development and utilization of marine resources and the prosperity of maritime trade, the traffic volume of ships has also increased significantly, making ship navigation more and more difficult, and navigation accidents caused by ship collisions occur from time to time. How to effectively prevent the risk of ship collisions, reduce collision accidents, and ensure navigation safety in limited navigable waters has become a challenge that needs to be solved urgently. At present, ship collision avoidance mainly relies on the experience and judgment of ship drivers. Various navigation facilities such as radar, AIS, electronic charts, etc. can only be used as auxiliary equipment to provide ship drivers with environmental information about the ship and the surroundings when taking collision avoidance strategies. However, with the increase in traffic volume and the increase in ship density, the time left for human decision-making in the process of ship collision avoidance is getting shorter and shorter, and the original ship collision avoidance method has gradually failed to meet the needs. Investigations and studies have found that human judgment errors are one of the main causes of ship collisions. Establishing a ship navigation decision-making assistance and autonomous navigation system is an effective means to solve this problem, and ship collision avoidance path planning is a key link. Therefore, the study of ship collision avoidance path planning algorithms has gradually become a hot topic in the field of ship navigation safety assurance.

[0003] Ship collision avoidance path planning is mainly divided into two important aspects: global path planning, based on static environmental information and known conditions, plans a safe and efficient route for the ship from the starting point to the target point, ensuring that the global path avoids known obstacles; local path planning, in response to unknown obstacles and environmental changes that appear during navigation, timely adjusts the route and performs local collision avoidance to adapt to the dynamic environment. When planning a ship collision avoidance path, it is necessary to combine global planning and local planning to plan a navigation route that takes into account safety, economy, timeliness and other aspects. If only one of the path planning methods is relied on, there may be some shortcomings in actual navigation applications, such as falling into the local optimal solution when planning the path; high computational complexity, long planning time; and the global path alone cannot meet the actual navigation needs of the ship.

[0004] The A* algorithm is widely used in intelligent navigation and other problems due to its efficient computing speed and excellent optimization ability. The dynamic window (DWA) algorithm takes into account the actual dynamic constraints of the ship, converts the constraints into restrictions on linear speed and acceleration, and does not produce overly drastic or unattainable control actions. It can realize local path planning in complex dynamic environments and has good real-time performance.

[0005] However, when solving the problem of ship collision avoidance path planning, the A-star and dynamic window algorithms have the following problems: (1) When planning the global path, the A-star algorithm only considers the node information of obstacles and treats the ship as a particle. However, in reality, ships have a certain volume. When they sail along the planned path, their edges may hit obstacles, causing the ship to run aground or collide. (2) When searching for a path, the A-star algorithm is restricted by the search direction, so the planned path will produce a large number of turning points. In addition, the search, node addition and node deletion operations need to be performed multiple times during the search, resulting in low efficiency in ship path planning. (3) The global path planned by the A-star algorithm has many turning points, which unnecessarily increases the ship's sailing distance, and the poor smoothness does not meet the actual navigation requirements. The ship cannot turn continuously and smoothly. (4) Due to the lack of local goal guidance, the dynamic window (DWA) algorithm will produce unnecessary detours when taking collision avoidance actions, fall into the local optimal solution, and cause a large deviation from the original global planned path. (5) The dynamic window (DWA) algorithm is too close to other ships or obstacles when performing ship collision avoidance operations. Summary of the invention

[0006] In view of this, the purpose of the present invention is to propose a ship collision avoidance path planning method that integrates the improved A-star and the improved dynamic window algorithm, so as to solve the problems of the prior art that the global path is too close to obstacles and there are too many redundant turning points, and improve the efficiency of global path planning. At the same time, the method solves the problem that the distance between the ship and other ships or obstacles during the collision avoidance operation in the local path is too close and the path deviates after the collision avoidance, and can perform safe avoidance operations according to the different encounter situations faced by the ship. In this way, the method of the present invention realizes the ship collision avoidance path planning in a dynamic unknown environment, which meets the requirements of ship collision avoidance rules and maritime navigation.

[0007] The technical means adopted by the present invention are as follows:

[0008] A ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm comprises the following steps:

[0009] S1. Add node evaluation and sub-node expansion optimization to the original A-star algorithm to obtain an improved A-star algorithm, thereby planning a global path from the starting position to the target position;

[0010] S2. Use the Floyd algorithm to smooth the global path to remove redundant turning points on the global path, reduce the number of ship turns, and improve the continuity of the global path;

[0011] S3. Based on the improved speed obstacle method in the quaternary ship field, a ship collision avoidance risk prediction model is established to monitor the collision risk of ships in real time during navigation along the global path;

[0012] S4. Add path deviation evaluation function and threat cone evaluation function to the original dynamic window algorithm to obtain an improved dynamic window algorithm. When the collision avoidance hazard prediction model detects that the ship is at risk of collision, the improved dynamic window algorithm is used to perform local collision avoidance path planning based on the global path.

[0013] Furthermore, in S1, the node evaluation specifically includes the following steps:

[0014] S11, adding obstacle distance cost function to evaluate the nodes, so that the ship and obstacles in the global path maintain a safe distance to avoid collision risk;

[0015] The calculation formula of the heuristic function of the A-star algorithm after adding the obstacle distance cost function is as follows:

[0016] F(n)=G(n)+H(n)+w*I(n)

[0017] Where: F(n) is the evaluation cost from the starting point to the target point; G(n) is the actual cost from the starting point to the current node n; H(n) is the estimated cost from the current node n to the target point; w is the obstacle weight factor, the larger w is, the greater the impact of the obstacle on the heuristic function; I(n) is the obstacle distance cost function;

[0018]

[0019] Where: D is the distance between the current node n and the center of the obstacle; R is the safety distance radius; C(n) is the relationship between the distance between the ship and the obstacle and the safety distance radius.

[0020] Furthermore, in S1, the subnode expansion optimization specifically includes the following steps:

[0021] S12. Use the pruning principle to optimize the expansion of child nodes and reduce some nodes that do not need to be expanded;

[0022] According to the relative position of the target node relative to the current node, the relative direction angle is calculated, and the nodes are selectively expanded, only the area toward the target node is expanded, and the remaining invalid nodes are pruned;

[0023] The formula for calculating the cosine value of the relative direction angle between the target node and the current node is as follows:

[0024]

[0025] Where: x n is the horizontal coordinate of the current node; y n is the ordinate of the current node; x g is the horizontal coordinate of the target node; g is the ordinate of the target node; α is the relative direction angle.

[0026] Furthermore, in S3, the boundary equation calculation formula of the quaternary ship field is as follows:

[0027] QSD k ={(x,y)|f k (x,y:Q)≤1,Q={R fore ,R aft ,R port ,R starb},k≥1}

[0028]

[0029] Where: k determines the shape of the ship area. The larger the k value, the closer the shape of the ship area is to a rectangle. fore , R aft , R port , R starb They represent the distances of the ship in the four directions of the bow, stern, left and right, respectively. The calculation formula is as follows:

[0030]

[0031] Where: k AD k is the coefficient of ship advance; DT A is the coefficient of the initial turning diameter of the ship; D D is the ship's advance distance; T is the initial turning diameter of the ship; V is the sailing speed of the ship; L is the length of the ship.

[0032] Furthermore, in S3, the improved speed barrier method specifically includes: combining the characteristics of the four-dimensional ship field, analyzing the ship's speed triangle, calculating the horizontal and vertical components of the relative speed vector, setting the corresponding safe navigation angle, and obtaining the speed area that may cause a collision, namely, the collision threat cone; adding an adaptive expansion radius to the periphery of the ship field to increase the safety of ship collision avoidance, thereby establishing a ship collision avoidance risk prediction model to predict whether there is a collision risk between two ships, and realizing real-time monitoring of collision risks during the navigation of ships along the global path.

[0033] Furthermore, in S4, the calculation formula of the path deviation evaluation function is as follows:

[0034] path(v,ω)=exp(-d)

[0035]

[0036] Where: (v, ω) is the velocity set; the coordinates of the predicted trajectory point at the current moment are (x t ,y t); the node coordinates on the original planned path are (x p ,y p );d represents the distance between the predicted trajectory point and the path at the current moment. The smaller the distance d is, the larger the value of the evaluation function is, and the better the planned local collision avoidance path is.

[0037] Furthermore, in S4, the threat cone evaluation function calculation formula is as follows:

[0038] avo(v,ω)=90°-σ

[0039] Where: σ is the velocity vector direction of each point on the ship's predicted trajectory and the ship's velocity V after adaptive expansion o' The angle between the vector directions; the smaller the angle, the higher the evaluation value and the better the comprehensive collision avoidance effect of the ship.

[0040] The present invention also provides a storage medium, which includes a stored program, wherein when the program is run, any of the above-mentioned ship collision avoidance path planning methods integrating the improved A-star and the improved dynamic window algorithm is executed.

[0041] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes any one of the above-mentioned ship collision avoidance path planning methods integrating the improved A-star and the improved dynamic window algorithm through the computer program.

[0042] Compared with the prior art, the present invention has the following advantages:

[0043] The method of the present invention adds an obstacle distance cost function to evaluate the nodes, ensuring that a safe distance is maintained between the ship and the obstacle to avoid the risk of collision, thereby solving the problem that the ship may be too close to the obstacle during global path planning in the A-star algorithm.

[0044] The method of the present invention optimizes the expansion of sub-nodes by utilizing the pruning principle, reduces some nodes that do not need to be expanded, and improves the planning efficiency of the A-star algorithm.

[0045] According to the actual navigation situation, the method of the present invention uses the Floyd algorithm to perform multiple iterative optimizations on the path, effectively removing the redundant turning points on the path planned by the A-star algorithm, thereby being more in line with the maneuvering characteristics of the ship and shortening the path length.

[0046] The method of the present invention adds a path deviation evaluation function, solves the problem that the path planned by the dynamic window algorithm may detour due to avoiding other ships and fall into a local optimal solution, thereby reducing the possibility of the ship detouring.

[0047] The method of the present invention adds a threat cone evaluation function to solve the problem that the dynamic window algorithm is too close to other ships or obstacles during the ship collision avoidance operation.

[0048] The method of the present invention can monitor the collision risk of a ship in real time during navigation, and effectively realize the ship's collision avoidance path planning in a dynamic unknown environment according to different encounter situations faced, and meets the requirements of ship navigation at sea. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] 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 some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0050] Figure 1 This is a flow chart of the ship collision avoidance path planning method of the present invention.

[0051] Figure 2 Schematic diagram of the sub-node expansion pruning strategy of the present invention.

[0052] Figure 3 This is a schematic diagram of the path optimization of the Floyd algorithm of the present invention.

[0053] Figure 4 It is a schematic diagram of the four-element ship field model of the present invention.

[0054] Figure 5 It is a schematic diagram of the improved speed obstacle method based on the quaternary ship field of the present invention.

[0055] Figure 6 It is a schematic diagram of the adaptive expansion radius of the present invention.

[0056] Figure 7 It is a schematic diagram of the effective velocity vector interval of the present invention, wherein Figure 7 a is the navigable speed set diagram, Figure 7 b is the velocity vector screening diagram.

[0057] Figure 8 This is a schematic diagram of dynamic window sampling of the present invention.

[0058] Fig. 9 Schematic diagram of path deviation of the present invention.

[0059] Fig.10 It is a schematic diagram of the threat cone evaluation function of the present invention.

[0060] Fig.11 This is an overall framework diagram of the ship collision avoidance path planning method of the present invention.

[0061] Fig.12 This is the flow chart of the improved A-star algorithm of the present invention.

[0062] Fig.13 The present invention improves the dynamic window algorithm flow chart.

[0063] Fig.14 is a simulation environment map according to an embodiment of the present invention, wherein Fig.14 a is the study site map, where Fig.14 b is a rasterized nautical chart.

[0064] Fig.15 This is a comparison diagram of the global planning paths according to an embodiment of the present invention.

[0065] Fig.16 FIG. 1 is a schematic diagram of a ship collision avoidance path planning simulation according to an embodiment of the present invention, wherein Fig.16 a is a schematic diagram of the rasterized chart ship collision avoidance simulation. Fig.16 b is a schematic diagram of ship collision avoidance simulation based on actual nautical chart. DETAILED DESCRIPTION

[0066] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings 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 should fall within the scope of protection of the present invention.

[0067] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0068] like Figure 1 As shown, the present invention provides a ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm, comprising the following steps:

[0069] S1. According to the safety and efficiency requirements of ship collision avoidance path planning, the A-star algorithm is improved to achieve global path planning from the starting position to the target position.

[0070] When planning the global path, the A-star algorithm only considers the node information of the obstacle. However, since the ship is not a particle and has a certain volume in reality, when it is too close to the obstacle or obliquely passes through the vertex of the obstacle, its edge may hit the obstacle, causing the ship to run aground or collide, which lacks practicality.

[0071] Therefore, in order to prevent the ship from colliding with obstacles in the shortest path, the method of the present invention improves and optimizes the heuristic function of the A-star algorithm, adds the influence term of nearby obstacles, that is, introduces the obstacle distance cost function, and assigns a certain weighting coefficient. The improved heuristic function expression is:

[0072] F(n)=G(n)+H(n)+w*I(n)

[0073] Among them, F(n) is the evaluation cost from the starting point to the target point; G(n) is the actual cost from the starting point to the current node; H(n) is the estimated cost from the current node to the target point; w is the obstacle weight factor, the larger w is, the greater the impact of obstacles on the heuristic function will be; I(n) is the obstacle distance cost function.

[0074] Considering that in the actual environment, the obstacle resistance of obstacles to ship navigation is not equivalent with the distance, a safe distance radius is introduced around the ship. During the search and expansion process, the obstacle cost value is calculated in real time according to the distance between the current node and the obstacle node. The relationship between the distance between the ship and the obstacle and the safe distance radius is shown in the following formula:

[0075]

[0076] Where D is the distance between the current node and the obstacle center; R is the safe distance radius. Therefore, the obstacle distance cost D(n) is the sum of the cost values ​​between the ship and all obstacles within the safe distance radius R during navigation, as shown in the following formula:

[0077]

[0078] When searching for and expanding subnodes around, the A-star algorithm does not differentiate between these candidate nodes, and expands them one by one each time, but this often results in certain invalid searches. Therefore, the method of the present invention adopts a subnode expansion pruning strategy to improve the efficiency of global path search, that is, according to the relative position of the target node relative to the current node, the node is selectively expanded through the relative direction angle, and only the area toward the target node is expanded, while the remaining invalid nodes are pruned to ensure that the search is focused on the path that is most likely to lead to the target.

[0079] The formula for calculating the cosine value of the relative direction angle between the target node and the current node is as follows:

[0080]

[0081] Among them, x n is the horizontal coordinate of the current node; y n is the ordinate of the current node; x g is the horizontal coordinate of the target node; g is the ordinate of the target node; α is the relative direction angle.

[0082] like Figure 2 As shown, the eight adjacent nodes around the current node are marked as A, B, C, D, E, F, G, and H, and divided into eight regions according to the relative direction angles. The specific pruning strategy is as follows:

[0083] (1) When 0°≤α≤22.5° or 337.5°<α<360°, prune nodes A, D, and F, and retain nodes B, C, E, G, and H;

[0084] (2) When 22.5°<α≤67.5°, prune nodes D, F, and G, and retain nodes A, B, C, E, and H;

[0085] (3) When 67.5°<α≤112.5°, prune nodes F, G, and H, and retain nodes A, B, C, D, and E;

[0086] (4) When 112.5°<α≤157.5°, prune nodes E, G, and H, and retain nodes A, B, C, D, and F;

[0087] (5) When 157.5°<α≤202.5°, prune nodes C, E, and H, and retain nodes A, B, D, F, and G;

[0088] (6) When 202.5°<α≤247.5°, prune nodes B, C, and E, and retain nodes A, D, F, G, and H;

[0089] (7) When 247.5°<α≤292.5°, prune nodes A, B, and C, and retain nodes D, E, F, G, and H;

[0090] (8) When 292.5° < α ≤ 337.5°, prune nodes A, B, and D, and retain nodes C, E, F, G, and H.

[0091] S2. Use the Floyd algorithm to smooth the global path to better conform to the maneuvering characteristics of the ship.

[0092] Due to the limitation of the search direction of the A* algorithm, there are many redundant turning points in the planned global path, resulting in poor path smoothness. At the same time, due to the poor turning flexibility of the ship, this kind of path does not conform to the navigation characteristics of the ship. Therefore, it is necessary to process the redundant turning points to reduce the number of ship heading changes and ensure the navigation safety of the ship.

[0093] The Floyd algorithm is a method based on the idea of dynamic programming to solve the shortest path between vertices in a given weighted graph, and it can obtain the shortest distance between any two points.

[0094] As Figure 3 shown, the path planned by the A* algorithm is S → n1 → n2 → n3 → T. First, it is necessary to remove the redundant nodes in the path, and the safety distance threshold from the obstacle is D. Starting from the starting point S, judge whether there is an obstacle between each path node in turn. If there is no obstacle, judge whether the vertical distance d between the obstacle and the path connecting the two nodes is greater than the safety distance threshold D. If d ≥ D, then delete the parent node of the node; if there is an obstacle or d < D, then regard this node as a key node, and continue to judge whether there is an obstacle between this node and each subsequent path node in turn until the target point T. Therefore, delete other redundant nodes in the red path except the starting point S, the target point T, and the key inflection points. The optimized path is: S → n3 → T.

[0095] Set the step size as k, and divide the path after the first optimization into countless segments according to the step size k. During the optimization process, starting from the starting point S, judge whether there is an obstacle between each break point in turn. When the distance d between the obstacle and the path break point is d ≥ D, then this break point meets the requirements. Find the break point of the final critical value where d ≥ D, and regard this break point as a key node. Repeat the iterative optimization many times, and finally the optimized path is: S → n4 → T.

[0096] S3. Based on the improved speed obstacle method of the four-element ship domain, establish a ship collision risk prediction model to monitor the collision risk in real time during the ship's navigation along the global path.

[0097] In actual navigation, the safety range around a ship is not a circular area, and the traditional speed barrier method that only considers the circular safety area is not suitable for establishing a ship collision avoidance model. Therefore, the method of the present invention is based on the actual situation of navigation. The bow of a ship is more dangerous than the stern, the starboard side is more dangerous than the port side, and the most dangerous area is mainly concentrated in the right front of the ship. The four-element ship area that is approximately elliptical is used as the safety area of ​​the ship, such as Figure 4 shown.

[0098] The boundary equation calculation formula of the quaternary ship field is:

[0099] QSD k ={(x,y)|f k (x,y:Q)≤1,Q={R fore ,R aft ,R port ,R starb},k≥1},

[0100]

[0101] in: k determines the shape of the ship area. The larger the k value, the closer the shape of the ship area is to a rectangle. fore , R aft , R port , R starb They represent the distances of the ship in the four directions of bow, stern, left and right, respectively. They need to be calculated and analyzed according to the actual water environment and needs. The calculation formula is:

[0102]

[0103] in: k AD k is the coefficient of ship advance; DT A is the coefficient of the initial turning diameter of the ship; D D is the ship's advance distance; T is the initial turning diameter of the ship; V is the sailing speed of the ship; L is the length of the ship.

[0104] Combined with the characteristics of the quaternary ship field, the speed obstacle method is improved. Figure 5 By analyzing the ship's velocity triangle, the horizontal and vertical components of the relative velocity vector are obtained, and the calculation formula is:

[0105]

[0106] Among them, γ is the azimuth between the own ship and the other ship; α is the angle between the own ship's heading and the positive direction of the x-axis; β is the angle between the other ship's heading and the positive direction of the x-axis; v o v is the ship speed;t For his ship's speed; S v is the velocity component of the relative velocity vector between the two ships in the horizontal direction of the line OT connecting the two ships; θ is the velocity component of the relative velocity vector between the two ships in the direction perpendicular to the line OT connecting the two ships. From this, we can get the velocity vector difference v ot The angle θ with the line OT connecting the two ships:

[0107]

[0108] At the same time, let the safe navigation angle of the ship be μ: Where D is the distance between the ship and the other ship; Q is the distance between the boundary of the ship domain and the intersection of the tangent and the other ship. Let k be 2, and substitute it into the boundary equation of the quaternary ship domain. The intersection points of the boundary of the ship domain and the tangent (x1, y1), (x2, y2) are solved as follows:

[0109]

[0110] Since the quaternion ship domain is irregular, the distances between the boundary of the ship domain and the two intersection points of the tangent line and other ships are as follows:

[0111]

[0112] The safe navigation angle μ will also get two results μ1 and μ2, thereby obtaining the speed area that may cause collision, namely the collision threat cone (VO). If (γ+θ)∈(γ-μ1,γ+μ2), there is a risk of collision between the two ships.

[0113] When there is a danger of collision between two ships, changing the speed or heading can make the relative velocity vector move out of the collision threat cone (VO), thus avoiding a collision. If the angle between the relative velocity vector of the ship and the boundary of the collision threat cone is larger, the distance between the ship and the other ship will be greater. Although safety can be guaranteed, the corresponding collision avoidance time will be longer, and the collision avoidance amplitude will be too large, and the real-time performance will be poor. If the angle between the relative velocity vector of the ship and the boundary of the collision threat cone is smaller, the ship will maintain a speed and heading that is tangent to the ship field of the other ship. At this time, if the speed of the other ship changes, a collision may still occur. In order to take into account both the real-time and safety of the ship in the process of collision avoidance, the method of the present invention adopts the speed barrier method of adaptive expansion radius, and adds an expansion radius to the periphery of the ship field. The collision threat cone formed at this time is called SD SA (self-adaptionship domain), such as Figure 6 shown.

[0114] The adaptive speed barrier method dynamically adjusts the size of the collision threat cone (VO) according to the relative speed and relative distance between the own ship and the other ship. When the distance between the own ship and the other ship is farther, the risk of collision is lower. At this time, the expansion radius should be smaller, so that the speed deviation of the ship is small and the driving trajectory is shorter. When the distance is closer, the collision threat is higher, and more attention should be paid to safety. At this time, the expansion radius should be larger to maintain a certain safe distance from the other ship. It is found that the size of the expansion radius is proportional to the relative speed between the two ships and inversely proportional to the shortest distance between the two ships. The adaptive expansion radius r is shown in the following formula:

[0115]

[0116] Among them, α is the adaptive gain; d is the relative distance between the own ship and other ships; V OT is the relative speed between own ship and other ships; is the relative speed V OT The angle between the relative distance d (rad); is the relative speed V OT Projection at the relative distance d. To ensure safety, when the relative distance d becomes smaller, the adaptive expansion radius increases exponentially. Let the denominator be in exponential form. β is used to adjust the denominator. Thus, an adaptive ship collision avoidance risk prediction model is established to predict whether there is a collision risk between two ships and to achieve real-time monitoring of collision risks during the navigation of ships along the global path.

[0117] S4. Improve the dynamic window algorithm and perform local collision avoidance path planning based on the global path, so that the collision avoidance path is more in line with maritime collision avoidance rules and navigation requirements, thereby effectively realizing ship collision avoidance path planning in dynamic unknown environments.

[0118] Based on the improved speed obstacle method in the quaternary ship field, the speed set of the ship that will collide with other ships can be obtained. By calculation, the speed V of the ship in the time interval Δt can be obtained. d and heading θ d However, the elements in this set are continuous, which is not conducive to the prediction trajectory and calculation. d and θ d Discretize it and set a suitable resolution to convert the speed set V d Discretized into M speeds, heading set θ d Discretized into N angles, we can get M×N velocity vectors (V i ,θ i ) is a set of reachable speeds, set to RV (reachable velocity), such as Figure 7(a) As shown in the figure. In the RV set, it is also necessary to screen it to determine whether there is a velocity vector in the VO and remove it, as shown in Figure 7 (b) As shown in the figure, the reachable avoidance velocity vector set RAV (reachable avoidance velocity) after screening is:

[0119]

[0120] By discretizing the velocity vector space in the dynamic window, the predicted trajectory can be represented as a circular arc trajectory formed by the velocity set (v, ω). Figure 8 shown.

[0121] Due to the lack of local target guidance, the planned path of the dynamic window (DWA) algorithm may produce unnecessary detours when avoiding other ships due to complex environments, and fall into the local optimal solution, resulting in excessive deviation from the original global planned path. Therefore, the method of the present invention adds a path deviation evaluation function to reduce the possibility of ship detours, and at the same time, it can return to the original route in time after completing the collision avoidance behavior. By improving the A-star algorithm, a global path composed of local sub-target points can be obtained, such as Fig. 9 As shown. The calculation method of path deviation evaluation function path(v,ω) is:

[0122]

[0123] Among them, (v, ω) is the velocity set; the coordinates of the predicted trajectory point at the current moment are (x t ,y t ); the node coordinates on the path are (x p ,y p );d is the distance between the predicted trajectory point and the path at the current moment. The smaller the distance d is, the larger the value of the evaluation function is, and the better the planned path is.

[0124] If the angle between the relative velocity vector and the collision threat cone boundary is larger, the distance from the other ship will be greater. Although safety can be guaranteed, the corresponding collision avoidance time will be longer, and the collision avoidance amplitude will be too large, which will deteriorate the real-time performance. In order to take into account the real-time performance and safety of the ship during collision avoidance, the method of the present invention adds a threat cone evaluation function, and the calculation formula is as follows: avo(v,ω)=90°-σ, where σ is the velocity vector direction of each point on the ship's predicted trajectory and the ship's own speed V after adaptive expansion. o' The smaller the angle between the vector directions, the higher the evaluation value and the better the comprehensive collision avoidance effect of the ship. Fig.10 shown.

[0125] The overall framework of the ship collision avoidance path planning method constructed by the method of the present invention is as follows: Fig.11 shown.

[0126] Improved A-star algorithm flow chart as follows Fig.12 shown.

[0127] The flow chart of the improved dynamic window (DWA) algorithm is as follows: Fig.13 shown.

[0128] Example

[0129] In order to verify the effectiveness of the proposed method in improving the global and local path planning algorithms, the sea area near Praslin Island was selected as the research site, and the nautical chart range was 20n mile × 20n mile. Fig.14 (a) In order to facilitate calculation and ship path planning, the actual nautical chart is gridded into 100×100 grids, and the length of each grid is 0.2n mile. The gridded nautical chart is shown in Fig.14 (b) as shown.

[0130] The improved A-star algorithm is used to plan a global path without collision risk and in line with the ship's maneuvering characteristics, such as Fig.15 As shown. In the figure, the red line path1 is the path planned by the A-star algorithm; the purple line path2 is the path planned by adding the obstacle distance cost function; the green line path3 is the path optimized for the first time using the Floyd algorithm; the blue line path4 is the path optimized three times using the Floyd algorithm. Local sub-target points are selected from its node sequence, and according to the different encounter situations faced, the improved dynamic window algorithm is used to plan the collision avoidance path for the local sub-target points, thereby effectively realizing the ship collision avoidance path planning in a dynamic unknown environment. As shown Fig.16 As shown, it is a schematic diagram of the simulation effect of the ship collision avoidance path planning method using the integrated improved A-star and improved dynamic window algorithms of the present application.

[0131] The route obtained by the ship collision avoidance path planning method in this application can quickly plan a safe global path, and during the navigation of the ship, according to the different encounter situations faced, timely local path corrections are made to avoid dynamic ships so that the ship can safely reach the target location. The integration of the global path planning of the improved A-star algorithm and the local path planning of the improved dynamic window algorithm can effectively realize the ship collision avoidance path planning in a dynamic unknown environment, and comply with the collision avoidance rules and the requirements of ship navigation at sea.

[0132] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm, characterized in that: The steps include: S1. Add node evaluation and sub-node expansion optimization to the original A-star algorithm to obtain an improved A-star algorithm, thereby planning a global path from the starting position to the target position; S2. Use the Floyd algorithm to smooth the global path to remove redundant turning points on the global path, reduce the number of ship turns, and improve the continuity of the global path; S3. Based on the improved speed obstacle method in the quaternary ship field, a ship collision avoidance risk prediction model is established to monitor the collision risk of ships in real time during navigation along the global path; S4. Add path deviation evaluation function and threat cone evaluation function to the original dynamic window algorithm to obtain an improved dynamic window algorithm. When the collision avoidance hazard prediction model detects that the ship is at risk of collision, the improved dynamic window algorithm is used to perform local collision avoidance path planning based on the global path.

2. The ship collision avoidance path planning method integrating the improved A-star and the improved dynamic window algorithm according to claim 1 is characterized in that: In S1, node evaluation specifically includes the following steps: S11, adding obstacle distance cost function to evaluate the nodes, so that the ship and obstacles in the global path maintain a safe distance to avoid collision risk; The calculation formula of the heuristic function of the A-star algorithm after adding the obstacle distance cost function is as follows: F(n)=G(n)+H(n)+w*I(n) Where: F(n) is the evaluation cost from the starting point to the target point; G(n) is the actual cost from the starting point to the current node n; H(n) is the estimated cost from the current node n to the target point; w is the obstacle weight factor. The larger w is, the greater the impact of the obstacle on the heuristic function. I(n) is the obstacle distance cost function. Where: D is the distance between the current node n and the center of the obstacle; R is the safety distance radius; C(n) is the relationship between the distance between the ship and the obstacle and the safety distance radius.

3. The ship collision avoidance path planning method integrating the improved A-star and the improved dynamic window algorithm according to claim 1 is characterized in that: In S1, subnode expansion optimization specifically includes the following steps: S12. Use the pruning principle to optimize the expansion of child nodes and reduce some nodes that do not need to be expanded; According to the relative position of the target node relative to the current node, the relative direction angle is calculated, and the nodes are selectively expanded, only the area toward the target node is expanded, and the remaining invalid nodes are pruned; The formula for calculating the cosine value of the relative direction angle between the target node and the current node is as follows: Where: x n is the horizontal coordinate of the current node; y n is the ordinate of the current node; x g is the horizontal coordinate of the target node; g is the ordinate of the target node; α is the relative direction angle.

4. The ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm according to claim 1 is characterized in that: In S3, the boundary equation calculation formula of the quaternary ship field is as follows: QSD k ={(x,y)|f k (x,y:Q)≤1,Q={R fore ,R aft ,R port ,R starb },k≥1} Where: k determines the shape of the ship area. The larger the k value, the closer the shape of the ship area is to a rectangle. fore , R aft , R port , R starb They represent the distances of the ship in the four directions of the bow, stern, left and right, respectively. The calculation formula is as follows: Where: k AD k is the coefficient of ship advance; DT A is the coefficient of the initial turning diameter of the ship; D D is the ship's advance distance; T is the initial turning diameter of the ship; V is the sailing speed of the ship; L is the length of the ship.

5. The ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm according to claim 1 is characterized in that: In S3, the improved speed barrier method specifically includes: combining the characteristics of the four-dimensional ship field, analyzing the ship's speed triangle, calculating the horizontal and vertical components of the relative speed vector, setting the corresponding safe navigation angle, and obtaining the speed area that may cause a collision, namely, the collision threat cone; adding an adaptive expansion radius to the periphery of the ship field to increase the safety of ship collision avoidance, thereby establishing a ship collision avoidance risk prediction model to predict whether there is a collision risk between two ships, and realizing real-time monitoring of collision risks during the navigation of ships along the global path.

6. The ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm according to claim 1 is characterized in that: In S4, the calculation formula of the path deviation evaluation function is as follows: path(v,ω)=exp(-d) Where: (v, ω) is the velocity set; the coordinates of the predicted trajectory point at the current moment are (x t ,y t ); the node coordinates on the original planned path are (x p ,y p );d represents the distance between the predicted trajectory point and the path at the current moment. The smaller the distance d is, the larger the value of the evaluation function is, and the better the planned local collision avoidance path is.

7. The ship collision avoidance path planning method integrating improved A-star and improved dynamic window algorithm according to claim 1 is characterized in that: In S4, the threat cone evaluation function calculation formula is as follows: avo(v,ω)=90°-σ Where: σ is the velocity vector direction of each point on the ship's predicted trajectory and the ship's velocity V after adaptive expansion o' The angle between the vector directions; the smaller the angle, the higher the evaluation value and the better the comprehensive collision avoidance effect of the ship.

8. A storage medium, characterized in that: The storage medium includes a stored program, wherein when the program is run, the ship collision avoidance path planning method integrating the improved A-star and the improved dynamic window algorithm as described in any one of claims 1 to 7 is executed.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: The processor executes the ship collision avoidance path planning method integrating the improved A-star and improved dynamic window algorithms as described in any one of claims 1 to 7 through the computer program.

Images