Improved ant colony algorithm introducing three-dimensional obstacle avoidance factor

Abstract

The invention provides an improved ant colony algorithm introducing a three-dimensional obstacle avoidance factor. The improved ant colony algorithm comprises the following steps: modeling a three-dimensional terrain environment; constructing an overall travel cost function; setting a starting point and an ending point in the three-dimensional terrain model, and initializing parameters; all ants are placed at a starting point, and a taboo table is constructed; the state transition probability formula of the ant colony algorithm is improved, the state transition probability is calculated through the formula, the next node where the ant wants to walk is determined, and the node where the ant wants walk is placed in a taboo table; improving a pheromone updating rule of the ant colony algorithm, and updating the pheromone content according to the rule; comparing and obtaining a current optimal driving route; whether the number of iterations reaches a set maximum value or not is judged, and if the number of iterations reaches the maximum value, an optimal route is output; according to the technical scheme provided by the invention, a three-dimensional obstacle avoidance factor is introduced, the problem that a deadlock phenomenon is easily caused by the limitation of a taboo table and the influence of obstacles in a complex environment in an ant colony algorithm is relieved, and the improved ant colony algorithm is more suitable for path planning in a three-dimensional terrain environment.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and relates to an improved ant colony algorithm introducing a three-dimensional obstacle avoidance factor. Background Art

[0002] With the rapid development of navigation technology and artificial intelligence technology, path planning technology based on ant colony algorithm has been widely applied in the fields of logistics, manufacturing, tourism, etc. At the same time, ant colony algorithm and its improved algorithms have become the research core of related technologies, and excellent algorithms can enable vehicles to efficiently plan the optimal path in a known map.

[0003] Ant colony algorithm is a probabilistic optimization algorithm based on swarm intelligence. Its effectiveness was first verified in solving the traveling salesman problem, and then this algorithm has received more attention and emphasis from researchers. Now this algorithm has shown good application prospects in path planning and unmanned driving.

[0004] Although the ant colony algorithm has advantages such as good robustness and strong adaptability, this algorithm still has the following disadvantages: First, the ant colony algorithm has strong blindness in the initial search, slow algorithm convergence speed, and long search time; Second, due to the positive feedback mechanism of the algorithm, if a sub-optimal solution is obtained at the beginning, then the positive feedback will make the sub-optimal solution quickly occupy a greater advantage, which makes the ants prone to stagnation during the search process and fall into the local optimal solution; Third, due to the limitation of the taboo list of the ant colony algorithm and the influence of obstacles in a complex environment, the ant colony search can only move forward and cannot move backward, which is very likely to cause the "deadlock" phenomenon; moreover, when considering the three-dimensional terrain factors in practical applications, this phenomenon will have serious consequences on the effect of path planning, directly leading to the failure of this path planning. Therefore, it is necessary to further improve the ant colony algorithm, introduce a three-dimensional obstacle avoidance factor, to get rid of the "deadlock" phenomenon and obtain an improved ant colony algorithm. Summary of the Invention

[0005] In order to overcome the deficiencies of the above traditional ant colony algorithm in path planning tasks, the present invention proposes an improved ant colony algorithm introducing a three-dimensional obstacle avoidance factor.

[0006] The technical solution adopted by the present invention is as follows:

[0007] Step 1: Use the grid method for environmental modeling, and the formula is:

[0008]

[0009] In the formula, N represents the total number of hillside terrains; (x oi , y oi ) is the center coordinate of the i-th small hillside; h i represents the height of the i-th small hillside; xsi and y si represent the components of the slope of the i-th hillside in two directions of the plane coordinates;

[0010] Step 2: Construct the overall cost function of the itinerary, which is used to calculate and measure the total cost of the vehicle traveling in the three-dimensional terrain model. The overall cost function of the itinerary is expressed as:

[0011]

[0012] where F represents the total cost of the itinerary; f L represents the cost brought by the driving distance; f S represents the cost brought by the slope magnitude during driving; f H represents the cost brought by the height change during driving; is a constant, representing the weight value of each cost;

[0013] Furthermore, the driving distance cost f L considers the driving distance of the vehicle from the starting point to the ending point, and can be specifically expressed as:

[0014]

[0015] where (x i , y i ) and (x i+1 , y i+1 ) respectively represent the plane coordinates of the i-th itinerary node and the adjacent next itinerary node;

[0016] Furthermore, the driving slope cost f S considers the influence of the slope magnitude during driving on its own loss; the driving slope cost can be specifically expressed as:

[0017]

[0018] where N represents the total number of itinerary nodes; z i and z i+1 respectively represent the z-axis coordinates of the current node and the next node;

[0019] Furthermore, the driving height change cost f H considers the loss brought by the change in altitude where the vehicle is located during driving; it can be specifically expressed as:

[0020]

[0021] where N represents the total number of itinerary nodes; z k represents the height value at the k-th itinerary node;

[0022] Step 3: Determine the starting point and ending point of the vehicle in the 3D terrain model, and initialize the parameters of the improved ant colony algorithm;

[0023] Step 4: Place all ants at the starting point and construct a taboo list simultaneously;

[0024] Step 5: Improve the state transition probability formula of the ant colony algorithm, mainly by introducing a 3D obstacle avoidance factor. The improved formula is as follows:

[0025]

[0026] where τ i,j (t) is the pheromone between node i and node j; η i,j (t) is the heuristic function; allowed i is the set of feasible nodes at node i; m is the ant label; t is the iteration number; i is the current node label, and j is the label of the next node to be transferred; α and β respectively represent the relative importance of pheromone and heuristic factor; ψ i,j is the travel direction guiding function, which can be specifically expressed as:

[0027]

[0028] where d s,i represents the distance from the starting point to node i, d i,j represents the distance from node i to the next node j; d j,e represents the distance from node j to the end point; γ is the direction factor, representing the relative importance of travel direction guidance. avoid j is the introduced 3D obstacle avoidance factor, which can be specifically expressed as:

[0029]

[0030] where O j is the total number of grids adjacent to node j and having obstacles in the 3D space, L j is the total number of grids adjacent to node j and restricted by the taboo list and cannot be passed through, A j is the total number of grids adjacent to node j in the 3D space. ε is the obstacle avoidance coefficient, and its value is a small positive number. Calculate the transfer probability through this formula to determine the next node for the ant to walk, and put the passed nodes into the taboo list; when the ant reaches the target node, a search is completed;

[0031] Step 6: Improve the pheromone update rule of the ant colony algorithm. The formula is:

[0032]

[0033]

[0034] Among them, ρ is the pheromone evaporation factor; Q is the pheromone constant; M is the total number of ants; F m represents the total travel cost function value of the path of the m-th ant in the t-th iteration; further, the pheromone content is updated according to the above formula;

[0035] Step 7: Compare the optimal driving routes obtained after each iteration to obtain the current optimal driving route;

[0036] Step 8: Determine whether the number of iterations reaches the set maximum value. If it reaches the maximum, output the optimal route; otherwise, continue the iteration.

[0037] Compared with the prior art, the beneficial effects of the present invention are:

[0038] (1) Considering the slope magnitude and the cost brought by the change in altitude faced by the vehicle when driving in a three-dimensional terrain environment, the pheromone update rule of the traditional ant colony algorithm is improved, making the application of the improved ant colony algorithm in vehicle path planning more reasonable and effective.

[0039] (2) Design and introduce a three-dimensional obstacle avoidance factor to alleviate the problems of the traditional ant colony algorithm due to the limitation of the taboo table and the influence of obstacles in a complex environment, which makes the ant colony search can only move forward and cannot move backward, easily causing the problem of the "deadlock" phenomenon. Description of the Drawings

[0040] Figure 1 It is: A flowchart of a vehicle path planning method based on an improved ant colony algorithm provided by the present invention. Specific Embodiments

[0041] The present invention will be further described below with reference to the drawings.

[0042] The overall flowchart of the improved ant colony algorithm of the present invention is as Figure 1 shown. First, the grid method is used for environmental modeling. For the construction of a three-dimensional terrain, it can be described by an exponential function, and the formula is:

[0043]

[0044] In the formula, N represents the total number of hillsides; (x oi , y oi ) is the center coordinate of the i-th small hillside; h i represents the height of the i-th small hillside; x si and y si represent the slope of the i-th small hillside; this formula can simulate the undulation of the actual environment where the vehicle travels. A three-dimensional terrain environment is constructed according to this formula.

[0045] Next, construct the overall cost function for vehicle driving, which is used to calculate and measure the total cost of the vehicle driving in the three-dimensional terrain model. Now, comprehensively consider the impacts of three factors, namely the vehicle driving distance, the slope magnitude, and the height change, on the vehicle driving cost. The constructed overall cost function for the vehicle itinerary is expressed as:

[0046]

[0047] Among them, F represents the total cost of the vehicle itinerary; f L represents the cost brought by the vehicle driving distance; f S represents the cost brought by the slope magnitude during the vehicle driving process; f H represents the cost brought by the height change during the vehicle driving process; is a constant, representing the weight values of each cost, and their sum is 1; in the three-dimensional terrain environment, the cost of vehicle driving is obviously related to both the altitude change and the slope magnitude during the driving process. The height change and the slope magnitude are two factors and should not be confused. When the two-dimensional planar distance and the height change degree of the itinerary are the same, the slope magnitude experienced by the vehicle is not necessarily the same. Therefore, when measuring the total cost of the vehicle driving in the three-dimensional terrain model, the height and the slope are considered as two factors.

[0048] Furthermore, the vehicle driving distance cost f L considers the driving distance of the vehicle from the starting point to the ending point. In the three-dimensional terrain model, the total distance consists of N nodes; the vehicle driving distance cost can be specifically expressed as:

[0049]

[0050] Among them, (x i , y i ) and (x i+1 , y i+1 ) respectively represent the planar coordinates of the i-th itinerary node and the adjacent next itinerary node; due to the impacts of the three-dimensional terrain on the vehicle driving cost, the two factors of height and slope have been considered to measure it. Therefore, when considering the distance factor, only the two-dimensional planar distance of the vehicle driving is considered. The three factors of two-dimensional planar distance, height, and slope restrict each other and jointly affect the final total cost function.

[0051] The slope cost f S of vehicle driving considers the impact of the slope magnitude during the vehicle driving process on the vehicle's own loss; when the slope is larger, the loss brought to the vehicle itself is obviously also larger; the slope cost of vehicle driving can be specifically expressed as:

[0052]

[0053] Among them, N represents the total number of travel nodes; z i and z i+1 respectively represent the z-axis coordinates of the current node and the next node; taking the absolute value of the slope between the current node and the next node and then summing them up, the larger the total slope value obtained, the greater the loss brought by this route to the vehicle itself.

[0054] The height change cost f of vehicle driving H considers the loss caused by the change in the altitude where the vehicle is located during the vehicle's driving; the standard deviation of the altitude is used to describe the height change cost of vehicle driving, which can be specifically expressed as:

[0055]

[0056] Among them, N represents the total number of travel nodes; z k represents the height value at the kth travel node; we hope that the vehicle has a smaller altitude change during the overall travel, so as to make the vehicle's driving more stable.

[0057] After completing the environmental modeling and the construction of the total cost function, set the starting point and the ending point of the vehicle in the three-dimensional terrain model, and initialize the various parameters of the improved ant colony algorithm. Then place all the ants at the starting point, and at the same time construct a taboo table, which is used to store the nodes that the ants have passed through to avoid these nodes being selected by the ants again.

[0058] Aiming at the problem that the traditional ant colony algorithm is prone to fall into the "deadlock" phenomenon, a three-dimensional obstacle avoidance factor is designed and introduced to improve the state probability transfer formula of the ant colony algorithm. The improved formula is:

[0059]

[0060] Among them, τ i,j (t) is the pheromone between node i and node j; η i,j (t) is the heuristic function; allowed i is the set of feasible nodes at node i; m is the ant label; t is the iteration number; i is the current node label, and j is the label of the next node to be transferred; α and β respectively represent the relative importance of the pheromone and the heuristic factor; different from the traditional ant colony algorithm, the improved formula introduces ψ i,j as the travel direction guiding function, which is specifically expressed as:

[0061]

[0062] Among them, d s,i represents the distance from the starting point to node i, d i,j represents the distance from node i to the next node j; d j,erepresents the distance from node j to the end point; γ is the direction factor, representing the relative importance of the travel direction guidance. The travel direction guidance function provides a general search direction towards the end point at the initial stage of the algorithm, making the search more purposeful in terms of direction and alleviating the low efficiency problem caused by blind search. avoid j is the introduced three-dimensional obstacle avoidance factor, specifically expressed as:

[0063]

[0064] where O j is the total number of grids adjacent to node j in three-dimensional space and having obstacles, L j is the total number of grids adjacent to node j in three-dimensional space and restricted by the taboo list and unable to pass through, A j is the total number of grids adjacent to node j in three-dimensional space. ε is the obstacle avoidance coefficient, taking a small positive value.

[0065] The transition probability is calculated through the improved state probability transition formula to determine the next node for the ant to move, and the passed nodes are put into the taboo list. When the ant reaches the target node, a search is completed.

[0066] To make the ant colony algorithm adapt to the path planning problem of vehicles in a three-dimensional terrain environment, the above-mentioned total vehicle travel cost is now introduced into the pheromone update rule of the ant colony algorithm. The formula for the improved pheromone update rule is:

[0067]

[0068]

[0069] where τ i,j (t) and τ i,j (t + 1) respectively represent the pheromone content between nodes i and j at the t-th iteration and the (t + 1)-th iteration; ρ is the pheromone evaporation factor; Q is the pheromone constant; M is the total number of ants; F m represents the value of the total travel cost function of the path of the m-th ant in the t-th iteration. Different from the traditional ant colony algorithm, F m is not only related to the distance, but also related to the two factors of height and slope. The smaller the total cost F m the more advantageous the route.

[0070] Furthermore, the pheromone content is updated according to the above formula. Then, the optimal vehicle driving routes obtained after each iteration are compared to obtain the current optimal vehicle driving route;

[0071] Finally, it is judged whether the number of iterations has reached the set maximum value. If it has not reached the maximum, the iteration continues; if it has reached the maximum, the route output at this time is taken as the optimal route.

[0072] As described above, it is only the specific implementation manner of the present invention. Any feature disclosed in this specification, unless specifically described, can be replaced by other equivalent or alternative features with similar purposes; all the features disclosed, or all the steps in all the methods or processes, except for the mutually exclusive features or / and steps, can be combined in any way.

Claims

1. An improved ant colony algorithm that introduces a three-dimensional obstacle avoidance factor. It is characterized by: The following steps are involved: Step 1: Use the grid method to model the environment. The formula is: Where N represents the total number of hillsides; (x oi ,y oi ) is the center coordinate of the i-th hillside; h i represents the height of the i-th hill; x si and si Represents the components of the slope of the i-th hill in two directions on the plane coordinate system; Step 2: Construct the overall cost function of the vehicle trip to calculate the total cost of the vehicle traveling in the three-dimensional terrain model. The overall cost function of the vehicle trip is expressed as: Where F represents the total cost of the vehicle trip; f L represents the cost of vehicle driving distance; f S Indicates the cost of the slope during vehicle driving; f H Indicates the cost of height change during vehicle driving; is a constant, representing the weight of each cost; the vehicle travel distance cost f L Specifically, it can be expressed as: Among them, (x i ,y i ) and (x i+1 ,y i+1 ) represent the plane coordinates of the i-th trip node and the next adjacent trip node respectively; further, the slope cost f S Specifically, it can be expressed as: Where N represents the total number of trip nodes; i and z i+1 Represents the z-axis coordinates of the current node and the next node respectively; the height change cost of the vehicle is f H Specifically, it can be expressed as: Where N represents the total number of trip nodes; k Indicates the height value at the kth trip node; Step 3: Set the starting point and end point of the vehicle in the 3D terrain model and initialize various parameters of the improved ant colony algorithm; Step 4: Place all ants at the starting point and build a taboo table; Step 5: Improve the state probability transfer formula of the ant colony algorithm, calculate the transfer probability through this formula to determine the next node that the ant will go to, and put the node that has been passed into the taboo table; when the ant reaches the target node, the search is completed; Step 6: Improve the pheromone update rule of the ant colony algorithm and update the pheromone content according to the rule; Step 7: Compare the optimal vehicle driving routes obtained after each iteration to obtain the current optimal vehicle driving route; Step 8: Determine whether the number of iterations reaches the set maximum value. If it reaches the maximum, the output is the optimal route; otherwise, continue to iterate.

2. The method according to claim 1, characterized in that: The state probability transfer formula of the improved ant colony algorithm in step 5 is specifically expressed as: Among them, τ i,j (t) is the pheromone between node i and node j; η i,j (t) is the heuristic function; alowed i is the set of feasible nodes at node i; m is the ant number; t is the number of iterations; i is the current node number, j is the next node number to be transferred; α and β represent the relative importance of pheromone and heuristic factors respectively; ψ i,j is the travel direction guidance function, which can be expressed as: Among them, d s,i represents the distance from the starting point to node i, d i,j represents the distance from node i to the next node j; d j,e represents the distance from node j to the end point; γ is the direction factor, which indicates the relative importance of the travel direction guidance. avoid j is the introduced three-dimensional obstacle avoidance factor, which is specifically expressed as: Among them, O j is the total number of grids adjacent to node j in three-dimensional space with obstacles, L j is the total number of grids in the three-dimensional space that are adjacent to node j and cannot be passed due to the taboo table, A j is the total number of grids adjacent to node j in three-dimensional space. ε is the obstacle avoidance coefficient, which is a small positive number.

3. The method according to claim 1, characterized in that: The pheromone update rule of the improved ant colony algorithm in step 6 is specifically expressed as: Where ρ is the pheromone volatility factor; Q is the pheromone constant; M is the total number of ants; F m Represents the total cost function value of the mth ant's path in the tth iteration.

Images