A path planning method for rescue unmanned vessels based on an optimized A* algorithm

Abstract

The present invention discloses a path planning method for a rescue unmanned ship based on an optimized A* algorithm, including: constructing a grid map model to generate an initial rescue path; creating an Open table and a Close table for the A* algorithm, putting the starting node into the Close table, and putting the nodes adjacent to the starting node and not coinciding with obstacles into the Open table, calculating the path cost of the starting node and storing it in the cost table; selecting the node with the minimum cost as the next parent node and storing it in the Close table; putting all adjacent nodes of the selected parent node into the Open table, and removing the points coinciding with the Close table and the impassable points, and running in a loop. When the preset condition is reached, the loop ends; according to the cost table, sequentially find the nodes with the lowest path cost in the process from the target node to the starting node to generate the final rescue path. The present invention speeds up the response speed of the rescue unmanned ship and improves the rescue success rate.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ship path planning, and particularly to a path planning method for a rescue unmanned ship based on an optimized A* algorithm. Background Art

[0002] With the rapid development of technology, unmanned ships, as an important part of intelligent marine equipment, are increasingly widely used in various fields, including environmental monitoring, ocean mapping, etc. However, the development of the rescue unmanned ship field is still relatively weak. One of the pain points lies in the insufficient rescue speed. The ability of an unmanned ship to navigate autonomously in complex waters, especially the path planning and obstacle avoidance capabilities, is directly related to the execution efficiency and success of its rescue mission. Traditionally, due to its heuristic search characteristics, the A* algorithm performs excellently in solving the shortest path in a static road network. It combines the advantages of algorithms such as the DFS algorithm and the BFS algorithm. However, as a heuristic search algorithm, although the A* algorithm performs well in a static environment, when dealing with a large-scale or dynamically changing environment, it may lead to a decrease in efficiency due to repeated exploration of similar path areas, affecting the operating speed of the unmanned ship. Summary of the Invention

[0003] To solve the above technical problems, the present invention proposes a path planning method for a rescue unmanned ship based on an optimized A* algorithm to optimize the problems existing in the above-mentioned prior art. It is proposed to introduce a DP variable table and a state transition equation in the path cost calculation process of the A* algorithm to simplify the path cost calculation process and avoid repeated calculation of nodes, thereby achieving the purpose of improving the calculation speed and faster convergence.

[0004] To achieve the above object, the present invention provides a path planning method for a rescue unmanned ship based on an optimized A* algorithm, including:

[0005] Construct a grid map model, determine the starting point and the target point of the rescue ship path based on the grid map model, and generate an initial rescue path before fitting optimization;

[0006] Create an Open table and a Close table for the A* algorithm, put the starting node of the initial rescue path into the Close table, put the nodes adjacent to the starting node and not coinciding with obstacles into the Open table, calculate the path cost of the starting node, and store it in the cost table, where the Close table is used to store the nodes that have been expanded as parent nodes, and the Open table is used to store the nodes from which the parent node is selected next time;

[0007] Take the starting node as the parent node, calculate the path costs of the child nodes of the starting node of the initial rescue path, and store them in the cost table. Sort all the nodes in the Open table according to the path costs, and select the node with the minimum cost as the next parent node and store it in the Close table;

[0008] Put all the adjacent nodes of the selected parent node into the Open table, and remove the overlapping points and impassable points with the Close table. Run the operation of calculating the parent node cost and expanding the child nodes in a loop. When a preset condition that a certain child node coincides with the path target point is reached, end the loop to obtain a complete path cost table;

[0009] According to the path cost table, sequentially find the nodes with the lowest path costs in the process from the target node to the starting node to generate the final rescue path.

[0010] Preferably, generate the initial rescue path before fitting optimization, including:

[0011] Construct the grid map model according to the operation waters of the rescue unmanned ship and the scene obstacle situation, determine the position of the drowning person based on the GPS positioning system carried by the life jacket of the drowning person, initialize the starting point and the target point of the path of the rescue ship in the grid map model, and use the static dilation algorithm to extend the obstacles outward by a safe distance to generate the initial rescue path before fitting optimization.

[0012] Preferably, calculate the path cost of the starting node, including:

[0013] Define the predecessor node of the starting node as p, and the weight of the edge from p to n as w(p, n), and calculate the cost g(n) of the current path:

[0014] g(n) = g(p) + w(p, n) (1)

[0015] where g(p) is the path cost of the predecessor node, and w(p, n) is the weight of the edge from the predecessor node to the current node;

[0016] Calculate the expected cost of the starting node through the Manhattan function, and calculate the path cost according to the expected cost:

[0017] h = |x1 - x2| + |y1 - y2| (2)

[0018] f(n) = g(n) + h(n) (3)

[0019] Wherein, f(n) represents the estimated cost of the distance from the starting node through node n to the target node e, g(n) represents the actual distance cost from the starting node to the currently traversed node n, h is the Manhattan distance between two points, x1 is the abscissa value of the first point, x2 is the abscissa value of the second point, y1 is the ordinate value of the first point, y2 is the ordinate value of the second point, and h(n) is the expected cost.

[0020] Preferably, taking the starting node as the parent node, calculating the path cost of the child node of the starting node of the initial rescue path includes:

[0021] Introduce a DP cost table, initialize the storage of path costs, and represent the initialized path cost state as a quadruple: State = (current node, target, expected cost, actual cost);

[0022] Define the state transition equation:

[0023] dp[i][j] = opt(dp[x][y] for some x,y related to i,j and other conditions)(4)

[0024] Wherein, dp[i][j] represents the current state, i and j represent the indices in the DP table, and dp[x][y] represents the parent node state;

[0025] Taking the starting node as the parent node, calculate the path cost of its child node according to the state transition equation and store it in the cost table.

[0026] Preferably, when a preset condition that a certain child node coincides with the path target point is reached, end the loop, including:

[0027] Sort all the nodes in the Open table according to the path cost, select the point with the minimum cost as the next parent node and store it in the Close table, and run in a loop. Among them, when sorting the nodes in the Open table each time, give priority to traversing the nodes in the cost table. If it has been calculated, directly call it;

[0028] When a preset condition that a certain child node coincides with the path target point is reached, end the loop, and according to the cost table, sequentially find the path with the shortest cost from the target node to the starting node, where the preset condition is that the selected parent node coincides with the target node or the Open table is empty.

[0029] Preferably, generating the final rescue path includes:

[0030] Optimize the initial rescue path using the vertical distance limit method, calculate the vertical distance between each node (excluding the start node and the target node) and its two adjacent nodes before and after. If the vertical distance is less than the threshold, delete the node. Then, sequentially find the node with the lowest path cost during the process of reaching the start node from the target node to generate the final rescue path, and introduce a Bessel curve to fit the final rescue path.

[0031] Preferably, the method for constructing the Bessel curve is as follows:

[0032] Establish a Bessel equation:

[0033]

[0034] In the formula, n is one less than the number of control points, P i is the control point, is the combination number, B(t) is the functional representation of the Bessel equation, i is the index variable, t is a fixed parameter in the Bessel curve, and t ∈ [0, 1];

[0035] Based on the Bessel equation, establish a cubic Bessel curve, specifically:

[0036] B(t) = (1 - t) 3 P0 + 3(1 - t) 2 tP1 + 3(1 - t)t 2 P2 + t 3 P3(6)

[0037] In the formula, t is any value from 0 to 1, representing the position on the curve; P0 is the starting point of the curve, P1 and P2 are the intermediate control points of the curve respectively, and P3 is the ending point of the curve.

[0038] Compared with the prior art, the present invention has the following advantages and technical effects:

[0039] By introducing a DP cost table, the present invention simplifies the calculation cost of node paths, avoids duplicate calculations of nodes, and speeds up the calculation. Compared with the prior art, it makes the path planning speed of the unmanned ship faster, accelerates the response speed of the rescue unmanned ship, and thus improves its rescue success rate. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The accompanying drawings forming a part of this application are used to provide a further understanding of this application. The illustrative embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation to this application. In the drawings:

[0041] Figure 1 is a flowchart of a rescue unmanned ship path planning method based on an optimized A* algorithm according to an embodiment of the present invention;

[0042] Figure 2The flowchart of the path planning for the transportation unmanned ship according to the embodiment of the present invention;

[0043] Figure 3 The schematic diagram of the path cost state transition according to the embodiment of the present invention;

[0044] Figure 4 The working schematic diagram of the second-order Bezier curve according to the embodiment of the present invention;

[0045] Figure 5 The post-processing workflow of the path according to the embodiment of the present invention. Detailed implementation manners

[0046] It should be noted that, without conflict, the embodiments in the present application and the features in the embodiments may be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and embodiments.

[0047] It should be noted that the steps shown in the flowchart of the accompanying drawings may be executed in a computer system such as a set of computer-executable instructions, and although the logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than here.

[0048] The dynamic programming algorithm is a method for solving complex problems by decomposing the original problem into relatively simple sub-problems. It saves the answers of the solved sub-problems, so that these answers can be reused when needed, avoiding repeated calculations, and it can be substituted into the A* algorithm for optimization. Therefore, the present invention can be used to generate a more smooth, safe and conforming to the actual navigation characteristics of the unmanned ship rescue path more quickly.

[0049] The present invention proposes a rescue unmanned ship path planning method based on an optimized A* algorithm, as Figure 1 - Figure 2 , including:

[0050] Construct a grid map model, determine the starting point and the target point of the rescue ship path based on the grid map model, and generate an initial rescue path before fitting optimization;

[0051] Create an Open table and a Close table for the A* algorithm, put the starting node of the initial rescue path into the Close table, put the nodes adjacent to the starting node and not coinciding with obstacles into the Open table, calculate the path cost of the starting node, and store it in the cost table, where the Close table is used to store the nodes that have been expanded as parent nodes, and the Open table is used to store the nodes from which the parent node is selected next time;

[0052] Taking the starting node as the parent node, calculating the path cost of the child nodes of the starting node of the initial rescue path and storing them in the cost table, sorting all the nodes in the Open table according to the path cost, selecting the node with the minimum cost as the next parent node and storing it in the Close table;

[0053] Put all the adjacent nodes of the selected parent node into the Open table, and remove the points that overlap with the Close table and the inaccessible points, and run the operation of calculating the cost of the parent node and expanding the child nodes in a loop. When the preset condition that a child node overlaps with the path target point is met, end the loop and obtain a complete path cost table;

[0054] According to the path cost table, nodes with the lowest path cost in the process of reaching the starting node from the target node are searched in sequence to generate a final rescue path.

[0055] The present invention introduces the DP cost table to simplify the node path calculation cost, avoids repeated calculation of nodes, and speeds up the calculation speed. Compared with the prior art, the path planning speed of the unmanned boat is faster, the response speed of the rescue unmanned boat is accelerated, and the rescue success rate is improved.

[0056] Furthermore, an initial rescue path before fitting optimization is generated, including:

[0057] According to the operating waters of the unmanned rescue boat and the obstacles in the scene, the grid map model is constructed, and the position of the drowning person is determined according to the GPS positioning system of the drowning person's life jacket. The starting point and target point of the rescue boat path are initialized in the grid map model, and the obstacles are extended outward by a safe distance using the static expansion algorithm to generate the initial rescue path before fitting optimization.

[0058] Specifically, according to the operating waters of the rescue unmanned boat and the obstacles in the scene, a grid map model is constructed, and the position of the drowning person is determined according to the GPS positioning system of the life jacket, and the starting point and target point of the rescue boat path are initialized in the grid map. The static expansion algorithm is used to extend the obstacle outward by a safe distance d to prevent the hull from colliding with the obstacle, and a path task is generated, that is, the initial rescue path before fitting optimization.

[0059] Furthermore, the Close table is used to store nodes that have been expanded as parent nodes, and the Open table is used to store nodes from which parent nodes are selected next time.

[0060] Specifically, create and initialize the Open table and Close table of the A* algorithm. The Close table is used to store the nodes that have been expanded as parent nodes, and the Open table is used to store the nodes from which the parent node will be selected next. Put the starting node into the Close table, and put the adjacent nodes that do not coincide with the obstacles into the Open table.

[0061] Furthermore, calculate the path cost of the starting node, including:

[0062] Define the predecessor node of the starting node as p, and the weight of the edge from p to n as w(p, n). Then, calculate the cost g(n) of the current path through Equation (1):

[0063] g(n) = g(p) + w(p, n) (1)

[0064] Where g(p) is the path cost of the predecessor node, and w(p, n) is the weight of the edge from the predecessor node to the current node;

[0065] Calculate the expected cost of the starting node through the Manhattan function, and calculate the path cost based on the expected cost:

[0066] h = |x1 - x2| + |y1 - y2| (2)

[0067] f(n) = g(n) + h(n) (3)

[0068] In the formula, f(n) represents the estimated cost of the distance from the starting node through node n to the target node e, g(n) represents the actual distance cost from the starting node to the currently traversed node n, h is the Manhattan distance between two points, x1 is the abscissa value of the first point, x2 is the abscissa value of the second point, y1 is the ordinate value of the first point, y2 is the ordinate value of the second point, and h(n) is the expected cost.

[0069] Specifically, define the predecessor node of the current node n as p, and the weight of the edge from p to n as w(p, n). Then, the cost of the current path can be calculated through the following formula:

[0070] g(n) = g(p) + w(p, n)

[0071] Where g(p) is the path cost of the predecessor node, and w(p, n) is the weight of the edge from the predecessor node to the current node.

[0072] Calculate the expected cost of the starting node through the heuristic function Manhattan function:

[0073] h = |x1 - x2| + |y1 - y2|

[0074] Estimated cost:

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

[0076] Among them, f(n) represents the estimated distance cost value from the starting node through node n to the target node e, and g(n) represents the actual distance cost from the starting node to the currently traversed node n. The actual cost of the starting node is 0.

[0077] Furthermore, taking the starting node as the parent node, calculating the path cost of the child node of the starting node of the initial rescue path includes:

[0078] Introduce a DP cost table, initialize the storage of path costs, and represent the initialized path cost status as a quadruple: Status = (current node, target, expected cost, actual cost);

[0079] Define the state transition equation:

[0080] dp[i][j] = opt(dp[x][y] for some x, y related to i, j and other conditions)(4)

[0081] Among them, dp[i][j] represents the current state, i and j represent the indices in the DP table, and dp[x][y] represents the parent node state;

[0082] Taking the starting node as the parent node, calculate the path cost of its child node according to the state transition equation and store it in the cost table.

[0083] Specifically, introduce a DP cost table, initialize the storage of path costs, and represent its status as a quadruple: Status = (current node, target, expected cost, actual cost).

[0084] Define the state transition equation:

[0085] dp[i][j] = opt(dp[x][y] for some x, y related to i, j and other conditions)

[0086] Among them, dp[i][j] represents the current state, i and j represent the indices in the DP table. Here, it represents the expected cost h(n) and the actual cost g(n) of the child node, and dp[x][y] represents the parent node state, corresponding to the indices p, q in the DP table.

[0087] As shown in the appendix Figure 1 , in this embodiment, the actual cost of the nodes in the four positive directions of the parent node is increased by 10, and the actual cost of the nodes in the diagonal direction is taken as an approximate integer plus 14.

[0088] Take the starting node as the parent node, calculate the path cost of its child nodes according to the state transition equation, and store it in the cost table. As Figure 3 .

[0089] Further, when a preset condition that a certain child node coincides with the path target point is reached, the loop ends, including:

[0090] Sort all the nodes in the Open table according to the path cost, select the node with the minimum cost as the next parent node and store it in the Close table, and run in a loop. Among them, when sorting the nodes in the Open table each time, give priority to traversing the nodes in the cost table. If it has been calculated, directly call it;

[0091] When a preset condition that a certain child node coincides with the path target point is reached, the loop ends, and according to the cost table, sequentially find the path with the shortest cost from the target node to the starting node, where the preset condition is that the selected parent node coincides with the target node or the Open table is empty.

[0092] Specifically, sort all the nodes in the Open table according to the path cost, select the node with the minimum cost as the next parent node and store it in the Close table. Run in a loop. When sorting the nodes in the Open table each time, give priority to traversing the nodes in the cost table. If it has been calculated, directly call it.

[0093] When the selected parent node coincides with the target node or the Open table is empty, the loop ends. And according to the cost table, sequentially find the path with the shortest cost from the target node to the starting node.

[0094] Further, generate the final rescue path, such as Figure 5 , including:

[0095] Use the vertical distance limit method to optimize the initial rescue path, calculate the vertical distance between each node except the starting node and the target node and its two adjacent nodes before and after. If it is less than the threshold, delete the node, sequentially find the node with the lowest path cost in the process from the target node to the starting node to generate the final rescue path, and introduce a B-spline curve to fit the final rescue path.

[0096] Specifically, use the vertical distance limit method to optimize the A* initial path, calculate the vertical distance between each node except the starting node and the target node and its two adjacent nodes before and after. After it is less than the threshold p, delete the node, and execute in a loop to generate a smooth path curve.

[0097] Further, the method for constructing a B-spline curve is (such as Figure 4 ):

[0098] Establish a B-spline equation:

[0099]

[0100] wherein, n is one less than the number of control points, P i is a control point, is a combination number, B(t) is the functional representation of the Bessel equation, i is an index variable, t is a fixed parameter in the Bessel curve, and t ∈ [0, 1];

[0101] A cubic Bessel curve is established based on the Bessel equation, specifically:

[0102] B(t) = (1 - t) 3 P0 + 3(1 - t) 2 tP1 + 3(1 - t)t 2 P2 + t 3 P3(6)

[0103] wherein, t is any value from 0 to 1, representing the position on the curve; P0 is the starting point of the curve, P1 and P2 are the intermediate control points of the curve respectively, and P3 is the ending point of the curve.

[0104] The above is only the preferred specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed in the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A path planning method for a rescue unmanned ship based on an optimized A* algorithm, characterized in that, include: Constructing a grid map model, determining the starting point and target point of the rescue ship path based on the grid map model, and generating an initial rescue path before fitting optimization; Create an Open table and a Close table of the A* algorithm, put the starting node of the initial rescue path into the Close table, put the nodes adjacent to the starting node and not overlapping with the obstacle into the Open table, calculate the path cost of the starting node, and store it in the cost table, wherein the Close table is used to store nodes that have been expanded as parent nodes, and the Open table is used to store nodes from which the parent node is selected next time; Taking the starting node as the parent node, calculating the path cost of the child nodes of the starting node of the initial rescue path and storing them in the cost table, sorting all the nodes in the Open table according to the path cost, selecting the node with the minimum cost as the next parent node and storing it in the Close table; Put all the adjacent nodes of the selected parent node into the Open table, and remove the points that overlap with the Close table and the inaccessible points, and run the operation of calculating the cost of the parent node and expanding the child nodes in a loop. When the preset condition that a child node overlaps with the path target point is met, end the loop and obtain a complete path cost table; According to the path cost table, nodes with the lowest path cost in the process of reaching the starting node from the target node are searched in sequence to generate a final rescue path.

2. The path planning method of the rescue unmanned ship based on the optimized A* algorithm according to claim 1, wherein Generate an initial rescue path before fitting optimization, including: According to the operating waters of the unmanned rescue boat and the obstacles in the scene, the grid map model is constructed, and the position of the drowning person is determined according to the GPS positioning system of the drowning person's life jacket. The starting point and target point of the rescue boat path are initialized in the grid map model, and the obstacles are extended outward by a safe distance using the static expansion algorithm to generate the initial rescue path before fitting optimization.

3. The rescue unmanned ship path planning method based on the optimized A* algorithm according to claim 1, wherein Calculating the path cost of the starting node includes: Define the predecessor node of the starting node as p, and the weight of the edge from p to n as w(p, n), and calculate the cost of the current path g(n): g(n)=g(p)+w(p,n)(1) Among them, g(p) is the path cost of the predecessor node, and w(p,n) is the weight of the edge from the predecessor node to the current node; The expected cost of the starting node is calculated by the Manhattan function, and the path cost is calculated based on the expected cost: h=|x1−x2|+|y1−y2|(2) f(n)=g(n)+h(n)(3) Where f(n) represents the estimated cost of the distance from the starting node to the target node e via node n, g(n) represents the actual distance cost from the starting node to the currently traversed node n, h is the Manhattan distance between the two points, x1 is the horizontal coordinate value of the first point, x2 is the horizontal coordinate value of the second point, y1 is the vertical coordinate value of the first point, y2 is the vertical coordinate value of the second point, and h(n) is the expected cost.

4. The rescue unmanned ship path planning method based on the optimized A* algorithm according to claim 1, wherein Taking the starting node as a parent node, calculating the path cost of the child nodes of the starting node of the initial rescue path includes: Introduce the DP cost table, initialize the storage of path costs, and represent the initialized path cost state as a quadruple: \text{State}=(current node, target, expected cost, actual cost); Define the state transition equation: dp[i][j]=opt(dp[x][y] for some x,y related to i,j and other conditions)(4) where dp[i][j] represents the current state, i and j represent the indices in the DP table, and dp[x][y] represents the parent node state; Take the starting node as the parent node, calculate the path costs of its child nodes according to the state transition equation, and store them in the cost table.

5. The rescue unmanned ship path planning method based on the optimized A* algorithm according to claim 1, wherein When a preset condition that a certain child node coincides with the path target point is reached, end the loop, including: Sort all the nodes in the Open table by path cost, select the point with the minimum cost as the next parent node and store it in the Close table, and run in a loop. Among them, when sorting the nodes in the Open table each time, give priority to traversing the nodes in the cost table. If it has been calculated, directly call it; When a preset condition that a certain child node coincides with the path target point is reached, end the loop, and according to the cost table, sequentially find the path with the shortest cost from the target node to the starting node, where the preset condition is that the selected parent node coincides with the target node or the Open table is empty.

6. The rescue unmanned ship path planning method based on the optimized A* algorithm according to claim 1, characterized in that Generate the final rescue path, including: Optimize the initial rescue path using the vertical distance limit method. Calculate the vertical distance between each node except the starting node and the target node and its two adjacent nodes before and after. If it is less than the threshold, delete the node. Sequentially find the node with the lowest path cost in the process from the target node to the starting node to generate the final rescue path, and introduce a Bezier curve to fit the final rescue path.

7. The rescue unmanned ship path planning method based on the optimized A* algorithm according to claim 6, characterized in that, The method for constructing the Bezier curve is: Establish the Bezier equation: where n is one less than the number of control points, P i is a control point, is the combination number, B(t) is the functional representation of the Bessel equation, i is the index variable, t is the fixed parameter in the Bessel curve, and t ∈ [0, 1]; Based on the Bezier equation, establish a cubic Bezier curve, specifically: B(t) = (1 - t) 3 P0 + 3(1 - t) 2 tP1 + 3(1 - t)t 2 P2 + t 3 P3(6) In the formula, t is any value from 0 to 1, representing the position on the curve; P0 is the starting point of the curve, P1 and P2 are the intermediate control points of the curve respectively, and P3 is the ending point of the curve.

Images