A path planning method for improving path safety and smoothness

Abstract

This invention discloses a path planning method to improve path safety and smoothness. The method initializes the costs from all non-obstacle nodes in a grid map to the starting point, no-turn markers, and the starting point cost. It then calculates the heuristic cost of the starting point and adds it to an open list. While the open list is not empty, a loop is executed, selecting the node with the lowest heuristic cost from the open list. now_s And add it to the closed list, if now_s It is the end point, ending the loop; for now_s Nodes within the eight-neighbor range sn ,like sn If not in the close list, check now_s Is it sn If the better parent node is found, update it. sn Find the parent node and the cost to the starting point, and determine the value of the no-turn sign; calculate sn The heuristic cost is reduced; after the loop ends, starting from the endpoint, a path is found based on the parent node and the no-turn sign. This invention incorporates obstacle proximity penalty, turn penalty, and turn restriction, ensuring the planned path stays away from obstacles and has fewer turns.

Description

Technical Field

[0001] This invention relates to a path planning method that improves path safety and smoothness, belonging to the field of path planning technology. Background Technology

[0002] Graph search methods are an important class of methods for implementing path planning. They expand from the starting point based on the connectivity between nodes, gradually expanding the nodes according to the principle of minimizing total cost, until the destination is reached. Then, they find the path from the destination based on the recorded parent node information. This method is applicable to both raster maps and network maps.

[0003] In grid maps, graph search path planning methods primarily use distance as the criterion for node expansion, neglecting the dangers of proximity to obstacles and the additional costs of turning paths. This often leads to problems such as paths passing through obstacle grid vertices, redundant turns, and frequent steering. Many studies have focused on optimizing these two issues by pre-expanding obstacles in the map and planning smooth paths afterward.

[0004] However, obstacles in inflated maps can further reduce the accuracy of grid maps, and their overly conservative approach can prevent the exploration of better paths. While post-planning smoothing can reduce path turns, it still relies on the initial planning and has limited optimization capabilities, failing to fundamentally prevent the creation of high-cost or even dangerous paths.

[0005] With the widespread application of autonomous vehicles, there is an urgent need for a safer and more flexible path planning method to plan safe and smooth driving routes for autonomous vehicles. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a path planning method that improves path safety and smoothness by adding obstacle proximity penalty, turning penalty and turning restriction, selecting safer nodes to generate a smoother path, so that the planned path is far away from obstacles and has fewer turns.

[0007] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0008] A path planning method to improve path safety and smoothness includes the following steps:

[0009] Step 1: Construct a grid map for the area to be planned, treat each grid as a node, determine the coordinates of the start point, end point and obstacle nodes, and define the remaining nodes as non-obstacle nodes;

[0010] Step 2: Initialize the cost from all non-obstacle nodes to the starting point to +∞ and the cost of the starting point to 0. Set the no-turn flag of all non-obstacle nodes to FALSE. Calculate the heuristic cost of the starting point and add the starting point to the open list.

[0011] Step 3: When the open list is not empty, execute the loop, remove the node now_s with the lowest heuristic cost from the open list and add it to the closed list; if now_s is the end point, end the loop and go to step 7.

[0012] Step 4: For the 8-neighbor node sn of now_s that is not in the closed list, determine whether now_s is a better parent node of sn. If it is a better parent node, update the parent node of sn and the cost from sn to the starting point, and set the no-turn flag of sn to FALSE, and go to step 5; otherwise, do nothing.

[0013] Step 5: If the turning cost of sn with now_s as its parent node is not 0, determine whether replacing the parent node of now_s with a neighboring node that makes the turning cost of sn 0 will increase the cost of sn to the starting point. If it does not increase the cost, set the no-turn flag of sn to TRUE; otherwise, do nothing.

[0014] Step 6: Calculate the heuristic cost of sn after updating the parent node. If sn is not in the open list, add it to the open list and go to step 3.

[0015] Step 7: Starting from the end point, find the planned path based on the parent node and the no-turn sign, and end at the starting point.

[0016] In a preferred embodiment of the present invention, the heuristic cost of the starting point in step 2 is calculated using the following formula:

[0017] f(s) = g(s) + h(s,goal)

[0018]

[0019] Where f(s) is the heuristic cost of any node s, g(s) is the cost from any node s to the starting point, h(s,goal) is the heuristic distance from any node s to the destination goal, and (x s ,y s ), (x goal ,y goal f(start) represents the coordinates of any node s and the endpoint goal, respectively; when s = start, f(start) is the heuristic cost of the starting point start.

[0020] In a preferred embodiment of the present invention, in step 4, determining whether now_s is a better parent node of sn, the specific process is as follows:

[0021] Step 4.1, calculate the obstacle penalty cost of sn.

[0022]

[0023] Where m is the number of obstacle nodes in the four nodes of sn (top, bottom, left, and right), and n is the number of obstacle nodes in the four nodes of sn (top left, top right, bottom left, and bottom right).

[0024] Step 4.2, calculate the Euclidean distance d(sn,now_s) between now_s and sn;

[0025] Step 4.3, calculate the turning cost of sn with now_s as the parent node.

[0026]

[0027] Where (now_s-sn) represents the direction of now_s relative to sn, (sf-now_s) represents the direction of sf relative to now_s, start represents the starting point, and sf represents the parent node of now_s;

[0028] Step 4.4, calculate the new cost of sn with now_s as the parent node:

[0029]

[0030] Where c(sn,now_s) is the new cost of sn with now_s as the parent node, r is the side length of a single grid cell in the raster map, and g(now_s) is the cost from now_s to the starting point;

[0031] Step 4.5: Determine the relationship between the new cost c(sn,now_s) and the current cost g(sn). If c(sn,now_s) is less than g(sn), or c(sn,now_s) is equal to g(sn) and the turning cost is less than g(sn), then the new cost c(sn,now_s) is less than g(sn). If the value is 0, then determine that now_s is the better parent node of sn, update the parent node of sn to now_s, update the cost of node sn to c(sn,now_s), and set the no-turn flag of node sn to FALSE.

[0032] As a preferred embodiment of the present invention, the specific process of step 5 is as follows:

[0033] If the turning cost of sn is not zero when now_s is its parent node, select the node sl in the neighborhood of now_s that makes the turning cost of sn zero, and calculate the non-turning cost of sn when now_s is its parent node.

[0034]

[0035] Where c(now_s, sl) represents the new cost of now_s with sl as its parent node; the cost without turning. If the cost is less than or equal to the current cost g(sn), then set the no-turn flag of node sn to TRUE and update the cost of node sn to... Otherwise, no action will be taken.

[0036] As a preferred embodiment of the present invention, the specific process of step 7 is as follows:

[0037] Start searching for a planned path from the endpoint. If the endpoint has no parent node, the path search fails; otherwise, add the endpoint to the path.

[0038] The newly added node is taken as the current path node. If the no-turn flag of the current path node is TRUE, the parent node of the current path node is added to the path as the next path node. Then, the neighboring nodes that are on the same straight line as the current path node and the parent node of the current path node are added to the path as the next next path node. The neighboring nodes are the neighboring nodes of the parent node of the current path node.

[0039] If the "no turn required" flag of the current path node is FALSE, then add the parent node of the current path node to the path as the next path node.

[0040] If the current path node is the starting point, the pathfinding ends and the planned path is output.

[0041] A computer device includes a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor executes the computer program to implement the steps of the path planning method for improving path safety and smoothness.

[0042] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the path planning method for improving path safety and smoothness.

[0043] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0044] 1. This invention does not require preprocessing of obstacles in the grid map, ensuring that the accuracy of the grid map is not reduced.

[0045] 2. This invention eliminates the need for path optimization after planning, fundamentally avoiding the generation of high-cost or even dangerous paths.

[0046] 3. This invention directly incorporates the penalty term into the cost function, fundamentally avoiding dangerous node target paths. Attached Figure Description

[0047] Figure 1 This is a flowchart of a path planning method for improving path safety and smoothness according to the present invention;

[0048] Figure 2 This is an embodiment of the path planning method of the present invention. Detailed Implementation

[0049] Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0050] like Figure 1 As shown, this invention proposes a path planning method to improve path safety and smoothness. By adding penalties for proximity to obstacles, turning penalties, and turning restrictions, it selects safer nodes to generate a smoother path. Specifically, it includes the following steps:

[0051] Step 1: Input the raster map, start and end coordinates, initialize the cost g(s) from all non-obstacle nodes to the start point to +∞, set the no-turn sign Turn(s) to FALSE, initialize the start point cost g(start) to 0, calculate the heuristic cost f(start) of the start point, add the start point to the open list, and calculate the heuristic cost of node s using the following formula:

[0052] f(s) = g(s) + h(s,goal)

[0053] In the formula, g(s) is the cost from s to the starting point, and h(s,goal) is the heuristic distance from s to goal, calculated using the following formula:

[0054]

[0055] In the formula, (x s ,y s ), (x goal ,y goal ) are the coordinates of s and goal, respectively.

[0056] Step 2: When the open list is not empty, execute the loop, remove the node now_s with the lowest heuristic cost from the open list and add it to the closed list. If now_s is the end point, end the loop and go to step 5.

[0057] Step 3: If the node sn in the 8-neighborhood of now_s is not in the closed list, determine whether now_s is a better parent node of sn. If it is a better parent node, and the turning cost of sn with now_s as its parent node is not 0, determine whether replacing the parent node of now_s with a neighboring node that makes the turning cost of sn 0 will increase the cost g(sn) from sn to the starting point. If it does not increase, set the no-turn flag Turn(sn) of sn to TRUE; otherwise, do nothing.

[0058] The determination of whether node now_s is a better parent node of its neighboring node sn includes the following steps:

[0059] Step 3.1: Calculate the obstacle penalty cost of sn using the following formula:

[0060]

[0061] In the formula, m is the number of obstacle nodes in the four nodes of sn (top, bottom, left, and right), and n is the number of obstacle nodes in the four nodes of sn (top left, top right, bottom left, and bottom right).

[0062] Step 3.2: Calculate the Euclidean distance d(sn,now_s) between now_s and sn;

[0063] Step 3.3: Calculate the turning cost of sn, now_s, and the parent node sf of now_s using the following formula:

[0064]

[0065] In the formula, (now_s-sn) represents the direction of now_s relative to sn, and (sf-now_s) is similar;

[0066] Step 3.4: Calculate the total cost of sn with now_s as the parent node using the following formula:

[0067]

[0068] In the formula, r represents the side length of a single grid in the raster map, and g(now_s) is the cost from now_s to the starting point;

[0069] Step 3.5: Determine the relationship between the new cost c(sn,now_s) and the current cost g(sn). If the new cost c(sn) is less than the current cost or equal to the current cost but the turning cost is less than the current cost, then the new cost c(sn) is less than the current cost. If the value is 0, then update the parent node of sn to now_s, update g(sn) to c(sn,now_s), and set the no-turn flag of node sn to FALSE.

[0070] When node now_s is a better parent node than its neighboring node sn, the value of the flag indicating that sn does not need to turn is:

[0071] If the turning cost of sn is not zero when now_s is its parent node, select the node sl in the neighborhood of now_s that makes the turning cost of sn zero, and calculate the non-turning cost using the following formula:

[0072]

[0073] The cost of not turning If the cost is not greater than the original cost g(sn), then set the no-turn flag Turn(sn) of node sn to TRUE, and update the cost g(sn) of node sn to...

[0074] Step 4: Calculate the heuristic cost of sn. If sn is not in the open list, add it to the open list and go to Step 2.

[0075] Step 5: Starting from the endpoint, find the path based on the parent node and the "no turns required" sign, and output the result.

[0076] When searching for a path based on the parent node and the "no turn required" flag, if the destination has no parent node record, the pathfinding fails; otherwise, the destination is added to the path. The latest node on the path is taken as the current node, and its parent node is added to the path. If the "no turn required" flag of the current node is TRUE, the neighboring nodes of the parent node in the straight line direction with the current node are added to the path. When the starting point is recorded, the pathfinding ends and the path is output; otherwise, the recording continues.

[0077] Example

[0078] This embodiment uses a simple 3×4 graph, where node [1,4] is an obstacle, node [2,1] is the starting point, node [3,4] is the ending point, and the grid side length is 1.

[0079] The first step, corresponding Figure 2 Step 1: Initialize the cost g(s) of all non-obstacle nodes to +∞, set the Turn(s) sign (no turning required) to FALSE, set the starting point cost g to 0, and calculate the heuristic distance from the starting point:

[0080]

[0081] Heuristic Cost:

[0082] f([2,1])=g([2,1])+h([2,1],[3,4])=0+3.414=3.414

[0083] Add the starting point to the open list openlist = [{[2,1],3.414}].

[0084] The second step is to stop the calculation when the open list is empty; otherwise, retrieve the node with the lowest heuristic cost from the open list and determine whether it is a better parent node of its neighboring nodes that are not in the closed list.

[0085] First round correspondence Figure 2 In step 2, extract node [2,1]. For its neighboring nodes [1,2], calculate the obstacle penalty cost:

[0086]

[0087] Turning costs:

[0088]

[0089] Total cost:

[0090]

[0091] and,

[0092] c([1,2],[2,1])<g([1,2])

[0093] Therefore, update g([1,2]) = 1.414 for node [1,2], with parent node [2,1]. Set the "no turn required" flag to FALSE. Since the turn cost is 0, no turn is constituted, and no further judgment is made. Calculate the heuristic cost:

[0094]

[0095] Add node [1,2] to the open list openlist=[{[1,2],4.242}].

[0096] Similarly, update nodes [2,2], [3,2], [3,1], and [1,1], and the open list is updated as follows:

[0097] openlist=[{[2,2],3.414},{[3,2],3.414},{[3,1],4},{[1,2],4.242},{[1,1],4.828}],

[0098] The closed list has been updated to:

[0099] closedlist = [[2,1]];

[0100] Second round corresponding Figure 2 In step 3, retrieve node [2,2], and update its neighboring nodes [1,3], [2,3], [3,3], [3,2], [3,1], [1,1], and [1,2]. The updated open list is as follows:

[0101] openlist=[{[3,2],3.414},{[3,3],3.914},{[3,1],4},{[1,2],4.242},{[2,3],4.414},{[1,1],4.828},{[1,3],7.328}],

[0102] The closed list has been updated to:

[0103] closedlist = [[2,1],[2,2]];

[0104] Third round of correspondence Figure 2 In step 4, retrieve node [3,2], update its neighboring nodes [2,3], [3,3], and [3,1]. The updated open list is as follows:

[0105] openlist=[{[3,3],3.914},{[3,1],4},{[1,2],4.242},{[2,3],4.414},{[1,1],4.828},{[1,3],7.328}],

[0106] The list of closed items is:

[0107] closedlist=[[2,1],[2,2],[3,2]];

[0108] Fourth round correspondence Figure 2 In step 5, retrieve node [3,3], update its neighboring nodes [2,4], [3,4], and [2,3]. The updated open list is as follows:

[0109] openlist=[{[3,4],3.914},{[3,1],4},{[1,2],4.242},{[2,3],4.414},{[1,1],4.828},{[1,3],7.328},{[2,4],7.828}],

[0110] The list of closed items is:

[0111] closedlist=[[2,1],[2,2],[3,2],[3,3]];

[0112] The no-turn flag for node [3,4] is TRUE (Turn([3,4])). The determination process is as follows:

[0113] Calculate obstacle costs:

[0114]

[0115] Turning costs:

[0116]

[0117] Total cost:

[0118]

[0119] and,

[0120] c([3,4],[3,3])<g([3,4])

[0121] Therefore, update node [3,4] with g([3,4]) = 4.414, the parent node is [3,3], and set the no-turn flag Turn([3,4]) = FALSE, since the turning cost... Since the value is not 0, further judgment is required:

[0122]

[0123]

[0124] and,

[0125]

[0126] Therefore, update node [3,4] with g([3,4]) = 3.914, and set the no-turn flag Turn([3,4]) = TRUE;

[0127] Fifth round correspondence Figure 2 In step 6, extract node [3,4] as the endpoint and stop the calculation.

[0128] The third step is to find a path based on the recorded successor nodes and turning constraints:

[0129] The parent node of the endpoint [3,4] is [3,3], and the path is {[3,3],[3,4]}.

[0130] The endpoint [3,4] does not require a turn, so the Turn([3,4]) is TRUE. Therefore, the next node is [3,3], which is in the direction of the turn that does not form a turn with [3,4]. The path is {[3,2],[3,3],[3,4]}.

[0131] The parent node of node [3,2] is [2,1], and the path is {[2,1],[3,2],[3,3],[3,4]}.

[0132] The endpoint is [2,1], so the path output ends and the final path is {[2,1],[3,2],[3,3],[3,4]}.

[0133] Based on the same inventive concept, embodiments of this application provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the aforementioned path planning method for improving path safety and smoothness.

[0134] Based on the same inventive concept, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the aforementioned path planning method for improving path safety and smoothness.

[0135] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0136] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0137] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0138] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0139] The above embodiments are merely illustrative of the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solutions based on the technical concept proposed in this invention shall fall within the scope of protection of this invention.

Claims

1. A path planning method for improving path safety and smoothness, characterized by, Includes the following steps: Step 1: Construct a grid map for the area to be planned, treat each grid as a node, determine the coordinates of the start point, end point and obstacle nodes, and define the remaining nodes as non-obstacle nodes; Step 2: Initialize the cost from all non-obstacle nodes to the starting point to +∞ and the cost of the starting point to 0. Set the no-turn flag of all non-obstacle nodes to FALSE. Calculate the heuristic cost of the starting point and add the starting point to the open list. Step 3: When the open list is not empty, execute the loop, remove the node now_s with the lowest heuristic cost from the open list and add it to the closed list; if now_s is the end point, end the loop and go to step 7. Step 4: For the 8-neighbor node sn of now_s that is not in the closed list, determine whether now_s is a better parent node of sn. If it is a better parent node, update the parent node of sn and the cost from sn to the starting point, and set the no-turn flag of sn to FALSE, and go to step 5; otherwise, do nothing. The specific process for determining whether now_s is a better parent node of sn is as follows: Step 4.1, calculate obstacle penalty cost of sn : ， Where m is the number of obstacle nodes in the four nodes of sn (top, bottom, left, and right), and n is the number of obstacle nodes in the four nodes of sn (top left, top right, bottom left, and bottom right). Step 4.2, compute the Euclidean distance between now_s and sn ; Step 4.3, compute sn as the turn cost with now_s as parent node : ， Where (now_s - sn) represents the direction of now_s relative to sn, (sf - now_s) represents the direction of sf relative to now_s, start represents the starting point, and sf represents the parent node of now_s; Step 4.4, calculate the new cost of sn with now_s as the parent node: ， wherein, r is the edge length of a single cell of the grid map, and g(now_s) is the cost from now_s to the start point. Step 4.5, judging new cost is related to the current cost g(sn), if is less than g(sn), or is equal to g(sn) and the turning cost is 0, then judge now_s as the more optimal parent node of sn, update the parent node of sn as now_s, update the cost of node sn as , and set the no-turning flag of node sn as FALSE; Step 5: If the turning cost of sn with now_s as its parent node is not 0, determine whether replacing the parent node of now_s with a neighboring node that makes the turning cost of sn 0 will increase the cost of sn to the starting point. If it does not increase the cost, set the no-turn flag of sn to TRUE; otherwise, do nothing. Step 6: Calculate the heuristic cost of sn after updating the parent node. If sn is not in the open list, add it to the open list and go to step 3. Step 7: Starting from the end point, find the planned path based on the parent node and the no-turn sign, and end at the starting point.

2. The path planning method for improving path safety and smoothness according to claim 1, wherein, In step 2, the heuristic cost of the starting point is calculated using the following formula: ， ， wherein, is the heuristic cost for any node s, is the cost from any node s to the start, is the heuristic distance from any node s to the goal goal, are the coordinates of any node s and the goal goal, respectively; when s = start, is the heuristic cost for the start start.

3. The path planning method for improving path safety and smoothness according to claim 1, characterized in that, The specific process of step 5 is as follows: If the turning cost of sn is not zero when now_s is its parent node, select the node sl in the neighborhood of now_s that makes the turning cost of sn zero, and calculate the non-turning cost of sn when now_s is its parent node. : ， in, This represents the new cost of now_s when sl is the parent node; the cost if no turn is made. If the cost is less than or equal to the current cost g(sn), then set the no-turn flag of node sn to TRUE and update the cost of node sn to... Otherwise, no action will be taken.

4. The path planning method for improving path safety and smoothness according to claim 1, characterized in that, The specific process of step 7 is as follows: Start searching for a planned path from the endpoint. If the endpoint has no parent node, the path search fails; otherwise, add the endpoint to the path. The newly added node is taken as the current path node. If the no-turn flag of the current path node is TRUE, the parent node of the current path node is added to the path as the next path node. Then, the neighboring nodes that are on the same straight line as the current path node and the parent node of the current path node are added to the path as the next next path node. The neighboring nodes are the neighboring nodes of the parent node of the current path node. If the "no turn required" flag of the current path node is FALSE, then add the parent node of the current path node to the path as the next path node. If the current path node is the starting point, the pathfinding ends and the planned path is output.

5. A computer device comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the path planning method for improving path safety and smoothness as described in any one of claims 1 to 4.

6. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the path planning method for improving path safety and smoothness as described in any one of claims 1 to 4.

Images