Path planning method, system, equipment and medium

Abstract

The invention discloses a path planning method, system and device and a medium, and relates to the technical field of path planning algorithms, and the method comprises the steps: adding a heuristic evaluation function and an inflection point penalty term for a cost evaluation function to construct an optimized Dijkstra algorithm; during use, the position of the automatic guide vehicle is used as a current node, and all nodes adjacent to the current node are put into an open list; distance costs of all nodes in the open list and the current node are obtained, five minimum distance costs are obtained from all the distance costs, one of the five distance costs is randomly selected, the node corresponding to the selected distance cost is used as a target node for vehicle operation, the target node is moved from the open list to a closed list, and the vehicle operation is completed. Selecting all nodes adjacent to the target node and putting the nodes into an open list; and the path planning is completed until the end point enters the open list. According to the invention, the algorithm can efficiently plan the shortest path and avoid falling into local optimum at the same time.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning algorithms, and in particular to a path planning method, system, device and medium. Background Art

[0002] As the industrial field gradually shifts towards digitalization, intelligence and informatization, most manufacturing workshops are also transforming towards unmanned operations, gradually transforming production lines into fully automated intelligent industries. In this era, automated guided vehicles (AGVs) have been widely used as a modern product of intelligent manufacturing and intelligent logistics. The study of AGV path planning algorithms can improve the working ability and efficiency of AGVs.

[0003] In the prior art, path planning algorithms use Dijkstra algorithm, A* algorithm, genetic algorithm and ant colony algorithm, etc.

[0004] However, the above algorithm is not efficient and may fall into local optimum. It is impossible to plan the shortest path efficiently while avoiding the algorithm from falling into local optimum. Summary of the invention

[0005] The embodiments of the present invention provide a path planning method, system, device and medium, which can solve the problem in the prior art that the shortest path cannot be planned with high efficiency while avoiding the algorithm from falling into the local optimum.

[0006] The embodiment of the present invention provides a path planning method, comprising the following steps: obtaining a grid map of a workshop, and allowing an automatic guided vehicle to run in the workshop; in the Dijkstra algorithm, adding a heuristic evaluation function and an inflection point penalty item to a cost evaluation function to construct an optimized Dijkstra algorithm; using the optimized Dijkstra algorithm to plan a path for the automatic guided vehicle to run from a starting point to an end point in the grid map; wherein, taking the position of the automatic guided vehicle as the current node, selecting all nodes adjacent to the current node and putting them into an open list; obtaining the distance costs of all nodes in the open list and the current node, and obtaining the minimum five distance costs from all the distance costs, randomly selecting one from the five distance costs, and taking the node corresponding to the selected distance cost as the target node for the vehicle to run; moving the target node from the open list into a closed list, selecting all nodes adjacent to the target node and putting them into the open list; guiding the vehicle to run to the target node, which is the current node; when the end point is selected and put into the open list, moving the end point into the closed list, counting all nodes in the closed list as planned path nodes, and completing the path planning from the start point to the end point.

[0007] Furthermore, the step of constructing the optimized Dijkstra algorithm comprises:

[0008] In the Dijkstra algorithm used to find the shortest path between a starting point and an end point, the cost evaluation function S is used to find the minimum path cost. 起点→当前点 Add a heuristic evaluation function S that can reduce the search space Q 当前点→终点 and the turning point penalty term P used to reduce the number of turns 拐点惩罚 , to construct the optimized Dijkstra algorithm, the formula is:

[0009] Cost 总 =S 起点→当前点 +S Q 当前点→终点 +P 拐点惩罚

[0010] P 拐点惩罚 =η×|d x -d y |

[0011] Among them, Cost 总 The cost evaluation function adds a heuristic evaluation function and an inflection point penalty term, η represents the penalty weight, d x Indicates the horizontal coordinate difference between the current node and the previous node, d y Indicates the vertical coordinate difference between the current node and the previous node.

[0012] Furthermore, the obtaining of the grid map of the workshop comprises the following specific steps: drawing a two-dimensional plane map of the workshop in the form of coordinates, and marking the workshop equipment and prohibited areas as obstacles on the two-dimensional plane map; and converting the two-dimensional plane map into a grid map using a Python program.

[0013] An embodiment of the present invention provides a path planning system, including:

[0014] The map acquisition module is used to obtain the grid map of the workshop and let the automatic guided vehicle run in the workshop; the algorithm optimization module is used to add a heuristic evaluation function and an inflection point penalty item to the cost evaluation function in the Dijkstra algorithm to construct an optimized Dijkstra algorithm; the path planning module is used to use the optimized Dijkstra algorithm to plan the path of the automatic guided vehicle from the starting point to the end point in the grid map; wherein, the location of the automatic guided vehicle is taken as the current node, all nodes adjacent to the current node are selected and put into the open list; the distance costs of all nodes in the open list and the current node are obtained, and the smallest five distance costs are obtained from all distance costs, one is randomly selected from the five distance costs, and the node corresponding to the selected distance cost is used as the target node for the vehicle to run; the target node is moved from the open list to the closed list, all nodes adjacent to the target node are selected and put into the open list; the vehicle is guided to run to the target node, which is the current node; when the end point is selected and put into the open list, the end point is moved into the closed list, all nodes in the closed list are counted as planned path nodes, and the path planning from the start point to the end point is completed.

[0015] An embodiment of the present invention provides a computer device, comprising: a memory and a processor; the memory stores a computer program, and the processor implements the above-mentioned path planning method when executing the computer program.

[0016] An embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the path planning method described above is implemented.

[0017] The embodiments of the present invention provide a path planning method, system, device and medium. Compared with the prior art, the advantages thereof are as follows:

[0018] In the cost evaluation function of the Dijkstra algorithm, a heuristic evaluation function and an inflection point penalty term are added to construct an optimized Dijkstra algorithm. The optimized Dijkstra algorithm can be used to plan the path to obtain the shortest path with high efficiency. In order to avoid the optimized Dijkstra algorithm from falling into the local optimal situation during the path planning process, after obtaining the distance costs of all nodes and the current node in the open list, the smallest five distance costs are obtained from all the distance costs, and one is randomly selected from the five distance costs. The node corresponding to the selected distance cost is used as the target node for the vehicle operation.

[0019] Local optimum means that the optimized Dijkstra algorithm always selects the smallest distance cost in the open list, and the random selection method can avoid this situation. Ultimately, the optimized Dijkstra algorithm can avoid falling into the local optimum while efficiently planning the shortest path. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 An algorithm flow chart provided for an embodiment of the present invention;

[0021] Figure 2 A two-dimensional grid map drawn for an actual workshop provided in an embodiment of the present invention, wherein (a) is a production workshop map, (b) is a warehouse map, (c) is an assembly workshop map, and (d) is a digitized workshop map;

[0022] Figure 3 A path comparison diagram of the improved algorithm provided in the embodiment of the present invention and the traditional algorithm, wherein (a) is the path diagram of the Dijkstra algorithm, (b) is the path diagram of the A* algorithm, and (c) is the path diagram of the improved Dijkstra algorithm;

[0023] Figure 4 A comparison chart of the operation time and path inflection points of the improved algorithm provided in the embodiment of the present invention and the traditional algorithm, wherein (a) is a comparison chart of the algorithm running time, and (b) is a comparison chart of the number of inflection points. DETAILED DESCRIPTION

[0024] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present invention, so the present invention is not limited by the specific embodiments disclosed below.

[0025] See also Figure 1 to Figure 4 , an embodiment of the present invention provides a path planning method, comprising the following steps:

[0026] Step 1: Obtain a grid map of the workshop and let the automated guided vehicle operate in the workshop.

[0027] Based on the actual workshop, a two-dimensional plane map of the actual workshop is established, and the map grid is visualized using programming languages ​​such as Python and matlab to construct a grid map. Specifically, the workshop layout is measured and counted, and it is drawn as a two-dimensional plane map in the form of coordinates, and the equipment and prohibited areas in the workshop are classified as obstacles. Then, the map grid is visualized using programming languages ​​such as Python and matlab, and converted into a grid map equivalent to the workshop.

[0028] Step 2: In the Dijkstra algorithm used to find the shortest path between the starting point and the end point, add a heuristic evaluation function that can reduce the search space and a turning point penalty item for reducing the number of turns to the cost evaluation function used to find the minimum path cost, so as to construct an optimized Dijkstra algorithm.

[0029] Specifically, on the basis of the original cost evaluation function of the Dijkstra algorithm, a heuristic evaluation function and an inflection point penalty term are added to form a cost evaluation function of the new algorithm.

[0030] The cost evaluation function calculation formula is:

[0031] Cost 总 =S 起点→当前点 +S Q 当前点→终点 +P 拐点惩罚

[0032] In the formula, S 起点→当前点 Represents the cost evaluation function of the original Dijkstra algorithm, S Q 当前点→终点 The Chebyshev distance from the current node to the end point is the final path that uses the total cost value as the smallest path, P 拐点惩罚 Represents the inflection point penalty, which is calculated as:

[0033] P 拐点惩罚 =η×d x -d y |

[0034] In the formula, η represents the penalty weight. Such a calculated value can ensure that the value of the inflection point penalty term and the value of the heuristic evaluation function are at the same order of magnitude. The role of the heuristic evaluation function is to provide directionality for the traversal of the algorithm, reduce the number of nodes that the algorithm needs to traverse, and thus shorten the algorithm operation time; the role of the inflection point penalty term is to increase the cost of turning in the path and reduce the number of turns in the path.

[0035] Step 3: Use the optimized Dijkstra algorithm to plan the path of the automatic guided vehicle from the starting point to the end point in the grid map; wherein, the location of the automatic guided vehicle is taken as the current node, all nodes adjacent to the current node are selected and put into the open list; the distance costs of all nodes in the open list and the current node are obtained, and the five smallest distance costs are obtained from all distance costs, one is randomly selected from the five distance costs, and the node corresponding to the selected distance cost is used as the target node for the vehicle to run; the target node is moved from the open list to the closed list, all nodes adjacent to the target node are selected and put into the open list; the vehicle is guided to run to the target node, which is the current node; when the end point is selected and put into the open list, the end point is moved into the closed list, and all nodes in the closed list are counted as planned path nodes to complete the path planning from the start point to the end point.

[0036] Specifically, starting from the starting point, the algorithm traverses the adjacent nodes in sequence, counting the distance cost until the end point, and taking the path with the smallest distance cost as the planned path. The detailed explanation is: starting from the starting point, counting all the nodes adjacent to the starting point, putting all the nodes in the open list, calculating the distance cost between the node and the starting point, calculating the node with the smallest distance cost in the open list, putting it in the closed list, putting all the adjacent nodes of the node included in the closed list in the open list, and repeating the above process until the end point is traversed.

[0037] When implementing Dijkstra's algorithm, open lists and closed lists are usually used to manage nodes. The open list contains all nodes that have been explored but not yet processed; the closed list contains all nodes that have been processed. Once a node is removed from the open list and processed, it will be added to the closed list.

[0038] With the help of the random idea of ​​the RRT* algorithm, one of the five nodes with the smallest cost value in the open list is randomly selected to help the algorithm jump out of the local optimum. During the traversal process, with the help of the random idea of ​​the RRT* algorithm, a random one of the five nodes with the smallest distance cost value in the open list is included in the closed list with a certain probability, instead of the node with the smallest distance cost value, to help the algorithm jump out of the local optimum.

[0039] The improved algorithm of the present invention incorporates the randomness idea of ​​the RRT* algorithm to help the algorithm escape from the local optimum; at the same time, an inflection point penalty item is added to make the number of inflection points of the calculated path as small as possible, which can shorten the transportation time of the automatic guided vehicle AGV and reduce the energy consumption of the automatic guided vehicle AGV in practice.

[0040] An embodiment of the present invention provides a path planning system, including:

[0041] The map acquisition module is used to obtain the grid map of the workshop and let the automatic guided vehicle run in the workshop. The algorithm optimization module is used to add a heuristic evaluation function and an inflection point penalty term to the cost evaluation function in the Dijkstra algorithm to build an optimized Dijkstra algorithm. The path planning module is used to use the optimized Dijkstra algorithm to plan the path of the automatic guided vehicle from the starting point to the end point in the grid map; wherein, the location of the automatic guided vehicle is taken as the current node, all nodes adjacent to the current node are selected and put into the open list; the distance costs of all nodes in the open list and the current node are obtained, and the minimum five distance costs are obtained from all distance costs, one is randomly selected from the five distance costs, and the node corresponding to the selected distance cost is used as the target node for the vehicle to run; the target node is moved from the open list to the closed list, all nodes adjacent to the target node are selected and put into the open list; the vehicle is guided to run to the target node, which is the current node; when the end point is selected and put into the open list, the end point is moved into the closed list, and all nodes in the closed list are counted as planned path nodes to complete the path planning from the start point to the end point.

[0042] An embodiment of the present invention provides a computer device, comprising: a memory and a processor; the memory stores a computer program, and the processor implements the steps of a path planning method when executing the computer program.

[0043] An embodiment of the present invention provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the steps of a path planning method are implemented.

[0044] A specific embodiment is as follows:

[0045] The present invention is based on the actual workshop, establishes a two-dimensional plane map of the actual workshop, and constructs a grid map.

[0046] Based on the existing Dijkstra algorithm, the actual operation of AGV is considered. For AGVs running in the workshop, turning will greatly reduce the running speed of AGV and increase the energy consumption of AGV. Therefore, the number of turns of AGV should be reduced as much as possible to avoid unnecessary turns. Therefore, it is considered to add an inflection point penalty term to the total cost evaluation function of the algorithm, which can help the algorithm avoid unnecessary turns as much as possible. After adding the inflection point penalty, the calculation formula of the total cost is:

[0047] Cost 总 =S 起点→当前点 +S Q 点前点→终点 +P turn

[0048] Where P turnIndicates the penalty value, which is calculated as follows: turn =η×|d x -d y |, where η represents the penalty weight, which can be adjusted according to the actual needs of the workshop.

[0049] like Figure 1 As shown in the figure, starting from the starting point, the algorithm traverses the adjacent nodes in sequence, counting the distance cost value until the end point, and takes the path with the smallest distance cost value as the planned path. Specifically:

[0050] (1) Start.

[0051] (2) Determine the starting and ending points of the AGV.

[0052] (3) Take the starting point as the first root node and traverse the adjacent nodes.

[0053] (4) Calculate the cost value of each adjacent node of the root node and select any number in a natural number set, the natural number set i = [0:100].

[0054] (5) Determine whether i is less than or equal to 5; if so, select a random node from the five adjacent nodes with the smallest cost value, and determine whether the path is better after adopting this node. If so, place the node in the closed list as a new root node; if not, select the node with the smallest cost value among the adjacent nodes as the new root node and place the node in the closed list.

[0055] Determine whether the new node is the end point:

[0056] If yes, determine the optimal path based on the cost value in the closed list and end; if no, recalculate the cost value of each adjacent node of the root node and repeat the above process until the end point is reached.

[0057] The present invention will be further described below in conjunction with the accompanying drawings.

[0058] S1: Figure 2 The figure shows a two-dimensional map formed by rasterizing the workshop map according to the layout of four actual production workshops. In the figure, the black area represents the area where AGV cannot operate, including machine tools, walls, stacking obstacles, etc. The white area represents the area where AGV can operate. The grid map is placed in a 100*100 coordinate system, and each grid node corresponds to an integer coordinate. According to the actual workshop layout, the actual map can be represented according to the range of the grid map in the coordinate system.

[0059] S2: According to actual production, the algorithm traverses the nodes with the shipping point of each production line as the starting point and the storage point of the warehouse as the end point, collects the adjacent nodes of the current traversed node in turn, and calculates and updates the cost value of the new node. In the process of traversal, the current node is recorded as the nth node, the previous node is recorded as the n-1th node, and the new adjacent node is recorded as the n+1th node. If the coordinates of the n+1th node are not collinear with the linear line formed by the nth node and the n-1th node, it is judged that a turn has occurred at the node, that is, an inflection point. Therefore, when calculating the cost value at this point, it is necessary to add an inflection point penalty item to increase its cost value. Calculate the cost values ​​of the adjacent nodes in turn, take the smallest value as the next node, and include the cost value of the node in the closed list.

[0060] S3: When the algorithm traverses the nodes, it randomly selects one of the five nodes with the smallest cost value (a root node has a maximum of eight adjacent nodes) as the next node instead of the node with the smallest cost value with a certain probability, so as to help the algorithm escape from the local optimum. Figure 3 In the simulation shown, the random probability is set to 5%, where (a) is the A* algorithm; (b) is the Dijkstra algorithm; and (c) is the proposed algorithm.

[0061] S4: Repeat the above process until the end point is reached, then count the minimum cost value in the closed list and display its path, which is the path planned by the algorithm.

[0062] According to the simulation results of the example, and compared with the Dijkstra algorithm and the A* algorithm, the results are shown in the following table:

[0063] Table 1 Map path comparison.

[0064]

[0065] Table 2 Map path comparison.

[0066]

[0067] Table 3. Map path comparison.

[0068]

[0069] Table 4 Map path comparison.

[0070]

[0071] According to the simulation results of the example, the results are shown in Tables 1 to 4. It can be seen that the algorithm has a great advantage in computing speed and can always calculate the results faster than the other two algorithms. In terms of the path, the algorithm can basically find the shortest path, and the number of inflection points is almost the same as that of the Dijkstra algorithm that traverses all nodes, which is far better than the number of inflection points of the A* algorithm.

[0072] The above-mentioned embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A path planning method, characterized in that: The following steps are involved: Obtain a grid map of the workshop and allow the automated guided vehicle to operate in the workshop; In the Dijkstra algorithm, a heuristic evaluation function and an inflection point penalty term are added to the cost evaluation function to construct an optimized Dijkstra algorithm; An optimized Dijkstra algorithm is used to plan the path of an automated guided vehicle from a starting point to an end point in a grid map; wherein the location of the automated guided vehicle is taken as the current node, all nodes adjacent to the current node are selected and put into an open list; the distance costs of all nodes in the open list and the current node are obtained, and the five smallest distance costs are obtained from all the distance costs, one is randomly selected from the five distance costs, and the node corresponding to the selected distance cost is used as the target node for the vehicle to run; the target node is moved from the open list to the closed list, and all nodes adjacent to the target node are selected and put into the open list; Guide the vehicle to run to the target node, which is the current node; when the end point is selected and placed in the open list, move the end point to the closed list, count all the nodes in the closed list as the planned path nodes, and complete the path planning from the start point to the end point.

2. A path planning method according to claim 1, characterized in that: The optimized Dijkstra algorithm is constructed, and the specific steps include: In the Dijkstra algorithm used to find the shortest path between a starting point and an end point, the cost evaluation function S is used to find the minimum path cost. 起点→当前点 Add a heuristic evaluation function S that can reduce the search space Q 当前点→终点 and the turning point penalty term P used to reduce the number of turns 拐点惩罚 , to construct the optimized Dijkstra algorithm, the formula is: Cost 总 =S 起点 → 当前点 +S Q 当前点 → 终点 +P 拐点惩罚 P 拐点惩罚 η×|d x -d y | Among them, Cost 总 The cost evaluation function adds a heuristic evaluation function and an inflection point penalty term, η represents the penalty weight, d x Indicates the horizontal coordinate difference between the current node and the previous node, d y Indicates the vertical coordinate difference between the current node and the previous node.

3. A path planning method according to claim 1, characterized in that: The specific steps of obtaining the grid map of the workshop include: Draw a two-dimensional plane map of the workshop in the form of coordinates, and mark the workshop equipment and prohibited areas as obstacles on the two-dimensional plane map; Use Python program to convert two-dimensional flat map into raster map.

4. A path planning system, characterized in that: include: A map acquisition module is used to obtain a grid map of the workshop and allow the automatic guided vehicle to operate in the workshop; The algorithm optimization module is used to add a heuristic evaluation function and an inflection point penalty term to the cost evaluation function in the Dijkstra algorithm to construct an optimized Dijkstra algorithm; The path planning module is used to plan the path of the automatic guided vehicle from the starting point to the end point in the grid map using the optimized Dijkstra algorithm; wherein, the location of the automatic guided vehicle is taken as the current node, all nodes adjacent to the current node are selected and put into the open list; the distance costs of all nodes in the open list and the current node are obtained, and the smallest five distance costs are obtained from all the distance costs, one is randomly selected from the five distance costs, and the node corresponding to the selected distance cost is used as the target node for the vehicle to run; the target node is moved from the open list to the closed list, and all nodes adjacent to the target node are selected and put into the open list; Guide the vehicle to run to the target node, which is the current node; when the end point is selected and placed in the open list, move the end point to the closed list, count all the nodes in the closed list as the planned path nodes, and complete the path planning from the start point to the end point.

5. A computer device comprising: Memory and processor; The memory stores a computer program, wherein the processor implements a path planning method according to any one of claims 1 to 3 when executing the computer program.

6. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, a path planning method according to any one of claims 1 to 3 is implemented.

Images