Path planning method based on improved A*

Abstract

The present invention discloses a path planning method based on improved A*, which relates to the technical field of path planning. The method comprises the following steps: initializing a map environment, determining a starting point and an end point of a path to be planned, initializing an obstacle list, an open list and a closed list, traversing all nodes in the map to obtain obstacle nodes, placing the obstacle nodes in the obstacle list, setting the closed list to be empty, and placing the starting point in the open list; starting from the starting point, searching for initial path points in eight directions of the current node, placing the initial path points in the open list, preferentially searching for at least one initial path point in combination with a cost function, taking the initial path point with the smallest cost value as the parent node of subsequent searches, and moving the point from the open list to the closed list; extracting all turning points in the initial path, judging whether the shortest path between two points before and after the turning point passes through an obstacle, and updating the initial path according to the judgment result to obtain an optimal path. The method can shorten the path length.

Description

Technical Field

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

[0002] Path planning is a crucial technology in the field of mobile robotics and a key technology in autonomous driving and navigation. Its research objective is to enable a mobile robot to plan a path from its initial position to its target location within a work scenario. This path must meet a series of requirements, including safety, efficiency, optimality, and rationality, and must be able to avoid obstacles along the way. Path planning algorithms are categorized into global path planning (for static environments) and local path planning (for dynamic environments). Global path planning algorithms primarily include graph search algorithms (BFS, DFS, Dijkstrα, and A*) and sampling planning algorithms (PRM and RRT). Local path planning algorithms primarily include artificial potential field (APF) and dynamic window approach (DWA).

[0003] Graph search algorithms are used to find paths, traverse nodes, or solve other graph-related problems in a graph or tree data structure. Breadth-first search (BFS) is a graph search algorithm that starts from a start node and visits all adjacent nodes layer by layer. It is suitable for the shortest path problem in unweighted graphs. Depth-first search (DFS) starts from a start node and continues down a branch until it reaches a dead end or the end point, then backtracks to the previous node to continue exploring. It is suitable for deep traversal and topological sorting of graphs. The Dijkstrα algorithm finds the shortest path from a start node to all other nodes in a weighted graph. It gradually expands to the nodes closest to the start node, using a priority queue to select nodes. The Alpha* algorithm is a heuristic search algorithm that combines actual cost and estimated cost (heuristic function) to find the shortest path from a start node to a target node. It uses a priority queue to manage nodes, with priority determined by the sum of actual cost and heuristic cost estimate.

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

[0005] ΑPF treats the target point and obstacles as attractive potential fields and repulsive potential fields, respectively. When the robot moves in the potential field, it is affected by the combined influence of the attraction of the target point and the repulsion of the obstacles, thereby selecting the optimal path. However, its local minimum problem and performance in complex environments may affect its effectiveness.

[0006] DWA selects an optimal speed that enables the robot to avoid obstacles and move toward the target in the next time by sampling and evaluating in the robot's current velocity space. Although it has shortcomings such as local minimum problems and sensitivity to parameters, it has shown good results 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 of the existing technology and provide a path planning method based on improved A*, thereby improving the search speed and obtaining a path with fewer nodes and shorter distance through path smoothing, which is more efficient.

[0008] The technical solution adopted by the present invention to solve its technical problem is:

[0009] According to one aspect of the present disclosure, a path planning method based on improved A* is provided, comprising the following steps:

[0010] 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, add the obstacle nodes to the obstacle list, set the closed list to empty, and add the starting point to the open list;

[0011] S2. Starting from the starting point, search for initial path points in eight directions from the current node. Place the initial path points that meet the conditions in the open list. Combined with the cost function, prioritize searching for at least one initial path point. Select the initial path point with the smallest cost as the parent node for subsequent searches, and move the point from the open list to the closed list. 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. The initial path point is a traversable neighbor node corresponding to the current parent node.

[0012] S3. Extract all turning points in the initial path, determine whether the shortest path between the two points before and after the turning point passes through an obstacle, and update the initial path based on the determination result to obtain the optimal path.

[0013] In one embodiment of the present disclosure, 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 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, the current turning point is ended, and the next turning point is judged. If the nodes do not contain an obstacle node, it means that this turning point can be optimized, 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 obtained is continuously 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.

[0014] In one embodiment of the present disclosure, searching for all turning points in the path according to the slope includes:

[0015] Initialize a turning point set, which stores turning points in the path, where the path points include a starting point, an end point, and intermediate path nodes between the starting point and the end point;

[0016] 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 , calculate the slope between the current path point and the next path node , determine the slope and Are they 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 it into the turning point set. If the slopes are the same, no turning occurs.

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

[0018] In one embodiment of the present disclosure, the method of calculating all nodes passed by the path according to the angle and orientation between two points includes: determining the nodes before and after 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 of the two nodes is used to find all the nodes through which the two nodes are connected, where the front and back nodes and It is determined by determining the position index of the turning point in the coordinates of the path point set.

[0019] In one embodiment of the present disclosure, the size of α and the node Relative to the node The direction of the two nodes is used to find all the nodes through which the connection passes, including:

[0020] Initialize the node list to store all the nodes that the line between two points passes through;

[0021] When 0°< When the angle is less than 45°, the vertical axis criterion is used to determine whether the node list contains an obstacle node. The vertical axis criterion is used to determine the position of the intersection of the two points and the vertical axis, and obtain the position coordinates of the nodes on the left and right sides of the intersection;

[0022] 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 above and below the intersection.

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

[0024] 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 perform the next step of path optimization.

[0025] 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, which correspond to the eight neighboring nodes of the parent node respectively, and the next path point of the current node is any one of the passable neighboring nodes 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 node to the target point With weight The product of

[0026] Find the total value of the open list F The minimum initial path point and the total cost value F The smallest initial path point is removed from the open list and placed in the closed list, and the total cost value is FThe smallest initial path point is used as the parent node of the next search, and a cyclic search is performed until the parent node of the next search is the end point, at which point the search ends.

[0027] In one embodiment of the present disclosure, during the search process, for each parent node, the eight neighboring nodes of the parent node are sequentially traversed, and the position coordinates of each neighboring node are sequentially calculated based on the position coordinates of the parent node;

[0028] 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 node. The coordinates of the eight neighbor nodes are: , .

[0029] In one embodiment of the present disclosure, in S2, based on the coordinates of the eight neighbor nodes, each of the neighbor nodes is judged in turn and 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 or 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 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 value 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 .

[0030] In one embodiment of the present disclosure, the cost value between the parent node and all neighbor nodes in the open list is calculated. , expressed as Euclidean distance, as follows:

[0031] (1),

[0032] in, is the current node coordinate value, The coordinate value of the current parent node.

[0033] In one embodiment of the present disclosure, the diagonal distance from the current neighbor node to the target point is calculated according to the diagonal distance formula. The values are as follows:

[0034] (2),

[0035] (3),

[0036] (4),

[0037] in, To obtain The minimum of the two, is the end point coordinate value, is the coordinate value of the current neighbor node;

[0038] Heuristic function cost Expressed as:

[0039] (5),

[0040] (6),

[0041] 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 diagonal distance;

[0042] Total value Expressed as:

[0043] (7),

[0044] in, The sum of the actual cost value of the previous path and the cost value of the current parent node, is the heuristic function cost.

[0045] The present invention has the following beneficial effects: the improved algorithm significantly improves search speed in global path planning for mobile robots compared to the original algorithm. Furthermore, the path smoothing process of the present invention can produce a path with fewer nodes and shorter distances. The improved A* algorithm also has greater stability and can be widely applied to various map scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0047] Figure 1 A flow chart of a path planning method provided by an embodiment of the present disclosure is shown.

[0048] Figure 2 A schematic structural diagram of a grid map provided by an embodiment of the present disclosure is shown.

[0049] Figure 3 A schematic diagram of the structure of neighbor nodes provided by an embodiment of the present disclosure is shown.

[0050] Figure 4 A flow chart of a turning point optimization algorithm provided by an embodiment of the present disclosure is shown.

[0051] Figure 5A schematic diagram illustrating the structure of a turning point that does not require optimization provided by an embodiment of the present disclosure.

[0052] Figure 6 An orientation diagram of the turning point optimization algorithm provided by an embodiment of the present disclosure is shown.

[0053] Figure 7 A schematic structural diagram of a grid in which a line connecting two points on the vertical axis criterion passes through eight regions is shown in an embodiment of the present disclosure.

[0054] Figure 8 A schematic diagram showing a grid in five areas where a line connecting two points on the vertical axis criterion provided by an embodiment of the present disclosure passes through.

[0055] Figure 9 A schematic diagram showing a line connecting two points along the vertical axis criterion provided by an embodiment of the present disclosure passing through a grid in area 1 is shown.

[0056] Figure 10 A schematic diagram showing a grid in four areas where a line connecting two points along the vertical axis criterion provided by an embodiment of the present disclosure passes through.

[0057] Figure 11 A schematic diagram showing a grid in 7 areas where a line connecting two points along the horizontal axis criterion provided by an embodiment of the present disclosure passes through.

[0058] Figure 12 A schematic diagram showing a line connecting two points on the horizontal axis criterion passing through a grid in six areas provided by an embodiment of the present disclosure.

[0059] Figure 13 A schematic diagram showing a grid in two areas where a line connecting two points along the horizontal axis criterion provided by an embodiment of the present disclosure passes through.

[0060] Figure 14 A schematic diagram showing a grid in three areas where a line connecting two points on the horizontal axis criterion provided by an embodiment of the present disclosure passes through.

[0061] Figure 15 A schematic diagram of the simulation effect of the A* algorithm before improvement provided by an embodiment of the present disclosure is shown.

[0062] Figure 16 A schematic diagram of simulation of the effect of the improved A* algorithm provided by an embodiment of the present disclosure is shown.

[0063] Figure 17 A schematic diagram of an experimental scenario provided by an embodiment of the present disclosure is shown.

[0064] Figure 18 A schematic diagram of the experimental path planning results of the improved A* algorithm provided in an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

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

[0066] Figure 1 FIG. 1 shows a flow chart of a path planning method provided by an embodiment of the present disclosure. Figure 1 As shown, in one embodiment, the present disclosure provides a path planning method, which mainly includes the following steps:

[0067] 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, add the obstacle nodes to the obstacle list, set the closed list to empty, and add the starting point to the open list;

[0068] S2. Starting from the starting point, search for initial path points in eight directions from the current node. Place the initial path points that meet the conditions in the open list. Combined with the cost function, prioritize searching for at least one initial path point. Use the initial path point with the smallest cost as the parent node for subsequent searches and move it from the open list to the closed list. 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. The initial path point is a traversable neighbor node corresponding to the current parent node.

[0069] S3. Extract all turning points in the initial path, determine whether the shortest path between the two points before and after the turning point passes through the obstacle, and update the initial path based on the determination result to obtain the final path.

[0070] In S1, the initialization process starts from the starting point and searches for traversable nodes in the eight directions of the current parent node, combined with the actual cost G and the heuristic function cost H sum F (Total value) is used to prioritize the path points that are most likely to lead to the goal, thereby effectively finding the shortest path from the starting point to the end point.

[0071] It should be noted that the map used by the A* algorithm is a grid map, such as Figure 2 As shown, the coordinate origin of the grid map is the upper left corner. From top to bottom and from left to right, the first grid index 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 connected more smoothly and beautifully, it is necessary to add 0.5 units to the horizontal and vertical coordinates of all nodes to obtain the coordinates of the grid center points, and use these center nodes to represent the final path. In this invention, the starting point coordinates are defined as , the end point coordinates are defined as , the coordinates of the parent node are defined as The nodes used in the turning point optimization algorithm are , , ,in, is the coordinate of the node before the turning point, is the point after the turning point, The coordinates of the grid center position are , = +0.5, = +0.5.

[0072] Furthermore, during the initialization process in S1, the map environment is initialized, the map size is determined, and the pathfinding start and end points are determined. Then, an obstacle list, an open list, and a closed list are initialized. The obstacle list stores all obstacle nodes, the open list stores traversable neighbor nodes, and the closed list stores the final path point. During the initialization process, all nodes in the map are traversed, all obstacle nodes are found, and they are added to the obstacle list. The closed list is set to empty, and the starting point is added to the open list.

[0073] Furthermore, 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, which correspond to the eight neighboring nodes of the parent node respectively. The next path point of the current 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 node to the target point With weight The product of ; find the total cost in the open list F The minimum initial path point and the total cost value F The smallest initial path point is removed from the open list and placed in the closed list, and the total cost value is F The smallest initial path point is used as the parent node of the next search, and a cyclic search is performed until the parent node of the next search is the end point, at which point the search ends.

[0074] Furthermore, during the search process, for each parent node, the eight neighbor nodes of the parent node are traversed in sequence, and the position coordinates of each neighbor node are calculated in sequence based on the position coordinates of the parent node.

[0075] Furthermore, the coordinates of the neighbor nodes are calculated based on the coordinates of the parent node. Within the map range, starting from the upper right corner, in a counterclockwise direction, the current node is obtained. The coordinates of the eight neighbor nodes are: , ,like Figure 3 shown.

[0076] Furthermore, based on the coordinates of the eight neighbor nodes, determine which points need to be placed in the open list. If the neighbor node is within the map range, is not an obstacle node, and is not in the open list or the closed list, it means that the neighbor node is a passable neighbor node, and this node is placed in 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, it means that the neighbor node is not executable, and the neighbor node is discarded; if the neighbor node is in the open list, and the total cost value is F If it is smaller than the neighbor node in the open list, update the total cost value of the neighbor node in the open list. F .

[0077] Furthermore, the cost value between the parent node and all neighbor nodes in the open list is calculated. , expressed as Euclidean distance, as follows:

[0078] (1),

[0079] in, is the current node coordinate value, The coordinate value of the current parent node.

[0080] In one embodiment, the cost between the parent node and all neighbor nodes in the open list is g The following method can also be used to assign values: when the neighbor node is at the top, bottom, left, and right of the parent node, the distance is 1, and at the other four positions it is 1.4. When the parent node is the starting point, equal ; When the parent node is not the starting point, The value is the sum of the actual cost value of the previous path and the cost value of the current parent node.

[0081] Furthermore, according to the diagonal distance formula, the diagonal distance from the current neighbor node to the target point is calculated The value of , formula (2)-(4), To take the minimum value of the two.

[0082] (2),

[0083] (3),

[0084] (4),

[0085] in, is the end point coordinate value, is the coordinate value of the current neighbor node.

[0086] Furthermore, the final heuristic function cost As shown in formulas (5)-(6):

[0087] (5),

[0088] (6),

[0089] in, is the distance from the current neighbor node to the end point, expressed as diagonal distance. The distance from the start point to the end point is expressed using the diagonal distance.

[0090] Furthermore, the total cost , as shown in formula (7),

[0091] (7),

[0092] Further, find the total cost value in the open list The smallest initial path point is removed from the open list and placed in the closed list. The node is used as the parent node for the next search and a circular search is performed until the parent node is the end point. The search ends.

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

[0094] In S3, all turning points in the initial path are extracted, and it is determined whether the shortest path between the two points before and after the turning point (the line connecting the two points) passes through an obstacle. The initial path is updated based on the judgment result and the obstacle list to obtain the optimal path, thereby smoothing the initial path and improving efficiency.

[0095] In the disclosed embodiments, the improved algorithm significantly improves search speed for mobile robot global path planning compared to the original algorithm. Furthermore, the path smoothing process employed by the present invention results in a path with fewer nodes and shorter distances. The improved A* algorithm also boasts greater stability and is widely applicable to various map scenarios.

[0096] like Figure 4As shown in the figure, in the turning point optimization algorithm, the turning point extraction method is to find all turning points in the initial path based on the slope. The shortest path between the two points before and after the turning point is determined to pass through the obstacle. The angle and orientation between the two points are used to calculate all the nodes passed by the path. If these nodes include obstacle nodes, it means that the current turning point cannot be optimized. The current turning point is ended and the next turning point is determined. If all nodes do not include obstacle nodes, it means that this turning point can be optimized. This 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 the updated path point set. The new path point set obtained 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.

[0097] Furthermore, all turning points in the path are found based on the slope. The path points include the starting point, the end point, and the intermediate path nodes between them. Figure 4 First, initialize a turning point set to store the turning points in the path. Select the first point of the intermediate path node and set it as the current path point. Calculate the slope of the current path point and the previous point (starting point). , calculate the slope between the current path point and the next point , determine the slope and Are they different? If they are, the current path point is a turning point. Record the coordinates of the current path point and add them to the turning point set. If the slopes are the same, no turning point occurs. Set the second point of the intermediate node as the current path point. Repeat the above steps until the current path point is the last intermediate node, and the turning point search is complete.

[0098] Furthermore, all nodes that the path passes through are calculated based on the angle and orientation between the two points, including: determining the intermediate path nodes before and after 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 of the two nodes is used to find all the nodes that the two nodes pass through, the front and back nodes and It is determined by determining the position index of the turning point in the coordinates of the path point set.

[0099] Further, determine the nodes before and after the turning point , ( ) is to determine the position index of the turning point in the path point set coordinates, thereby determining the location of the turning point and then obtaining its front and rear node coordinates.

[0100] Furthermore, computing nodes Relative to the node The absolute value of the angle , first you need to calculate the absolute value of the difference between the horizontal and vertical coordinates of the two nodes .if , then the following four situations occur, such as Figure 5 As shown in the four figures above, the horizontal coordinates of the two nodes are the same and the connection passes through the obstacle node. In this case, the turning point cannot be discarded. Similarly, if , then the following four situations occur, such as Figure 5 As shown in the following four figures, the vertical coordinates of the two nodes are the same and the connection passes through the obstacle. In this case, the turning point cannot be optimized. The absolute value of the slope is and The inverse tangent function is used to calculate the absolute value of the angle, as shown in Formula 8 to Formula 11.

[0101] (8),

[0102] (9),

[0103] (10),

[0104] (11),

[0105] Further, according to 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. The position is fixed, regarded as the origin, and the judgment Location and area. First, the node Relative to the node The orientation is divided into 8. Among them, the relative position of the two nodes is: exist There are four situations: directly above, directly below, directly to the left, and directly to the right. =0° and =90°, as described above, is a situation that cannot be optimized, such as Figure 5 As shown. Secondly, the relative positions of the two nodes are: exist The four situations of upper right, lower right, upper left and lower left are =45°, at this time, 、 In the path formed by the turning point, it is obvious that the path directly from arrive It is the shortest path, so there is no need for a turning point, and the reason for the existence of a turning point is that and The connection passes through the obstacle, so =45° is a situation that does not need to be considered.

[0106] Furthermore, angle judgment, yes 、 The absolute value of the angle between two points and the horizontal direction has two cases, one is 0°< <45°, the other is 45°< <90°, we divide the eight areas according to the eight directions, and the angle of each area is 45°. The corresponding position areas of the two situations are: 0°< <45° corresponds to areas 1, 4, 5, and 8, 45°< <90° corresponds to areas 2, 3, 6, and 7, such as Figure 6 shown.

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

[0108] Furthermore, by judging the above angle and orientation, it is possible to determine the two nodes and The relative position of In which area.

[0109] Furthermore, to find all nodes that two nodes are connected through, we need to follow the connection crossing coordinate axis rule. Here we define two criteria, namely the horizontal axis criterion and the vertical axis criterion.

[0110] Furthermore, the horizontal axis criterion and the vertical axis criterion are to find the intersection of the two nodes and the horizontal axis or vertical axis. =45°, passing through the intersection of the horizontal and vertical axes, when When the angle is less than 45°, the intersection of the line and the vertical axis is obviously greater than the horizontal axis, which is defined as the vertical axis criterion; on the contrary, when When the angle is >45°, the intersection of the connecting line and the horizontal axis is more than that of the vertical axis, which is defined as the horizontal axis criterion.

[0111] Furthermore, the above two criteria are described in detail:

[0112] First, initialize a node list to store all the nodes that the line between two points passes through.

[0113] Vertical axis criteria

[0114] The vertical axis criterion is to determine the position of the intersection of the two points and the vertical axis by calculation, and obtain the position coordinates of the nodes on the left and right sides of the intersection.

[0115] The vertical axis criterion indicates that the angle range has been determined to be 0°< <45°. Then, according to the obtained node coordinates , ( )judge location area.

[0116] like > and > ,but Located in area 8.

[0117] like > and < ,but Located in area 5.

[0118] like < and > ,but Located in Area 1.

[0119] like < and < ,but Located in area 4.

[0120] Further, we get After the area is located, it is calculated to be All nodes passed through by the connection are further explained here by the following formula. Each area is divided into two cases.

[0121] like Located in area 8, when When , the coordinates of the nodes on the left and right sides of the intersection of the connecting line and the vertical axis are: ( , ), ( , );when When , since area 8 is located at the lower right, the nodes on both sides of the intersection should be located at the upper left and lower right positions, and the coordinates of the nodes on the left and right sides are: ( , ), ( +1, +1). Figure 7 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0122] like Located in area 5, when When , the coordinates of the nodes on the left and right sides are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the lower left and upper right positions, and the coordinates of the nodes on the left and right sides are: ( , ), ( , ).like Figure 8 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0123] like Located in area 1, when When , the coordinates of the nodes on the left and right sides are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the lower left and upper right positions, and the coordinates of the nodes on the left and right sides are: ( , ), ( , ).like Figure 9 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0124] like Located in area 4, when When , the coordinates of the nodes on the left and right sides are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the upper left and lower right positions, and the coordinates of the nodes on the left and right sides are: ( , ), ( , ).like Figure 10 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0125] Among them, the judgment condition of the slope is to determine the intersection position of the connection and the vertical coordinate. 、 is the number of iterations, and the ranges are , , Each iteration, The calculation is iterated until a point that satisfies the above slope criteria is found. After a finite number of iterations, all nodes that the line connecting the two points passes through can be found. However, during the calculation process, some points will be obtained repeatedly, so for the repeated nodes, it is necessary to discard them and keep only one to ensure the uniqueness of these points. These points are then stored in the node list for the next step of judgment.

[0126] Horizontal axis criteria

[0127] The horizontal axis criterion is to determine the position of the intersection of the two points and the horizontal axis by calculation, and obtain the coordinates of the nodes on the upper and lower sides of the intersection.

[0128] The horizontal axis criterion indicates that the angle range has been determined to be 45°< <90°. Then, according to the obtained node coordinates , judge location area.

[0129] like > and > ,but Located in area 7.

[0130] like > and < ,but Located in area 6.

[0131] like < and > ,but Located in Area 2.

[0132] like < and < ,but Located in area 3.

[0133] get After the area is located, it is calculated to be All nodes passed through by the connection are further explained here by the following formula. Each area is divided into two cases.

[0134] like Located in area 7, when When , the coordinates of the upper and lower nodes are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the upper left and lower right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ).like Figure 11 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0135] like Located in area 6, when When , the coordinates of the upper and lower nodes are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the lower left and upper right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ).like Figure 12 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0136] like Located in area 2, when When , the coordinates of the upper and lower nodes are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the lower left and upper right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ).like Figure 13 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0137] like Located in area 3, when When , the coordinates of the upper and lower nodes are: ( , ), ( , );when When , the nodes on both sides of the intersection should be located at the upper left and lower right positions, and the coordinates of the nodes on the upper and lower sides are: ( , ), ( , ).like Figure 14 As shown, the coordinates of the nodes on the left and right sides of the intersection are calculated in sequence to obtain and All nodes that the line passes through.

[0138] Among them, the judgment condition of the slope is to determine the intersection position of the connection and the vertical coordinate. 、 is the number of iterations, and the ranges are , , Each iteration, Iterate until you find a criterion that satisfies the above slope. After a finite number of iterations, you can find all the nodes. Similarly, for repeated nodes, we only need to keep one and put these nodes into the node list for the next step of judgment.

[0139] Next, the algorithm checks to see if any of the points in the node list are in the obstacle list. If any are, the optimization of the current turning point ends. If no points in the node list are in the obstacle list, meaning all nodes are free, this turning point can be optimized. This turning point is deleted from the pathpoint set, and the optimization process continues for the next turning point. After optimizing all turning points, a new pathpoint set is obtained, and the turning point optimization process repeats. If the new pathpoint set does not change from the previous optimization result, the path is optimized, and the final optimization result is output as the final path.

[0140] The improved A* algorithm of the present invention is simulated in matlab, as shown in Figure 15 、 16 As shown, Figure 15 For the unimproved A* algorithm, the time used for path planning is 0.31s, the path length is 27.38, the number of path nodes is 23, and the number of turning points is 5. Figure 16 For the improved A* algorithm, path planning took 0.18 seconds, the path length was 26.21, the number of path nodes was 11, and the number of turning points was 3. Simulation results show that the improved A* algorithm improves planning time and path length. Furthermore, the number of redundant path points and turning points is significantly reduced, significantly improving path smoothness.

[0141] In order to verify the effectiveness and feasibility of the improved algorithm of the present invention, the improved A* algorithm is applied to a mobile robot platform based on ROS. Figure 17 After using LiDAR to acquire experimental scene information, 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 through plugin registration to replace the global_planner for global path planning.

[0142] During the experiment, we first start the keyboard control node to move the robot and build a two-dimensional map of the experimental scene. Then we start the map_saver node to save the map. Then we start the map_server node to import the map. Then we use the 2D_Nav_Estimate function in rviz to calibrate the robot's position. Then, we use the current position as the starting point and specify any position in the map as the target point to perform path planning. The planning results are as follows: Figure 18 shown.

[0143] The above description is merely a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Any simple modifications, equivalent changes and modifications 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, add the obstacle nodes to the obstacle list, set the closed list to empty, and add the starting point to the open list; S2. Starting from the starting point, search for initial path points in eight directions from the current node. Place the initial path points that meet the conditions in the open list. Combined with the cost function, prioritize searching for at least one initial path point. Use the initial path point with the smallest cost as the parent node for subsequent searches and move it from the open list to the closed list. 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. The initial path point is a traversable neighbor node corresponding to the current parent node. S3, extracting all turning points in the initial path, determining whether the shortest path between the two points before and after the turning point passes through an obstacle, and updating the initial path based on the determination result to obtain the optimal path; Among them, in the 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 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, the current turning point is ended, and the next turning point is determined. If the nodes do not contain an obstacle node, it means that this turning point can be optimized, 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 obtained is continuously 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; The method of calculating all nodes passed by the path based on the angle and orientation between the two points includes: determining the nodes before and after 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 of the two nodes is used to find all the nodes through which the two nodes are connected, where the front and back nodes and The turning point is determined by determining the position index of the turning point in the coordinates of the path point set; 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 connection passes, 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 vertical axis criterion is used to determine whether the node list contains an obstacle node. The vertical axis criterion is used to determine the position of the intersection of 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°< 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 above and below 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.

2. The method according to claim 1, characterized in that The method of searching for all turning points in the path according to the slope includes: Initialize a turning point set, which stores turning points in the path, where the path points include a starting point, an end point, and intermediate path nodes 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.

3. 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, which correspond to the eight neighboring nodes of the parent node respectively. 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 cyclic search is performed until the parent node of the next search is the end point, at which point the search ends.

4. The method according to claim 3, characterized in that During the search process, for each parent node, the eight neighboring nodes of the parent node are traversed in turn, and the position coordinates of each neighboring 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: , .

5. The method according to claim 3, characterized in that In S2, based on the coordinates of the eight neighbor nodes, each of the neighbor nodes is judged in turn and placed in an open list. If a neighbor node is within the map range, is not an obstacle node, and is not in the open list or the closed list, the neighbor node is placed in the open list as the initial path point; if the neighbor node is not a point in the map, is an obstacle node, or is already in the closed list, the neighbor node is discarded. If the node is in the open list and the total cost value 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 .

6. The method according to claim 3, 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.

7. The method according to claim 3, characterized in that According to the diagonal distance formula, calculate the diagonal distance from the current neighbor node to the target point The values are 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 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 diagonal distance; Total value Expressed as: （7）， in, 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