CN109945881B - A mobile robot path planning method based on ant colony algorithm - Google Patents

Abstract

The invention discloses a mobile robot path planning method based on ant colony algorithm. The factor forms the expectation heuristic function, and the pheromone is updated according to the pheromone update formula. The parameters of the pheromone heuristic factor and the expected heuristic factor are dynamically adjusted according to the fuzzy algorithm, and the pheromone volatility coefficient is dynamically adjusted. The technical solution provided by the present invention can obtain a better optimal path solution, and the convergence speed is faster. In addition, the present invention proves that the above technical solution is feasible and effective through simulation experiments.

Description

A mobile robot path planning method based on ant colony algorithm

technical field

The invention relates to the field of robot technology, in particular to a path planning method for a mobile robot based on an ant colony algorithm.

Background technique

Mobile robots have attracted extensive attention due to their great potential and research value in industrial applications, manufacturing, search and rescue, medical services, and intelligent logistics. Navigation is at the heart of research in mobile robotics-related technologies, as it defines how a mobile robot perceives its environment, how it locates in the environment, and how it plans a path in a known map. Path planning is an important part in the research of navigation technology, and its main purpose is to construct a collision-free optimal path from the starting point to the target point for a mobile robot in a known environment.

Path planning problems generally consider both static and dynamic environments: a static environment means that the positions of the starting point and the target point are known, and the obstacles are stationary. However, in a dynamic environment, since the location of obstacles and target points may change over time, mobile robots need to make corresponding decisions with the help of sensor information. According to the different perceptions of the robot to the environment, path planning can be divided into two categories: the first category, the robot first builds a map of the environmental information, so the path can be planned offline according to the known map information, this type of path planning is called global path planning; In the second category, the robot does not input environmental information in advance, and needs to use sensors to build an environmental map in real time to avoid obstacles to find a suitable path. This type of path planning is called local path planning. This embodiment studies the global path planning problem in a static environment.

In recent years, many scholars have done a lot of research on path planning and proposed some feasible methods, such as artificial potential field method, fuzzy algorithm, DWA algorithm, genetic algorithm, immune algorithm, neural network algorithm and so on. However, there are some shortcomings in these technical methods, such as poor stability, local optimum, and poor adaptability.

SUMMARY OF THE INVENTION

In order to solve the limitations and defects of the prior art, the present invention provides a mobile robot path planning method based on ant colony algorithm, including:

The grid map of the mobile robot is formed according to the grid method, and the calculation formula of the sequence code of the mobile robot in the environment map is:

Wherein, the coordinates of the robot are (x _g , y _g ), Num is the serial number, N _x is the number of rows of the grid map, and N _y is the number of columns of the grid map;

The distribution of initial pheromone is formed according to the obstacle information of the starting point and the ending point;

An expectation heuristic function is formed according to the critical obstacle influence factor, and the expectation heuristic function is:

Among them, d _ij is the distance between two adjacent grids, _gi is the critical obstacle influence factor at node i, and A constant;

The pheromone is updated according to the pheromone update formula, and the pheromone update formula is:

为所有蚂蚁的可行路径解的平均值；Among them, Q is the pheromone enhancement coefficient, L _k is the total length of the path traversed by the kth ant in one iteration, Δτ _ij (t) is the pheromone added on the path ij at time t, B is a constant, and n is the iteration frequency,

is the average value of the feasible path solutions of all ants;

Dynamically adjust α/β parameters according to fuzzy algorithm;

Among them, α is the pheromone heuristic factor, and β is the expected heuristic factor;

Output the optimal path.

Optionally, after the step of dynamically adjusting the α/β parameter according to the fuzzy algorithm, it includes:

Determine whether the ant colony algorithm falls into a local optimum;

If the ant colony algorithm falls into a local optimum, the pheromone volatilization coefficient is dynamically adjusted according to the adjustment formula. The adjustment formula is:

ρ'=Cρ (12)

Among them, C is a constant greater than 0 and less than 1, ρ∈(0,1);

If the ant colony algorithm does not fall into a local optimum, the pheromone volatility coefficient remains unchanged.

Optionally, it also includes: outputting the iterative convergence curve and the optimal path length.

Optionally, the value range of α is [1, 4], and the value range of β is [7, 9].

The present invention has the following beneficial effects:

The mobile robot path planning method of the ant colony algorithm provided by the present invention forms the grid map of the mobile robot according to the grid method, forms the distribution of the initial pheromone according to the obstacle information of the starting point and the end point, and forms the expected inspiration according to the critical obstacle influence factor Function, update the pheromone according to the pheromone update formula, dynamically adjust the parameters of the pheromone heuristic factor and the expected heuristic factor according to the fuzzy algorithm, and dynamically adjust the pheromone volatilization coefficient. The technical solution provided by this embodiment can obtain a better optimal path solution, and the convergence speed is faster. In addition, the present embodiment proves that the above technical solution is feasible and effective through simulation experiments.

Description of drawings

FIG. 1 is a schematic diagram of a grid map model according to Embodiment 1 of the present invention.

FIG. 2 is a schematic diagram illustrating the influence of different α values on algorithm performance according to Embodiment 1 of the present invention.

FIG. 3 is a schematic diagram illustrating the influence of different β values on algorithm performance according to Embodiment 1 of the present invention.

FIG. 4 is a block diagram of fuzzy reasoning provided by Embodiment 1 of the present invention.

FIG. 5 is a graph of an α fuzzy control rule table provided by Embodiment 1 of the present invention.

FIG. 6 is a graph of a β fuzzy control rule table provided by Embodiment 1 of the present invention.

FIG. 7 is a schematic diagram of a simple grid map provided by Embodiment 1 of the present invention.

FIG. 8 is a schematic diagram of a complex grid map according to Embodiment 1 of the present invention.

FIG. 9 is a flowchart of an ant colony algorithm according to Embodiment 1 of the present invention.

10a to 10c are optimal path trajectory diagrams of the mobile robot according to Embodiment 1 of the present invention.

11a to 11c are comparison diagrams of the optimal path convergence curves of the basic ant colony algorithm and the improved ant colony algorithm according to Embodiment 1 of the present invention.

Detailed ways

In order for those skilled in the art to better understand the technical solutions of the present invention, the mobile robot path planning method provided by the ant colony algorithm provided by the present invention will be described in detail below with reference to the accompanying drawings.

Example 1

The ant colony algorithm is inspired by the foraging behavior of real ants in nature, and is a heuristic intelligent evolutionary algorithm. Today's ant colony algorithm has been gradually applied in the field of mobile robot path planning due to its advantages of parallel processing, distributed computing and strong robustness. Although the ant colony algorithm has shown good results in the field of path planning, it still cannot solve the shortcomings of long search time, easy stagnation, slow convergence, and local optimization. In order to improve the performance of the algorithm, many scientists have done related research. Yen and Cheng proposed a fuzzy ant colony algorithm to minimize the iterative learning error of the ant colony algorithm under fuzzy control. Imen et al. combined the advantages of ant colony algorithm and genetic algorithm, and proposed a new hybrid GA-ACO algorithm. Cheng et al. verified the efficiency of the ant colony algorithm under the TSP model. Wu et al. improved the performance of traditional ant colony algorithm by applying the rollback strategy. Rajput and Kumari combined the directional motion of the mobile robot in the grid map with the vector motion, and proposed a fast ant colony algorithm. Li et al. proposed an improved ant colony algorithm based on dynamic parameters and pheromone update mechanism. Khaled and Farid proposed an ant colony algorithm based on infinite step size. GAN et al. proposed a planning method based on ant colony expansion path optimization. Based on the principle of odor diffusion, Chen et al. proposed a fast two-stage ant colony algorithm, which overcomes the inherent problems of traditional ant colony algorithms. Aiming at the characteristics of the three-dimensional path problem of explosion-proof mobile robots and the shortcomings of traditional ant colony algorithm in path planning, Che et al. proposed an improved ant colony optimization algorithm (ACO).

According to the above research, this embodiment proposes a global path planning method based on the improved ant colony algorithm. First, the robot environment map is established by the grid method, and then the obstacle influence factor is introduced, and a new pheromone distribution and update strategy is proposed. Fuzzy algorithm controls key parameters and dynamically adjusts the volatility coefficient in sections. Finally, simulation experiments are carried out in a variety of complex environments, and the results show that the algorithm is feasible and effective.

Mobile robot environment modeling methods mainly include grid method, visual method and free space method. The grid method can describe any form of obstacles well, and the generated binary information features are convenient for computer storage and updating. Therefore, the grid method has significant advantages for environmental modeling.

FIG. 1 is a schematic diagram of a grid map model according to Embodiment 1 of the present invention. As shown in Figure 1, the grid method divides the working space of the mobile robot into grid units. In the grid map, the moving direction of the robot is no longer any direction, but the 8 travel directions represented by the octree. . There are only two grid states of the map, namely occupied or free. The black grid in the figure represents the obstacle information, the white grid is the movable area of the robot, the blue grid is the starting point of the robot, and the red grid is the goal of the robot. point. Assuming that the coordinates of the robot are (x _g , y _g ), the sequence code of the robot on the map can be calculated as follows:

Among them, Num represents the serial number, N _x represents the number of raster map rows, and N _y represents the number of raster map columns.

Ant colony algorithm (ACO) imitates the foraging behavior of ant colony and finds the optimal path in an unknown environment. It is a typical heuristic intelligent search algorithm. The existing research results show that the cooperative communication between ants is based on pheromone, and the concentration of pheromone is inversely proportional to the path length. During random exploration of the environment in search of food, ants tend to move toward paths with higher pheromone concentrations because they can sense the intensity of the pheromone. As the number of ants walking on the same path increases, the concentration of pheromones on the path also increases, so that more ants are attracted by the path. This behavior represents the principle for ants to choose the optimal path.

In order to improve the efficiency of path planning, the concepts of heuristic function η and tabu table _tabuk are introduced into the artificial ant colony model. In the random search algorithm, the use of heuristic function can improve the search efficiency of the algorithm. The tabu table _tabuk is used to record the nodes that the ants have walked through to ensure that the ants will not return to the previous nodes.

At time t, ant k moves from the current node i to an unvisited node j according to the distance information from the target point and the pheromone intensity on the path. If there are multiple unvisited nodes, ant k will move according to the following formula Determine state transition probabilities between nodes

Among them, U={C-tabu _k } is the set of optional nodes for the next step of ant k; α is the pheromone heuristic factor, the higher the value, the greater the influence of the pheromone concentration between nodes when the ant chooses the path; β is the expectation heuristic factor, the higher the value, the stronger the preference of the ants for the distance between the current node and the next node; τ _ij (t) represents the pheromone concentration on the path ij at time t; η _ij (t) is the expectation The heuristic function, defined as the reciprocal of the Euclidean distance between node i and node j, is:

where d _ij is the distance between two adjacent grids, defined as:

When all the ants complete the path search, the pheromone will continue to evaporate over time, and at the same time, the pheromone on the path that the ants pass through will increase, and the pheromone left on each path will be updated. The update formula is as follows:

τ _ij (t+1)=(1-ρ)τ _ij (t)+Δτ _ij (4)

表示蚂蚁k在t时刻经过路径ij后增加的信息素，定义为：where ρ is the pheromone volatility coefficient to avoid excessive accumulation of pheromone, ρ∈(0,1); Δτ _ij (t) represents the increased pheromone on the path ij at time t;

represents the pheromone added by ant k after passing the path ij at time t, which is defined as:

Among them, Q is the pheromone enhancement coefficient, which is a constant; L _k is the total length of the path traversed by the kth ant in one iteration.

According to the needs of the fuzzy control algorithm, this embodiment defines the following variables:

Definition 1 The quality of the path solution obtained by the ant colony

Value= _Lbest (n)-min{ _Lbest (n-1), _Lbest (n-2),......, _Lbest (1)},Value∈[-6,6](7 )

Definition 2 The degree of evolution of the ant colony

Among them, n is the current number of iterations, N is the total number of iterations, and L _best is the shortest path in the contemporary ant colony.

The fuzzy controller designed in this embodiment uses the values of Value and Iter as the fuzzy input, and the value of α/β is a fuzzy controller with dual inputs and dual outputs. The α/β parameters are dynamically adjusted by the convergence state of the ant colony in different stages, so that it can improve the early convergence speed and avoid falling into the local optimum of the algorithm.

The whole fuzzy control algorithm is divided into three stages. In the initial stage of the algorithm, the accumulation of pheromone on each path is insufficient, and the difference is not large. At this time, the ant colony selection path is mainly related to the expected heuristic factor β, so the value of β should be set larger and the value of α is smaller. With the progress of iteration, in the middle stage of the algorithm, the concentration of pheromone on the shortest path is gradually higher than that of other paths. At this time, the positive feedback effect of pheromone should be increased, that is, the value of α should be increased, and β should be decreased accordingly. value of . In the later stage of the algorithm, the pheromone concentration on some paths is much higher than other paths, and the algorithm has the greatest risk of falling into a local optimum. Therefore, the proportion of pheromone concentration in path selection should be reduced and the randomness of the algorithm should be increased. So α decreases again and β should decrease further. In the process of adjusting α/β, Value reflects the optimization ability of the ant colony, combined with the shortest path solution calculated each time, more accurate control parameters.

α and β in the traditional ant colony algorithm usually take values from [1, 9]. Through a large number of experiments, this embodiment conducts relevant research on the optimal values of α and β. As can be seen from Figure 2, when α=1,3, the ant colony algorithm can ensure a certain convergence speed while obtaining a shorter path length, and the effect is better. In order to further enhance the dynamic performance of the algorithm, this embodiment sets the value range of α as [1, 4]. The experimental results in Figure 3 show that the performance of the ant colony algorithm has been significantly improved with the continuous increase of the β value. When β=7, 9, the ant colony algorithm can finally converge stably, and the minimum path length can also be obtained. Therefore, in this embodiment, the value range of β is selected to be [7, 9].

The fuzzy controller provided in this embodiment adopts mamdani inference, and the flowchart of the fuzzy inference is shown in FIG. 4 . The input and output rules are established according to the fuzzy control algorithm. The membership function adopts the uniform distribution of symmetrical triangles. The fuzzy rules take the following form:

If Iter is A AND Value is B THENα/βis C

The fuzzy inference rules are shown in Table 1, and Figure 5 and Figure 6 are the graphical forms of the fuzzy control rule table.

Table 1 Fuzzy rule table for the value of α/β

The reasonable distribution of the initial pheromone is conducive to speeding up the search efficiency in the initial stage of the algorithm. The improvement made in this embodiment is to determine the distribution of the initial pheromone according to the obstacle information of the starting point and the ending point. In the absence of obstacles, the shortest path is the connection between the starting point and the target point. In the presence of obstacles, just follow the direction of the connection and get as close to the obstacle as possible while avoiding the obstacle. In this case, the path is also the shortest. Therefore, in the initial state, a larger initial value of pheromone than other grids is given to the places near the connection or the obstacle, which can greatly speed up the search efficiency in the early stage.

Figures 7 and 8 illustrate how to distribute the initial pheromone by taking different grid maps as examples. In the figures, "1" and "2" represent the size of the critical obstacle influence factor. The more pheromone. The critical obstacle influence factor will be generated around the two-point connection and the obstacles that the connection passes through, and the grid on the connection is more important than the grid around the obstacle, so take a larger value.

The expectation heuristic function is the reciprocal of the Euclidean distance between two points, which means that the probability of ants moving to the grid closer to the target is greater. At the same time, the information of obstacles is also taken into account, which is expressed by formula (9):

Among them, the critical obstacle influence factor at g _i node i, A is a constant, which can be adjusted appropriately according to the demand.

对小于平均值的蚂蚁经过的路径进行信息素的更新。当n≥B时，仅对蚂蚁经过的最优路径进行信息素的更新。上述的改进机制如公式(10)和公式(11)所示，B是一个与总迭代次数N有关的常数。In the traditional ant colony algorithm, when all the ants have completed their search, the pheromone on each path traversed by the ants will be updated. However, in the later stage of the algorithm, the pheromone on the optimal path has accumulated to a certain extent, and the selection of the shortest path by the ants has become stable. Decrease the stability of the optimal value and affect the convergence speed in the later stage of the algorithm. This embodiment proposes a method for updating pheromone in stages, which can effectively solve this problem. For this purpose, a constant B is introduced. When the number of iterations is n<B, the average value of the feasible path solutions obtained by all ants in this generation is calculated.

Update the pheromone for the paths traversed by the ants that are smaller than the average value. When n≥B, only update the pheromone for the optimal path traversed by the ants. The above improvement mechanism is shown in Equation (10) and Equation (11), where B is a constant related to the total number of iterations N.

It can be seen from formula (11) that the improved update mechanism only updates the pheromone for some ants that have obtained relatively short paths in the early stage of the algorithm, and only allows the ants to update the pheromone on the optimal path passed by the ants in the later stage of the algorithm , these improvements can speed up the algorithm convergence speed and optimization ability, while avoiding the algorithm premature.

Local optimality means that when the pheromone concentration on the path increases with the number of iterations, if one ant finds a better path at this time, the ants after it will receive strong information from the suboptimal path. Therefore, it will not walk along the optimal path. At this time, it is called the local optimal solution of the algorithm. If the optimal solution is not found for several consecutive generations, the algorithm will stagnate. The ant colony algorithm is easy to fall into the dilemma of the local optimal solution in the later stage of iteration. Therefore, in order to solve this phenomenon and strengthen the global search capability in the later stage of the algorithm, this embodiment dynamically adjusts the pheromone volatilization coefficient ρ.

The pheromone volatilization coefficient ρ in the improved ant colony algorithm is no longer constant. When the algorithm falls into a local optimum, the ρ value will decrease accordingly, which will reduce the positive feedback effect of pheromone concentration and increase the randomness of the algorithm. . As shown in equation (12), where C is a constant greater than 0 and less than 1.

ρ'=Cρ (12)

FIG. 9 is a flowchart of an ant colony algorithm according to Embodiment 1 of the present invention. As shown in FIG. 9 , this embodiment specifically describes the process of improving the ant colony algorithm as follows:

Step 1: Initialize the parameters of the algorithm, build a grid map according to the surrounding environment, the number of ants is m, the total number of iterations is N, the current number of iterations is n, the pheromone inspiration factor is α, the pheromone expectation factor is β, and the initial pheromone is The distribution is determined according to the method in 4.2, the pheromone volatilization coefficient is ρ, the pheromone intensity is Q, and the starting point and target point of the mobile robot are set.

Step 2: Place the ant at the starting point and add the starting point to the Tabu list. Calculate the transition probability between nodes ij according to formula (1), use the roulette method to select the next node j, and add the current node to the tabu tabu list.

Step 3: Determine whether the ant has reached the target point, if so, calculate its length according to the path nodes recorded in the taboo table, otherwise continue to search for the next node until reaching the target point. Loop through all ants in this generation until all ant traversal is complete, go to step 4.

Step 4: Calculate the evolution degree Iter of the ant colony and the solution quality Value of the ant colony path according to equations (7) and (8). Determine whether the algorithm falls into the local optimum, if so, adjust the value of ρ according to formula (12), otherwise the value of ρ remains unchanged. The pheromone is updated segmentally according to the strategy of formula (11).

Step 5: Determine whether the number of iterations of the algorithm satisfies n≥N. If otherwise, go to step 2 and let the ants start iterating over from the starting point. If yes, go to step 6

Step 6: Output the optimal path, the iterative convergence curve and the optimal path length, and the algorithm ends.

In order to verify the correctness of the improved algorithm provided in this embodiment, this embodiment uses MATALB R2014b to build three 20x20 grid maps of different complexity, and observes whether the mobile robot can find a feasible and optimal path in a static obstacle environment. The calculation results of the improved ant colony algorithm (IACO) and the basic ant colony algorithm (ACO) are compared, and the corresponding analysis is given. The main parameters of the algorithm are shown in Table 2.

Table 2 Initial parameter settings of the algorithm

The two algorithms are simulated under the same parameters. Figures 10a-10c are the optimal path trajectories of the mobile robot. The left figure is the experimental result of the basic ant colony algorithm, and the right figure is the improved ant colony proposed in this embodiment. The experimental results of the algorithm, Figure 11a-Figure 11c are the comparison diagrams of the optimal path convergence curves of the two algorithms. The technical solution provided by this embodiment can obtain a better optimal path solution, and the convergence speed is faster. In addition, the present embodiment proves that the above technical solution is feasible and effective through simulation experiments.

In Figure 11a, the number of iterations for the two algorithms to find the optimal path is not much different, but the shortest path found by the improved ant colony algorithm is better than the basic ant colony algorithm, and the convergence speed is faster, as can be seen from the figure d It can be seen that the optimal path of the basic ant colony algorithm still fluctuates slightly at the later stage of the iteration, while the improved ant colony algorithm can maintain the optimal path stably due to the change of the pheromone update mechanism.

In Figure 11b, the improved ant colony algorithm finds the shortest path earlier than the basic ant colony algorithm. In the later stage of the algorithm, the basic ant colony algorithm falls into the local optimal solution, while the improved ant colony algorithm jumps out of the local optimal solution by dynamically adjusting the parameters. Excellent trap.

In Figure 11c, the basic ant colony algorithm finds the optimal path and remains stable, but the improved ant colony algorithm uses a shorter number of iterations, which means that we can reduce the number of iterations N and shorten the running time of the algorithm.

Table 3 Comparison of algorithm experimental results

As shown in the experimental data in Table 3, the improved ant colony algorithm proposed in this embodiment can obtain a better optimal path solution than the basic ant colony algorithm, and the convergence speed is faster. Through simulation experiments, the effectiveness of the algorithm proposed in this embodiment is verified, and the performance of the algorithm is better than that of the basic ant colony algorithm.

The present embodiment discloses a mobile robot path planning method based on ant colony algorithm, which includes: forming a grid map of the mobile robot according to the grid method, forming initial pheromone distribution according to the obstacle information of the starting point and the ending point, and forming an initial pheromone distribution according to the critical obstacle information. The influence factor forms the expectation heuristic function, the pheromone is updated according to the pheromone update formula, the parameters of the pheromone heuristic factor and the expectation heuristic factor are dynamically adjusted according to the fuzzy algorithm, and the pheromone volatility coefficient is dynamically adjusted. The technical solution provided by this embodiment can obtain a better optimal path solution, and the convergence speed is faster. In addition, the present embodiment proves that the above technical solution is feasible and effective through simulation experiments.

It can be understood that the above embodiments are only exemplary embodiments adopted to illustrate the principle of the present invention, but the present invention is not limited thereto. For those skilled in the art, without departing from the spirit and essence of the present invention, various modifications and improvements can be made, and these modifications and improvements are also regarded as the protection scope of the present invention.

Claims (4)

1. A mobile robot path planning method of an ant colony algorithm is characterized by comprising the following steps:

forming a grid map of the mobile robot according to a grid method, wherein a calculation formula of sequence coding of the mobile robot on an environment map is as follows:

wherein the robot has coordinates of (x)_g，y_g) Num is sequence number, N_xFor the number of lines of the grid map, N_yThe number of columns of the grid map is obtained;

forming initial pheromone distribution according to the barrier information of the starting point and the end point;

forming an expected heuristic function according to the critical obstacle impact factor, the expected heuristic function being:

wherein d is_ijIs the distance between two adjacent grids, g_iIs a critical obstacle impact factor, a constant, at node i;

updating the pheromone according to a pheromone updating formula, wherein the pheromone updating formula is as follows:

wherein m is the number of ants, Q is the pheromone enhancement coefficient, L_kTotal length of path traveled by the kth ant in one iteration, Δ τ_ij(t) pheromones added to the path ij at time t, B is a constant, n is the number of iterations,

average value of feasible path solutions for all ants; l is_bestIs the shortest path in the contemporary ant colony;

dynamically adjusting the alpha/beta parameter according to a fuzzy algorithm;

wherein, alpha is pheromone elicitor, beta is expectation elicitor;

and outputting the optimal path.

2. The ant colony algorithm mobile robot path planning method of claim 1, wherein the step of dynamically adjusting the α/β parameters according to the fuzzy algorithm is followed by:

judging whether the ant colony algorithm falls into local optimum or not;

if the ant colony algorithm falls into local optimum, dynamically adjusting the pheromone volatilization coefficient according to an adjustment formula, wherein the adjustment formula is as follows:

ρ'＝Cρ (12)

wherein C is a constant which is more than 0 and less than 1, and rho epsilon (0, 1);

if the ant colony algorithm does not fall into the local optimum, the pheromone volatilization coefficient is kept unchanged.

3. The ant colony algorithm mobile robot path planning method according to claim 2, further comprising: and outputting an iterative convergence curve and the optimal path length.

4. The method for planning a path of a mobile robot according to the ant colony algorithm of claim 1, wherein a is in a range of [1,4] and β is in a range of [7,9 ].

Images