Rescue unmanned ship path planning method based on optimization A* algorithm

Abstract

The invention discloses a rescue unmanned ship path planning method based on an optimization A * algorithm, and the method comprises the steps: constructing a grid map model, and generating an initial rescue path; creating an Open table and a Close table of an A * algorithm, putting the starting node into the Close table, putting a node which is adjacent to the starting node and is not overlapped with the obstacle into the Open table, calculating the path cost of the starting node, and storing the path cost in a cost table; the node with the minimum cost is selected as a father node of the next time to be stored in the Close table; all adjacent nodes in the selected father nodes are put into an Open table, points and impassable points which coincide with the points and the impassable points in the Close table are removed, circulating operation is carried out, and when a preset condition is met, circulation is ended; according to the cost table, nodes with the lowest path cost in the process from the target node to the starting node are searched in sequence, and a final rescue path is generated. The response speed of the rescue unmanned ship is increased, and the rescue success rate is increased.

Description

Technical Field

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

[0002] With the rapid development of science and technology, unmanned ships, as an important part of intelligent marine equipment, have been increasingly applied in a wide range of fields, including environmental monitoring, marine mapping, etc. However, the development of the field of rescue unmanned ships is still relatively weak. One of the pain points is the insufficient rescue speed. The ability of unmanned ships to navigate autonomously in complex waters, especially path planning and obstacle avoidance, is directly related to the efficiency and success of their rescue missions. Traditionally, the A* algorithm has performed well in solving the shortest path in a static road network due to its heuristic search characteristics. It combines the advantages of algorithms such as the DFS algorithm and the BFS algorithm. However, as a heuristic search algorithm, the A* algorithm performs well in static environments, but when dealing with large-scale or dynamically changing environments, it may cause 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] In order to solve the above technical problems, the present invention proposes a rescue unmanned boat path planning method based on the optimized A* algorithm to optimize the problems existing in the above-mentioned prior art, and proposes to introduce DP variable table and state transfer 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 calculation speed and faster convergence.

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

[0005] 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;

[0006] 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;

[0007] 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;

[0008] 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;

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

[0010] Preferably, generating an initial rescue path before fitting optimization includes:

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

[0012] Preferably, calculating the path cost of the starting node includes:

[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 of the current path g(n):

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

[0015] 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;

[0016] The expected cost of the starting node is calculated by the Manhattan function, and the path cost is calculated based on the expected cost:

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

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

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

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

[0021] The DP cost table is introduced to initialize the storage of the path cost, and the initialized path cost state is represented as a four-tuple: \text{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 otherconditions)(4)

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

[0025] The starting node is taken as the parent node, and the path cost of its child nodes is calculated according to the state transfer equation and stored in the cost table.

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

[0027] Sort all 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 it in a loop. Each time the nodes in the Open table are sorted, the nodes in the cost table are traversed first, and if they have been calculated, they are directly called;

[0028] When the preset condition that a child node coincides with the path target point is reached, the loop ends, and according to the cost table, the shortest cost path from the target node to the start node is searched in sequence, where the preset condition is when the selected parent node coincides with the target node or the Open table is empty.

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

[0030] The initial rescue path is optimized using the vertical distance limit method. The vertical distance between each node except the starting node and the target node and the two preceding and succeeding nodes is calculated. If the vertical distance is less than the threshold, the node is deleted. The nodes with the lowest path cost in the process of reaching the starting node from the target node are sequentially searched to generate the final rescue path. The Bezier curve is introduced to fit the final rescue path.

[0031] Preferably, the method for constructing the Bezier curve is:

[0032] Set up the Bessel equation:

[0033]

[0034] Where n is the number of control points minus one, P i is the control point, is the number of combinations, B(t) is the function representation of the Bezier equation, i is the index variable, t is the fixed parameter in the Bezier curve, t∈[0,1];

[0035] A cubic Bezier curve is established based on the Bezier equation, specifically:

[0036] B(t)=(1-t) 3 P 0 +3(1-t) 2 Tp 1 +3(1-t)t 2 P 2 +t 3 P 3 (6)

[0037] Where t is any value from 0 to 1, representing the position on the curve; P 0 is the starting point of the curve, P 1 and P 2 are the middle control points of the curve, P 3 is the end point of the curve.

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

[0039] 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. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0041] Figure 1 A flow chart of a method for path planning of an unmanned rescue boat based on an optimized A* algorithm according to an embodiment of the present invention;

[0042] Figure 2 This is a workflow diagram of the transport unmanned ship path planning according to an embodiment of the present invention;

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

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

[0045] Figure 5 This is the path post-processing workflow of an embodiment of the present invention. DETAILED DESCRIPTION

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

[0047] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown 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 to the solved sub-problems, so that these answers can be used again when needed to avoid repeated calculations. They can be substituted into the A* algorithm for optimization. Therefore, the present invention can generate a smoother, safer rescue path that meets the actual navigation characteristics of the unmanned ship more quickly.

[0049] The present invention proposes a rescue unmanned boat path planning method based on the optimized A* algorithm. Figure 1-Figure 2 ,include:

[0050] 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;

[0051] 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;

[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 nodes that have been expanded as parent nodes, and the Open table is used to store points from which the parent node is selected next time. Put the starting node into the Close table, and put its adjacent nodes that do not overlap with obstacles into the Open table.

[0061] Furthermore, the path cost of the starting node is calculated, 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 formula (1):

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

[0064] 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;

[0065] The expected cost of the starting node is calculated by the Manhattan function, and the path cost is calculated based on the expected cost:

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

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

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

[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 by the following formula:

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

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

[0072] The expected cost of the starting node is calculated by 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 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, and the actual cost of the starting node is 0.

[0077] Further, 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:

[0078] Introduce the DP cost table, initialize the storage of path cost, and represent the initialized path cost state as a four-tuple: \text{state} = (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 otherconditions)(4)

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

[0082] The starting node is taken as the parent node, and the path cost of its child nodes is calculated according to the state transfer equation and stored in the cost table.

[0083] Specifically, the DP cost table is introduced, the storage of path cost is initialized, and its state is represented as a four-tuple: \text{state} = (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 otherconditions)

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

[0087] As attached Figure 1 In this embodiment, the actual cost of the nodes located in the four positive directions of the parent node is added by 10, and the actual cost of the nodes located in the diagonal direction is 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. Figure 3 .

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

[0090] Sort all 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 it in a loop. Each time the nodes in the Open table are sorted, the nodes in the cost table are traversed first, and if they have been calculated, they are directly called;

[0091] When the preset condition that a child node coincides with the path target point is reached, the loop ends, and according to the cost table, the shortest cost path from the target node to the start node is searched in sequence, where the preset condition is when the selected parent node coincides with the target node or the Open table is empty.

[0092] Specifically, all nodes in the Open table are sorted by path cost, and the point with the minimum cost is selected as the parent node for the next time and stored in the Close table. The loop runs, and each time the nodes in the Open table are sorted, the nodes in the cost table are traversed first, and if they have been calculated, they are called directly.

[0093] When the selected parent node coincides with the target node or the Open table is empty, the loop ends, and the shortest cost path from the target node to the start node is searched in turn according to the cost table.

[0094] Furthermore, the final rescue path is generated, such as Figure 5 ,include:

[0095] The initial rescue path is optimized using the vertical distance limit method. The vertical distance between each node except the starting node and the target node and the two preceding and succeeding nodes is calculated. If the vertical distance is less than the threshold, the node is deleted. The nodes with the lowest path cost in the process of reaching the starting node from the target node are sequentially searched to generate the final rescue path. The Bezier curve is introduced to fit the final rescue path.

[0096] Specifically, the vertical distance limit method is used to optimize the A* initial path. The vertical distance between each node except the start node and the target node and the two previous and next nodes is calculated. When it is less than the threshold p, the node is deleted and the process is repeated to generate a smooth path curve.

[0097] Further, the method of constructing the Bezier curve is (such as Figure 4 ):

[0098] Set up the Bessel equation:

[0099]

[0100] Where n is the number of control points minus one, P i is the control point, is the number of combinations, B(t) is the function representation of the Bezier equation, i is the index variable, t is the fixed parameter in the Bezier curve, t∈[0,1];

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

[0102] B(t)=(1-t) 3 P 0 +3(1-t) 2 Tp 1 +3(1-t)t 2 P 2 +t 3 P 3 (6)

[0103] Where t is any value from 0 to 1, representing the position on the curve; P 0 is the starting point of the curve, P 1 and P 2 are the middle control points of the curve, P 3 is the end point of the curve.

[0104] The above are only preferred specific implementations 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 a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A rescue unmanned boat path planning method based on 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 method for rescue unmanned boat path planning based on optimized A* algorithm according to claim 1 is characterized in that: 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 boat path planning method based on the optimized A* algorithm according to claim 1 is characterized in that: 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 method for unmanned rescue boat path planning based on optimized A* algorithm according to claim 1, characterized in that: 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: The DP cost table is introduced to initialize the storage of the path cost, and the initialized path cost state is represented as a four-tuple: \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 index in the DP table, and dp[x][y] represents the parent node state; The starting node is taken as the parent node, and the path cost of its child nodes is calculated according to the state transfer equation and stored in the cost table.

5. The method for rescue unmanned boat path planning based on optimized A* algorithm according to claim 1, characterized in that: When the preset condition that a child node coincides with the path target point is met, the loop ends, including: Sort all 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 it in a loop. Each time the nodes in the Open table are sorted, the nodes in the cost table are traversed first, and if they have been calculated, they are directly called; When the preset condition that a child node coincides with the path target point is reached, the loop ends, and according to the cost table, the shortest cost path from the target node to the start node is searched in sequence, where the preset condition is when the selected parent node coincides with the target node or the Open table is empty.

6. The method for unmanned rescue boat path planning based on optimized A* algorithm according to claim 1, characterized in that: Generating the final rescue path includes: The initial rescue path is optimized using the vertical distance limit method. The vertical distance between each node except the starting node and the target node and the two preceding and succeeding nodes is calculated. If the vertical distance is less than the threshold, the node is deleted. The nodes with the lowest path cost in the process of reaching the starting node from the target node are sequentially searched to generate the final rescue path. The Bezier curve is introduced to fit the final rescue path.

7. The method for rescue unmanned boat path planning based on optimized A* algorithm according to claim 6, characterized in that: The method for constructing the Bezier curve is: Set up the Bessel equation: Where n is the number of control points minus one, P i is the control point, is the number of combinations, B(t) is the function representation of the Bezier equation, i is the index variable, t is the fixed parameter in the Bezier curve, t∈[0,1]; A cubic Bezier curve is established based on the Bezier equation, specifically: B(t)=(1-t) 3 P0+3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P3(6) Where 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 middle control points of the curve, and P3 is the end point of the curve.

Images