An efficient path planning method based on improved A* algorithm

Abstract

The present invention seeks to protect an efficient path planning method based on an improved A* algorithm, which is an improved A* algorithm that quickly obtains an optimal or suboptimal path in a large-area map environment. First, compared with the 8-neighborhood search method of the traditional A* algorithm, this paper uses a 4-neighborhood search method suitable for large-area grid maps, which greatly reduces the calculation of some unnecessary nodes. Secondly, this paper improves the heuristic function and gives different weights to the estimated cost of the heuristic function, so that the mobile robot can flexibly calculate the estimated cost of the node according to the distance from the starting point and the target point during the planning process. Finally, by selecting multiple groups of target points with different distances, the A* algorithm before and after the improvement was simulated and tested, and the planning time, the number of visited nodes, and the smoothness of the path of each group were compared to verify the efficiency of the proposed algorithm in path planning.

Description

Technical Field

[0001] The invention belongs to the technical field of mobile robot path planning, and relates to an efficient path planning method based on an improved A* algorithm. Background Art

[0002] Path planning is a method of finding a relatively optimal path between two locations in a specific environment. In the process of mobile robot navigation, the path planning algorithm is used to determine the nodes that need to be traversed, and the current path is gradually determined by calculating the cost value of the nodes until the target point is reached or the search fails. In an actual environment where the distance between the starting point and the target point is far, as the number of grids increases, the amount of calculation increases exponentially, and it is particularly important to efficiently plan a feasible path.

[0003] The A* algorithm is a heuristic search algorithm. The entire workplace of the mobile robot is divided into a grid, and each grid represents a node on a certain path. The A* algorithm calculates the cost value of each node in its working area starting from the starting point to obtain the optimal solution, and finally obtains the current optimal or suboptimal path through step-by-step iteration. However, the traditional A* algorithm has certain limitations. In 2018, B. Fu et al. proposed an improved A* algorithm that shortens the real-time planned path by judging whether there are obstacles between the nodes on the current path and the target point; however, the calculation of a large number of nodes in the path makes this improved A* algorithm less efficient, and the planned path has frequent corners and poor smoothness. In 2019, R. Song et al. used three different smoothing factors to reduce the number of turning points, thereby deleting some redundant nodes in the path planning, and repeated iterative calculations were performed on some fixed nodes. In 2020, R. Saeed et al. introduced a path enhancement method that shortens the path length by judging whether the connection between adjacent path nodes passes through obstacles; in a complex obstacle environment, more nodes will be traversed, which means that more path planning time is required.

[0004] CN111272187B, an optimal driving path planning method and system based on an improved A* algorithm, comprising: obtaining the starting node and target node of the path to be planned; based on the starting node and target node of the path to be planned, using the improved A* algorithm to obtain the optimal driving path; the improved A* algorithm is an improvement on the evaluation function of the A* algorithm; the improved evaluation function is: the product of the evaluation function of the traditional A* algorithm and the direction parameter. Starting from the evaluation function of the A* algorithm, the A* algorithm is improved using straight lines and direction parameters to plan a shorter travel path for the driver, saving the user's travel time and improving travel efficiency. This patent improves the cost function of the A* algorithm, multiplying the direction parameter on the traditional A* cost function, in order to shorten the path distance after planning. Its technical differences and shortcomings from the present invention and the improvement methods of the present invention are described as follows:

[0005] First, there are Euler distance, Manhattan distance, diagonal distance and other calculation methods for calculating the estimated cost of the A* algorithm. Through simulation experiments on different calculation formulas, the present invention finally selected the Manhattan distance, which takes a shorter time, as the calculation formula for the estimated cost;

[0006] Secondly, in terms of the algorithm search method, the patent uses the traditional A* algorithm search method, that is, 8 neighborhood searches around the current node. However, the present invention has made improvements on the search algorithm, using a 4-neighborhood search method, which greatly reduces the visits to unnecessary nodes and the calculation of node cost values, and improves the overall planning efficiency;

[0007] Again, the patent multiplies the direction parameter B(n) on the traditional A* cost function. The angle θ between the line from the current node to the next node and the line between the current node and the previous node is calculated as a parameter. According to the formula Direction parameters are calculated to correct the path direction, shortening the planned path distance. The present invention proposes an environmental position coefficient τ, which is calculated based on the number of obstacles around the node and the total number of obstacles in the environment when the algorithm searches for a node, increasing the weight of the cost function while giving the algorithm adaptability in the environment.

[0008] Finally, the patent adds the direction parameter of the current node as the weight of the cost function of the A* algorithm, aiming to correct the planning direction in planning to obtain the shortest path. Starting from optimizing the search complexity of the A* algorithm, the present invention adopts a search method of 4 neighborhoods around the node; it proposes an environmental position coefficient that is adaptive to environmental information and acts on the cost function of the A* algorithm, further reducing planning time and improving algorithm efficiency. Summary of the invention

[0009] The present invention aims to solve the above problems of the prior art. An efficient path planning method based on an improved A* algorithm is proposed. The technical solution of the present invention is as follows:

[0010] An efficient path planning method based on an improved A* algorithm comprises the following steps:

[0011] S1. Establish a map; obtain the initial position of the mobile robot as the starting point S, and obtain its coordinates on the grid map;

[0012] S2, taking the node to be expanded as the parent node, and performing expansion search in four non-diagonal directions of its surrounding neighborhood;

[0013] S3. According to the distribution of obstacles in the grid map, the number of obstacle grids within the search range from the current node to the target point is obtained;

[0014] S4. Combined with the total number of obstacles in the grid map, an environmental position coefficient τ acting on the estimated cost h(n) is proposed. The environmental position coefficient τ corresponding to the estimated cost of each node will change with the change of the number of surrounding obstacles.

[0015] S5. Calculate the total cost of each expanded node, wherein the Manhattan distance is used as a calculation formula for calculating the estimated cost h(n);

[0016] S6. After searching the target point in the map using the improved A* algorithm of S2-S5, the final path is obtained.

[0017] Furthermore, the step S1 of establishing the map specifically includes: expanding the number of grids of the simulation map to 100×100, the obstacle coverage rate to 20%, selecting 5 different target points from near to far, and comparing and analyzing the A* algorithm before the improvement and the A* algorithm with the estimated cost of the position environment coefficient added; obtaining the coordinates S(x s ,y x ), the coordinates of the five different targets are G(x g ,y g ).

[0018] Furthermore, the step S1 also includes: initializing an OPEN_LIST list to store unexpanded nodes, and a CLOSE_LIST list to store calculated nodes, and adding the starting position node S to the OPEN_LIST.

[0019] Furthermore, S2 searches in four directions around the parent node as follows: the starting position coordinates of the mobile robot are taken as the first parent node, and the actual running direction of the robot in the map is: horizontal direction, vertical direction, and 360-degree turn on the spot. Combined with the running direction of the robot, the neighboring nodes around the parent node are searched in the vertical and horizontal directions, and non-obstacle nodes are added to OPEN_LIST.

[0020] Furthermore, the step S3 obtains the distribution of obstacles in the grid map as follows: when the current node is far away from the target point, the estimated cost h(n) at this time is much smaller than the actual value, increasing the proportion of h(n) in the total cost; conversely, when the search route gradually approaches the target node, the proportion of h(n) is reduced; in order to allow the mobile robot to adaptively adjust the proportion of h(n) at different positions in the environment, a mobile robot environmental position coefficient τ is proposed; the number of grids composed of the robot's starting position and the target point is defined as Q, and the number of grid obstacles within the search range from the current node n to the target point G is defined as P, then the τ value at a certain position is the ratio of P to Q.

[0021] Furthermore, the step S4 starts from the starting position node and calculates the estimated cost h(n) of each extended node. Specifically, an OPEN_LIST list is initialized to store unexpanded nodes, and a CLOSE_LIST list is used to store calculated nodes. The starting position node S is added to the OPEN_LIST, and the starting position node is used as the parent node. The four neighboring nodes around it that are not occupied by obstacles are added to the OPEN_LIST; the total cost f(n) of the nodes added to the OPEN_LIST is calculated, and the starting position node S is deleted from the OPEN_LIST and added to the CLOSE_LIST; the node with the lowest total cost is selected as the current node, and the node is deleted from the OPEN_LIST and added to the CLOSE_LIST; the calculation is continued in a loop until the target node appears in the OPEN_LIST and the search ends.

[0022] Furthermore, the calculation formula of the total cost f(n) is as follows:

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

[0024] g(n) is the actual cost of the current node from the starting position, and h(n) is the estimated cost of the current node from the target point;

[0025] In order to adaptively adjust the effect of the cost function at different distances, the environmental position coefficient τ of the mobile robot is introduced. The number of grids composed of the robot's starting position and the target point is defined as Q; the number of grid obstacles within the search range from the current node n to the target point G is P, and the environmental position coefficient of the robot can be expressed as:

[0026]

[0027] Step S5 combines Manhattan distance as the calculation function of the estimated cost. At this time, the estimated cost function is optimized as follows:

[0028] The estimated cost h(n) is:

[0029] h(n)=e τ (x n -x g |+|y n -y g |)

[0030] τ is the environmental position coefficient: (x n ,y n ) and (x g ,y g ) are the coordinates of the current node n and the target point G respectively.

[0031] The advantages and beneficial effects of the present invention are as follows:

[0032] The present invention proposes a location environment coefficient τ acting on the estimated cost function of the A* algorithm, and improves the traditional A* algorithm. Through simulation experiments, the final planned path length and processing time before and after the A* algorithm was improved were compared and calculated. The test results show that the improved A* algorithm has a 63.8% increase in search efficiency compared to the algorithm before the improvement. When the distance between the starting position coordinates and the target point coordinates increases, the time taken by the improved A* algorithm to plan the path is significantly less than that of the A* algorithm before the improvement, and the number of nodes expanded in the heuristic path search process is greatly reduced, thereby improving the search efficiency.

[0033] Based on the traditional A* algorithm, this paper optimizes the search method of the current node neighborhood, changing the traditional 8-way search to a 4-way search suitable for long distances and static obstacles, reducing the number of node searches and improving the calculation efficiency; S2, through comparison and selection, Manhattan distance is selected as the calculation method for the estimated cost h(n), which has fewer nodes and shorter calculation time than the other two calculation methods; S3, in order to adaptively adjust the weight of the estimated cost h(n) at different positions from the target point during path planning, this paper introduces the environmental position coefficient τ and improves the heuristic function of the traditional A* algorithm; S4, based on a grid map of size 100×100 and the same obstacle ratio, the A* algorithm before and after the improvement is compared with the target point from near to far for path planning simulation comparison test. The experimental results show that when the target point is far away and it is ensured that an optimal or suboptimal path can be found, the time of the improved A* algorithm is still greatly reduced compared with the original one. Therefore, the improved A* algorithm is suitable for large-area and long-distance path planning.

[0034] The present invention firstly improves the search method of the A* algorithm, changing the original 8-neighborhood search method to a 4-neighborhood search method, thereby reducing the calculation of the total cost value of redundant nodes; secondly, the cost function of the A* algorithm is improved, and the environmental position coefficient τ is calculated according to the relationship between the number of obstacles in the environment and the total number of obstacles. The coefficient is expressed as eτ The form of is used as the weight coefficient of the cost function, so that the algorithm can change the size of the weight value according to different environments, which is a dynamic weight coefficient. This dynamic weight calculation method that changes with environmental obstacles is the innovation point of the present invention. Its advantage is that it balances the proportion of the actual cost g(n) and the estimated cost h(n) in the total cost formula at different node positions, so that the algorithm can ensure that an optimal or suboptimal path is searched out while significantly improving its search efficiency. It is specifically reflected in the definition and calculation of the weight of the estimated cost function. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flow chart of the improved A* algorithm according to the preferred embodiment of the present invention;

[0036] Figure 2 This is a planning comparison chart of three different target points before and after the A* algorithm improvement. DETAILED DESCRIPTION

[0037] The following will describe the technical solutions in the embodiments of the present invention in detail in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention.

[0038] The technical solution of the present invention to solve the above technical problems is:

[0039] This paper proposes an improved A* algorithm that can quickly obtain an optimal or suboptimal path in a large-area map environment. First, compared with the 8-neighborhood search method of the traditional A* algorithm, this paper uses a 4-neighborhood search method suitable for large-area grid maps, which greatly reduces the calculation of some unnecessary nodes. Secondly, this paper improves the heuristic function and gives different weights to the estimated cost of the heuristic function, so that the mobile robot can flexibly calculate the estimated cost of the node according to the distance from the starting point and the target point during the planning process. Finally, by selecting multiple groups of target points with different distances, the A* algorithm before and after the improvement was simulated and tested, and the planning time, the number of visited nodes, and the smoothness of the path of each group were compared to verify the efficiency of the proposed algorithm in path planning.

[0040] The technical solution of the present invention is as follows:

[0041] S1, the node to be expanded is taken as the parent node, and the four non-diagonal directions of its surrounding neighborhood are expanded and searched;

[0042] S2, according to the distribution of obstacles in the grid map, obtain the number of obstacles within the search range from the current node to the target point and the total number of obstacles in the map;

[0043] S3, combined with the total number of obstacles in the grid map, proposes the environmental position coefficient τ acting on the estimated cost h(n). The coefficient corresponding to the estimated cost of nodes at different positions is adaptive to the change of environmental position.

[0044] S4, both use Manhattan distance as the calculation formula to calculate the estimated cost h(n) and the total cost f(n) of each expanded node.

[0045] S5, after the improved A* algorithm searches for the target point in the map, the final path is obtained.

[0046] Furthermore, the above search in four directions around the parent node is specifically that the starting position coordinates of the mobile robot are used as the first parent node. The actual running directions of the robot in the map are: horizontal direction, vertical direction, and 360-degree turn on the spot. Combined with the running direction of the robot, the neighboring nodes around the parent node are searched in the vertical and horizontal directions, and the non-obstacle nodes are added to the OPEN_LIST.

[0047] Furthermore, the distribution of obstacles in the above-mentioned grid map is specifically that when the current node is far away from the target point, the estimated cost h(n) at this time is much smaller than the actual value. Therefore, in order to improve the search efficiency, the proportion of h(n) in the total cost value should be appropriately increased; on the contrary, when the search route gradually approaches the target node, the large h(n) value easily causes the path planning to fall into the local optimum, and at this time, the proportion of h(n) should be reduced. In order to allow the mobile robot to adaptively adjust the proportion of h(n) at different positions in the environment to improve the planning efficiency and ensure the robustness of the algorithm, the mobile robot environment position coefficient τ is proposed. Define the number of grids composed of the robot's starting position and the target point as Q, and the number of grid obstacles in the search range from the current node n to the target point G as P. Then the τ value of a certain position is the ratio of P and Q.

[0048] Further, starting from the starting position node, the estimated cost h(n) of each extended node is calculated as follows: an OPEN_LIST list is initialized to store the unexpanded nodes, and a CLOSE_LIST list is used to store the calculated nodes, and the starting position node S is added to the OPEN_LIST. The starting position node is used as the parent node, and the four neighboring nodes around it that are not occupied by obstacles are added to the OPEN_LIST. Start calculating the total cost f(n) of the nodes added to the OPEN_LIST, and delete the starting position node S from the OPEN_LIST and add it to the CLOSE_LIST. Select the node with the lowest total cost as the current node, delete the node from the OPEN_LIST and add it to the CLOSE_LIST. Continue the calculation in a loop until the target node appears in the OPEN_LIST and the search ends. During the search process, the improved A* algorithm has an environmental position coefficient τ, which avoids the path planning from falling into the local optimum and improves the algorithm operation efficiency.

[0049] Preferably, Figure 1 As shown, the present invention provides an efficient path planning method based on an improved A* algorithm, which comprises the following steps:

[0050] S1, build a map. In order to simulate the path planning of the improved A* algorithm in a large area environment, the number of grids in the simulation map is expanded to 100×100, the obstacle coverage rate is 20%, and 5 different target points are selected from near to far to compare and analyze the A* algorithm before improvement and the A* algorithm with the estimated cost of the position environment coefficient. Get the coordinates S(x s ,y x ), the coordinates of the five different targets are G(x g ,y g ), the parameters are shown in Table 1.

[0051] S2, initialize an OPEN_LIST list to store unexpanded nodes and a CLOSE_LIST list to store calculated nodes, and add the starting position node S to the OPEN_LIST.

[0052] S3, the starting position node S is used as the parent node, and the four neighboring nodes around it that are not occupied by obstacles are added to the OPEN_LIST.

[0053] S4, start calculating the total cost f(n) of adding nodes to OPEN_LIST, and delete the starting node S from OPEN_LIST and add it to CLOSE_LIST. The total cost function of the improved A* algorithm is: f(n) = g(n) + h(n) where the estimated cost h(n) is:

[0054] h(n)=e τ (x n -x g |+|y n -y g |) (6)

[0055] τ is the environmental position coefficient: the number of grids formed by the robot's starting position S and the target point is Q, the number of grid obstacles within the search range from the current node n to the target point G is P, then the value of a certain position is the ratio of P and Q.

[0056] S5, select the node with the lowest total cost value as the current node, delete the node from OPEN_LIST, and add the current node n to CLOSE_LIST.

[0057] S6, determine whether the node n in CLOSE_LIST is the target node G. If it is the target node, the search ends, and the final path is output by recursive backtracking; if it is not the target node, the current node n is taken as the parent node.

[0058] S7, add all nodes in the surrounding area of ​​the parent node that are not in OPEN_LIST and are not occupied by obstacles to OPEN_LIST. Calculate f(n) of the nodes in OPEN_LIST, and delete the node with the smallest calculated f(n) value from OPEN_LIST and add it to CLOSE_LIST.

[0059] S8, return to S6, continue the loop search until the target node G is found, output the shortest path, and end the search.

[0060] Table 2 shows the simulation results of 5 groups of target points. The results in the table show that the improved A* algorithm takes less time to plan the path when the optimal or suboptimal path can be found. Figure 2 As shown in Table 2, under the same starting position, 5 different target nodes were selected, and the paths were planned using the A* algorithm before and after the improvement. When the paths can be obtained, as the distance between the target point and the starting position increases, the planning time of the improved A* algorithm is half of that before the improvement or even shorter, and the number of expanded nodes is reduced exponentially. The simulation results show that in long-distance path planning, the improved A* algorithm is more efficient while ensuring the robustness of the algorithm.

[0061]

[0062] Table 1

[0063]

[0064] Table 2

[0065] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0066] The above embodiments should be understood to be only used to illustrate the present invention and not to limit the protection scope of the present invention. After reading the contents of the present invention, technicians can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the present invention.

Claims

1. An efficient path planning method based on an improved A* algorithm, characterized in that: The following steps are involved: S1. Establish a map; obtain the initial position of the mobile robot as the starting point S, and obtain its coordinates on the grid map; S2, taking the node to be expanded as the parent node, and performing expansion search in four non-diagonal directions of its surrounding neighborhood; S3. According to the distribution of obstacles in the grid map, the number of obstacle grids within the search range from the current node to the target point is obtained; S4. Combined with the total number of obstacles in the grid map, an environmental position coefficient τ acting on the estimated cost h(n) is proposed. The environmental position coefficient τ corresponding to the estimated cost of each node will change with the change of the number of surrounding obstacles. S5. Calculate the total cost of each expanded node, wherein the Manhattan distance is used as the calculation formula for calculating the estimated cost h(n); S6, after searching the target point in the map using the A* algorithm improved by S2-S5, the final path is obtained; The step S4 starts from the starting position node and calculates the estimated cost h(n) of each expanded node. Specifically, an OPEN_LIST list is initialized to store unexpanded nodes, and a CLOSE_LIST list is used to store calculated nodes. The starting position node S is added to the OPEN_LIST, and the starting position node is used as a parent node. The four neighboring nodes around it that are not occupied by obstacles are added to the OPEN_LIST; the total cost f(n) of the nodes added to the OPEN_LIST is calculated, and the starting position node S is deleted from the OPEN_LIST and added to the CLOSE_LIST; the node with the lowest total cost is selected as the current node, and the node is deleted from the OPEN_LIST and added to the CLOSE_LIST; the calculation is continued in a loop until the target node appears in the OPEN_LIST and the search ends; The calculation formula of the total cost f(n) is as follows: f(n)=g(n)+h(n) g(n) is the actual cost of the current node from the starting position, and h(n) is the estimated cost of the current node from the target point; In order to adaptively adjust the effect of the cost function at different distances, the environmental position coefficient τ of the mobile robot is introduced. The number of grids composed of the robot's starting position and the target point is defined as Q; the number of grid obstacles within the search range from the current node n to the target point G is P, and the environmental position coefficient of the robot can be expressed as: Step S5 combines Manhattan distance as the calculation function of the estimated cost. At this time, the estimated cost function is optimized as follows: The estimated cost h(n) is: h(n)=e τ (x n -x g |+|y n -y g |) τ is the environmental position coefficient: (x n ,y n ) and (x g ,y g ) are the coordinates of the current node n and the target point G respectively.

2. The efficient path planning method based on the improved A* algorithm according to claim 1, characterized in that: The step S1 of establishing a map specifically includes: expanding the number of grids in the simulation map to 100×100, with an obstacle coverage rate of 20%, selecting 5 different target points from near to far to compare and analyze the A* algorithm before the improvement and the A* algorithm with the estimated cost of the position environment coefficient added; obtaining the coordinates S(x s ,y x ), the coordinates of the five different targets are G(x g ,y g ).

3. The efficient path planning method based on the improved A* algorithm according to claim 2, characterized in that: The step S1 also includes: initializing an OPEN_LIST list to store unexpanded nodes, and a CLOSE_LIST list to store calculated nodes, and adding the starting position node S to the OPEN_LIST.

4. The efficient path planning method based on the improved A* algorithm according to claim 3 is characterized in that: The S2 searches in four directions around the parent node as follows: the starting position coordinates of the mobile robot are taken as the first parent node, and the actual running direction of the robot in the map is: horizontal direction, vertical direction, and 360-degree turn on the spot. Combined with the running direction of the robot, the neighboring nodes around the parent node are searched in the vertical and horizontal directions, and non-obstacle nodes are added to the OPEN_LIST.

5. The efficient path planning method based on the improved A* algorithm according to claim 4, characterized in that: The step S3 obtains the distribution of obstacles in the grid map as follows: when the current node is far away from the target point, the estimated cost h(n) at this time is much smaller than the actual value, increasing the proportion of h(n) in the total cost value; conversely, when the search route gradually approaches the target node, the proportion of h(n) is reduced; in order to allow the mobile robot to adaptively adjust the proportion of h(n) at different positions in the environment, a mobile robot environment position coefficient τ is proposed; the number of grids composed of the robot's starting position and the target point is defined as Q, and the number of grid obstacles within the search range from the current node n to the target point G is defined as P, then the τ value of a certain position is the ratio of P to Q.

Images