A path planning method for unmanned ship based on improved A-star algorithm

Abstract

The present invention discloses an unmanned ship path planning method based on an improved A-star algorithm. According to the location information of the island and its shoal area, the obstacles are expanded and processed by image processing technology, and corner points, i.e. key points, are extracted, and used as potential child nodes to participate in the path planning, and potential child nodes that the current node can directly reach in a straight line and meet the safety distance are screened out, and defined as child nodes of the current node. This method can not only effectively avoid collisions caused by unmanned ships passing through high-risk areas of islands, but also reduce the number of redundant points in the path and optimize the path smoothness; at the same time, the path cost is also reduced to a certain extent.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ship path planning, and more specifically, to an unmanned ship path planning method based on an improved A-star algorithm. Background Art

[0002] At present, collision and grounding are the main problems faced by unmanned ships. In the actual navigation of unmanned ships, due to the influence of tides, the closer the hull is to the island, the higher the risk of collision and grounding of the unmanned ship. Therefore, the path of the unmanned ship should maintain a certain safe distance from the island to avoid passing through high-risk areas. In addition to considering the influence of tides, the division of dangerous areas also needs to consider the draft of the unmanned ship and retain a certain excess depth (UKC) to ensure the maneuverability of the unmanned ship and prevent accidents caused by the hull touching the bottom. Therefore, optimizing the safety of the planned path is of great significance to avoiding collisions and groundings of unmanned ships.

[0003] The automatic navigation technology of unmanned ships has broad prospects in the marine environment, and the path planning algorithm is a research hotspot in this technical field. In order to adapt to the actual marine environment, researchers have developed various path planning technologies. These path planning technologies cover a variety of strategies, such as grid-based planning, random sampling-based planning, model-based planning, and machine learning technology. Among them, the strategy based on grid planning path has been widely used in the field of path planning for unmanned ships because of its relatively simple calculation and better results. Among them, the most commonly used algorithms include Algorithm, Dijkstra algorithm, ant colony algorithm, etc. Figure 1 As shown in Figure 2, due to the simple calculation process and outstanding path planning performance, The algorithm has been widely used in the field of path planning, but its planned path has many shortcomings, such as too many redundant points and inflection points, low path smoothness, and long planning search time. In order to solve the above problems, Yu et al. The weight coefficient is added to the cost function of the algorithm to reduce the search frequency and improve the search efficiency, but there is still a lot of room for optimization in the number of inflection points and path smoothness; Zhang et al. used the grid method to model the spatial environment. The search direction of the algorithm is expanded to 12 to increase the search direction and sub-nodes and shorten the path, but there is a problem of crossing obstacles; Wang et al. used the jump point search strategy to optimize The algorithm significantly reduces the search time, but its planned path is usually slower than traditional The algorithm is longer; Tang et al. filtered out the nodes in the closed list from a geometric perspective to eliminate nodes that do not meet the requirements and avoid generating irregular paths, but the path cost still has room for optimization; Singh used a bidirectional search strategy to screen potential trajectories with fewer turning points to plan a short and smooth path, but it has the disadvantages of too much calculation, too long planning time and low efficiency.

[0004] Based on the analysis and experiments of the above research literature, the inventors found that if the unmanned ship sails along the path planned by the above method, there will still be problems of collision and grounding. The algorithm usually uses a grid map to plan the path, and the child nodes are selected from the neighboring points in the eight-connected domain. This causes the unmanned ship's navigation path to be too close to the island, that is, the unmanned ship will pass through the high-risk area of ​​the island, which undoubtedly increases the risk of collision and grounding of the unmanned ship.

[0005] Therefore, it is necessary to propose a new solution to solve the above problems. Summary of the invention

[0006] The purpose of the present invention is to overcome the deficiencies of the prior art and provide an unmanned ship path planning method based on an improved A-star algorithm.

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

[0008] A method for unmanned ship path planning based on an improved A-star algorithm comprises the following steps:

[0009] Step 1: Create an island map based on the information of the island and its shoal area, and convert it into a raster map. The raster map includes obstacle areas that the unmanned boat cannot pass through and passable areas;

[0010] Step 2: Expand the obstacles in the grid map outward by one grid, extract the corner points of the expanded area, and define them as key points;

[0011] Step 3, redefine the child nodes of the A-star algorithm, take all key points as potential child nodes of the current node to participate in the path search, then filter out the potential child nodes that the current node can reach directly in a straight line and meet the safety distance, and define them as the child nodes of the current node;

[0012] Step 4: Define the minimum distance of the connection prediction path of the first n segments The reciprocal sum of is the path hazard value , the path hazard value Introduced into the cost function value of the A-star algorithm;

[0013] Step 5: Use the cost function to evaluate the total cost of each child node , choose the total cost The smallest node is taken as the parent node; in each loop, the existence of the child nodes is checked, and it is determined whether the target point appears. If the target point appears, the path finding is successful, the loop ends, and the path is backtracked; if there are no more child nodes to choose from and the target point still does not appear, the path finding fails and the loop ends.

[0014] Furthermore, in step 2, the extraction of key points includes: Figure 2 The obstacle grid is assigned a value of 1 and the rest are 0, and the image matrix A to be expanded is obtained; through image processing technology, the two 3×3 structural elements in formula (3) are used and Dilate the image, using Expand the obstacle outward by one grid. Extract the corner points of the obstacle expansion area and build a key point list;

[0015] (3)

[0016] In formula (3), To expand the obstacle in 8 directions by one grid; To expand the obstacle in four directions, upward, downward, left and right, by one grid; For expansion processing operations; is the binary map matrix after the obstacle is expanded; It is a binary map matrix that only retains the corner points of the dilated area.

[0017] Furthermore, in step 3, the selection of child nodes includes: if the obstacle center, current node and potential child node are not collinear, the distance d from the obstacle center to the potential path is calculated using the Heron triangle area formula and height formula of formula (4); if the three points are collinear, the distance is 0; if all the distances If both are greater than or equal to the safety distance, it means that the current point can directly reach the potential child node, and the node is defined as the neighbor point of the current point, that is, the child node; otherwise, the potential path is prone to collision with obstacles;

[0018] (4)

[0019] In formula (4), AB, BC, and AC are the three side lengths of the triangle formed by the current node, potential child node, and the center of the interval obstacle; is half the perimeter of the triangle; is the area of ​​the triangle; is the distance from the center of the interval obstacle to the current node and the potential child node.

[0020] Further, step 4 includes: using formula (5), from the set Filter out the minimum distance between intervals , the minimum distance between intervals The reciprocal sum of is defined as the path hazard value , and introduce it into the cost function value of the A-star algorithm;

[0021] (5)

[0022] In formula (5), To predict the number of obstacles in the path interval; is the set of distances between the obstacle center and the predicted path; , Respectively before Duanheqian The sum of the reciprocals of the minimum distance values ​​of the segment paths; is the weight coefficient of the path hazard value.

[0023] The beneficial effects of the present invention are:

[0024] 1. Based on the location information of the island and its shoal area (obstacle area), the present invention uses image processing technology to expand the obstacles and extract corner points, i.e. key points, and uses them as potential child nodes to participate in path planning. Potential child nodes that can be directly reached by the current node in a straight line and meet the safety distance are screened out, and defined as child nodes of the current node. This method can effectively reduce the number of nodes that need to be traversed, improve path search efficiency, and shorten path length.

[0025] 2. In the present invention, the minimum distance of the connection prediction path of the first n segments is defined as The reciprocal sum of is the path hazard value , path hazard value The larger the value, the higher the risk of collision and grounding of the unmanned ship; To judge the collision risk and select a predicted path with relatively low risk, thereby effectively avoiding collision and grounding of unmanned ships.

[0026] 3. The present invention improves the A-star algorithm by introducing the path hazard value into the cost function , in order to take into account both path cost and safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 A flow chart of the traditional A-star algorithm in the prior art;

[0028] Figure 2 A schematic diagram of a navigation of an unmanned ship in the prior art;

[0029] Figure 3 This is a flow chart of the unmanned ship path planning method based on the improved A-star algorithm in this embodiment;

[0030] Figure 4 A schematic diagram of an island map in this embodiment;

[0031] Figure 5 A schematic diagram of a grid map in this embodiment;

[0032] Figure 6 is a schematic diagram of an obstacle grid in this embodiment;

[0033] Figure 7 A schematic diagram of an obstacle expansion and expansion of a grid in this embodiment;

[0034] Figure 8 A schematic diagram of the key points extracted in this embodiment;

[0035] Fig. 9 A schematic diagram of sub-node selection of the traditional A-star algorithm in the prior art;

[0036] Fig.10 A schematic diagram of improving the sub-node selection of the A-star algorithm in this embodiment;

[0037] Fig.11 A schematic diagram of the distance from the interval obstacle to the predicted path in this embodiment;

[0038] Fig.12 A schematic diagram of a simple grid map in this embodiment;

[0039] Fig.13 A schematic diagram of the path planning of the unmanned ship on a simple grid map in this embodiment;

[0040] Fig.14 A schematic diagram of a complex grid map in this embodiment;

[0041] Fig.15 A schematic diagram of the path planning of the unmanned ship on a complex grid map in this embodiment. DETAILED DESCRIPTION

[0042] 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.

[0043] Embodiment: A method for unmanned ship path planning based on improved A-star algorithm, such as Figure 3 As shown, the following steps are included:

[0044] Step 1: Create an island map based on the information of the island and its shoal area, and convert it into a raster map. The raster map includes obstacle areas that the unmanned boat cannot pass through and passable areas. Specifically:

[0045] In the actual navigation of unmanned ships, due to the influence of tides, the closer the ship is to the island, the higher the risk of collision and grounding. Therefore, the path of the unmanned ship should maintain a certain safe distance from the island to avoid passing through high-risk areas. In addition, if Figure 2 As shown in Figure 2, the division of dangerous areas also needs to consider the draft depth of the unmanned ship ( ), and retain a certain amount of excess depth ( ) to ensure the maneuverability of the unmanned boat and prevent the boat from hitting the bottom and causing damage. ) should satisfy formula (1):

[0046] (1)

[0047] Assuming that the navigation environment of the unmanned ship remains unchanged, It can be calculated using formula (2), that is, by subtracting the draft ( ) multiplied by a constant ratio ( ) is used for calculation. In general, the coefficient is used for coastal waters. The minimum value of ).

[0048] (2)

[0049] Taking into account the impact of tides, draft depth and excess depth on the navigable sea area, the division and rasterization process of the island map is as follows: Figure 4-Figure 5 As shown. On the island map ( Figure 4 ), the area marked with "X" is an island, and the area marked with "√" is an ocean. Through image processing technology, the island map is rasterized to form a raster map ( Figure 5 ); In the grid map, the side length of each grid is 10m. The area marked with "√" is a barrier-free area and can be passed freely. The area marked with "X" is an obstacle area and cannot be passed. The edge of the obstacle area is a high-risk area with a width of 2m. The unmanned ship should avoid passing through this area.

[0050] By converting the island map into a raster map, the unmanned ship can not only complete the path search quickly, but also plan a relatively smooth path.

[0051] Step 2: Expand the obstacles in the grid map outward by one grid, extract the corner points of the expanded area, and define them as key points. These key points will participate in the path planning of the algorithm as potential child nodes of each parent node. This method aims to avoid the influence of grid size on the planned path as much as possible and reduce the number of redundant nodes caused by avoiding obstacles formed by islands and shoals.

[0052] like Figure 6-Figure 8 As shown in the figure, the key point extraction process is as follows:

[0053] Among them, Figure 6 In the process, rectangular and irregular obstacle shapes are generated according to the island environment information. The grid marked with "X" is an obstacle, and the rest of the area is a passable area. Figure 2 The obstacle grid is assigned a value of 1 and the rest are 0, and the image matrix A to be expanded is obtained; through image processing technology, the two 3×3 structural elements in formula (3) are used and Dilate the image, using Expand the obstacle outward by one grid, such as Figure 7 As shown in the figure, the grid area marked with "△" is the obstacle area after expansion; then, with the help of Extract the corner points of the obstacle expansion area, such as Figure 8 As shown, “•” is the key point, and a key point list is constructed;

[0054] (3)

[0055] In formula (3), To expand the obstacle in 8 directions by one grid; To expand the obstacle in four directions, upward, downward, left and right, by one grid; For expansion processing operations; is the binary map matrix after the obstacle is expanded; It is a binary map matrix that only retains the corner points of the dilated area.

[0056] Step 3, redefine the child nodes of the A-star algorithm, and use all key points as potential child nodes of the current node to participate in the path search. Then, filter out the potential child nodes that the current node can reach directly in a straight line and meet the safety distance, and define them as child nodes of the current node.

[0057] The subnodes of the traditional A-star algorithm are usually arranged in eight-connected domains, and the nodes in the obstacle grid are eliminated6, such as Fig. 9 As shown in Figure 2, since the extracted key points cannot be arranged according to the traditional eight-connected domain, a suitable screening strategy is needed to select potential child nodes that meet the requirements.

[0058] like Fig.10 As shown in the figure, taking a potential child node as an example, the selection of its child node includes: if the three points of the obstacle center, the current node and the potential child node are not collinear, that is, the three points can form a triangle, then the distance d from the obstacle center to the potential path is calculated using the Heron triangle area formula and the height formula of formula (4); if the three points are collinear, then the distance is 0; if all the distances If both are greater than or equal to the safety distance, it means that the current point can directly reach the potential child node, and the node is defined as the neighbor point of the current point, that is, the child node; otherwise, the potential path is prone to collision with obstacles. That is, different distance calculation methods are used according to whether the center of the obstacle in the interval is colinear with the current node and the potential child node;

[0059] (4)

[0060] In formula (4), AB, BC, and AC are the three side lengths of the triangle formed by the current node, potential child node, and the center of the interval obstacle; is half the perimeter of the triangle; is the area of ​​the triangle; is the distance from the center of the interval obstacle to the current node and the potential child node.

[0061] Step 4: Define the minimum distance of the connection prediction path of the first n segments The reciprocal sum of is the path hazard value , the path hazard value Introduced into the cost function value of the A-star algorithm.

[0062] The planning path should consider the safe distance between the ship and the island to avoid collisions when the unmanned ship passes through high-risk areas. At the same time, the planning path also needs to consider the length of the navigation path to take into account the cost of the navigation path. Fig.11 It can be seen that the distance from the obstacle in the estimated path interval to the predicted path It is an important basis for measuring the safety of unmanned ships. Since the predicted path interval usually has multiple distance values , evaluate the minimum value among them , to better show the safety of the path. Minimum distance value The smaller it is, the higher the risk of the unmanned ship colliding with the shoal.

[0063] As shown in formula (5), from the set Filter out the minimum distance between intervals , the minimum distance between intervals The reciprocal sum of is defined as the path hazard value , and introduce it into the cost function value of the A-star algorithm, the path hazard value The larger the value, the higher the collision risk of the unmanned ship. By taking into account both path cost and safety through the new cost function, the unmanned ship can plan a navigation path with lower cost and collision risk.

[0064] (5)

[0065] In formula (5), To predict the number of obstacles in the path interval; is the set of distances between the obstacle center and the predicted path; , Respectively before Duanheqian The sum of the reciprocals of the minimum distance values ​​of the segment paths; is the weight coefficient of the path hazard value.

[0066] Step 5: The path search cycle is the same as the traditional A-star algorithm. This embodiment improves the A-star algorithm by using the cost function to evaluate the total cost of each child node. , choose the total cost The smallest node is taken as the parent node; in each loop, the existence of the child nodes is checked, and it is determined whether the target point appears. If the target point appears, the path finding is successful, the loop ends, and the path is backtracked; if there are no more child nodes to choose from and the target point still does not appear, the path finding fails and the loop ends.

[0067] The following analysis is based on a simple island grid map of 100×100 and a complex island grid map of 200×200 simulated on MATLAB:

[0068] Fig.12 For a simple island grid map, Fig.14 For complex island grid maps, Fig.12 and Fig.14 In the figure, “△” represents the starting point and “•” represents the target point.

[0069] like Fig.13 and Fig.15 As shown in the figure, the paths planned by the improved A-star algorithm in this embodiment in a simple island grid map and a complex island grid map are respectively shown. From the paths in the above two figures, it can be seen that the unmanned ship navigation path planned by the improved A-star algorithm in this embodiment can effectively avoid high collision risk areas, improve the safety of the unmanned ship navigation path, and the navigation path has fewer turning points and a smoother path.

[0070] The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A method for unmanned ship path planning based on improved A-star algorithm, characterized in that: The following steps are involved: Step 1: Create an island map based on the information of the island and its shoal area, and convert it into a raster map. The raster map includes obstacle areas that the unmanned boat cannot pass through and passable areas; Step 2: Expand the obstacles in the grid map outward by one grid, extract the corner points of the expanded area, and define them as key points; Step 3, redefine the child nodes of the A-star algorithm, take all key points as potential child nodes of the current node to participate in the path search, then filter out the potential child nodes that the current node can reach directly in a straight line and meet the safety distance, and define them as the child nodes of the current node; Step 4: Define the minimum distance of the connection prediction path of the first n segments The reciprocal sum of is the path hazard value , the path hazard value Introduced into the cost function value of the A-star algorithm; Step 5: Use the cost function to evaluate the total cost of each child node , choose the total cost The smallest node is used as the parent node; In each loop, check the existence of the child nodes and determine whether the target point appears. If the target point appears, the path finding is successful, the loop ends, and the path is backtracked; If there are no more child nodes to choose from and the target point still does not appear, the path finding fails and the loop ends; In step 2, the extraction of key points includes: binarizing the grid map, assigning the obstacle grid a value of 1 and the rest to 0, and obtaining the image matrix A to be expanded; using image processing technology, using the two 3×3 structural elements in formula (3) and Dilate the image, using Expand the obstacle outward by one grid. Extract the corner points of the obstacle expansion area and build a key point list; (3) In formula (3), To expand the obstacle in 8 directions by one grid; To expand the obstacle in four directions, upward, downward, left and right, by one grid; For expansion processing operations; is the binary map matrix after the obstacle is expanded; It is a binary map matrix that only retains the corner points of the dilated area; In step 3, the selection of child nodes includes: if the obstacle center, current node and potential child node are not collinear, the distance d from the obstacle center to the predicted path is calculated using the Heron triangle area formula and height formula in formula (4); if the three points are collinear, the distance is 0; if all the distances If both are greater than or equal to the safety distance, it means that the current point can directly reach the potential child node, and the node is defined as the neighbor point of the current point, that is, the child node; otherwise, the predicted path is prone to collision with obstacles; (4) In formula (4), AB, BC, and AC are the three side lengths of the triangle formed by the current node, potential child node, and the center of the interval obstacle; is half the perimeter of the triangle; is the area of ​​the triangle; is the distance from the center of the interval obstacle to the predicted path; Step 4 includes: using formula (5), from the set Filter out the minimum distance between intervals , the minimum distance between intervals The reciprocal sum of is defined as the path hazard value , and introduce it into the cost function value of the A-star algorithm; (5) In formula (5), To predict the number of obstacles in the path interval; is the set of distances between the obstacle center and the predicted path; For practical cost, Euclidean distance is used; To estimate the cost, Mann's distance is used; is the current parent node; is the next parent node; is the target point; , Respectively before Duanheqian The path hazard value of the segment; is the weight coefficient of the path hazard value.

Images