Path planning method, device, equipment, and medium

Abstract

The embodiments of the present application disclose a path planning method, device, equipment and storage medium to solve the problem that the traditional path planning method cannot consider the influence of special areas and traverses many nodes and turning points during path planning. The present application improves the A* algorithm based on the regular hexagonal grid map modeling, optimizes the calculation method of the mobile cost, so that the environmental information can be fully considered during the search, the mobile cost of the node can be flexibly set, and the optimality of the planned path can be maintained. By introducing an adaptive heuristic, the number of traversed nodes is reduced and the search speed is increased; by designing a turning penalty measure, the number of turning points in the planned path is reduced to avoid unnecessary turns. Therefore, the present application can not only ensure the accuracy of path planning in complex scenarios, but also realize the flexible setting of the mobile cost, improve the search efficiency, reduce the number of turns, and meet the requirements of high-quality path planning of robots.

Description

Technical Field

[0001] The present application relates to a path planning method, device, equipment and storage medium, and belongs to the field of path planning. Background Art

[0002] Path planning is a key part of realizing the movement of intelligent robots. The task of path planning is to find a path from the starting point to the end point with the least time consumption, the shortest path, and no collision. In reality, the complex surface environment often makes the planned path inconsistent with the actual road conditions. Therefore, scientific and reasonable path planning methods have become a hot topic in academic research.

[0003] Existing path planning methods mainly include two parts: (1) construction of abstract maps; (2) path search algorithms. There are mainly graphical map representation, navigation grid representation, grid map method, etc. for map construction. At present, most navigation systems adopt graphical map representation, that is, the path planning algorithm relies on a vector road network, whose traversable paths are pre-set and have low degrees of freedom. Some special areas that can be passed are not included in the planning scope, such as grasslands, squares, mountains, etc., which are passable areas in many application scenarios. The navigation grid abstracts the original map into a collection of triangular grids and uses the connection relationship between triangles for path planning. This method is more accurate in map expression, but the structure is complex, difficult to implement and modify, and the construction process consumes a lot of memory. It is now mostly used in three-dimensional scenes such as electronic games. The grid method divides the environment into grids of equal size, and each grid is regarded as a node of an abstract graph. Then a search algorithm is used to search in this abstract graph. This method has a high degree of freedom, can cover the passable area well, and include all of them in the path planning scope. It is simple in principle and easy to implement.

[0004] In a two-dimensional plane, only three polygons, square, equilateral triangle and regular hexagon, can evenly cover all areas. The traditional grid method is mainly based on the square grid. In the square grid, there are 4 or 8 directions for the target to move from the current node to the neighbor node. When the diagonal direction cannot be moved, there are only 4 directions, namely, front, back, left and right. At this time, the moving cost is 1 unit. The 8-neighbor square grid adds four diagonal directions, and the moving cost is units. In the hexagonal grid, there are only 6 target directions for moving from the current point, and the moving cost is 1 unit. In the 4-neighbor square grid map, the path is jagged and has low smoothness. In the 8-neighbor square grid map, the path is smoother, but the path may contact obstacles when crossing the diagonal line, which is easy to collide with obstacles in practical applications. The hexagonal grid map does not have the problems of the first two. Its path is smooth and beautiful, and does not feel stiff. Compared with the 8-neighbor square map, the planned path is farther away from the obstacle, and each node has only one adjacent relationship with the neighbor node, which simplifies the implementation of the algorithm and improves the efficiency of the search. Therefore, a hexagonal grid map is used to replace the traditional square grid map to improve the improved A* algorithm.

[0005] In terms of path search, there are many algorithms currently being applied, including breadth-first algorithm (BFS), depth-first algorithm (DFS), Djikstra algorithm, genetic algorithm, ant colony algorithm, etc. Among them, the A* algorithm is recognized as the most effective method for solving the optimal path problem in a static environment, with excellent stability, path optimality, and high search efficiency. However, due to the fixed heuristic function and limited heuristic effect, it cannot respond to the impact of special areas on the movement cost during path search, and often traverses a large number of unnecessary nodes, accompanied by the generation of a large number of inflection points. Since the A* algorithm was proposed in 1968, domestic and foreign scholars have never stopped their research on its optimization. At present, the improvement of the A* algorithm mainly focuses on optimizing the distance calculation method, improving the heuristic function, and secondary optimization of the path. The optimized distance calculation method is to improve the estimation method of the distance between the current point and the end point, make more full and reasonable use of the end point position information, reduce the path length, and improve the efficiency of the algorithm. However, this method takes the distance cost as the main evaluation criterion and ignores the impact of special terrain on the movement of the robot. Improvements to the heuristic function: Weighting the two terms of the heuristic function or adding a new guide solves the problems of slow search speed and too many traversed nodes, but it is not adaptable to maps and often loses its optimality in complex maps, resulting in local optimal solutions. The secondary optimization of the path performs additional processing on the path planned by the A* algorithm, shortens the path length, and improves the path smoothness, but this method requires the design of additional path smoothing methods, increases the complexity of the algorithm, and reduces the path-finding efficiency accordingly. In addition, none of the above improvements consider the impact of special terrain on the algorithm, but focus more on obstacles.

[0006] In fact, maps of actual application scenarios often have special terrains, and the above-mentioned improvements to the algorithm cannot adapt to such complex environments. Based on this, it is particularly necessary to develop a path planning solution that can adapt to complex environments. Summary of the invention

[0007] In order to solve the above technical problems, the embodiments of the present application respectively provide a path planning method, device, equipment and storage medium, so that the intelligent robot can fully consider the environmental information in the path search process, improve the calculation efficiency and the smoothness of the planned path. The present application is aimed at regular hexagonal grid maps, and incorporates terrain information into the map to guide the path search; in view of the problems that the traditional A* algorithm cannot be affected by special terrain in the grid map, the number of traversed nodes is large and the number of inflection points is large, by improving the various items of the algorithm heuristic function, designing new items and introducing environmental information, the efficiency and environmental adaptability are improved while maintaining the optimality of the planned path, thereby improving the quality of the path.

[0008] Other features and advantages of the present application will become apparent from the following detailed description, or may be learned in part by the practice of the present application.

[0009] According to one aspect of an embodiment of the present application, a path planning method is provided, the method comprising:

[0010] Construct a raster path planning map;

[0011] Construct the first set and the second set, and add the starting point to the first set, where g'(n)=0 of the starting point, and g'(n) is the sum of the costs of each planned path, indicating the movement cost;

[0012] When the first set is not empty, determining a node with the smallest heuristic function value in the first set as the current node;

[0013] If the current node is not the end point, remove the current node from the first set and add it to the second set, and traverse all neighbor nodes of the current node that are not in the second set;

[0014] If the neighbor node is not in the first set, add the neighbor node to the first set, calculate the single-step movement cost, turning penalty, adaptive heuristic and heuristic function value of the neighbor node, and use the current node as the parent node of the neighbor node;

[0015] In the case where the neighbor node is in the first set, the current node is used as the parent node of the current neighbor node, and the sum of the movement cost and the turning penalty of the current neighbor node is calculated. If the sum of the movement cost and the turning penalty calculated at present is less than the sum of the movement cost and the turning penalty of the current neighbor node last time, the current node is used as the parent node of the current neighbor node, and the adaptive heuristic amount of the current neighbor node is recalculated, and the heuristic function value of the current neighbor node is updated according to the adaptive heuristic amount of the current neighbor node;

[0016] When the current node is the end point, the parent nodes are traced back in sequence to obtain a path.

[0017] Furthermore, the single-step moving cost is calculated by the following method:

[0018] When node i is in the region with an impact factor of K, the single-step moving cost S'(i) from the parent node of node i to node i is calculated by the following formula:

[0019] S'(i)=1+K(K>-1)

[0020] Among them, the constant term 1 is the distance cost of moving to node i. When K=0, node i is an ordinary node; when K=1, the cost of moving to node i is twice that of moving to an ordinary node.

[0021] Furthermore, the single-step moving cost is calculated by the following method:

[0022] When there are m special areas superimposed at the location of node i, the single-step moving cost S'(i) from the parent node of node i to node i is calculated by the following formula:

[0023]

[0024] Among them, K j is the impact factor corresponding to special region j.

[0025] Furthermore, the movement cost g'(n) is calculated by the following formula:

[0026]

[0027] Among them, n represents the current node traversed during the search process.

[0028] Furthermore, the turning penalty is calculated by the following formula:

[0029] punish(n+1)=β(1-cosθ)

[0030]

[0031] Among them, punish(n+1) represents the turning penalty of the new node (n+1), n ​​is the current node, θ is the angle between the turning angle of the route and the direction of the previous path and the direction of the new node, and β is the penalty intensity coefficient. is the forward direction from the parent node to the current node, The possible directions from the current node to the new node.

[0032] Furthermore, the adaptive heuristic is calculated by the following formula:

[0033] h'(n)=(1-α)×[h(n)+h(n-1)]

[0034]

[0035] Where h'(n) represents the adaptive heuristic, (1-α) represents the adaptive weight multiplier of the heuristic, α is the map complexity, h(n) represents the estimated cost between the current node n and the end point, h(n-1) represents the estimated cost between the parent node (n-1) of the current node n and the end point, m represents the number of special area types, K i represents the impact factor corresponding to special area i, x is the number of nodes in the map, y is the number of obstacle nodes, ε i Indicates the number of nodes with special region i attribute.

[0036] Furthermore, the heuristic function value f(n) is calculated by the following formula:

[0037] f(n)=g'(n)+(1-α)[h(n)+h(n-1)]+punish(n)

[0038] Wherein, punish(n) represents the turning penalty of node n.

[0039] According to one aspect of an embodiment of the present application, a path planning device is provided, including:

[0040] A map construction unit is configured to construct a raster path planning map;

[0041] A set construction unit is configured to construct a first set and a second set, and add the starting point to the first set, wherein g'(n) of the starting point is 0, and g'(n) is the sum of the costs of each planned path segment, indicating the movement cost;

[0042] a node determination unit, configured to determine, when the first set is not empty, a node with a minimum heuristic function value in the first set as a current node;

[0043] a node traversal unit, configured to, when the current node is not an end point, remove the current node from the first set and add it to the second set, and traverse all neighbor nodes of the current node that are not in the second set;

[0044] A first calculation unit is configured to, if the neighbor node is not in the first set, add the neighbor node to the first set, calculate the single-step movement cost, the turning penalty, the adaptive heuristic and the heuristic function value of the neighbor node, and use the current node as the parent node of the neighbor node;

[0045] A second calculation unit is configured to, when the neighbor node is in the first set, use the current node as the parent node of the current neighbor node, and calculate the sum of the movement cost and the turning penalty of the current neighbor node; if the sum of the movement cost and the turning penalty currently calculated is less than the sum of the movement cost and the turning penalty of the current neighbor node last time, use the current node as the parent node of the current neighbor node, recalculate the adaptive heuristic of the current neighbor node, and update the heuristic function value of the current neighbor node according to the adaptive heuristic of the current neighbor node;

[0046] The node backtracking unit is configured to, when the current node is the end point, backtrack the parent nodes in sequence to obtain a path.

[0047] According to one aspect of an embodiment of the present application, an electronic device is provided, comprising: a controller; and a memory for storing one or more programs, wherein when the one or more programs are executed by the controller, the controller implements the path planning method described above.

[0048] According to one aspect of an embodiment of the present application, a computer-readable storage medium is also provided, on which computer-readable instructions are stored. When the computer-readable instructions are executed by a processor of a computer, the computer executes the above-mentioned path planning method.

[0049] According to one aspect of the embodiments of the present application, a computer program product or a computer program is also provided, the computer program product or the computer program includes a computer instruction, and the computer instruction is stored in a computer-readable storage medium. The processor of the computer device reads the computer instruction from the computer-readable storage medium, and the processor executes the computer instruction, so that the computer device executes the above-mentioned path planning method.

[0050] The technical solution provided in the embodiments of the present application has at least the following advantages:

[0051] (1) In view of the problems that the traditional A* algorithm cannot be affected by special terrain in grid maps, the number of traversed nodes is large, and there are many turning points, the movement cost calculation method is first improved. The present application flexibly sets the movement cost by introducing an influencing factor, so that the path search can fully consider the influence of special terrain; then, an adaptive heuristic is introduced, and the heuristic after adding the influence of the parent node is adjusted using map information, thereby speeding up the search while maintaining the optimal path; finally, a turning penalty is designed to reduce the number of path turns.

[0052] (2) This application has good adaptability when facing maps with different numbers of obstacles and special terrains. On the premise of keeping the path movement cost to a minimum, it can adjust the search speed according to the map conditions, reduce the traversal of nodes, inflection points and turning angles, improve the algorithm efficiency and path quality, and has good stability.

[0053] (3) The path planning effect is best for complex maps with special areas such as grasslands and hills. Compared with the traditional A* algorithm and other improved algorithms in the literature, this application traverses fewer nodes, searches faster, and significantly reduces the number of turning points and turning angles, thereby improving the smoothness of the path.

[0054] In summary, the present application has obvious advantages and adaptability, and can better provide a good solution for intelligent robot path planning in complex environments.

[0055] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The drawings herein are incorporated into the specification and constitute a part of the specification, showing embodiments consistent with the present application, and together with the specification, are used to explain the principles of the present application. Obviously, the drawings described below are only some embodiments of the present application, and for those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative work. In the drawings:

[0057] Figure 1 is a flow chart of a path planning method shown in an exemplary embodiment of the present application;

[0058] Figure 2 is a schematic diagram of an adaptive heuristic test result shown in an exemplary embodiment of the present application;

[0059] Figure 3 is a schematic diagram of the path forward direction shown in an exemplary embodiment of the present application;

[0060] Figure 4 is a schematic diagram of a cube coordinate system shown in an exemplary embodiment of the present application;

[0061] Figure 5 is an experimental map shown in an exemplary embodiment of the present application;

[0062] Figure 6 is a schematic diagram of search results of different algorithms shown in an exemplary embodiment of the present application;

[0063] Figure 7 It is a schematic diagram of the structure of a path planning device shown in an exemplary embodiment of the present application. DETAILED DESCRIPTION

[0064] Here, exemplary embodiments will be described in detail, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application. Instead, they are only examples of devices and methods consistent with some aspects of the present application as detailed in the attached claims.

[0065] The block diagrams shown in the accompanying drawings are merely functional entities and do not necessarily correspond to physically independent entities. That is, these functional entities may be implemented in software form, or in one or more hardware modules or integrated circuits, or in different networks and / or processor devices and / or microcontroller devices.

[0066] The flowcharts shown in the accompanying drawings are only exemplary and do not necessarily include all the contents and operations / steps, nor must they be executed in the order described. For example, some operations / steps can be decomposed, and some operations / steps can be combined or partially combined, so the actual execution order may change according to actual conditions.

[0067] The term "multiple" as used in this application refers to two or more than two. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the related objects are in an "or" relationship.

[0068] Embodiment 1:

[0069] See also Figure 1 , Figure 1 FIG. 1 is a flow chart of a path planning method shown in an exemplary embodiment of the present application. An embodiment of the present invention provides a path planning method, which includes the following steps:

[0070] Step S1, constructing a grid path planning map.

[0071] Step S3, constructing a first set and a second set, adding the starting point to the first set, wherein g'(n) of the starting point is 0, and g'(n) is the sum of the costs of each planned path segment, indicating the movement cost.

[0072] The first set and the second set in the embodiment of the present application are described as Open List and Closed List sets.

[0073] Step S4: when the first set is not empty, determine the node with the smallest heuristic function value in the first set as the current node.

[0074] Step S5, when the current node is not the end point, remove the current node from the first set and add it to the second set, and traverse all neighbor nodes of the current node that are not in the second set.

[0075] Step S6, when the neighbor node is not in the first set, add the neighbor node to the first set, calculate the single-step movement cost, turning penalty, adaptive heuristic and heuristic function value of the neighbor node, and use the current node as the parent node of the neighbor node.

[0076] Step S7, when the neighbor node is in the first set, the current node is used as the parent node of the current neighbor node, and the sum of the movement cost and the turning penalty of the current neighbor node is calculated. If the sum of the movement cost and the turning penalty calculated at present is less than the sum of the movement cost and the turning penalty of the current neighbor node last time, the current node is used as the parent node of the current neighbor node, and the adaptive heuristic amount of the current neighbor node is recalculated, and the heuristic function value of the current neighbor node is updated according to the adaptive heuristic amount of the current neighbor node.

[0077] Step S8, when the current node is the end point, backtrack to the parent nodes in sequence to obtain a path.

[0078] It should be noted that the technical problem to be solved by the path planning method proposed in the embodiment of the present application is that the traditional A* algorithm cannot be affected by special terrain, and there are problems such as a large number of traversed nodes, a large number of turning points, and a large total turning angle during the path search process. Improvements are made by designing a flexible cost strategy, an adaptive heuristic function, and a turning penalty function, adding the impact of special terrain on path search, reducing the number of traversed nodes, improving calculation efficiency, reducing the turning points in the path, and the total turning angle, while ensuring the optimal path, so as to quickly and stably plan a more reasonable intelligent robot path.

[0079] Therefore, the core improvements of this application are mainly reflected in four aspects: flexible cost setting, increasing adaptive heuristics, increasing turning penalty strategies, and improving heuristic functions. The four aspects described above will be elaborated in detail below.

[0080] First, the cost can be set flexibly. Traditional grid methods usually define nodes with only two attributes: passable and inaccessible. In practical applications, some special areas are often included in the scope of path planning. These areas are passable, but due to their particularity, the distance cannot be simply used as the movement cost of the area. Therefore, grid nodes are defined in three states:

[0081] 1. Ordinary nodes;

[0082] 2. Nodes in special areas;

[0083] 3. Obstacle node.

[0084] When abstracting the map, the quantitative information of the communication difficulty is stored. The calculation method of g(n) is further improved by introducing the influence factor K into the calculation of the heuristic function. K represents the difficulty of passing through a special area, and its value can be determined according to the actual situation or experience of the special area. If node i is in an area with an influence factor of K, the single-step movement cost S'(i) from the parent node of node i to node i is designed to be:

[0085] S'(i)=1+K (K>-1)

[0086] The constant term 1 is the distance cost of moving to node i, and the moving cost, that is, the difficulty of passing, increases with the increase of K, and decreases with the decrease of K. When K = 0, this node is an ordinary node; when K = 1, it can be generally understood that the cost of moving to node i is twice that of moving to an ordinary node. When the K value is large enough, it can be determined that there is an obstacle at the location of node i and it is not passable.

[0087] Moreover, a path node may be located at the superposition of multiple types of regions, and the impact of multiple regions on the cost calculation of the node needs to be comprehensively considered. If the location of node i has a total of m special regions superimposed, the expression of S'(i) is:

[0088]

[0089] Where K j is the impact factor corresponding to the special area j, and this formula is used to calculate the cost g'(n) of moving to the current node after being affected.

[0090] Secondly, the embodiment of the present application increases the adaptive heuristic by introducing the influence of the parent node (n-1), adding the heuristic of the parent node to the heuristic function of the current node, and increasing the overall proportion of the heuristic in the heuristic function. This can improve the search speed, traverse fewer nodes during the search, and reduce the amount of calculation, but too fast a search speed may lead to the emergence of a local optimal solution. In order to speed up the search speed of the algorithm in simple maps and take a relatively cautious approach in more complex maps, it is necessary to set weights for the heuristic according to the map information so that it can adaptively control the search speed. If the number of nodes in the map is x, the number of obstacle nodes is y, the number of special area types is m, the number of nodes with corresponding area attributes is ε, and the corresponding special area impact factor is K, and the above parameters are all determined by the map information, then the improved heuristic h'(n) expression is as follows:

[0091] h'(n)=(1-α)×[h(n)+h(n-1)]

[0092]

[0093] Where (1-α) is the adaptive weight multiplier of the heuristic; α is the map complexity, which ranges from [0,1]. When the obstacles and special areas are large, the map complexity increases, the heuristic will decrease, and the search speed will decrease.

[0094] In order to verify the effect of adaptive heuristics on search speed, a 30*30 regular hexagonal map was constructed for experimental testing. The test results are as follows: Figure 2 shown. Figure 2 The a in the middle is the traditional A * The algorithm's search results, Figure 2 b is the search result that only introduces the parent node heuristic. Figure 2 c is the search result after the introduction of the adaptive heuristic in this paper. The figure shows the common nodes, obstacle nodes, path nodes, nodes traversed during the search process, path starting point and path end point, and the calculated value is 0.276. The comparison of search results is shown in Table 1.

[0095] Table 1 Comparison of search results

[0096]

[0097] The results show that after the introduction of the parent node heuristic, the number of traversed nodes is greatly reduced and the search speed is improved, but the path planned loses its optimality; while the function introducing the adaptive heuristic maintains the optimality of the path and the number of traversed nodes is significantly reduced, proving the feasibility of the solution proposed in this application. However, it can be seen that after the introduction of the adaptive heuristic, there is still inflection point redundancy, and there is room for further optimization and improvement.

[0098] Therefore, the embodiment of the present application also adds a turning penalty strategy to deal with the situation of redundant turning points. The formation of redundant corners is because the algorithm is less stable during search. Among multiple paths with the same moving cost, the algorithm cannot select a route with the smallest number of turning points and total turning angles. The design adds a penalty amount to control the formation of turning points. While maintaining the optimality of the path, the algorithm has a search tendency to maintain the forward direction of the path search and not turn easily.

[0099] like Figure 3 As shown in the figure, the current node n-1, the parent node n-2 of the current node, and the neighbor node n of the current node are shown, that is, the new node to be traversed in the next traversal. The black arrow is the direction from the parent node to the current node, with vector Indicates; the gray arrow is the possible direction from the current node to the new node, with vector express.

[0100] When traversing a new node, there are six directions of expansion. Compared with the previously planned route, these six directions will produce the following results:

[0101] 1. Continue along the previous route;

[0102] 2. The new node direction differs by 60° from the previous route direction;

[0103] 3. The direction of the new node differs by 120° from the direction of the previous route.

[0104] It can be seen that the turning angle of the route is related to the angle θ between the previous path direction and the new node direction. Let the current node be n, its parent node be (n-1), and the new node be (n+1), then:

[0105]

[0106] in is the vector from the current node to the new node, A vector from the parent node to the current node.

[0107] Add a penalty amount punish(n+1) to each new node, punish(n+1) = β(1-cosθ), the penalty value range is [0, 2β], β is the penalty intensity coefficient, and the penalty intensity increases with the increase of β. On this basis, new nodes with unchanged forward direction will be expanded first to ensure that the number of turns and inflection points are reduced as much as possible during the search.

[0108] Finally, the embodiment of the present application also improves the heuristic function. The improvement of the heuristic function is based on the proposal of flexible setting of movement cost, adaptive heuristic amount and turning penalty strategy, so that the present application can adapt to complex maps, give priority to planning paths in areas with low difficulty of passage, improve search speed, reduce the number of traversed nodes, and minimize path turning points and turning angles.

[0109] The heuristic function f(n) is expressed as follows:

[0110] f(n)=g'(n)+(1-α)[h(n)+h(n-1)]+punish(n)

[0111] Comparative Example:

[0112] In order to verify the performance of the path planning method proposed in the embodiment of the present application, the traditional A* algorithm, the algorithm with flexible cost setting, the algorithm introducing adaptive heuristics, the algorithm in document [1], and the algorithm in document [2] are used as comparative examples 1 to 5 respectively and simulated and verified in the same map with the method proposed in embodiment 1 of the present application.

[0113] Comparative Example 1: The working principle of the traditional A* algorithm is as follows:

[0114] Path search algorithms can be divided into blind and heuristic algorithms according to whether they are heuristic or not. Blind algorithms search according to a predetermined search strategy without considering the characteristics of the map itself and the location of the destination. They mainly include breadth-first search algorithm (BFS) and Dijkstra algorithm. Heuristic algorithms use the estimated cost of reaching the destination as an aid to calculate the heuristic function value and speed up the search. Representative algorithms include greedy algorithm, A *The most typical one is the greedy algorithm, which selects the node with the lowest estimated cost to reach the end point at each step of its expansion. * The algorithm has the characteristics of both Dijkstra and greedy algorithms, and its heuristic function is:

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

[0116] Where: n represents the current node traversed during the search process, g(n) represents the cost of moving from the starting point to the current node, h(n) represents the estimated cost between the current node and the end point, and f(n) is A * The heuristic function value of the algorithm.

[0117]

[0118] S(i) represents the moving distance of the corresponding node. For example, S(n) represents the actual moving distance from the parent node (n-1) of the current node to the current node n. Then g(n) is the sum of the cost of each planned path. In a hexagonal grid, the distance between adjacent nodes is the same, so S(i) = 1 can be set to facilitate the operation of the algorithm.

[0119] The heuristic algorithm needs to calculate the distance between nodes. The distance calculation requires the assistance of the coordinate system. Common hexagonal grid coordinate systems mainly include cube coordinates, offset coordinates, and axis coordinates. The cube coordinate system is easy to understand and the distance calculation formula is simple and clear. Therefore, the cube coordinate system is used to calculate the distance. Different from the two axes x and y in the cube grid Cartesian coordinate system, such as Figure 4 As shown, there are three axes q, r, and s in the cube coordinate system, and the corresponding coordinates of the grid are (q, r, s).

[0120] Common distance calculation methods include Euclidean distance, Manhattan distance, diagonal distance, etc. In a hexagonal grid, the distance between two nodes can be intuitively measured in steps, so Manhattan distance is used to estimate the moving cost, that is, to calculate the distance from the current node to the end point. Assume that grid A (q, r, s) and grid B (q, r, s), the Manhattan distance between A and B is expressed as:

[0121]

[0122] A * During the search process, the algorithm finds the node with the smallest heuristic function value each time for expansion. The specific steps of the algorithm are as follows:

[0123] 1) Create an Open List and a Closed List set, add the starting point to the Open List, and set the starting point g(n) = 0.

[0124] 2) Find the node with the smallest f(n) value in the Open List. If the Open List is empty, the search fails and ends.

[0125] 3) Remove the current node from the Open List and add it to the Closed List, and traverse all neighbor nodes of the current node that are not in the Closed List:

[0126] a. If the neighbor node is not in the Open List, add the neighbor node to the Open List, calculate its move cost f(n), and set the current node as the parent node of this neighbor node;

[0127] b. If the neighbor node is in the Open List, and the g(n) calculated from the current node to this neighbor node is smaller than the previous one, update the value of f(n) and set the current node as the parent node of this neighbor node; if it is larger, do not update.

[0128] 4) Repeat steps 2 and 3 until the node found in step 2 is the end point.

[0129] From the end point, trace back to the parent node in sequence. If the parent node is the starting point, the process ends and the path planning result is obtained.

[0130] Comparative Example 2: The algorithm for flexibly setting the cost refers to the method proposed in the embodiment of the present application that only considers the flexible setting of the cost, but does not consider the adaptive heuristic and the turning penalty strategy, that is, the improvement of the traditional A* algorithm only considers the flexible setting of the cost.

[0131] Comparative Example 3: The algorithm that introduces the adaptive heuristic refers to the method proposed in the embodiment of the present application that only introduces the adaptive heuristic, that is, only the adaptive heuristic is introduced to improve the traditional A* algorithm.

[0132] Comparative Example 4: Reference [1] is "Chen Chen. Research on robot shortest path planning based on improved A* algorithm [J]. Computers and Digital Engineering, 2023, 51(8): 1697-1701."

[0133] Comparative Example 5: Reference [2] is "Wu Shanping, Huang Yanyan, Chen Tiande. Improved A* algorithm for static route planning of surface ships [J]. Computer Engineering and Applications, 2022, 58(23): 307-315."

[0134] The path planning method proposed in Example 1 of the present application is simulated using Unreal Engine UE5 (Unreal Engine 5). A grid map of 30*30 is constructed, in which there are 191 obstacle nodes. A special area of ​​grass is added to the map, and its influence factor is set to 1. There are 248 grass nodes. Under the premise of introducing flexible cost setting and adaptive heuristics, the value of the turning penalty strength β is experimented, and the value of α is calculated to be 0.35. The map and the starting point and end point are selected as follows: Figure 5 As shown. The algorithm search results for different β values ​​are shown in Table 2. From the search results of each β value in the analysis table, it can be seen that after the introduction of the turning penalty, the path turning point and the total turning angle are reduced. When the value exceeds 0.5, the number of traversed nodes will become larger and larger. This is because the excessive penalty causes the algorithm to traverse more nodes whose forward direction has not changed rather than nodes closer to the end point. When the value is between 0.5 and 1, the path has the least turning points and the smallest total turning angle. The number of turning points is reduced by 41%, while the number of redundant turning points is reduced by 54%. The optimization effect is obvious, and 0.5 is selected as the value of β for simulation in the embodiment of the present application.

[0135] Table 2 Performance comparison of different β value algorithms

[0136] β value 0 0.25 0.5 0.75 1 1.5 2 Number of path nodes 44 44 44 44 44 44 44 Path movement cost / step 60 60 60 60 60 60 60 Number of nodes traversed 263 261 261 267 267 276 280 Number of inflection points 17 12 10 10 10 12 12 Total turning angle / ° 1020 720 600 600 600 720 720

[0137] Similarly, Unreal Engine 5 is used to simulate the same grid map in Comparative Examples 1 to Comparative Examples 5 mentioned above. The search results of the paths in each comparative example are as follows: Figure 6 The comparison between Example 1 of the present application and various comparative examples is shown in Table 3.

[0138] It can be seen that Example 1 only found the path with the shortest distance between the starting point and the end point, and chose to travel a long distance in the grass area, which increased the overall movement cost and lost the optimality; after taking the method of setting the cost flexibly, the algorithm searched for the path with the minimum cost and achieved optimality, but due to the influence of the special terrain, the number of traversed nodes and turning points increased a lot. After introducing the adaptive heuristic, the searched path maintained optimality and the number of traversed nodes was also reduced a lot, but the number of turning points and turning angles of the A* algorithm were already redundant. After introducing the adaptive heuristic, the number of turning points increased by three, which needs further optimization. Comparative Example 4 uses the average of Manhattan distance and Euclidean distance as the distance calculation method. Although this will make the path visually approach the end point faster, it will also lead to the generation of too many turning points; and it introduces the parent node information into the heuristic calculation. It can be seen that the number of traversed nodes is greatly reduced, which is 59.0% less than the A* algorithm with flexible cost settings, and the search speed is greatly improved, but this is produced at the expense of the optimality and stability of the algorithm. It can be seen from the figure that the algorithm chooses to move more on the grass in the front section of the path, the movement cost increases by 3, the number of turning points also increases by 3, and the path loses its optimality. Comparative Example 5 introduces the orientation information of the target node into the heuristic function. First, the cosine value of the angle between the forward direction and the target direction is calculated. By judging in advance whether there are obstacles in the forward direction, the cosine value is determined to increase or decrease the cost, so as to quickly bypass the obstacles. It can be seen that Comparative Example 5 searches for the optimal path, the turning points and the total turning angle are reduced by more than half, and the number of traversed nodes is also reduced compared with the algorithm with flexible cost settings, but the reduction is small, and the search efficiency is not high.

[0139] The path planning method proposed in this application searches for the optimal path on the map with special areas. Compared with the algorithm with flexible cost setting, the number of traversed nodes, the number of inflection points and the turning angle are greatly improved; compared with comparative example 4, although the number of traversed nodes is large, the optimal path is planned and the number of inflection points is also reduced a lot; compared with comparative example 5, there are more inflection points, but the number of traversed nodes in comparative example 5 is very different from that in embodiment 1, and this application has a significant improvement in pathfinding efficiency. It can be seen from the analysis of Table 3 that the improved algorithm of this application will be more superior than the traditional A* algorithm, which is specifically manifested in that the overall cost of the path movement searched by the improved A* algorithm is reduced by 6, a decrease of 9.1%. On the basis of maintaining the optimality of the path, the number of traversed nodes is reduced by an average of 21.8%, and the number of inflection points and the total turning angle are reduced by an average of 25.0%.

[0140] Table 3 Performance comparison of different algorithms

[0141] index Comparative Example 1 Comparative Example 2 Comparative Example 3 Comparative Example 4 Comparative Example 5 Example 1 Number of path nodes 43 44 44 44 44 44 Path movement cost / step 66 60 60 63 60 60 Number of nodes traversed 261 420 263 172 409 261 Number of inflection points 9 15 17 18 7 10 Total turning angle / ° 540 900 1020 1080 420 600

[0142] In order to verify the performance changes of the improved algorithm when facing different obstacles and grass, obstacle nodes and grass nodes are generated at random positions with different probabilities on a 30*30 map. Simulation experiments are carried out on the solutions proposed in Comparative Example 1, Comparative Example 2 and Example 1 under the same starting point. The experimental results are shown in Table 4.

[0143] Table 4 Comparison of experimental results under different maps

[0144]

[0145]

[0146] According to the experimental results shown in Table 4, it can be seen that the path planned by Comparative Example 1 is shorter, but the influence of the grass is ignored, and the movement cost is greater. However, when facing the changes of obstacles and grass, the path planned by Example 1 still has the minimum movement cost. When the number of obstacles and grass is small, the present application traverses fewer nodes in the path search, and the planned path contains fewer turning points than the algorithm with flexible cost setting. This is because the map is not particularly complex at this time, and the path is less obstructed by obstacles and grass. The adaptive heuristic and turning penalty strategy will make the algorithm quickly approach the end point, effectively improving the search efficiency.

[0147] As the probability of generating obstacles and grass increases, the map complexity gradually increases. The difference between the number of traversed nodes and the number of turning points between Example 1 and Comparative Example 2 becomes smaller. When the probability of generating obstacles and grass is 0.4, the map becomes complex enough and the complexity exceeds 0.5. When searching for a path, it will be more cautious than Comparative Example 1, and the number of traversed nodes exceeds that of Comparative Example 2 to ensure that the path with the lowest movement cost is searched out. When the map complexity is large, the path selection space becomes smaller, and the appearance of turning points is sometimes inevitable. However, the turning points of the path planned by the present application are always less than or equal to those of Comparative Example 2, and a smoother path can be planned.

[0148] The above experimental analysis fully demonstrates that Example 1 of the present application can adapt to complex map environments, has good adaptability and stability when facing maps with different numbers of obstacles and special terrains, can plan the optimal path, has higher search efficiency, and has fewer planned path inflection points and turning angles, making it more suitable for path planning of intelligent robots.

[0149] Embodiment 2:

[0150] Another aspect of the present application also provides a path planning device, such as Figure 7 As shown, the device 700 includes:

[0151] A map construction unit 701 is configured to construct a raster path planning map;

[0152] The set construction unit 702 is configured to construct a first set and a second set, and add the starting point to the first set, wherein g'(n) of the starting point is 0, and g'(n) is the sum of the costs of each planned path segment, indicating the movement cost;

[0153] A node determination unit 703 is configured to determine, when the first set is not empty, a node with a minimum heuristic function value in the first set as a current node;

[0154] The node traversal unit 704 is configured to, when the current node is not an end point, remove the current node from the first set and add it to the second set, and traverse all neighbor nodes of the current node that are not in the second set;

[0155] The first calculation unit 705 is configured to add the neighbor node to the first set if the neighbor node is not in the first set, calculate the single-step movement cost, turning penalty, adaptive heuristic and heuristic function value of the neighbor node, and use the current node as the parent node of the neighbor node;

[0156] The second calculation unit 706 is configured to, when the neighbor node is in the first set, take the current node as the parent node of the current neighbor node, and calculate the sum of the movement cost and the turning penalty of the current neighbor node; if the sum of the movement cost and the turning penalty currently calculated is less than the sum of the movement cost and the turning penalty of the current neighbor node last time, take the current node as the parent node of the current neighbor node, recalculate the adaptive heuristic of the current neighbor node, and update the heuristic function value of the current neighbor node according to the adaptive heuristic of the current neighbor node;

[0157] The node backtracking unit 707 is configured to, when the current node is the end point, backtrack the parent nodes in sequence to obtain a path.

[0158] It should be noted that the path planning device provided in the above embodiment and the path planning method provided in the above embodiment belong to the same concept, wherein the specific manner in which each module or unit performs the operation has been described in detail in the method embodiment and will not be repeated here.

[0159] Embodiment 3:

[0160] Another aspect of the present application provides an electronic device, including: a controller; a memory for storing one or more programs, which, when executed by the controller, executes the path planning in the above-mentioned embodiments.

[0161] In particular, according to an embodiment of the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present application includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a computer program for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication part, and / or installed from a removable medium. When the computer program is executed by a central processing unit (CPU) 701, various functions defined in the system of the present application are executed.

[0162] It should be noted that the computer-readable medium shown in the embodiment of the present application can be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium can be, for example, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of computer-readable storage media can include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM), a flash memory, an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present application, a computer-readable storage medium can be any tangible medium containing or storing a program, which can be used by an instruction execution system, device or device or used in combination with it. In the present application, a computer-readable signal medium can include a data signal propagated in a baseband or as a part of a carrier wave, wherein a computer-readable computer program is carried. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than computer-readable storage media, which may send, propagate, or transmit programs for use by or in conjunction with an instruction execution system, apparatus, or device. The computer program contained on the computer-readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wired, etc., or any suitable combination of the above.

[0163] The flowchart and block diagram in the accompanying drawings illustrate the possible architecture, functions and operations of the system, method and computer program product according to various embodiments of the present application. Wherein, each box in the flowchart or block diagram can represent a module, a program segment, or a part of the code, and the above-mentioned module, program segment, or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order than the order marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flowchart, and the combination of boxes in the block diagram or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0164] The units involved in the embodiments described in this application may be implemented by software or hardware, and the units described may also be set in a processor. The names of these units do not, in some cases, constitute limitations on the units themselves.

[0165] Embodiment 4:

[0166] Another aspect of the present application further provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the path planning method as described above is implemented. The computer-readable storage medium may be included in the electronic device described in the above embodiment, or may exist independently without being assembled into the electronic device.

[0167] Embodiment 5:

[0168] Another aspect of the present application also provides a computer program product or a computer program, which includes a computer instruction stored in a computer-readable storage medium. A processor of a computer device reads the computer instruction from the computer-readable storage medium, and the processor executes the computer instruction, so that the computer device executes the path planning method provided in each of the above embodiments.

[0169] Embodiment 6:

[0170] According to one aspect of an embodiment of the present application, a computer system is also provided, including a central processing unit (CPU), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) or a program loaded from a storage portion into a random access memory (RAM), such as executing the method in the above embodiment. In RAM, various programs and data required for system operation are also stored. CPU, ROM and RAM are connected to each other via a bus. An input / output (I / O) interface is also connected to the bus.

[0171] The following components are connected to the I / O interface: an input part including a keyboard, a mouse, etc.; an output part including a cathode ray tube (CRT), a liquid crystal display (LCD), etc., and a speaker; a storage part including a hard disk, etc.; and a communication part including a network interface card such as a LAN (Local Area Network) card, a modem, etc. The communication part performs communication processing via a network such as the Internet. A drive is also connected to the I / O interface as needed. Removable media, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., are installed on the drive as needed so that the computer program read therefrom is installed into the storage part as needed.

[0172] The above content is only a preferred exemplary embodiment of the present application and is not intended to limit the implementation scheme of the present application. A person skilled in the art can easily make corresponding changes or modifications based on the main concept and spirit of the present application. Therefore, the scope of protection of the present application shall be based on the scope of protection required by the claims.

Claims

1. A path planning method, characterized in that: The method comprises: Construct a raster path planning map; Construct the first set and the second set, and add the starting point to the first set, where g'(n)=0 of the starting point, and g'(n) is the sum of the costs of each planned path, indicating the movement cost; When the first set is not empty, determining a node with the smallest heuristic function value in the first set as the current node; If the current node is not the end point, remove the current node from the first set and add it to the second set, and traverse all neighbor nodes of the current node that are not in the second set; If the neighbor node is not in the first set, add the neighbor node to the first set, calculate the single-step movement cost, turning penalty, adaptive heuristic and heuristic function value of the neighbor node, and use the current node as the parent node of the neighbor node; In the case where the neighbor node is in the first set, the current node is used as the parent node of the current neighbor node, and the sum of the movement cost and the turning penalty of the current neighbor node is calculated. If the sum of the movement cost and the turning penalty calculated at present is less than the sum of the movement cost and the turning penalty of the current neighbor node last time, the current node is used as the parent node of the current neighbor node, and the adaptive heuristic amount of the current neighbor node is recalculated, and the heuristic function value of the current neighbor node is updated according to the adaptive heuristic amount of the current neighbor node; When the current node is the end point, trace back to the parent nodes in sequence to obtain the path; The single-step movement cost is calculated as follows: When node i is in the region with an impact factor of K, the single-step moving cost S'(i) from the parent node of node i to node i is calculated by the following formula: S'(i)=1+K(K>-1) Among them, the constant term 1 is the distance cost of moving to node i. When K = 0, node i is an ordinary node; when K = 1, the cost of moving to node i is twice that of moving to an ordinary node; When there are m special areas superimposed at the location of node i, the single-step moving cost S'(i) from the parent node of node i to node i is calculated by the following formula: Among them, K j is the impact factor corresponding to special region j; The turning penalty is calculated by the following formula: punish(n+1)=β(1-cosθ) Among them, punish(n+1) represents the turning penalty of the new node (n+1), n ​​is the current node, θ is the angle between the turning angle of the route and the direction of the previous path and the direction of the new node, and β is the penalty intensity coefficient. is the forward direction from the parent node to the current node, The possible directions from the current node to the new node; The adaptive heuristic is calculated by the following formula: h'(n)=(1-α)×[h(n)+h(n-1)] Where h'(n) represents the adaptive heuristic, (1-α) represents the adaptive weight multiplier of the heuristic, α is the map complexity, h(n) represents the estimated cost between the current node n and the end point, h(n-1) represents the estimated cost between the parent node (n-1) of the current node n and the end point, m represents the number of special area types, K i represents the impact factor corresponding to special area i, x is the number of nodes in the map, y is the number of obstacle nodes, ε i Indicates the number of nodes with special region i attributes; The heuristic function value f(n) is calculated by the following formula: f(n)=g'(n)+(1-α)[h(n)+h(n-1)]+punish(n) Wherein, punish(n) represents the turning penalty of node n.

2. The path planning method according to claim 1, characterized in that: The movement cost g'(n) is calculated by the following formula: Among them, n represents the current node traversed during the search process.

3. A path planning device, characterized in that: include: A map construction unit is configured to construct a raster path planning map; A set construction unit is configured to construct a first set and a second set, and add the starting point to the first set, wherein g'(n) of the starting point is 0, and g'(n) is the sum of the costs of each planned path segment, indicating the movement cost; a node determination unit, configured to determine, when the first set is not empty, a node with a minimum heuristic function value in the first set as a current node; a node traversal unit, configured to, when the current node is not an end point, remove the current node from the first set and add it to the second set, and traverse all neighbor nodes of the current node that are not in the second set; A first calculation unit is configured to, if the neighbor node is not in the first set, add the neighbor node to the first set, calculate the single-step movement cost, the turning penalty, the adaptive heuristic and the heuristic function value of the neighbor node, and use the current node as the parent node of the neighbor node; A second calculation unit is configured to, when the neighbor node is in the first set, use the current node as the parent node of the current neighbor node, and calculate the sum of the movement cost and the turning penalty of the current neighbor node; if the sum of the movement cost and the turning penalty currently calculated is less than the sum of the movement cost and the turning penalty of the current neighbor node last time, use the current node as the parent node of the current neighbor node, recalculate the adaptive heuristic of the current neighbor node, and update the heuristic function value of the current neighbor node according to the adaptive heuristic of the current neighbor node; A node backtracking unit is configured to, when the current node is the end point, backtrack the parent nodes in sequence to obtain a path; The first computing unit is further configured to: The single-step movement cost is calculated as follows: When node i is in the region with an impact factor of K, the single-step moving cost S'(i) from the parent node of node i to node i is calculated by the following formula: S'(i)=1+K(K>-1) Among them, the constant term 1 is the distance cost of moving to node i. When K = 0, node i is an ordinary node; when K = 1, the cost of moving to node i is twice that of moving to an ordinary node; When there are m special areas superimposed at the location of node i, the single-step moving cost S'(i) from the parent node of node i to node i is calculated by the following formula: Among them, K j is the impact factor corresponding to special region j; The turning penalty is calculated by the following formula: punish(n+1)=β(1-cosθ) Among them, punish(n+1) represents the turning penalty of the new node (n+1), n ​​is the current node, θ is the angle between the turning angle of the route and the direction of the previous path and the direction of the new node, and β is the penalty intensity coefficient. is the forward direction from the parent node to the current node, The possible directions from the current node to the new node; The adaptive heuristic is calculated by the following formula: h'(n)=(1-α)×[h(n)+h(n-1)] Where h'(n) represents the adaptive heuristic, (1-α) represents the adaptive weight multiplier of the heuristic, α is the map complexity, h(n) represents the estimated cost between the current node n and the end point, h(n-1) represents the estimated cost between the parent node (n-1) of the current node n and the end point, m represents the number of special area types, K i represents the impact factor corresponding to special area i, x is the number of nodes in the map, y is the number of obstacle nodes, ε i Indicates the number of nodes with special region i attributes; The heuristic function value f(n) is calculated by the following formula: f(n)=g'(n)+(1-α)[h(n)+h(n-1)]+punish(n) Wherein, punish(n) represents the turning penalty of node n.

4. An electronic device, characterized in that: include: Controller; A memory for storing one or more programs, which, when executed by the controller, enables the controller to implement the path planning method described in any one of claims 1 to 2.

5. A computer-readable storage medium, characterized in that: Computer-readable instructions are stored thereon, and when the computer-readable instructions are executed by a processor of a computer, the computer is caused to execute the path planning method according to any one of claims 1 to 2.

Images