Multi-road condition optimization way-finding method based on ant colony algorithm

Abstract

The invention provides a multi-road condition optimization way-finding method based on an ant colony algorithm. The method is specially designed for route planning and scheduling of an automatic driving unmanned vehicle in an actual production environment. According to the method, on the basis of a traditional heuristic algorithm, heuristic factors such as road width, total path curvature and traffic conflict probability which have great influence on vehicle driving time are added as consideration factors to be added into a way-finding method, and a driving route with good comprehensive road conditions is obtained by sacrificing part of road length. The method designs a penalty mechanism to improve the convergence speed of the algorithm aiming at the situation that the convergence speed is reduced after a traditional heuristic algorithm enters a dead path. Meanwhile, path smoothing processing is performed on an abstract driving path obtained by an algorithm based on a path smoothing technology of a road condition, so that the problem of path difference from an abstract scheduling path to actual driving of the vehicle caused by physical limitation of a vehicle walking mechanism is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning scheme design, and in particular to a multi-road condition optimization path finding method based on ant colony algorithm. Background Art

[0002] In the actual production environment, the environment is complex and changeable, and robots are often required to carry production tools, production materials or auxiliary equipment. The complex road environment will have a huge impact on the robot's driving route and driving speed. Therefore, in view of the complex road conditions and the physical limitations of the vehicle chassis, it is necessary to design a reasonable, efficient and fast driving route. Therefore, how to reasonably plan the path without collision and meet customer needs, so that the robot can maximize the use of road conditions during operation and avoid the low operating speed caused by the physical structure limitations of the robot itself, is still an unresolved problem. In addition, in the prior art, there is a lack of solutions for further optimizing the driving path according to the actual operation of the robot and the actual conditions of the road. In the actual production environment, many different types of robots will be used in the production process, and the physical structure designs of different robots and the data during driving are often different. The robot needs to obey the dispatch when multiple robots work together, so it has no right to choose the route independently, and cannot predict the specific situation of the route it is driving. When the robot is not suitable for the current robot to pass due to the poor path, the robot may move slowly or even stop, resulting in a lot of time wasted.

[0003] Heuristic algorithms are proposed in contrast to optimization algorithms. The optimal algorithm for a problem finds the optimal solution for each instance of the problem. A heuristic algorithm can be defined as follows: an algorithm based on intuition or experience that gives a feasible solution to each instance of the combinatorial optimization problem to be solved at an acceptable cost (referring to computing time and space), and the degree of deviation between the feasible solution and the optimal solution cannot generally be predicted. At present, heuristic algorithms are mainly based on natural body algorithms, including ant colony algorithms, simulated annealing methods, neural networks, etc. Although heuristic algorithms have been repeatedly applied in path-finding systems of complex maps, the actual road environment is often affected by multiple factors, which makes traditional heuristic algorithms that are only sensitive to path length face the problem of difficulty in successfully transforming from abstract theoretical paths to actual production paths. Therefore, the existing technology needs further development. Summary of the invention

[0004] The purpose of the present invention is to overcome the above technical deficiencies and provide a multi-road condition optimization path finding method based on ant colony algorithm to solve the problems existing in the prior art.

[0005] The technical solution adopted by the method of the present invention is: a multi-road condition optimization path finding method based on ant colony algorithm, comprising the following steps:

[0006] S1: Obtain two-dimensional map data, and calculate a linear function equation of the path between nodes according to the coordinates of all nodes in the map and the path relationship between all nodes, and according to the node coordinates and the path relationship between nodes;

[0007] S2: Construct and initialize the pheromone matrix, set the ant colony size, maximum number of iterations, starting point and end point, and set the path smoothing threshold;

[0008] S3: performing an iteration of the ant colony based on the map data, the path relationship between all nodes, the function equation of the path between nodes, the turning angle between adjacent paths and the pheromone matrix;

[0009] S4: updating the pheromone matrix based on the driving path explored by each ant in the ant colony;

[0010] S5: Repeat steps S3-S4 until the maximum number of iterations is reached;

[0011] S6: Obtain the driving path with the best comprehensive road conditions in all iterations as the optimal driving path;

[0012] S7: Based on the optimal driving path, a path smoothing process is performed on a path in which a steering angle exceeds a set threshold in the driving path, and a final driving path is output based on the path after the path smoothing process.

[0013] The map data includes the horizontal and vertical coordinates of each node and whether there is a path between each node; the pheromone matrix is ​​a matrix of N rows and N columns, where N represents the number of all nodes; the element in the ath row and bth column of the pheromone matrix represents the pheromone concentration between directed paths ab; each element in the initialized pheromone matrix is ​​equal and equal to 0.

[0014] The detailed steps for calculating the function equation of the path between nodes based on the node coordinates and the path relationship between nodes include:

[0015] S101: confirming that there is a feasible path between two nodes;

[0016] S102: For nodes a and b that have a path, obtain the horizontal coordinates and vertical coordinates of the two nodes, and set the node with the smaller horizontal coordinate as node a, and the node with the larger horizontal coordinate as node b;

[0017] S103: Calculate the path function between nodes. The formula is as follows:

[0018]

[0019] Among them, y b represents the ordinate of node b, x b Indicates the horizontal coordinate of node b, y a represents the ordinate of node a, x a Represents the horizontal coordinate of node a. The entire function expression is a linear function expression of the path function.

[0020] S104: Order The path function can be simplified to the form y ab =k ab *x+m ab A linear function of .

[0021] The detailed steps for one iteration of the ant colony algorithm include:

[0022] S301: Initialize the taboo node table and the historical arrival node table for each ant;

[0023] S302: Place each ant at the starting point, add the starting point to the taboo node table, and add the starting point to the historical arrival node table;

[0024] S303: selecting the next node to go to for each ant based on the pheromone matrix and heuristic information;

[0025] S304: updating the taboo node table and the historical arrival node table for each ant;

[0026] S305: Repeat steps S303-S304 until each ant reaches the terminal node or reaches a non-terminal node and has no other next node to choose and go to.

[0027] The taboo node table includes reached nodes and unreachable nodes, wherein the reached node represents that the node is in the ant's historical reached node table, and the unreachable node represents that there is no drivable path between the node and the node where the ant is currently located; the historical reached node table includes the nodes that the ant chooses to go to each time in sequence starting from the starting point, and is sorted and stored in chronological order according to the time of selection.

[0028] The detailed steps for selecting the next node for each ant to go to include:

[0029] S3031: Calculate the probability that the ant selects any node as the next node to move forward. The formula is as follows:

[0030]

[0031] Among them, P ijis the probability of selecting the path finding factor to go from the current node i to the next node j, k belongs to the set allowed of all nodes that can be reached from the current node, τ ik is the pheromone concentration of the road between node i and the reachable node k, η ik is the heuristic information of the path between node i and the reachable node k, α is the pheromone adjustment factor; β is the heuristic information adjustment factor;

[0032] S3032: Based on the probability of the ant selecting each reachable node as the next node, the next node to be selected is determined by a roulette algorithm;

[0033] S3033: Update the current node where the ant is located to the next node selected. The heuristic factor expression is as follows:

[0034]

[0035] where η ij represents the heuristic information value of the current node i to reach the reachable node j; p d is the proportional coefficient of the importance of distance in heuristic information; D' ij is the weighted distance of the path; A' hij is the path weighted steering angle; p d is the proportional coefficient of the importance of distance in heuristic information; p a is the proportion coefficient of the total steering angle in the heuristic information; if node i is the starting point, the heuristic factor calculation formula is as follows:

[0036]

[0037] where η ij represents the heuristic information value of the current node i to reach the reachable node j; D' ij Weighted distance for the path.

[0038] The multi-road condition optimization path finding method based on ant colony algorithm according to claim 7 is characterized in that the path weighted distance calculation formula is as follows:

[0039] D' ij =1+log2(D ij +3600);

[0040] Among them, D' ij is the weighted distance between node i and node j; D ij is the Euclidean distance calculated based on the horizontal and vertical coordinates of node i and node h.

[0041] The path weighted steering angle calculation formula is as follows:

[0042] A' hij =1+log2(A hij +1);

[0043] Where A' hij is a vector consisting of the node h that can be reached before the current node, the current node i, and the node j that can be reached With vector The weighted path turning angle between hij is a vector consisting of the node h that can be reached before the current node, the current node i, and the node j that can be reached With vector The path turning angle between them.

[0044] The angle between the vectors is calculated as follows:

[0045]

[0046] Among them, the dot product formula is:

[0047]

[0048] vector The modulus length is vector The modulus length is

[0049] The detailed steps of updating the taboo node table and the reached node table for each ant include:

[0050] S3041: Put the selected next node into the taboo node table;

[0051] S3042: Put the selected next node to be visited into the historical arrival node table.

[0052] The detailed steps of updating the pheromone matrix include:

[0053] S401: Get the historical path node table that the ants have walked;

[0054] S402: determining whether the node finally reached by the ant is consistent with the designated terminal node; if consistent, executing the pheromone precipitation method according to the path length information and the total turning angle; if inconsistent, executing the pheromone elimination method for the pheromones on part of the path;

[0055] S403: Repeat steps S401-S402 until all ants are taken out and the pheromone update strategy is implemented.

[0056] The calculation formula of the pheromone precipitation method is as follows:

[0057] τ nm (t)=(1-ρ)·τ nm (t-1)+Δτ ij (t);

[0058] Among them, τ nm (t-1) represents the concentration of any pheromone in the pheromone matrix in this round of iteration; nm is τ nm The corresponding directed path; τ nm (t) represents τ nm (t-1) updated value; Δτ ij (t) represents the pheromone concentration left by the ants between the directed paths nm in this iteration, ρ is the pheromone evaporation rate; for the pheromone concentration Δτ left by the ants ij The calculation formula of (t) is as follows:

[0059]

[0060] Among them, Q is the pheromone intensity, D' is the weighted total path distance, and A' is the weighted total turning angle.

[0061] The calculation formula of the pheromone elimination method is as follows:

[0062] τ nm (t) = R*τ nm (t-1) (1≥R≥0);

[0063] Among them, m represents the last terminal node m reached, n represents the node in the current ant's historical path that is closest to the terminal node m and is located in the historical arrival node table of other ants that successfully reached the end point, τ nm (t) is the residual concentration of pheromone on all roads between node n and terminal node m after executing the pheromone penalty strategy in the current iteration. R is the penalty coefficient. When R is 0, it means that all pheromones between node n and node m are not retained at all. τ nm (t-1) is the pheromone concentration in the road after the last iteration.

[0064] The method for obtaining the driving path with the best comprehensive road conditions in all iterations as the optimal driving path is as follows: for any two different routes from ant 1 and ant 2 starting from a specified starting point and ending at a specified end point, first obtain the total lengths D1 and D2 of the two paths respectively, then obtain the total steering angles A1 and A2 of the two paths, and calculate the total steering angles A1 and A2 of the two paths according to the importance ratio parameter P of the total length of the paths. a The importance ratio parameter P of the total steering angle of the path b Perform path superiority comparison if and only if When it is established, the path of Ant 1 is considered superior to the path of Ant 2, and the path of Ant 1 is taken as the better driving path.

[0065] The detailed steps of path smoothing for the optimal driving path include:

[0066] S701: starting from the starting point, three adjacent path nodes are selected in sequence, which are the path starting point, the path turning point, and the path end point, and for two paths formed by the three nodes, a turning angle between the two paths is calculated;

[0067] S702: Based on the turning angle between the two paths, determine whether the turning angle is greater than a set path smoothing threshold, if so, perform path smoothing optimization, otherwise proceed to step S707;

[0067] S703: Obtain the coordinates of the path starting point, the path turning point, the path end point and the linear function equation of the two roads, and select the path smoothing starting point and the path smoothing end point;

[0068] S704: using a cubic spline interpolation method to perform path smoothing processing to obtain a smoothed path equation;

[0070] S705: Sampling a set of real numbers in the region defined by the horizontal coordinates of the path smoothing start point and the path smoothing end point to obtain the horizontal coordinates x1…x of several intermediate nodes located on the path smoothing function. n , and according to the smooth function of each intermediate node's horizontal coordinate, all the intermediate node's horizontal coordinates are brought back to the smooth function in turn to solve the vertical coordinates y1…y n ;

[0071] S706: Replace the original path turning point node with the intermediate node and update the driving path;

[0072] S707: Update the path starting point, path turning point, and path end point to the next node adjacent to the current node respectively;

[0073] S708: repeating steps S701-707 until the end point of the path reaches the end point of the optimal driving path;

[0074] S709: Outputting a final driving route based on the updated driving route.

[0075] The method for selecting the starting point and the end point of the path smoothing includes: first obtaining the path starting point node, the path turning point node and the path end point node and their coordinate information, then taking 90% of the road length before reaching the turning point as the path smoothing starting point, recorded as point S, and taking 10% of the road length after passing the path turning point as the path smoothing end point, recorded as point E, and the formula for obtaining the starting point and the end point coordinates is as follows:

[0076]

[0077] Among them, (x3, y3) are the coordinates of the undetermined point. When the undetermined point is the starting point of the path smoothing, let (x1, y1) be the coordinates of the path starting point node, let (x2, y2) be the coordinates of the path turning point node, L is the Euclidean distance between the path starting point node and the path turning point node, l is the distance parameter, when l = 0.9L, it means that the distance between the path smoothing starting point and the path turning point is 10% of the length of the road before passing the turning point; when the undetermined point is the end point of the path smoothing, let (x1, y1) be the coordinates of the path turning point, let (x2, y2) be the coordinates of the path end point, L is the Euclidean distance between the path turning point node and the path end point node, l is the distance parameter, when l = 0.1L, it means that the distance between the path smoothing end point and the path turning point node is 10% of the length of the road after passing the turning point.

[0078] The method of using cubic spline interpolation to smooth the path is as follows: let the coordinates of the path smoothing starting point be (x1, y1), the coordinates of the path turning point be (x2, y2), and the coordinates of the path smoothing end point be (x3, y3). The path smoothing formula is as follows:

[0079]

[0080] Solving the equation gives S1(x a )、S2(x b ), where S1(x a ) represents the approximate smoothing function of the path between the path smoothing starting point and the path turning point, S2(x b ) represents the approximate smooth function between the path inflection point and the path smoothing end point. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 A flowchart of the multi-road condition optimization pathfinding method based on ant colony algorithm provided by the present invention;

[0082] Figure 2 A flow chart of a method for selecting the next node to go to provided by the present invention;

[0083] Figure 3 A flow chart of a method for performing path smoothing processing on an optimal driving path provided by the present invention. DETAILED DESCRIPTION

[0084] The technical solution of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations of the technical solution of the present invention. The embodiments of the present invention and the technical features in the embodiments can be combined with each other without conflict.

[0085] This embodiment introduces a multi-road condition optimization path finding method based on ant colony algorithm. Figure 1 , the method comprises the following steps:

[0086] S1: Obtain two-dimensional map data, and calculate a linear function equation of the path between nodes according to the coordinates of all nodes in the map and the path relationship between all nodes, and according to the node coordinates and the path relationship between nodes;

[0087] S2: Construct and initialize the pheromone matrix, set the ant colony size, maximum number of iterations, starting and ending points, and path smoothing threshold;

[0088] The map data includes the horizontal and vertical coordinates of each node and whether there is a path between each node; the pheromone matrix is ​​a matrix of N rows and N columns, where N represents the number of all nodes; the element in the ath row and bth column of the pheromone matrix represents the pheromone concentration between directed paths ab; each element in the initialized pheromone matrix is ​​equal and equal to 0.

[0089] The detailed steps for calculating the function equation of the path between nodes based on the node coordinates and the path relationship between nodes include:

[0090] S101: confirming that there is a feasible path between two nodes;

[0091] S102: For nodes a and b that have a path, obtain the horizontal coordinates and vertical coordinates of the two nodes, and set the node with the smaller horizontal coordinate as node a, and the node with the larger horizontal coordinate as node b;

[0092] S103: Calculate the path function between nodes. The formula is as follows:

[0093]

[0094] Among them, y b represents the ordinate of node b, x b Indicates the horizontal coordinate of node b, y a represents the ordinate of node a, x a Represents the horizontal coordinate of node a, and the entire function expression is a linear function expression of the path function;

[0095] S104: Order The path function can be simplified to the form y ab =k ab *x+m ab A linear function of .

[0096] S3: performing an iteration of the ant colony based on the map data, the path relationship between all nodes, the function equation of the path between nodes, the turning angle between adjacent paths and the pheromone matrix;

[0097] The detailed steps for one iteration of the ant colony algorithm include:

[0098] S301: Initialize the taboo node table and the historical arrival node table for each ant;

[0099] The taboo node table includes reached nodes and unreachable nodes, wherein the reached nodes represent nodes in the ant's historical reached node table, and the unreachable nodes represent nodes that have no drivable path to the ant's current node; the historical reached node table includes nodes that the ant chooses to go to each time in sequence starting from the starting point, and the nodes are sorted and stored in chronological order according to the time of selection;

[0100] S302: Place each ant at the starting point, add the starting point to the taboo node table, and add the starting point to the historical arrival node table;

[0101] S303: selecting the next node to go to for each ant based on the pheromone matrix and heuristic information;

[0102] The detailed steps for selecting the next node for each ant to go to include:

[0103] S3031: Calculate the probability that the ant selects any node as the next node to move forward. The formula is as follows:

[0104]

[0105] Among them, P ij is the probability of selecting the path finding factor to go from the current node i to the next node j, k belongs to the set allowed of all nodes that can be reached from the current node, τ ik is the pheromone concentration of the road between node i and the reachable node k, η ik is the heuristic information of the path between node i and the reachable node k, α is the pheromone adjustment factor; β is the heuristic information adjustment factor;

[0106] The heuristic factor expression is as follows:

[0107]

[0108] where η ij represents the heuristic information value of the current node i to reach the reachable node j; p d is the proportional coefficient of the importance of distance in heuristic information; D' ij is the weighted distance of the path; A' hij is the path weighted steering angle; p d is the proportional coefficient of the importance of distance in heuristic information; p ais the proportion coefficient of the total steering angle in the heuristic information; if node i is the starting point, the heuristic factor calculation formula is as follows:

[0109]

[0110] where η ij represents the heuristic information value of the current node i to reach the reachable node j; D' ij is the path weighted distance, and the calculation formula of the path weighted distance is as follows:

[0111] D' ij =1+log2(D ij +3600);

[0112] Among them, D' ij is the weighted distance between node i and node j; D ij is the Euclidean distance calculated based on the horizontal and vertical coordinates of node i and node j.

[0113] The path weighted steering angle calculation formula is as follows:

[0114] A' hij =1+log2(A hij +1);

[0115] Where A' hij is a vector consisting of the node h that can be reached before the current node, the current node i, and the node j that can be reached With vector The weighted path turning angle between hij is a vector consisting of the node h that can be reached before the current node, the current node i, and the node j that can be reached With vector The path turning angle between them.

[0116] The angle between the vectors is calculated as follows:

[0117]

[0118] Among them, the dot product formula is:

[0119]

[0120] vector The modulus length is vector The modulus length is

[0121] S3032: Based on the probability of the ant selecting each reachable node as the next node, the next node to be selected is determined by a roulette algorithm;

[0122] S3033: Update the current node where the ant is located to the next node selected to go to.

[0123] S304: updating the taboo node table and the historical arrival node table for each ant;

[0124] The detailed steps of updating the taboo node table and the reached node table for each ant include:

[0125] S3041: Put the selected next node into the taboo node table;

[0126] S3042: Put the selected next node to be visited into the historical arrival node table.

[0127] S4: updating the pheromone matrix based on the driving path explored by each ant in the ant colony;

[0128] The detailed steps of updating the pheromone matrix include:

[0129] S401: Get the historical path node table that the ants have walked;

[0130] S402: determining whether the node finally reached by the ant is consistent with the designated terminal node; if consistent, executing the pheromone precipitation method according to the path length information and the total turning angle; if inconsistent, executing the pheromone elimination method for the pheromones on part of the path;

[0131] The calculation formula of the pheromone precipitation method is as follows:

[0132] τ nm (t)=(1-ρ)·τ nm (t-1)+Δτ ij (t);

[0133] Among them, τ nm (t-1) represents the concentration of any pheromone in the pheromone matrix in this round of iteration; nm is τ nm The corresponding directed path; τ nm (t) represents τ nm (t-1) updated value; Δτ ij (t) represents the pheromone concentration left by the ants between the directed paths nm in this iteration, ρ is the pheromone evaporation rate; for the pheromone concentration Δτ left by the ants ij The calculation formula of (t) is as follows:

[0134]

[0135] Among them, Q is the pheromone intensity, D' is the weighted total path distance, and A' is the weighted total turning angle.

[0136] The calculation formula of the pheromone elimination method is as follows:

[0137] τ nm (t) = R*τ nm (t-1) (1≥R≥0);

[0138] Among them, m represents the last terminal node m reached, n represents the node in the current ant's historical path that is closest to the terminal node m and is located in the historical arrival node table of other ants that successfully reached the end point, τ nm (t) is the residual concentration of pheromone on all roads between node n and terminal node m after executing the pheromone penalty strategy in the current iteration. R is the penalty coefficient. When R is 0, it means that all pheromones between node n and node m are not retained at all. τ nm (t-1) is the pheromone concentration in the road after the last iteration.

[0139] S5: Repeat steps S3-S4 until the maximum number of iterations is reached;

[0140] S6: Obtain the driving path with the best comprehensive road conditions in all iterations as the optimal driving path;

[0141] The method for obtaining the driving path with the best comprehensive road conditions in all iterations as the optimal driving path is as follows: for any two different routes from ant 1 and ant 2 starting from a specified starting point and ending at a specified end point, first obtain the total lengths D1 and D2 of the two paths respectively, then obtain the total steering angles A1 and A2 of the two paths, and calculate the total steering angles A1 and A2 of the two paths according to the importance ratio parameter P of the total length of the paths. a The importance ratio parameter P of the total steering angle of the path b Perform path superiority comparison if and only if When it is established, the path of Ant 1 is considered superior to the path of Ant 2, and the path of Ant 1 is taken as the better driving path.

[0142] S7: Based on the optimal driving path, a path smoothing process is performed on a path in which a steering angle exceeds a set threshold in the driving path, and a final driving path is output based on the path after the path smoothing process.

[0143] The detailed steps of path smoothing for the optimal driving path include:

[0144] S701: starting from the starting point, three adjacent path nodes are selected in sequence, which are the path starting point, the path turning point, and the path end point, and for two paths formed by the three nodes, a turning angle between the two paths is calculated;

[0145] S702: Based on the turning angle between the two paths, determine whether the turning angle is greater than a set path smoothing threshold, if so, perform path smoothing optimization, otherwise proceed to step S707;

[0146] Obtain the coordinates of the path starting point, the path turning point, the path end point and the linear function equation of the two roads, and select the path smoothing starting point and the path smoothing end point;

[0147] The method for selecting the starting point and the end point of the path smoothing includes: first obtaining the path starting point node, the path turning point node and the path end point node and their coordinate information, then taking 90% of the road length before reaching the turning point as the path smoothing starting point, recorded as point S, and taking 10% of the road length after passing the path turning point as the path smoothing end point, recorded as point E, and the formula for obtaining the starting point and the end point coordinates is as follows:

[0148]

[0149] Among them, (x3, y3) are the coordinates of the undetermined point. When the undetermined point is the starting point of the path smoothing, let (x1, y1) be the coordinates of the path starting point node, let (x2, y2) be the coordinates of the path turning point node, L is the Euclidean distance between the path starting point node and the path turning point node, l is the distance parameter, when l = 0.9L, it means that the distance between the path smoothing starting point and the path turning point is 10% of the length of the road before passing the turning point; when the undetermined point is the end point of the path smoothing, let (x1, y1) be the coordinates of the path turning point, let (x2, y2) be the coordinates of the path end point, L is the Euclidean distance between the path turning point node and the path end point node, l is the distance parameter, when l = 0.1L, it means that the distance between the path smoothing end point and the path turning point node is 10% of the length of the road after passing the turning point.

[0150] S703: Obtain the coordinates of the path starting point, the path turning point, the path end point and the linear function equation of the two roads, and select the path smoothing starting point and the path smoothing end point;

[0151] S704: using a cubic spline interpolation method to perform path smoothing processing to obtain a smoothed path equation;

[0152] The method of using cubic spline interpolation to smooth the path is as follows: let the coordinates of the path smoothing starting point be (x1, y1), the coordinates of the path turning point be (x2, y2), and the coordinates of the path smoothing end point be (x3, y3). The path smoothing formula is as follows:

[0153]

[0154] Solving the equation gives S1(x a )、S2(x b), where S1(x a ) represents the approximate smoothing function of the path between the path smoothing starting point and the path turning point, S2(x b ) represents the approximate smooth function between the path inflection point and the path smoothing end point;

[0155] S705: Sampling the horizontal coordinates of the path smoothing start point and the end point to obtain the horizontal coordinates x1…x of several intermediate nodes located on the path smoothing function. n , and according to the smooth function of each intermediate node's horizontal coordinate, all the intermediate node's horizontal coordinates are brought back to the smooth function in turn to solve the vertical coordinates y1…y n ;

[0156] S706: Replace the original path turning point node with the intermediate node and update the driving path;

[0157] S707: Update the path starting point, path turning point, and path end point to the next node adjacent to the current node respectively;

[0158] S708: repeating steps S701-707 until the end point of the path reaches the end point of the optimal driving path;

[0159] S709: Outputting a final driving route based on the updated driving route.

[0160] It should be noted here that the present invention performs global path planning based on the ant colony algorithm, and uses the spline interpolation method to smoothly optimize the path turning points to obtain the final moving path; different from the previous factor based on the shortest path as the only reference, the present invention takes into account the impact of other road conditions on the vehicle's driving speed in actual situations, and therefore further incorporates other road factors in the actual production environment as reference factors into the path-finding process, which solves the problem of the large gap between the vehicle driving time and the theoretical running time under multiple road conditions, and further improves the mobility and work efficiency of the AGV robot.

[0161] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0162] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the purpose and scope of protection of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A multi-road condition optimization pathfinding method based on ant colony algorithm, characterized in that: The following steps are involved: S1: Obtain two-dimensional map data, and calculate a linear function equation of the path between nodes according to the coordinates of all nodes in the map and the path relationship between all nodes, and according to the node coordinates and the path relationship between nodes; S2: Construct and initialize the pheromone matrix, set the ant colony size, maximum number of iterations, starting and ending points, and path smoothing threshold; S3: performing an iteration of the ant colony based on the map data, the path relationship between all nodes, the function equation of the path between nodes, the turning angle between adjacent paths and the pheromone matrix; S4: updating the pheromone matrix based on the driving path explored by each ant in the ant colony; S5: Repeat steps S3-S4 until the maximum number of iterations is reached; S6: Obtain the driving path with the best comprehensive road conditions in all iterations as the optimal driving path; S7: Based on the optimal driving path, a path smoothing process is performed on a path in which a steering angle exceeds a set threshold in the driving path, and a final driving path is output based on the path after the path smoothing process.

2. The method for optimizing pathfinding under multiple road conditions based on ant colony algorithm according to claim 1, characterized in that: The map data includes the horizontal coordinate and the vertical coordinate of each node and whether there is a path between each node; The pheromone matrix is ​​a matrix of N rows and N columns, wherein N represents the number of all nodes; the elements of the th row and th column of the pheromone matrix represent the pheromone concentration between directed paths; each element in the initialized pheromone matrix is ​​equal and equal to 0.

3. The method for optimizing pathfinding under multiple road conditions based on ant colony algorithm according to claim 1, characterized in that: The detailed steps for calculating the function equation of the path between nodes based on the node coordinates and the path relationship between nodes include: S101: confirming that there is a feasible path between two nodes; S102: For nodes a and b that have a path, obtain the horizontal coordinates and vertical coordinates of the two nodes, and set the node with the smaller horizontal coordinate as node a, and the node with the larger horizontal coordinate as node b; S103: Calculate the path function between nodes. The formula is as follows: Among them, y b represents the ordinate of node b, x b Indicates the horizontal coordinate of node b, y a represents the ordinate of node a, x a Represents the horizontal coordinate of node a, and the entire function expression is a linear function expression of the path function; S104: Order The path function can be simplified to the form y ab =k ab *x+m ab A linear function of .

4. The method for optimizing pathfinding under multiple road conditions based on ant colony algorithm according to claim 1, characterized in that: The detailed steps for one iteration of the ant colony algorithm include: S301: Initialize the taboo node table and the historical arrival node table for each ant; S302: Place each ant at the starting point, add the starting point to the taboo node table, and add the starting point to the historical arrival node table; S303: selecting the next node to go to for each ant based on the pheromone matrix and heuristic information; S304: updating the taboo node table and the historical arrival node table for each ant; S305: Repeat steps S303-S304 until each ant reaches the terminal node or reaches a non-terminal node and has no other next node to choose and go to.

5. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 4, characterized in that: The taboo node table includes reached nodes and unreachable nodes, wherein the reached node represents that the node is in the ant's historical reached node table, and the unreachable node represents that there is no drivable path between the node and the node where the ant is currently located; The historical arrival node table includes the nodes that the ants choose to go to each time starting from the starting point, and is sorted and stored in chronological order according to the selection time.

6. The method for optimizing pathfinding under multiple road conditions based on ant colony algorithm according to claim 4, characterized in that: The detailed steps for selecting the next node for each ant to go to include: S3031: Calculate the probability that the ant selects any node as the next node to move forward. The formula is as follows: Among them, P ij is the probability of selecting the path finding factor to go from the current node i to the next node j, k belongs to the set allowed of all nodes that can be reached from the current node, τ ik is the pheromone concentration of the road between node i and the reachable node k, η ik is the heuristic information of the path between node i and the reachable node k, α is the pheromone adjustment factor; β is the heuristic information adjustment factor; S3032: Based on the probability of the ant selecting each reachable node as the next node, the next node to be selected is determined by a roulette algorithm; S3033: Update the current node where the ant is located to the next node selected to go to.

7. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 6, characterized in that: The heuristic factor expression is as follows: where η ij represents the heuristic information value of the current node i to reach the reachable node j; p d is the proportional coefficient of the importance of distance in heuristic information; D′ ij is the weighted distance of the path; A′ hij is the path weighted steering angle; p d is the proportional coefficient of the importance of distance in heuristic information; p a is the proportion coefficient of the total steering angle in the heuristic information; if node i is the starting point, the heuristic factor calculation formula is as follows: where η ij represents the heuristic information value of the current node i to reach the reachable node j; D′ ij Weighted distance for the path.

8. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 7, characterized in that: The weighted distance calculation formula is as follows: D′ ij =1+log2(D ij +3600) Among them, D′ ij is the weighted distance between node i and node j; D ij is the Euclidean distance calculated based on the horizontal and vertical coordinates of node i and node j.

9. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 7, characterized in that: The weighted distance calculation formula is as follows: A′ hij =1+log2(A hij +1) Where A′ hij is a vector consisting of the node h that can be reached before the current node, the current node i, and the node j that can be reached With vector The weighted path turning angle between hij is a vector consisting of the node h that can be reached before the current node, the current node i, and the node j that can be reached With vector The path turning angle between them.

10. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 9, characterized in that: The angle between the vectors is calculated as follows: Among them, the dot product formula is: vector The modulus length is vector The modulus length is 11. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 4, characterized in that: The detailed steps of updating the taboo node table and the reached node table for each ant include: S3041: Put the selected next node into the taboo node table; S3042: Put the selected next node to be visited into the historical arrival node table.

12. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 1, characterized in that: The detailed steps of updating the pheromone matrix include: S401: Get the historical path node table that the ants have walked; S402: determining whether the node finally reached by the ant is consistent with the designated terminal node; if consistent, executing the pheromone precipitation method according to the path length information and the total turning angle; if inconsistent, executing the pheromone elimination method for the pheromones on part of the path; S403: Repeat steps S401-S402 until all ants are taken out and the pheromone update strategy is implemented.

13. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 12, characterized in that: The calculation formula of the pheromone precipitation method is as follows: t nm (t)=(1-ρ)·τ nm (t-1)+Δτ ij (t) Among them, τ nm (t-1) represents the concentration of any pheromone in the pheromone matrix in this iteration, and nm is τ nm The corresponding directed path, τ nm (t) represents τ nm (t-1) The updated value, Δτ ij (t) represents the pheromone concentration left by the ants between the directed paths nm in this iteration, ρ is the pheromone evaporation rate; for the pheromone concentration Δτ left by the ants ij The calculation formula of (t) is as follows: Among them, Q is the pheromone intensity, D′ is the weighted total path distance, and A′ is the weighted total turning angle.

14. The method for optimizing path finding under multiple road conditions based on ant colony algorithm according to claim 12, characterized in that: The calculation formula of the pheromone elimination method is as follows: t nm (t)=R*τ nm (t-1)1≥R≥0 Among them, m represents the last terminal node m reached, n represents the node in the current ant's historical path that is closest to the terminal node m and is located in the historical arrival node table of other ants that successfully reached the end point, τ nm (t) is the residual concentration of pheromone on all roads between node n and terminal node m after executing the pheromone penalty strategy in the current iteration, R is the penalty coefficient; when R is 0, it means that all pheromones between node n and node m are not retained at all, τ nm (t-1) is the pheromone concentration in the road after the last iteration.

15. The path finding method based on multiple road condition optimization according to claim 1, characterized in that: The method for obtaining the driving path with the best comprehensive road conditions in all iterations as the optimal driving path is as follows: for any two different routes from ant 1 and ant 2 starting from a specified starting point and ending at a specified end point, first obtain the total lengths D1 and D2 of the two paths respectively, then obtain the total steering angles A1 and A2 of the two paths, and calculate the total steering angles A1 and A2 of the two paths according to the importance ratio parameter P of the total length of the paths. a The importance ratio parameter P of the total steering angle of the path b Perform path superiority comparison if and only if When it is established, the path of Ant 1 is considered superior to the path of Ant 2, and the path of Ant 1 is taken as the better driving path.

16. The path finding method based on multiple road condition optimization according to claim 1, characterized in that: The detailed steps for path smoothing the final path include: S701: Starting from the starting point, the turning angles of all adjacent paths on the path are calculated. If the turning angles are greater than a set path smoothing threshold, path smoothing optimization is performed on them. S702: Obtain the coordinates of the path starting point, the path turning point, the path end point and the linear function equation of the two roads, and select the path smoothing starting point and the path smoothing end point; S703: Using a cubic spline interpolation method to perform path smoothing processing to obtain a smoothed path equation; S704: Sampling the horizontal coordinates of the path smoothing start point and the end point to obtain the horizontal coordinates x1...x of several intermediate nodes located on the path smoothing function n , and according to the smooth function of each intermediate node's horizontal coordinate, all the intermediate node's horizontal coordinates are brought back to the smooth function in turn to solve the vertical coordinates y1...y n ; S705: Replace the original path turning point node with the intermediate node and update the driving path; S706: Repeat steps S701-705 until reaching a node adjacent to the end point.

17. A path finding method based on multi-road condition optimization according to claim 16, characterized in that: The method for selecting the starting point and the end point of the path smoothing includes: first obtaining the path starting point node, the path turning point node and the path end point node and their coordinate information, then taking 90% of the road length before reaching the turning point as the path smoothing starting point, recorded as point S, and taking 10% of the road length after passing the path turning point as the path smoothing end point, recorded as point E, and the formula for obtaining the starting point and the end point coordinates is as follows: Among them, (x3, y3) are the coordinates of the undetermined point. When the undetermined point is the starting point of the path smoothing, let (x1, y1) be the coordinates of the path starting point node, let (x2, y2) be the coordinates of the path turning point node, L is the Euclidean distance between the path starting point node and the path turning point node, l is the distance parameter, and when l=0.9L, it means that the distance between the path smoothing starting point and the path turning point is 10% of the length of the road before passing the turning point; when the undetermined point is the end point of the path smoothing, let (x1, y1) be the coordinates of the path turning point, let (x2, y2) be the coordinates of the path end point, L is the Euclidean distance between the path turning point node and the path end point node, 1 is the distance parameter, and when 1=0.1L, it means that the distance between the path smoothing end point and the path turning point node is 10% of the length of the road after passing the turning point.

18. The path finding method based on multiple road condition optimization according to claim 16, characterized in that: The method of using cubic spline interpolation to smooth the path is as follows: let the coordinates of the path smoothing starting point be (x1, y1), the coordinates of the path turning point be (x2, y2), and the coordinates of the path smoothing end point be (x3, y3). The path smoothing formula is as follows: Solving the equation gives S1(x a )、S2(x b ), where S1(x a ) represents the approximate smoothing function of the path between the path smoothing starting point and the path turning point, S2(x b ) represents the approximate smooth function between the path inflection point and the path smoothing end point.

Images