A path planning method based on multi-objective optimization smooth ant colony algorithm

Abstract

The application provides a path planning method based on a multi-target optimization smooth ant colony algorithm, comprising: establishing a grid map, determining a starting point and a target point, and initializing ant colony algorithm parameters; initializing a pheromone matrix according to guide path information generated by a Floyd algorithm and initializing a taboo table; constructing a candidate solution according to the taboo table and an optimized state transition function, and selecting a next node according to a roulette wheel principle; updating the taboo table according to the next node, recording path nodes and path lengths of ants, judging whether the ants reach the target node, and judging whether a preset maximum number of ants is reached, if yes, performing global updating according to an optimized pheromone updating mode, and performing smoothing processing on the path; and performing iteration according to a preset maximum iteration number, and obtaining an optimal path. The application optimizes and improves a traditional ant colony algorithm, so as to achieve the effects of accelerating the convergence speed of the algorithm, avoiding a local optimal solution and smoothing a path.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robotics, and particularly relates to a path planning method based on a multi-objective optimization smooth ant colony algorithm. BACKGROUND

[0002] Mobile robot path planning refers to planning an optimal path from a starting point to a target point without collision with obstacles in a specified area. Path planning mainly relies on various algorithms, and path planning algorithms can be divided into two categories: traditional algorithms and intelligent algorithms. Traditional algorithms include A* algorithm, tabu search algorithm and D* algorithm, etc.; intelligent algorithms include ant colony algorithm, particle swarm algorithm and genetic algorithm, etc. Among them, the ant colony algorithm has simple parameter setting and is suitable for various obstacle environments, and is widely used to solve robot path planning problems.

[0003] The ant colony algorithm was first proposed by Italian scholar Marco Dorigo in 1992, and the algorithm is derived from the behavior of ants in nature seeking the optimal path to food sources for food. The biggest feature of the ant colony algorithm is the positive feedback mechanism, which is conducive to continuously converging in the path search process and finally approaching the optimal solution. Secondly, individuals in the ant colony can change the surrounding environment by releasing pheromones, and individuals can perceive real-time changes in the surrounding environment, which can realize indirect communication between individuals and is conducive to quickly converging to the optimal solution. Finally, the ant colony uses a distributed computing method for the path search process, and multiple individuals perform parallel computing at the same time, which greatly improves the computing power and running efficiency of the algorithm. However, the slow convergence speed and the problem of easily falling into a local optimal solution of the ant colony algorithm in solving path planning problems cannot be ignored.

[0004] In order to further improve the performance of the ant colony algorithm to solve the robot path planning problem, scholars at home and abroad have proposed a number of optimization algorithms. Luo Qiang et al. proposed an improved ant colony optimization algorithm. The algorithm constructs an unequal initial pheromone distribution at the beginning of path planning, avoiding blind search in the early planning. At the same time, the pseudo-random state transition rule is used to select the path, and the state transition probability is calculated according to the current optimal solution and the number of iterations, and the proportion of deterministic or random selection is adjusted adaptively. The disadvantage is that the algorithm only sets the initial pheromone according to the position information of the node, which is not conducive to avoiding obstacles in the process of ant searching path and the guiding effect of the ant colony is not direct enough. Li Ka-rong et al. proposed an improved ant colony algorithm based on turning angle constraint. First, the initial pheromone concentration of the region between the starting point and the target point is increased to avoid blind search in the early stage; then, the evaluation function of A* algorithm and the turning angle constraint factor are added to the heuristic function, which is convenient for selecting the node with optimal path length and turning angle number; finally, the distribution principle of wolf swarm algorithm is introduced in the pheromone update part to strengthen the influence of high-quality population, and the maximum and minimum ant colony algorithm is used to limit the pheromone concentration to avoid falling into local optimal solution. The algorithm effectively avoids falling into local optimal solution, but the convergence speed in complex environment cannot meet the requirements. Akka Khaled et al. proposed an improved ant colony algorithm. The algorithm uses stimulation probability to help ants select the next grid, and uses new heuristic information based on infinite step length principle to expand the field of view and improve the visibility accuracy. In addition, the improved algorithm uses a new pheromone update rule and dynamically adjusts the evaporation rate to speed up the convergence speed and expand the search space. The algorithm effectively speeds up the convergence speed without considering the requirement of path smoothness, which is not conducive to reducing the energy consumption and mechanical loss of the robot. SUMMARY

[0005] To solve the above problems, the application provides a path planning method based on multi-objective optimization smoothing ant colony algorithm, which optimizes and improves the traditional ant colony algorithm to achieve the effects of accelerating the convergence speed of the algorithm, avoiding local optimal solution and smoothing the path.

[0006] To achieve the above purpose, the application provides the following technical scheme.

[0007] A path planning method based on multi-objective optimization smoothing ant colony algorithm, comprising the following steps:

[0008] Establish a grid map, determine the starting point and the target point, and initialize the ant colony algorithm parameters;

[0009] According to the starting point and the target point, the guide path information generated by Floyd algorithm is used to initialize the pheromone matrix and the taboo table;

[0010] The selectable nodes are constructed according to the tabu list and obstacle information, and the candidate solution is constructed according to the improved state transition function, and the next node is selected according to the roulette principle; wherein, a new heuristic function is constructed according to the distance from the selectable node to the target node, the heuristic function is optimized by introducing an artificial potential field method, and the heuristic function in the original state transition function is replaced;

[0011] According to the next node, the tabu list is updated, and the path node and path length of the ant are recorded;

[0012] According to the path length, safety and energy consumption, the pheromone updating mode is optimized by combining the multi-objective optimization idea and the dynamic principle;

[0013] It is judged whether the ant reaches the target node and whether the preset maximum number of ants is reached, if yes, the global updating is performed according to the optimized pheromone updating mode;

[0014] According to the preset maximum iteration number, iteration is performed to obtain an optimal path.

[0015] Preferably, the ant colony algorithm parameters include: a starting point, a target point, an iteration number, an ant number, a pheromone heuristic factor, an expected heuristic factor and a pheromone evaporation factor.

[0016] Preferably, the guide path information generated according to the Floyd algorithm and the initialization of the pheromone matrix include the following steps:

[0017] The shortest distance matrix Dist is initialized as the adjacency matrix of the map, and the path node matrix path is set as an empty matrix; the elements in the adjacency matrix are initialized as infinity, and if there is an edge between two nodes, the corresponding element is set as a weight value;

[0018] For node i and node j, it is judged whether there is a node k that makes the distance from node i to node k and then to node j shorter than the known path, if yes, the matrix Dist and the matrix path are updated;

[0019] The state transition equation is as follows:

[0020]

[0021] After the starting node and the target node are determined, the optimal path including the path length and the path node is obtained by the Floyd algorithm, and then the pheromone concentration on the path is increased according to the generated path;

[0022] The initial pheromone matrix setting mode is as follows:

[0023]

[0024] In the formula, τ ij(0) is the initial pheromone matrix, tour F The guide path generated by the Floyd algorithm, and the pheromone concentration of the guide path is set to k times that of other nodes.

[0025] Preferably, it further comprises:

[0026] When the ant falls into a deadlock, a backtracking strategy is executed until it escapes from the trap; including the following steps:

[0027] When the ant falls into a deadlock, the following formula will be satisfied:

[0028] allowed i ∩Obs=allowed i

[0029] In the formula, allowed i Obs is the list of forbidden nodes;

[0030] When the ant does not reach the target node and there is no optional node, a backtracking strategy is executed, the current node is added to the forbidden table and returned to the previous node, and the pheromone concentration at the current node is reduced; if there is a new optional node at this time, the backtracking strategy ends, if not, it continues to be executed until there is an optional node for the ant to choose, so as to escape from the deadlock dilemma;

[0031] The pheromone updating method when backtracking is as follows:

[0032] τ ij (t+1)=(1-λ)*τ ij (t)

[0033] In the formula, λ is the pheromone punishment evaporation coefficient.

[0034] Preferably, the new heuristic function is constructed according to the distance from the optional node to the target node, and the artificial potential field method is introduced to optimize the heuristic function, including the following steps:

[0035] The concept of distance from the optional node to the target node is proposed to replace the original heuristic function, as follows:

[0036]

[0037] On the basis of the new heuristic function, the concept of the target node's gravity to the robot in the artificial potential field method is introduced to optimize the heuristic function, as follows:

[0038]

[0039]

[0040]

[0041] In the formula, Gra is the attractive effect of the target node on the optional node, sigma is the gravitational constant, power is the power function, which returns the value of the exponential power of a given base; j is the optional node, and E is the target node;

[0042] The normal distribution function is introduced, which is combined with the heuristic function as follows:

[0043] Nd_function=e (-((k / K)^2) / 2)

[0044]

[0045] In the formula, Nd_function is a deformation of the standard normal distribution function, omitting the coefficient part, k is the current iteration number, K is the maximum iteration number, is the optimized state transition probability.

[0046] Preferably, the pheromone update mode is optimized according to the three indexes of path length, safety and energy consumption, combined with the multi-objective optimization idea and dynamic principle, including the following steps:

[0047] Based on the multi-objective optimization idea, three optimization objectives of path length Length, path safety degree Risk and path energy consumption Consumption are proposed, and the pheromone matrix is updated according to the above three indexes; the multi-objective optimization function is as follows:

[0048]

[0049] Risk=∑D_nodes

[0050] Consumption=∑0.5*N_corner+0.5*T_angle

[0051] J_quality=k1*Length+k2*Risk+k3*Consumption

[0052] k1+k2+k3=1

[0053] In the formula, Length is the sum of the Euclidean distances of all nodes in the path, Risk is the sum of dangerous nodes in the path, and the dangerous node is the node whose ratio of optional node to obstacle node is less than 1; Consumption is the sum of the number of turns and the angle of turns; J_quality is the comprehensive quality, which is a comprehensive index composed of the three optimization objectives of path length, path safety degree and path energy consumption with different proportions;

[0054] On this basis, the improved pheromone update mode is as follows:

[0055]

[0056]

[0057] τ ij (t+1)=(1-ρ)*τ ij (t)+Δτ ij (t)

[0058] In the formula, J best is the comprehensive quality of the local best path, b is the number of ants on the local best path, and BEST is the comprehensive quality of the global best path; J worst is the comprehensive quality of the local worst path, w is the number of ants on the local worst path, and WORST is the comprehensive quality of the global worst path.

[0059] The concept of dynamically updating the evaporation coefficient is introduced, and the dynamic evaporation coefficient is as follows:

[0060]

[0061] In the formula, J(k) and J(k-5) are the comprehensive quality of the optimal path in this iteration and five iterations before, respectively. In the late iteration, if the quality of the best path does not change in the last five iterations, the evaporation coefficient ρ increases, the evaporation effect is enhanced, the ants explore better solutions, and the upper limit is 0.8.

[0062] Preferably, the path is smoothed by using the connectivity principle and the quadratic B-spline method, including the following steps:

[0063] The nodes between the starting node and the most distant connectable node are all redundant nodes and will be discarded; the most distant node is used as the starting node for the next connectivity judgment, and the connectivity processing is ended until the target node is connected.

[0064] The quadratic B-spline curve optimization method is introduced, and the definition of the B-spline curve is as follows:

[0065]

[0066]

[0067]

[0068] In the formula, P i is the original endpoint, N i,k (t) is the basis function, and P(t) is the set of points on the curve.

[0069] When n=2 in the formula, the B-spline curve is quadratic, and the spline basis function is obtained as follows:

[0070]

[0071] The beneficial effects of the present application are as follows:

[0072] The present application provides a path planning method based on multi-objective optimization smooth ant colony algorithm, which has the following beneficial effects: first, the Floyd algorithm is introduced to generate a guide path, the pheromone content on the guide path is increased to enhance the guiding effect of the initial pheromone on the ant colony, and the convergence speed of the algorithm is improved; second, the backtracking strategy is applied to reduce the number of ants that die due to falling into traps and improve the success rate of ants finding the target node; third, the artificial potential field method and the concept of the distance from the optional node to the target node are introduced to optimize the heuristic function and compensate for the influence of the backtracking strategy on the convergence speed of the algorithm; fourth, the present application proposes a multi-objective optimization function, which comprehensively considers three indexes of path length, safety and energy consumption, combines the multi-objective optimization idea and the dynamic principle to optimize the pheromone update mode, avoids the algorithm falling into a local optimal solution and improves the comprehensive quality of the path; finally, the present application optimizes the path nodes according to the connectivity principle and the quadratic B-spline curve optimization method, effectively shortens the path length. BRIEF DESCRIPTION OF DRAWINGS

[0073] Figure 1 The algorithm flowchart provided for the embodiments of the present application is as follows:

[0074] Figure 2 The two-dimensional grid method schematic diagram provided for the embodiments of the present application is as follows:

[0075] Figure 3 The obstacle rule processing schematic diagram provided for the embodiments of the present application is as follows:

[0076] Figure 4 The guide path comparison diagram generated by the Floyd algorithm and the artificial potential field method provided for the embodiments of the present application is as follows:

[0077] Figure 5 The ant colony deadlock problem schematic diagram provided for the embodiments of the present application is as follows:

[0078] Figure 6 The ant colony backtracking strategy schematic diagram provided for the embodiments of the present application is as follows:

[0079] Figure 7 The artificial potential field method schematic diagram provided for the embodiments of the present application is as follows:

[0080] Figure 8 The connectivity processing schematic diagram provided for the embodiments of the present application is as follows:

[0081] Figure 9 The quadratic B-spline method node optimization schematic diagram provided for the embodiments of the present application is as follows:

[0082] Figure 10 A centralized obstacle environment schematic diagram provided for an embodiment of the present application;

[0083] Figure 11 A partially decentralized obstacle environment schematic diagram provided for an embodiment of the present application;

[0084] Figure 12 A decentralized obstacle environment schematic diagram provided for an embodiment of the present application;

[0085] Figure 13 A traditional algorithm path planning graph in a centralized obstacle environment provided for an embodiment of the present application;

[0086] Figure 14 An improved algorithm path planning graph in a centralized obstacle environment provided for an embodiment of the present application;

[0087] Figure 15 An algorithm convergence comparison graph in a centralized obstacle environment provided for an embodiment of the present application;

[0088] Figure 16 A traditional algorithm path planning graph in a partially decentralized obstacle environment provided for an embodiment of the present application;

[0089] Figure 17 An improved algorithm path planning graph in a partially decentralized obstacle environment provided for an embodiment of the present application;

[0090] Figure 18 An algorithm convergence comparison graph in a partially decentralized obstacle environment provided for an embodiment of the present application.

[0091] Figure 19 A traditional algorithm path planning graph in a decentralized obstacle environment provided for an embodiment of the present application;

[0092] Figure 20 An improved algorithm path planning graph in a decentralized obstacle environment provided for an embodiment of the present application;

[0093] Figure 21 An algorithm convergence comparison graph in a decentralized obstacle environment provided for an embodiment of the present application. DETAILED DESCRIPTION

[0094] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and should not be used to limit the present application.

[0095] Embodiment 1

[0096] A path planning method based on a multi-objective optimization smoothing ant colony algorithm, a flowchart is as follows Figure 1As shown, comprising the following steps:

[0097] S1: Establish a grid map, initialize algorithm parameters, including start point, target point, iteration number, ant number, pheromone heuristic factor, expected heuristic factor, pheromone evaporation factor, etc.

[0098] S2: Initialize the pheromone matrix according to the guide path information generated by the Floyd algorithm, and initialize the taboo list.

[0099] Specifically, the Floyd algorithm can calculate the shortest path between each node in the map environment, and its core idea is to solve the shortest path matrix. The shortest path from any node i to node j has only two possibilities, one is the Euclidean distance between the two nodes, that is, the two nodes are connected, and the other is from node i to node j through several intermediate nodes. Therefore, let Dis(i,j) be the Euclidean distance from node i to node j, then judge all nodes k except these two nodes, if Dis(i,k)+Dis(k,j)<Dis(i,j) is true, it is proved that the path from node i to node k and then to node j is shorter than the path from node i to node j. Directly, let Dis(i,j)=Dis(i,k)+Dis(k,j). After traversing node k, Dis(i,j) records the shortest distance from node i to node j.

[0100] The implementation of the Floyd algorithm mainly includes the following two steps.

[0101] (1) Initialize the shortest distance matrix Dist as the adjacency matrix of the map, and the path node matrix path as the empty matrix. The elements in the adjacency matrix are initialized to infinity, and if there is an edge between two nodes, the corresponding element is set to the weight value.

[0102] (2) For node i and node j, traverse the remaining nodes to determine whether there is a node k that makes the distance from node i to node k and then to node j shorter than the known path. If so, update the matrix Dist and the matrix path.

[0103] The state transition equation is as follows.

[0104]

[0105] After the start node and the target node are determined, the Floyd algorithm can quickly obtain the optimal path, including the path length and the path node, and then generate the path to increase the pheromone concentration on the path, which is obviously different from other paths. Since the ants are affected by the pheromone when selecting the path and are more likely to select the path with high pheromone concentration, the difference in pheromone between the guide path and other paths will make the ants tend to the former, thereby avoiding the blind search of the ants in the initial stage of the algorithm and quickly finding the target node. The initial pheromone matrix is set as follows.

[0106]

[0107] In the formula, τ ij (0) is the initial pheromone matrix, tour F The guide path generated by the Floyd algorithm is set to k times the pheromone concentration of other nodes.

[0108] S3: According to the taboo table and obstacle information, the selectable nodes are constructed, and the candidate solution is constructed according to the improved state transition function, and the next node is selected according to the roulette principle.

[0109] In the formula, the state transition function is improved - the concept of the distance from the selectable node to the target node is proposed, which replaces the original heuristic function, as follows.

[0110]

[0111] The new heuristic function refers to the target node information, and the selectable node closest to the target node has the maximum heuristic value, which is the optimal node, and has the maximum probability of being selected after the roulette. The ants have a clear direction in the exploration process, and the convergence speed will be improved.

[0112] On the basis of the new heuristic function, the artificial potential field method is considered to further improve the convergence speed of the algorithm.

[0113] The basic idea of the artificial potential field method is to regard the movement environment of the robot as a virtual force field, and the target node and the obstacle generate attractive force and repulsive force in the movement process of the robot, and the movement of the robot is controlled by the resultant force. Therefore, the concept of the attractive force of the target node on the robot in the artificial potential field method is used to optimize the heuristic function, as follows.

[0114]

[0115]

[0116]

[0117] In the formula, Gra is the attractive effect of the target node on the optional node, sigma is the gravitational constant, power is the power function, and j is the optional node. E is the target node. After adding the potential field gravity factor, the convergence speed of the algorithm is improved again.

[0118] The heuristic function needs to be adaptively adjusted to cooperate with the pheromone matrix, and the updating method of the state transition probability matrix is improved. The adaptive adjustment of the heuristic function is related to the number of iterations, so the normal distribution function is introduced, which is combined with the heuristic function as follows.

[0119] Nd_function=e (-((k / K)^2) / 2)

[0120]

[0121] In the formula, Nd_function is a modified standard normal distribution function, and the coefficient part is omitted. k is the current iteration number, K is the maximum iteration number, is the optimized state transition probability function.

[0122] S4: Determine whether the ant is in deadlock, if so, execute the rollback strategy until the ant escapes from the trap; otherwise, execute the subsequent steps.

[0123] Among them, the obstacle that will form the ant deadlock is usually a concave obstacle, because the ant follows the rule of putting the node passed into the taboo table when exploring the path, so as to reduce the generation of redundant nodes. When the ant encounters a concave obstacle, this rule will cause the ant to be unable to move away from the obstacle, so as to be trapped near the obstacle. And the self-lock is due to the fact that the ant has no clear direction of the target node at the beginning of iteration, and can only search blindly, and finally faces the dilemma of no optional node, forming a self-lock dilemma. The ant will meet the following formula when it is in deadlock.

[0124] allowed i ∩Obs=allowed i

[0125] In the formula, allowed i is the optional node list, and Obs is the taboo list.

[0126] In order to solve the deadlock problem encountered by the ant, the ant rollback strategy is proposed. When the ant does not reach the target node and has no optional node, the rollback strategy is executed. The current node is added to the taboo table and returned to the last node, and the pheromone concentration at the current node is reduced. If there is a new optional node at this time, the rollback strategy is ended, if not, it is continued to be executed until there is an optional node for the ant to choose, so as to escape from the deadlock dilemma. The processing of self-lock is similar.

[0127] The pheromone updating method when backtracking is as follows.

[0128] τ ij (t+1) = (1 - λ) * τ ij (t)

[0129] In the formula, λ is the pheromone punishment evaporation coefficient, which reduces the pheromone concentration of the trap node and helps the ants to avoid the trap.

[0130] S5: updating the taboo list and recording the path nodes and path length of the ants.

[0131] S6: judging whether the ants reach the target node and whether the preset maximum number of ants is reached, if yes, performing the pheromone global updating according to the improved pheromone updating method, and performing the path smoothing according to the connectivity processing and the secondary B-spline method; otherwise, continuing the node construction and selection.

[0132] The improvement of the pheromone updating method is to propose the pheromone updating method based on the multi-objective optimization idea and the dynamic principle.

[0133] Based on the multi-objective optimization idea, taking into account various requirements, three optimization objectives of path length, path safety degree and path energy consumption degree are proposed, and the pheromone matrix is updated according to the three optimization objectives. Among them, the path length is the same as in the classical ant colony algorithm, which is recorded as Length; the path safety degree is the number of dangerous nodes passed by the path, which is recorded as Risk; the path energy consumption degree depends on the number of corners and the turning angle of the path, which is recorded as Consumption. The multi-objective optimization function is as follows.

[0134]

[0135] Risk = ∑D_nodes

[0136] Consumption = ∑0.5*N_corner + 0.5*T_angle

[0137] J_quality = k1*Length + k2*Risk + k3*Consumption

[0138] k1 + k2 + k3 = 1

[0139] In the formula, Length is the sum of the Euclidean distances of all nodes of the path, the smaller the value is, the shorter the path is, Risk is the sum of the dangerous nodes that the path passes through, the dangerous node is the node whose ratio of optional node to obstacle node is less than 1, the smaller the value is, the safer the path is, Consumption is the sum of the number of turning angles and the turning angle of the path, the smaller the value is, the lower the energy consumption of the robot is. J_quality is the comprehensive quality, which is a comprehensive index composed of the length of the path, the safety degree of the path and the energy consumption degree of the path with different proportions, the smaller the value is, the higher the comprehensive quality of the path is.

[0140] On this basis, the pheromone updating method is improved as follows.

[0141]

[0142]

[0143] τ ij (t+1)=(1-ρ)*τ ij (t)+Δτ ij (t)

[0144] In the formula, J best is the comprehensive quality of the local optimal path, b is the number of ants on the local optimal path, and BEST is the comprehensive quality of the global optimal path; J worst is the comprehensive quality of the local worst path, w is the number of ants on the local worst path, and WORST is the comprehensive quality of the global worst path. The pheromone of the best path, the worst path and other paths in this iteration is updated according to the three forms in the formula.

[0145] In order to solve the problem of local optimal solution, the concept of dynamic updating of evaporation coefficient is proposed. In the later stage of iteration, if the quality of the best path does not change for five consecutive iterations, the evaporation coefficient is dynamically updated, the evaporation amount of pheromone is increased, the attraction of pheromone is weakened, the ants are helped to explore other paths, and the dynamic evaporation coefficient is as follows.

[0146]

[0147] In the formula, J(k) and J(k-5) are the comprehensive quality of the optimal path in this iteration and five iterations before, in the later stage of iteration, if the quality of the best path does not change for five consecutive iterations, the evaporation coefficient p increases, the evaporation effect is enhanced, the ants explore better solutions. The evaporation effect is not increased unlimitedly, and the upper limit is 0.8.

[0148] Further, the path is smoothed - the connectivity of the path nodes is processed and the secondary B-spline method is optimized.

[0149] The connectivity processing is an effective method for solving the redundant nodes. The first node in the path is taken as a starting node, and then it is judged whether the last node is connected with the starting node, and the connection means whether the connection between the two nodes crosses the obstacle. If not, it is considered to be connected. The nodes between the starting node and the farthest connectable node are all redundant nodes, and will be discarded. The farthest node is taken as the starting node for the next connectivity judgment until it is connected with the target node, and then the connectivity processing is ended.

[0150] In addition to the redundant nodes, the smoothing processing of the path also includes the smoothness operation of the corner, and therefore the quadratic B-spline curve optimization method is introduced to optimize the corner nodes.

[0151] The definition of the B-spline curve is shown as follows.

[0152]

[0153]

[0154]

[0155] In the formula, P i is the original endpoint, N i,k (t) is the base function, and P(t) is the set of points on the curve.

[0156] When n = 2 in the formula, the B-spline curve is quadratic, and the following form can be obtained through the spline base function.

[0157]

[0158] The quadratic B-spline method smoothes the path corner, and further shortens the path length.

[0159] S7: According to the preset maximum iteration number, the optimal path is obtained, and the optimal path iteration diagram and the optimal path diagram are drawn.

[0160] In the embodiment, the path is obtained by the following steps.

[0161] Referring to Figure 1The improved algorithm in the embodiment is improved on the basis of the traditional ant colony algorithm, mainly including optimization of the initial pheromone matrix, improvement of the heuristic function, treatment of the deadlock problem, multi-objective dynamic optimization of the pheromone updating mode, and smoothing of the path node. The optimization of the initial pheromone matrix relies on the guide path generated by the Floyd algorithm, and the pheromone on the guide path will be significantly higher than that in other areas, so as to strengthen the directivity of the algorithm and accelerate the initial convergence speed. The improvement of the heuristic function mainly introduces the artificial potential field method and the distance concept from the optional node to the target node, optimizes the structure of the heuristic function, improves the state transition probability, and accelerates the convergence speed of the algorithm. Considering that the ants are prone to fall into the deadlock and self-locking state and die in the initial stage of algorithm iteration, which is not conducive to the algorithm to solve the path, a backtracking strategy is proposed to reduce the number of dead ants and help improve the success rate of the algorithm to solve the path. The optimization of the pheromone updating mode mainly proposes a multi-objective optimization method, which takes into account the path length, path safety and path energy consumption, to solve the path with the highest comprehensive quality. Based on the multi-objective optimization method and the dynamic principle, the pheromone updating mode is improved to avoid the algorithm falling into the local optimal solution to the greatest extent. Finally, the path smoothing strategy introduces the connectivity principle and the quadratic B-spline method to optimize the corner nodes in the path, further shorten the path length, and reduce the mechanical loss of the robot in the working process.

[0162] Reference Figures 2-3 The path planning in the embodiment is established on the basis of a two-dimensional grid map. The two-dimensional grid map can convert obstacle information into a matrix form, which is conducive to simplifying the node calculation in path planning. The processing of irregular obstacles is the key. In the embodiment, the irregular obstacles are regularly processed by the dilation method, which can further reduce the description difficulty of the obstacle information.

[0163] Reference Figure 4 The initial pheromone matrix in the embodiment is mainly determined according to the guide path generated by the Floyd algorithm. The Floyd algorithm, also known as the insert point method, is an algorithm for finding the shortest path between multiple source points in a given weighted graph by using the idea of dynamic programming. The Floyd algorithm can calculate the shortest path between each node in the map environment, and its core idea is to solve the shortest path matrix, which is suitable for dense maps, simple and effective, and easy to implement. Therefore, the Floyd algorithm is introduced to generate a guide path in the initialization stage of the algorithm, and the pheromone on the path is increased to form a pheromone difference, enhance the directivity of the pheromone to the ants, reduce the time required for the ants to find the target node, accelerate the accumulation of pheromone, and thus significantly improve the initial convergence speed of the algorithm.

[0164] The guide path generated by the Floyd algorithm is shown in Figure 4 Figure 4 ​It can be seen that, as path planning algorithms, the path of artificial potential field method has the phenomenon of crossing obstacles, which is not allowed for the working path of mobile robots, while the optimal path of Floyd algorithm completely realizes the requirement of obstacle avoidance. In addition, the path of Floyd algorithm has no excessive redundant nodes, and the comprehensive performance is much stronger than that of artificial potential field method. Therefore, introducing the optimal path of Floyd algorithm as the guide path of ant colony algorithm will help the ant colony algorithm quickly find the target node in the early iteration and speed up the convergence speed of the algorithm.

[0165] With reference to Figure 5 and Figure 6 , the ant colony individuals often encounter deadlock problems when exploring the path, including the deadlock caused by obstacles and self-lock. The deadlock problem will cause excessive death of ants, reduce the ability of ant colony to explore the path, and is not conducive to the algorithm to find the optimal path. The obstacle that will form the ant deadlock is usually a concave obstacle, because the ant follows the rule of putting the node passed into the taboo table when exploring the path, so as to reduce the generation of redundant nodes. When the ant encounters a concave obstacle, this rule will cause the ant to be unable to move away from the obstacle, so as to be trapped near the obstacle. The self-lock is due to the fact that the ant has no clear direction of the target node in the early iteration, and can only search blindly, and finally faces the dilemma of no selectable node, forming a self-lock dilemma. The above two deadlock problems are shown in Figure 5 .

[0166] In order to solve the deadlock problem encountered by the ant in the embodiment, an ant backtracking strategy is proposed. When the ant does not reach the target node and has no selectable node, the backtracking strategy is executed. The current node is added to the taboo table and returned to the last node, and the pheromone concentration at the current node is reduced. If a new selectable node appears at this time, the backtracking strategy is ended, and if not, the backtracking strategy is continued until there is a selectable node for the ant to choose, so as to escape from the deadlock dilemma. The backtracking strategy is shown in Figure 6 . In Figure 6 , the ant at P3 node is trapped in deadlock, and starts to execute the backtracking strategy. P3 node is added to the taboo table, and the ant returns to P2 node. At this time, there is no selectable node for the ant to choose, so the ant continues to execute the strategy. When the ant returns to P1 node, the ant finds a new selectable node, and at this time the execution of the backtracking strategy is ended, and the ant selects P4 node far away from the obstacle by the roulette rule, and continues to explore the new path. Since the trap has been added to the taboo table, the subsequent ants will not be trapped in the deadlock dilemma because of the obstacle at this place. The processing of self-lock is similar to this.

[0167] With reference to Figure 7In the embodiment, the concept of optional node to target node distance is proposed, a new heuristic function is proposed, and the concept of target node gravity to robot in artificial potential field method is used to further optimize the heuristic function. In addition, the heuristic function needs to be adaptively adjusted to cooperate with the pheromone matrix, and the updating method of the state transition probability matrix is improved. The adaptive adjustment of the heuristic function is related to the number of iterations, so the normal distribution function is introduced and combined with the heuristic function.

[0168] The new heuristic function refers to the target node information, and the heuristic value of the optional node closest to the target node is the largest, which is the optimal node, and the probability of being selected after roulette is also the largest. The ant colony has a clear direction in the exploration process, and the convergence speed is improved. After adding the potential field gravity factor, the convergence speed of the algorithm is improved again. After the combination of the heuristic function and the normal distribution function, adaptive adjustment can be performed, which is more in line with the iteration law of the ant colony.

[0169] In the present application, the pheromone updating method is dynamically optimized for multiple targets. In order to improve the comprehensive quality of the path, the present application introduces the idea of multi-objective optimization, takes into account multiple requirements, proposes three optimization targets of path length, path safety degree and path energy consumption degree, and updates the pheromone matrix according to these standards. Replacing path length with path comprehensive quality is the implementation approach of the multi-objective optimization idea. The optimized pheromone updating method takes the comprehensive quality of the path as the basis for judgment, implements a reward and punishment system for the optimal path and the worst path, and gradually creates a gap between the pheromones of the optimal path and the worst path, so that subsequent ants are more likely to choose a better path, greatly speeding up the convergence speed of the algorithm based on ensuring the quality of the path.

[0170] For the local optimal solution problem of the classic ant colony algorithm, the present application proposes the concept of dynamically updating the evaporation coefficient. If the quality of the best path does not change for five consecutive iterations in the later iteration stage, the evaporation coefficient is dynamically updated, the evaporation amount of the pheromone is increased, and the attraction of the pheromone is weakened to help ants explore other better paths.

[0171] Referring to Figure 8 and Figure 9 In the embodiment, the connectivity principle and the quadratic B-spline curve optimization method are proposed to solve the problems of redundant nodes and corner smoothing, and the corner nodes in the path are optimized to further shorten the path length and reduce the energy consumption and mechanical loss of the robot in the working process.

[0172] Connectivity is proposed for the path characteristics of the ant colony algorithm. The path obtained by the ant colony algorithm is a series of nodes, and due to the limitation of the step length, there are many redundant nodes in the node set, which is not conducive to the movement process of the robot, and the connectivity processing is an effective method to solve the problem of redundant nodes, and the principle is as follows Figure 8The corner is reduced and the path length is shortened after the connectivity processing, and the working efficiency of the robot is improved.

[0173] The B-spline curve is a generalization of the Bezier curve, solves the problem that the Bezier curve is difficult to smoothly transit at the end point, and has higher precision. Figure 9 The corner processed by the B-spline method is smoother, and is more in line with the working characteristics of the robot.

[0174] The path processed by the connectivity and the B-spline method has almost no redundant nodes, is shorter than the path generated by the classical ant colony algorithm, is more suitable for the mobile robot, and has remarkable effects of path smoothing processing.

[0175] The experimental verification of the present application is as follows:

[0176] Referring to Figure 10 , Figure 11 and Figure 12 , the experimental environment of the present embodiment is divided into a centralized obstacle environment, a partially dispersed obstacle environment and a dispersed obstacle environment, so as to verify the effectiveness of the improved algorithm. Referring to Figure 13 and Figure 14 , the traditional ant colony algorithm and the improved algorithm of the present embodiment can both plan a feasible solution path in the centralized obstacle environment, however, the optimal path length of the improved algorithm of the present embodiment is 4.29% less than that of the classical ant colony algorithm, the average path length is 4.43% less, and the number of corners is 41.67% less. Referring to Figure 15 , the convergence speed of the improved algorithm is obviously faster than that of the traditional algorithm, and the optimal solution can be converged only by an average of 7 iterations, which is only 10% of the iteration number of the traditional ant colony algorithm, and the performance is better.

[0177] Referring to Figure 16 and Figure 17 , the traditional ant colony algorithm and the improved algorithm of the present embodiment also have a large gap in the partially dispersed obstacle environment. The optimal path length of the improved algorithm of the present embodiment is 2.78% less than that of the classical ant colony algorithm, the average path length is 5.88% less, and the number of corners is 35.71% less. Referring to Figure 18 , the convergence speed of the improved algorithm is significantly faster than that of the traditional algorithm, and the optimal solution can be converged only by an average of 11 iterations, which is only 16.92% of the iteration number of the traditional ant colony algorithm, and the comprehensive performance is better.

[0178] Referring to Figure 19 and Figure 20The improved algorithm of the embodiment obtains a solution path in a centralized obstacle environment, which is superior to the traditional ant colony algorithm, and the optimal path length is reduced by 4.27%, the average path length is reduced by 8.29%, and the number of corners is reduced by 38.89%. The path is smoother, and is more suitable for the working environment of a mobile robot, and can effectively reduce the energy consumption and mechanical loss of the robot. Figure 21 In the centralized obstacle environment, the improved algorithm maintains a faster convergence speed, and can converge to the optimal solution only by 10 iterations on average, which is only 13.33% of the traditional ant colony algorithm. The improved algorithm is more stable, and can well adapt to the requirement of the mobile robot to obtain an optimal path in a short time.

[0179] The path planning results of the traditional algorithm and the improved algorithm in the centralized obstacle environment, the partially dispersed obstacle environment and the dispersed obstacle environment are shown in Table 1, Table 2 and Table 3.

[0180] Table 1 Comparison of results of two algorithms in centralized obstacle environment

[0181]

[0182] Table 2 Comparison of results of two algorithms in partially dispersed obstacle environment

[0183]

[0184] Table 3 Comparison of results of two algorithms in dispersed obstacle environment

[0185]

[0186] The above is only a preferred embodiment of the present application, and is not used to limit the present application. Any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A path planning method based on multi-objective optimization smoothing ant colony algorithm, characterized in that, It comprises the following steps: A grid map is established to determine a starting point and a target point, and parameters of an ant colony algorithm are initialized; According to the starting point and the target point, a guide path information generated by a Floyd algorithm is adopted, and a pheromone matrix is initialized, and a taboo table is initialized; According to the taboo table and obstacle information, selectable nodes are constructed, and a candidate solution is constructed according to an improved state transition function, and the next node is selected according to the roulette principle; wherein, a new heuristic function is constructed according to the distance from the selectable node to the target node, the heuristic function is optimized by introducing an artificial potential field method, and the heuristic function in the original state transition function is replaced; According to the next node, the taboo table is updated, and the path node and the path length of the ant are recorded; According to three indexes of path length, safety and energy consumption, a pheromone updating mode is optimized in combination with a multi-objective optimization idea and a dynamic principle; It is judged whether the ant reaches the target node and whether a preset maximum ant number is reached, if yes, global updating is performed according to the optimized pheromone updating mode; Iteration is performed according to a preset maximum iteration number, and an optimal path is obtained; The new heuristic function is constructed according to the distance from the selectable node to the target node, and the heuristic function is optimized by introducing an artificial potential field method, comprising the following steps: The concept of the distance from the selectable node to the target node is proposed, and the original heuristic function is replaced, as follows: ； On the basis of the new heuristic function, the concept of the attraction of the target node to the robot in the artificial potential field method is introduced to optimize the heuristic function, as follows: ； ； ； wherein, is the gravitational effect of the optional node on the target node pair, is the gravitational constant, is the power function, returning the value of a given base raised to the power of an exponent; is the optional node, is the target node; A normal distribution function is introduced, which is combined with the heuristic function, as follows: ； In the formula, is a deformation of the standard normal distribution function, omitting the coefficient part, is the current iteration number, is the maximum iteration number, is the optimized state transition probability; The pheromone updating mode is optimized in combination with a multi-objective optimization idea and a dynamic principle according to three indexes of path length, safety and energy consumption, comprising the following steps: Based on the idea of multi-objective optimization, three optimization objectives of path length , path safety and path energy consumption are proposed, and the pheromone matrix is updated according to the three optimization objectives; the multi-objective optimization function is shown as follows: ； ； ； ； ； In the formula, is the sum of the Euclidean distances of all nodes in the path, is the sum of dangerous nodes in the path, the dangerous node is a node whose ratio of optional nodes to obstacle nodes is less than 1; is the sum of the number of turns and the angle of turns in the path; is the comprehensive quality, which is a comprehensive index composed of the path length, the path safety degree and the path energy consumption degree with different proportions. On this basis, the pheromone updating mode is improved as follows: ； ； ； wherein is the overall quality of the locally best path, is the number of ants on the locally best path, is the overall quality of the globally best path; is the overall quality of the locally worst path, is the number of ants on the locally worst path, is the overall quality of the globally worst path; The concept of a dynamic updating evaporation coefficient is introduced, and the dynamic evaporation coefficient is as follows: ； wherein, and Q and Q5are the overall quality of the best path in this iteration and the best path in the previous five iterations, respectively. In the later iterations, if the quality of the best path does not change in five consecutive iterations, the evaporation coefficient is increased to enhance the evaporation effect, making the ants explore better solutions, and the upper limit is 0.

8.

2. The path planning method based on multi-objective optimization smoothing ant colony algorithm according to claim 1, characterized in that, The ant colony algorithm parameters comprise a starting point, a target point, an iteration number, an ant number, a pheromone heuristic factor, an expected heuristic factor and a pheromone evaporation factor.

3. The path planning method based on multi-objective optimization smoothing ant colony algorithm according to claim 1, characterized in that, The guide path information generated by the Floyd algorithm is used to initialize the pheromone matrix, comprising the following steps: Initialize the shortest distance matrix is the adjacency matrix of the graph, the path node matrix is the empty matrix; the elements in the adjacency matrix are initialized to infinity, and if there is an edge between two nodes, the corresponding element is set to the weight value; For nodes With nodes Iterate through the remaining nodes to determine if any node exists. Let the node To the node Then to the node If the distance is shorter than the known path, then update the matrix. sum matrix ; The state transition equation is as follows: ； After the starting node and the target node are determined, the optimal path including the path length and the path node is obtained by the Floyd algorithm, and then the pheromone concentration on the generated path is increased; The initial pheromone matrix setting mode is as follows: ； In the formula, is the initial pheromone matrix, is the guide path generated by the Floyd algorithm, and the pheromone concentration of the guide path is set to times the pheromone concentration of other nodes. times the pheromone concentration of other nodes.

4. The path planning method based on multi-objective optimization smoothing ant colony algorithm according to claim 1, characterized in that, It also comprises: When the ant is trapped in a deadlock, a backtracking strategy is executed until it escapes from the trap; comprising the following steps: When the ant is trapped in a deadlock, the following formula is satisfied: ； wherein is a list of optional nodes, is a list of forbidden nodes; When the ant does not reach the target node and there is no selectable node, a backtracking strategy is executed, the current node is added to the taboo table and returned to the previous node, and the pheromone concentration at the current node is reduced; if there is a new selectable node at this time, the backtracking strategy is ended, if not, it is continuously executed until there is a selectable node for the ant to select, so as to escape from the deadlock dilemma; The pheromone updating mode during backtracking is as follows: ； wherein is the pheromone penalty evaporation coefficient.

5. The path planning method based on multi-objective optimization smoothing ant colony algorithm according to claim 1, characterized in that, The path is smoothed by using the connectivity principle and the quadratic B-spline method, comprising the following steps: The nodes between the starting node and the most distant connectable node are all redundant nodes, which will be discarded; the most distant node is taken as the starting node for the next connectivity judgment until the target node is connected, and then the connectivity processing is ended; The quadratic B-spline curve optimization method is introduced, and the definition of B-spline curve is shown as follows: ； ； ； wherein is the original endpoint, is the basis function, is the set of points on the curve; When the formula is B-spline curve is quadratic, and the following form is obtained by spline basis function: 。

Images