Path planning method based on improved A*

Abstract

The invention discloses a path planning method based on improved A *, and relates to the technical field of path planning, and the method comprises the steps: initializing a map environment, determining a starting point and an ending point of a to-be-planned path, initializing an obstacle list, opening the list and closing the list, traversing all nodes in a map, obtaining obstacle nodes, putting the obstacle nodes into the obstacle list, and completing the path planning. Setting the closed list to be empty, and putting the starting point into an open list; searching initial path points from the starting point in eight directions of the current node, putting the initial path points into an open list, preferentially searching at least one initial path point in combination with a cost function, taking the initial path point with the minimum cost value as a father node for subsequent searching, and moving the point into a closed list from the open list; all turning points in the initial path are extracted, whether the shortest path between the two points before and after the turning points passes through the obstacle is judged, the initial path is updated according to the judgment result to obtain the optimal path, and the path length can be shortened.

Description

Technical Field

[0001] The present disclosure relates to the technical field of path planning, and particularly to a path planning method based on improved A*. Background Art

[0002] Path planning is one of the important technologies in the field of mobile robots and is also one of the key technologies in the fields of autonomous driving and navigation. The research objective is that a mobile robot needs to plan a path from the initial position to the target position in the working scenario. This path should meet a series of requirements such as safety, efficiency, optimality, and reasonableness, and must be able to avoid obstacles along the way. Path planning algorithms are divided into global path planning (static environment) and local path planning (dynamic environment). Global path planning algorithms mainly include: graph search algorithms (BFS, DFS, Dijkstrα, A*), sampling planning algorithms (PRM, RRT), etc. Local path planning algorithms mainly include: artificial potential field method (Artificial Potential Field, APF), dynamic window method (Dynamic Window Approach, DWA), etc.

[0003] Graph search algorithms are algorithms used to find paths, traverse nodes, or solve other graph-related problems in graph or tree data structures. Breadth First Search (BFS) is a graph search algorithm that starts from the starting node and visits all adjacent nodes layer by layer, suitable for the shortest path problem of unweighted graphs; Depth-First-Search (DFS) is an algorithm that starts from the starting node and continuously delves along a branch until it encounters a dead end or the end point, and then backtracks to the previous node to continue exploration, suitable for the depth traversal and topological sorting of graphs; Dijkstrα algorithm is an algorithm for finding the shortest paths from the starting point to all other nodes in a weighted graph. It gradually expands the nodes closest to the starting point and uses a priority queue to select nodes; A* algorithm is a heuristic search algorithm that finds the shortest path from the starting point to the target node by combining the actual cost and the estimated cost (heuristic function). It uses a priority queue to manage nodes, and the priority is determined by the sum of the actual cost and the heuristic estimated cost.

[0004] Sampling planning algorithms are a class of algorithms for path planning in high-dimensional spaces. They find feasible paths through random sampling methods and are particularly suitable for path planning problems in complex environments. PRM is an algorithm for path planning in high-dimensional spaces. It generates a graph (road map) through random sampling and then searches for paths in the graph, suitable for static environments and high-dimensional spaces; RRT is an algorithm for path planning in high-dimensional spaces. It explores the space by gradually expanding a tree-like structure and is suitable for path planning in dynamic environments and high-dimensional spaces.

[0005] APF regards the target point and obstacles as an attractive potential field and a repulsive potential field respectively. When the robot moves in the potential field, it is comprehensively affected by the attractive force of the target point and the repulsive force of the obstacles, so as to select the optimal path. However, its local minimum problem and performance in complex environments may affect its effect.

[0006] DWΑ selects an optimal speed that enables the robot to avoid obstacles and move towards the target in the next time by sampling and evaluating in the current speed space of the robot. Although it has disadvantages such as local minimum problems and sensitivity to parameters, it has shown good effects in many practical applications. Summary of the Invention

[0007] The main technical problem to be solved by the present invention is to overcome the above-mentioned defects existing in the prior art, and provide a path planning method based on improved A*, so as to improve the search speed, obtain a path with fewer nodes and shorter distance through path smoothing processing, and higher efficiency.

[0008] The technical solution adopted by the present invention to solve its technical problems is: According to one aspect of the present disclosure, there is provided a path planning method based on improved A*, including the following steps: S1. Initialize the map environment, determine the starting point and the ending point of the path to be planned, initialize the obstacle list, the open list and the closed list, traverse all nodes in the map, obtain the obstacle nodes, put the obstacle nodes into the obstacle list, set the closed list to be empty, and put the starting point into the open list; S2. Starting from the starting point, search for initial path points in eight directions of the current node, put the initial path points that meet the conditions into the open list, combine the cost function to preferentially search for at least one initial path point, take the initial path point with the smallest cost value as the parent node for subsequent search, and move this point from the open list to the closed list. Among them, the path formed by the initial path points in the closed list is the initial path from the starting point to the ending point, the cost function is the sum of the actual cost and the heuristic function cost, and the initial path point is a passable neighbor node corresponding to the current parent node; S3. Extract all turning points in the initial path, judge whether the shortest path between two points before and after the turning point passes through an obstacle, and update the path according to the judgment result to obtain the optimal path.

[0009] In one embodiment of the present disclosure, in step S3, all turning points in the path are found according to the slope. To determine whether the shortest path between the two points before and after the turning point passes through an obstacle, all nodes passed by the path are calculated based on the angle and orientation between the two points. If the node set contains an obstacle node, it indicates that the current turning point cannot be optimized. End the current turning point and start to judge the next turning point. If none of the nodes contain an obstacle node, it means that this turning point can be optimized. Delete this turning point from the path set, update the path point set, and then continue with the next turning point until all turning points have been traversed once to obtain the updated path point set. The obtained new path point set is continuously optimized using the turning point optimization algorithm until the path cannot be optimized, that is, the path remains unchanged before and after optimization, to obtain the optimal path.

[0010] In one embodiment of the present disclosure, finding all turning points in the path according to the slope includes: Initialize a turning point set that stores the turning points in the path. The path points of the path include the starting point, the ending point, and the intermediate path nodes between the starting point and the ending point; Select the first point of the intermediate path nodes and set it as the current path point, and calculate the slope between the current path point and the previous path node , and calculate the slope between the current path point and the next path node , and judge the slope and whether they are different. If they are different, it means that the current path point is a turning point, record the coordinates of the current path point, and put them into the turning point set; if the slopes are the same, there is no turning; Set the second point of the intermediate path nodes as the current path point, and repeat the above steps until the current path point is the last intermediate path node, and the search for turning points ends.

[0011] In one embodiment of the present disclosure, calculating all nodes passed by the path based on the angle and orientation between two points includes: determining the front and rear nodes of the turning point , ( ), calculate the absolute value of the angle of node relative to node , according to 's magnitude and the orientation of node relative to node to find all nodes passed by the connection between the two nodes, where the front and rear nodes and are determined by determining the position index of the turning point in the path point set coordinates. ​

[0012] In one embodiment of the present disclosure, according to the magnitude of α and the node relative to the node to find all the nodes passed through by the connection of two nodes, including: Initialize a node list for storing all the nodes passed through by the line connecting the two points; When 0° < < 45°, use the vertical axis criterion to determine whether the node list contains obstacle nodes. The vertical axis criterion is used to determine the position of the intersection of the connection between the two points and the vertical axis, and obtain the position coordinates of the nodes on the left and right sides of the intersection; When 45° < < 90°, use the horizontal axis criterion to determine whether the node list contains obstacle nodes. The horizontal axis criterion is used to determine the position of the intersection of the connection between the two points and the horizontal axis, and obtain the position coordinates of the nodes above and below the intersection; If the node list does not contain obstacle nodes, delete the turning point and update the path; if the node list contains obstacle nodes, end the current turning point; Judge whether the new path is the same as the original path. If it is the same, output the new path as the optimal path and end; if it is different, continue to find all the turning points in the path set and perform the next optimization on the path.

[0013] In one embodiment of the present disclosure, in S2, the current node is determined as the parent node in the search process. The first current node is the starting point of the initial path, and the last current node is the target point; the eight directions are up, down, left, right, upper right, upper left, lower right, and lower left, respectively corresponding to the eight neighbor nodes of the parent node. The next path point of the current node is an accessible neighbor node in any of these eight directions, denoted as the initial path point; the actual cost is the Euclidean distance from the starting point to the current neighbor node; the heuristic function cost is the product of the diagonal distance from the current node to the target point and the weight ; Find the initial path point with the smallest total cost value F in the open list, and remove the initial path point with the smallest total cost value F from the open list and put it into the closed list. Take the initial path point with the smallest total cost value F as the parent node for the next search, and perform loop search until the parent node for the next search is the end point, then the search ends.

[0014] In one embodiment of the present disclosure, during the search process, for each parent node, its eight neighbor nodes need to be traversed in sequence, and the position coordinates of each neighbor node are calculated in sequence according to the position coordinates of the parent node; Calculate the coordinates of the neighbor nodes based on the coordinates of the parent node. Within the map range, starting from the upper right and in a counterclockwise direction, obtain the current node The coordinates of the eight neighbor nodes are successively: , .

[0015] In one embodiment of the present disclosure, in the step S2, according to the obtained coordinates of the eight neighbor nodes, each of the neighbor nodes is successively judged whether to be placed in the open list. If a neighbor node is within the map range, is not an obstacle node and is not in the open list and the closed list, then the neighbor node is placed in the open list as the initial path point; if the neighbor node is not a point within the map, or is an obstacle node, or the node is already in the closed list, then the neighbor node is discarded; if the node is in the open list and the total cost value F is smaller than that of this point in the open list, then update the total cost value of this node in the open list F .

[0016] In one embodiment of the present disclosure, calculate the cost values between the parent node and all neighbor nodes in the open list , which is represented by the Euclidean distance, as follows: (1), wherein, is the coordinate value of the current node, is the coordinate value of the current parent node.

[0017] In one embodiment of the present disclosure, according to the diagonal distance formula, calculate the diagonal distance value from the current neighbor node to the target point, as follows: (2), (3), (4), wherein, is to take the minimum of the two, is the end point coordinate value, is the current neighbor node coordinate value; The heuristic function cost is expressed as: (5), (6), Wherein, is the distance from the current neighbor node to the end point, represented by the diagonal distance, is the distance from the starting point to the end point, represented by the diagonal distance; The total cost value is expressed as: (7), Wherein, is the sum of the actual cost value of the previous path and the cost value of the current parent node, is the cost of the heuristic function.

[0018] The beneficial effects of the present invention are as follows: In the global path planning of the mobile robot, compared with before the improvement, the improved algorithm has a significant improvement in the search speed. At the same time, through the path smoothing process of the present invention, a path with fewer nodes and shorter distance can be obtained. The improved A* algorithm has better stability and can be widely applied to different map scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The present invention will be further described below with reference to the drawings and embodiments.

[0020] Figure 1 shows a flowchart of a path planning method provided by an embodiment of the present disclosure.

[0021] Figure 2 shows a schematic structural diagram of a grid map provided by an embodiment of the present disclosure.

[0022] Figure 3 shows a schematic structural diagram of a neighbor node provided by an embodiment of the present disclosure.

[0023] Figure 4 shows a flowchart of a turning point optimization algorithm provided by an embodiment of the present disclosure.

[0024] Figure 5 shows a schematic structural diagram of a turning point that does not need to be optimized provided by an embodiment of the present disclosure.

[0025] Figure 6 shows an azimuth diagram of a turning point optimization algorithm provided by an embodiment of the present disclosure.

[0026] Figure 7 shows a schematic structural diagram of the connection line between two points on the vertical axis criterion passing through the grid in the 8-region.

[0027] Figure 8 shows a schematic diagram of the connection line between two points on the vertical axis criterion passing through the grid in the 5-region.

[0028] Figure 9Schematic diagram showing that the line connecting two points according to the vertical axis criterion passes through the grid in area 1 provided by an embodiment of the present disclosure.

[0029] Figure 10 Schematic diagram showing that the line connecting two points according to the vertical axis criterion passes through the grid in area 4 provided by an embodiment of the present disclosure.

[0030] Figure 11 Schematic diagram showing that the line connecting two points according to the horizontal axis criterion passes through the grid in area 7 provided by an embodiment of the present disclosure.

[0031] Figure 12 Schematic diagram showing that the line connecting two points according to the horizontal axis criterion passes through the grid in area 6 provided by an embodiment of the present disclosure.

[0032] Figure 13 Schematic diagram showing that the line connecting two points according to the horizontal axis criterion passes through the grid in area 2 provided by an embodiment of the present disclosure.

[0033] Figure 14 Schematic diagram showing that the line connecting two points according to the horizontal axis criterion passes through the grid in area 3 provided by an embodiment of the present disclosure.

[0034] Figure 15 Schematic diagram of the simulation result of the A* algorithm effect before improvement provided by an embodiment of the present disclosure.

[0035] Figure 16 Schematic diagram of the simulation result of the A* algorithm effect after improvement provided by an embodiment of the present disclosure.

[0036] Figure 17 Schematic diagram of the experimental scenario provided by an embodiment of the present disclosure.

[0037] Figure 18 Schematic diagram of the experimental path planning result of the improved A* algorithm provided by an embodiment of the present disclosure. Detailed implementation manners

[0038] The present invention will be further described below with reference to the accompanying drawings.

[0039] Figure 1 A flowchart of a path planning method provided by an embodiment of the present disclosure is shown. As Figure 1 shown, in one embodiment, the present disclosure provides a path planning method, which mainly includes the following steps: S1. Initialize the map environment, determine the start point and end point of the path to be planned, initialize the obstacle list, open list and closed list, traverse all nodes in the map to obtain obstacle nodes, put the obstacle nodes into the obstacle list, set the closed list to be empty, and put the start point into the open list; S2. Starting from the starting point, search for initial path points in the eight directions of the current node, put the initial path points that meet the conditions into the open list, combine the cost function to preferentially search for at least one initial path point, take the initial path point with the smallest cost value as the parent node for subsequent search, and move this point from the open list to the closed list. Among them, the path formed by the initial path points in the closed list is the initial path from the starting point to the ending point. The cost function is the sum of the actual cost and the heuristic function cost. The initial path point is a passable neighbor node corresponding to the current parent node; S3. Extract all turning points in the initial path, judge whether the shortest path between two points before and after the turning point passes through obstacles, and update the path of the initial path according to the judgment result to obtain the final path.

[0040] In S1, in the initialization process, then starting from the starting point, search for passable nodes in the eight directions of the current parent node, combining the actual cost G and the heuristic function cost H sum F (total cost value) to preferentially explore the path points most likely to lead to the target, so as to effectively find the shortest path from the starting point to the ending point.

[0041] It should be noted that the map used by the A* algorithm is a grid map. As Figure 2 shown, the coordinate origin of the grid map is the upper left corner, arranged from top to bottom and from left to right, and the index of the first grid in the upper left corner is 1. Secondly, due to the nature of the grid map, the path nodes obtained after the search are the intersections of the horizontal and vertical axes of the map. To make these points connect more smoothly and beautifully, it is necessary to add 0.5 units to the horizontal and vertical coordinates of all nodes to obtain the grid center point coordinates, and use these center nodes to represent the final path. In the present invention, the starting point coordinates are defined as , the ending point coordinates are defined as , and the coordinates of the parent node are defined as . The nodes used in the turning point optimization algorithm are , , , among which, is the coordinate of the node before the turning point, is the point after the turning point, and the coordinate of the grid center position of is , = +0.5, = +0.5.

[0042] Further, in the initialization process of S1, first, initialize the map environment, determine the map size, the starting point and the ending point for pathfinding. Then, initialize an obstacle list, an open list, and a closed list. All obstacle nodes are stored in the obstacle list, passable neighbor nodes are stored in the open list, and the final path points are stored in the closed list. During the initialization process, traverse all nodes in the map, find all obstacle nodes and put them into the obstacle list, set the closed list to be empty, and put the starting point into the open list.

[0043] Further, in S2, determine the current node as the parent node in the search process. The first current node is the starting point of the initial path, and the last current node is the target point. The eight directions are up, down, left, right, upper right, upper left, lower right, and lower left, respectively corresponding to the eight neighbor nodes of the parent node. The next path point of the current node is a passable neighbor node towards any one of these eight directions, denoted as the initial path point; the actual cost is the Euclidean distance from the starting point to the current neighbor node; the heuristic function cost is the diagonal distance from the current node to the target point multiplied by the weight ; find the initial path point with the smallest total cost value F in the open list, and remove the initial path point with the smallest total cost value F from the open list and put it into the closed list. Take the initial path point with the smallest total cost value F as the parent node for the next search, and perform cyclic search until the parent node for the next search is the ending point, then the search ends.

[0044] Further, for the eight neighbor nodes, during the search process, for each parent node, traverse the eight neighbor nodes of this parent node in turn, and calculate the position coordinates of each neighbor node according to the position coordinates of the parent node.

[0045] Further, calculate the coordinates of the neighbor nodes according to the parent node coordinates. Within the map range, starting from the upper right and in the counterclockwise direction, the coordinates of the eight neighbor nodes of the current node are in turn: , , as Figure 3 shown.

[0046] Further, based on the coordinates of the eight neighboring nodes obtained, determine in sequence which points need to be placed in the open list. If the neighboring node is within the map range, is not an obstacle node, and is not in the open list or the closed list, it indicates that the neighboring node is a passable neighboring node, and this node is placed in the open list as an initial path point; if the neighboring node is not a point within the map, or is an obstacle node, or this node is already in the closed list, it indicates that the neighboring node is not executable, and then this neighboring node is discarded; if the neighboring node is in the open list and the total cost value F is smaller than that of this neighboring node in the open list, then update the total cost value of this neighboring node in the open list F .

[0047] Further, calculate the cost value between the parent node and all neighboring nodes in the open list , which is represented by the Euclidean distance, as follows: (1), where is the coordinate value of the current node, is the coordinate value of the current parent node.

[0048] In one embodiment, the cost value between the parent node and all neighboring nodes in the open list g can also be assigned in the following way: when the neighboring node is at the four positions of up, down, left, and right of the parent node, this distance is 1, and at the other four positions, it is 1.4. When the parent node is the starting point, is equal to ; when the parent node is not the starting point, is the sum of the actual cost value of the previous path and the cost value of the current parent node.

[0049] Further, according to the diagonal distance formula, calculate the diagonal distance value of the current neighboring node to the target point, as shown in formulas (2)-(4), is the minimum of the two.

[0050] (2), (3), (4), where is the end point coordinate value, is the current neighboring node coordinate value.

[0051] Further, the final heuristic function cost is shown in formulas (5)-(6) as follows:

[0052] (5), (6), wherein, is the distance from the current neighbor node to the end point, represented by the diagonal distance, is the distance from the starting point to the end point, represented by the diagonal distance.

[0053] Furthermore, the total cost value , as shown in Equation (7), (7), Furthermore, find the initial path point with the smallest total cost value in the open list, remove it from the open list, put it into the closed list, use this node as the parent node for the next search, and perform loop search until the parent node is the end point, then the search ends.

[0054] In one embodiment, the turning point optimization algorithm is used to smooth the path.

[0055] In S3, extract all turning points in the initial path, determine whether the shortest path (the line connecting the two points) between the two points before and after the turning point passes through an obstacle, and update the path of the initial path according to the judgment result and the obstacle list to obtain the optimal path, realizing the smoothing of the initial path and improving the efficiency.

[0056] In the embodiment of the present disclosure, in the global path planning of the mobile robot, compared with before the improvement, the improved algorithm has a significant improvement in the search speed. At the same time, through the path smoothing process of the present invention, a path with fewer nodes and shorter distance can be obtained. The improved A* algorithm has better stability and can be widely applied to different map scenarios.

[0057] As Figure 4 shown, in the turning point optimization algorithm, the turning point extraction method is to find all turning points in the initial path according to the slope. To determine whether the shortest path between the two points before and after the turning point passes through an obstacle, it is to calculate all the nodes passed by the path according to the angle and azimuth between the two points. If these nodes contain obstacle nodes, it means that the current turning point cannot be optimized, end the current turning point, and start judging the next turning point. If all nodes do not contain obstacle nodes, it means that this turning point can be optimized. Delete this turning point from the path set, update the path point set, and then continue with the next turning point until all turning points are traversed once to obtain the updated path point set. Use the turning point optimization algorithm to continue to optimize the obtained new path point set until the path cannot be optimized, that is, the path before and after optimization does not change, and the optimal path is obtained.

[0058] Further, all turning points on the path are found according to the slope. The path points include the starting point, the ending point, and the intermediate path nodes between the two. Continuing to refer to Figure 4 , first, initialize a set of turning points to store the turning points on the path. Select the first point of the intermediate path nodes and set it as the current path point. Calculate the slope between the current path point and the previous point (starting point) , calculate the slope between the current path point and the next point , and judge whether the slopes and are different. If they are different, it means that the current path point is a turning point. Record the coordinates of the current path point and put them into the set of turning points. If the slopes are the same, there is no turning. Set the second point of the intermediate node as the current path point and repeat the above steps until the current path point is the last intermediate node, and the search for turning points ends.

[0059] Further, all the nodes passed by the path are calculated according to the angle and azimuth between two points, including: determining the front and rear intermediate path nodes of the turning point , ( ), calculate the absolute value of the angle of node relative to node , and find all the nodes passed through by the connection of the two nodes according to the magnitude of and the azimuth of node relative to node relative to node . The front and rear nodes and are determined by determining the position index of the turning point in the set of path point coordinates.

[0060] Further, determine the front and rear nodes of the turning point , ( ) is determined by determining the position index of the turning point in the set of path point coordinates, so as to determine the position where the turning point is located, and then obtain the coordinates of its front and rear nodes.

[0061] Further, calculate the absolute value of the angle of node relative to node , and first, the absolute values of the differences in the horizontal and vertical coordinates of the two nodes need to be calculated . If , then the following four situations occur, as shown in the four figures above . The abscissas of the two nodes are the same, and the connection passes through the obstacle node. At this time, the turning point cannot be discarded. Similarly, if Figure 5 , then the following four situations occur, as shown in , as shown in Figure 5As shown in the following four figures, the ordinates of the two nodes are the same, and the connection passes through the obstacle. At this time, the turning point cannot be optimized. The absolute value of the slope is and The ratio of is used to calculate the absolute value of the angle with the arctangent function, as shown in Formulas 8 to 11.

[0062] (8), (9), (10), (11), Furthermore, according to the magnitude of and the node relative to the node to find all the nodes that the connection between the two nodes passes through, that is, fix the position as the origin, and judge the location and area. First, divide the orientation of the node relative to the node into 8. Among them, the relative positions of the two nodes are: is directly above, directly below, directly to the left, and directly to the right of = 0° and = 90°, which have been described above as situations that cannot be optimized, as Figure 5 shown. Secondly, the relative positions of the two nodes are: is in the upper right, lower right, upper left, and lower left of = 45°. At this time, in the , and the path formed by the turning point, obviously going directly from to is the shortest path and there is no need for a turning point. The reason for the existence of the turning point is that and the connection passes through the obstacle. Therefore, = 45° is a situation that does not need to be considered.

[0063] Furthermore, for angle judgment, is , the absolute value of the angle between the two points and the horizontal direction. There are two situations. One is 0° < < 45°, and the other is 45° < < 90°. According to the eight regions divided by the eight orientations, the included angle of each region is 45°. The position regions corresponding to the two situations are: 0° < <The 45° corresponding regions are 1, 4, 5, 8, and 45° < <The 90° corresponding regions are 2, 3, 6, 7, such as Figure 6 shown.

[0064] Furthermore, there are four cases for azimuth judgment, corresponding to the four regions in each case of the above angle judgment. If the calculated angle is 0° < < 45°, the azimuth corresponding to region 1 is the upper right, the azimuth corresponding to region 4 is the upper left, the azimuth corresponding to region 5 is the lower left, and the azimuth corresponding to region 8 is the lower right; similarly, if the calculated angle is 45° < < 90°, then the upper right corresponds to region 2, the upper left corresponds to region 3, the lower left corresponds to region 6, and the lower right corresponds to region 7.

[0065] Furthermore, through the above judgments of angles and azimuths, the relative positions of two nodes and can be determined, that is, which region they are located in.

[0066] Furthermore, when looking for all the nodes that the connection between two nodes passes through, the rule of passing through the coordinate axes should be followed. Here, two criteria are defined, namely the horizontal axis criterion and the vertical axis criterion.

[0067] Furthermore, the horizontal axis criterion and the vertical axis criterion are to find the intersection points of the connection between the two nodes and the horizontal axis or the vertical axis. When = 45°, it passes through the intersection point of the horizontal axis and the vertical axis. When < 45°, the number of intersection points of the connection line and the vertical axis is significantly more than that of the horizontal axis, which is defined as the vertical axis criterion; on the contrary, when > 45°, the number of intersection points of the connection line and the horizontal axis is more than that of the vertical axis, which is defined as the horizontal axis criterion.

[0068] Furthermore, a detailed description of the above two criteria is as follows: First, initialize a node list to store all the nodes passed through by the connection line between the two points.

[0069] Vertical axis criterion The vertical axis criterion is to determine the position of the intersection point of the connection between the two points and the vertical axis through calculation, and obtain the position coordinates of the nodes on both sides of the intersection point.

[0070] Using the vertical axis criterion indicates that the angle range has been determined to be 0° < < 45° at this time. Then, according to the obtained node coordinates , ( ) judge 's position area.

[0071] If > and > , then is located in Region 8.

[0072] If > and < , then is located in Region 5.

[0073] If < and > , then is located in Region 1.

[0074] If < and < , then is located in Region 4.

[0075] Furthermore, after obtaining the region where is located, all the nodes connected and passed through by it are calculated. The following formula is used for further explanation. Each region is divided into two cases.

[0076] If is located in Region 8, when , the node coordinates on the left and right sides of the intersection point of the connection line and the vertical axis are: ( , ), ( , ); when , since Region 8 is located in the lower right, the nodes on both sides of the intersection point should take the upper left and lower right positions, and the node coordinates on the left and right sides are obtained as: ( , ), ( + 1, Figure 7 and

[0077] If is located in Region 5, when , the node coordinates on the left and right sides are: ( , , ); when When, the nodes on both sides of the intersection point should take the two positions of the lower left and the upper right, and the coordinates of the nodes on the left and right sides are obtained as: ( , ), ( , ). As Figure 8 shown, calculate the coordinates of the nodes on both sides of the intersection point in turn, and all the nodes passed by the connection between and can be obtained.

[0078] If is located in Region 1, when , the coordinates of the nodes on the left and right sides are: ( , ), ( , ); when , the nodes on both sides of the intersection point should take the two positions of the lower left and the upper right, and the coordinates of the nodes on the left and right sides are obtained as: ( , ), ( , ). As Figure 9 shown, calculate the coordinates of the nodes on both sides of the intersection point in turn, and all the nodes passed by the connection between and can be obtained.

[0079] If is located in Region 4, when , the coordinates of the nodes on the left and right sides are: ( , ), ( , ); when , the nodes on both sides of the intersection point should take the two positions of the upper left and the lower right, and the coordinates of the nodes on the left and right sides are obtained as: ( , ), ( , ). As Figure 10 shown, calculate the coordinates of the nodes on both sides of the intersection point in turn, and all the nodes passed by the connection between and can be obtained.

[0080] Among them, the discriminant condition of the slope is to determine the intersection position of the connection with the ordinate, , are the number of iterations, and the ranges are , , Each time an iteration is performed, Perform iterative calculations until the discriminant condition that satisfies the above slope is found. After a finite number of iterations, all the nodes passed through by the line connecting the two points can be found. However, during the calculation process, some points will be taken repeatedly. Therefore, for the repeated nodes, only one needs to be retained after discarding the duplicates to ensure the uniqueness of these points. Then, store these points in the node list for the next judgment.

[0081] Horizontal axis criterion The horizontal axis criterion is to determine the position of the intersection of the line connecting two points with the horizontal axis through calculation, and obtain the coordinates of the nodes on both sides of the intersection.

[0082] Using the horizontal axis criterion indicates that the angle range has been determined to be 45° < < 90°. Then, according to the obtained node coordinates , Judge 's position area.

[0083] If > and > , then is located in area 7.

[0084] If > and < , then is located in area 6.

[0085] If < and > , then is located in area 2.

[0086] If < and < , then is located in area 3.

[0087] After obtaining the area where is located, calculate all the nodes passed through by its connection with . Here, it is further explained by the following formula. Each area is divided into two cases.

[0088] If is located in area 7, when , the node coordinates on both sides are: ( , ), ( , ); when When, the nodes on both sides of the intersection point should take the upper left and lower right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ). As Figure 11 shown, calculate the coordinates of the nodes on the left and right sides of the intersection point in sequence, and all the nodes passed through by the connection line between and can be obtained.

[0089] If is located in area 6, when , the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ); when , the nodes on both sides of the intersection point should take the lower left and upper right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ). As Figure 12 shown, calculate the coordinates of the nodes on the left and right sides of the intersection point in sequence, and all the nodes passed through by the connection line between and can be obtained.

[0090] If is located in area 2, when , the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ); when , the nodes on both sides of the intersection point should take the lower left and upper right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ). As Figure 13 shown, calculate the coordinates of the nodes on the left and right sides of the intersection point in sequence, and all the nodes passed through by the connection line between and can be obtained.

[0091] If is located in area 3, when , the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ); when , the nodes on both sides of the intersection point should take the upper left and lower right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ),( , ). As Figure 14 shown, by calculating the coordinates of the nodes on both sides of the intersection point in sequence, all the nodes passed through by the connection between and can be obtained.

[0092] Among them, the discrimination condition of the slope is to determine the position of the intersection point of the connection and the ordinate. , are the number of iterations, and the ranges are respectively , , For each iteration, needs to be iterated until the discrimination condition of the above slope is satisfied. After a finite number of iterations, all the nodes can be found. Similarly, for the repeated nodes, we only need to keep one, put these nodes into the node list, and make the next judgment.

[0093] Furthermore, judge whether all the points in the node list are in the obstacle list. If there is a node in the obstacle list, the optimization of the current turning point is ended. If all the points in the node list are not in the obstacle list, that is, all the nodes are free, it means that this turning point can be optimized. Delete this turning point from the path point set, and then continue to optimize the next turning point. After optimizing all the turning points once, a new path point set is obtained, and the above turning point optimization process is repeated again. If the obtained new path point set does not change compared with the previous optimization result, it means that the path has been optimized, output the last optimization result, and use it as the final path.

[0094] Simulate the improved Α* algorithm of the present invention in matlab. As Figure 15 , 16 shown, Figure 15 is the unimproved Α* algorithm. The time used for path planning is 0.31 s, the path length is 27.38, the number of path nodes is 23, and the number of turning points is 5. Figure 16 is the improved Α* algorithm. The time used for path planning is 0.18 s, the path length is 26.21, the number of path nodes is 11, and the number of turning points is 3. The results before and after simulation show that the improved Α* algorithm of the present invention has improvements in both the planning time and the path length. Secondly, the number of redundant path nodes and turning points is significantly reduced, and the path smoothness is significantly improved.

[0095] To verify the effectiveness and feasibility of the improved algorithm of the present invention, apply the improved Α* algorithm to the ROS-based mobile robot platform. The experimental scenario is as Figure 17As shown in the figure. After obtaining the experimental scene information using lidar, the amcl and gmapping modules are used for positioning and mapping. Then, the algorithm proposed in this paper is transplanted into the move_base module by registering it through a plugin to replace the global_planner for global path planning.

[0096] During the experiment, first start the keyboard control node to move the robot to construct a 2D map of the experimental scene, then start the map_saver node to save the map, then start the map_server node to import the map, and use the 2D_Nav_Estimate function in rviz to calibrate the position of the robot. Then, starting from the current position, specify any position in the map as the target point for path planning. The planning results are as Figure 18 shown.

[0097] The above are only the preferred embodiments of the present invention, and do not impose any form of limitation on the present invention. Any simple modifications, equivalent changes, and decorations made to the above embodiments based on the technical essence of the present invention still fall within the scope of the technical solution of the present invention.

Claims

1. A path planning method based on improved A*, characterized in that: The following steps are involved: S1. Initialize the map environment, determine the starting point and end point of the path to be planned, initialize the obstacle list, open list and closed list, traverse all nodes in the map, obtain obstacle nodes, put the obstacle nodes into the obstacle list, set the closed list to empty, and put the starting point into the open list; S2. Starting from the starting point, search for initial path points in eight directions of the current node, put the initial path points that meet the conditions into the open list, combine the cost function to preferentially search for at least one initial path point, use the initial path point with the smallest cost value as the parent node for subsequent searches, and move the point from the open list to the closed list, where the path formed by the initial path points in the closed list is the initial path from the starting point to the end point, the cost function is the sum of the actual cost and the heuristic function cost, and the initial path point is the traversable neighbor node corresponding to the current parent node; S3. Extract all turning points in the initial path, determine whether the shortest path between two points before and after the turning point passes through an obstacle, and update the initial path according to the determination result to obtain an optimal path.

2. The method according to claim 1, characterized in that In S3, all turning points in the path are found according to the slope, and whether the shortest path between the two points before and after the turning point passes through the obstacle is determined by calculating all the nodes passed by the path based on the angle and orientation between the two points. If the node contains an obstacle node, it means that the current turning point cannot be optimized, and the current turning point is ended, and the next turning point is judged. If none of the nodes contain an obstacle node, it means that this turning point can be optimized, and the turning point is deleted from the path set, the path point set is updated, and then the next turning point is continued until all turning points are traversed once to obtain an updated path point set; the new path point set is optimized using the turning point optimization algorithm until the path cannot be optimized, that is, the path does not change before and after optimization, and the optimal path is obtained.

3. The method according to claim 2, characterized in that The step of searching for all turning points in the path according to the slope includes: Initialize a turning point set, which stores the turning points in the path, where the path points of the path include a starting point, an end point, and an intermediate path node between the starting point and the end point; Select the first point of the intermediate path node, set it as the current path point, and calculate the slope of the current path point and the previous path node K 1. Calculate the slope between the current path point and the next path node K 2. Determine the slope K 1 and K 2. Are they different? If they are different, it means that the current path point is a turning point. The coordinates of the current path point are recorded and put into the turning point set. If the slopes are the same, no turning occurs. Set the second point of the intermediate path node as the current path point, and repeat the above steps until the current path point is the last intermediate path node, and the turning point search is completed.

4. The method according to claim 2, characterized in that: The method of calculating all nodes passed by the path according to the angle and orientation between the two points includes: determining the front and rear nodes of the turning point , ( ), compute nodes Relative to the node The absolute value of the angle ,according to Size and nodes Relative to the node The direction is used to find all the nodes that connect the two nodes, where the front and rear nodes and It is determined by determining the position index of the turning point in the coordinates of the path point set.

5. The method according to claim 4, characterized in that The basis Size and nodes Relative to the node The direction of the two nodes is used to find all the nodes through which the two nodes are connected, including: Initialize the node list to store all the nodes that the line between two points passes through; When 0°< When the angle is less than 45°, the longitudinal axis criterion is used to determine whether the node list contains an obstacle node. The longitudinal axis criterion is used to determine the position of the intersection of the two points and the longitudinal axis, and obtain the position coordinates of the nodes on the left and right sides of the intersection; When 45°< When the angle is less than 90°, the horizontal axis criterion is used to determine whether the node list contains an obstacle node. The horizontal axis criterion is used to determine the position of the intersection of the two points and the horizontal axis, and obtain the position coordinates of the nodes on the upper and lower sides of the intersection; If the node list does not contain an obstacle node, delete the turning point and update the path; if the node list contains an obstacle node, end the current turning point; Determine whether the new path is the same as the original path. If they are the same, output the new path as the optimal path and end; if they are different, continue to search for all turning points in the path set and further optimize the path.

6. The method according to claim 1, characterized in that In S2, the current node is determined as the parent node in the search process, the first current node is the starting point of the initial path, and the last current node is the target point; the eight directions are up, down, left, right, upper right, upper left, lower right, and lower left, respectively corresponding to the eight neighboring nodes of the parent node, and the next path point of the current parent node is any passable neighboring node in these eight directions, which is recorded as the initial path point; the actual cost is the Euclidean distance from the starting point to the current neighbor node; the heuristic function cost is the diagonal distance from the current neighbor node to the target point With weight The product of Find the total value of the open list The minimum initial path point and the total cost value The smallest initial path point is removed from the open list and placed in the closed list, and the total cost value is The smallest initial path point is used as the parent node of the next search, and a circular search is performed until the parent node of the next search is the end point, at which point the search ends.

7. The method according to claim 6, characterized in that During the search process, for each parent node, the eight neighbor nodes of the parent node must be traversed in turn, and the position coordinates of each neighbor node are calculated in turn according to the position coordinates of the parent node; Calculate the coordinates of the neighbor node based on the coordinates of the parent node. Within the map range, start from the upper right corner and go counterclockwise to get the current parent node. The coordinates of the eight neighbor nodes are: , .

8. The method according to claim 6, characterized in that In S2, according to the coordinates of the eight neighbor nodes, each of the neighbor nodes is judged in turn and put into the open list. If a neighbor node is within the map range, is not an obstacle node, and is not in the open list and the closed list, then the neighbor node is put into the open list as the initial path point; if the neighbor node is not a point in the map, or is an obstacle node, or the node is already in the closed list, then the neighbor node is discarded; If the node is in the open list and the total cost is F If it is smaller than the point in the open list, update the total cost value of the node in the open list. F .

9. The method according to claim 6, characterized in that Calculate the cost between the parent node and all neighbor nodes in the open list , expressed as Euclidean distance, as follows: （1）， in, is the coordinate value of the current neighbor node, The coordinate value of the current parent node.

10. The method according to claim 6, characterized in that According to the diagonal distance formula, calculate the diagonal distance from the current neighbor node to the target point The value is as follows: （2）， （3）， （4）， in, To obtain The minimum of the two, is the end point coordinate value, is the coordinate value of the current neighbor node; Heuristic function cost It is expressed as: （5）， （6）， in, is the distance from the current neighbor node to the end point, expressed as diagonal distance. is the distance from the starting point to the end point, expressed as the diagonal distance; Total value It is expressed as: （7）， in, It is the sum of the actual cost value of the previous path and the cost value from the current parent node to the neighbor node. is the heuristic function cost.

Images