CN109506655B - Improved ant colony path planning algorithm based on non-uniform modeling - Google Patents
Improved ant colony path planning algorithm based on non-uniform modeling Download PDFInfo
- Publication number
- CN109506655B CN109506655B CN201811413526.7A CN201811413526A CN109506655B CN 109506655 B CN109506655 B CN 109506655B CN 201811413526 A CN201811413526 A CN 201811413526A CN 109506655 B CN109506655 B CN 109506655B
- Authority
- CN
- China
- Prior art keywords
- grid
- path
- ants
- algorithm
- neighborhood
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Biophysics (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Biomedical Technology (AREA)
- Health & Medical Sciences (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention relates to the technical field of path planning methods, in particular to an improved ant colony path planning algorithm based on non-uniform modeling, which considers the defect of a uniform modeling scheme on complex environment modeling, provides an improved ant colony path planning algorithm based on a non-uniform grid model, improves a basic ant colony algorithm based on the non-uniform grid model, considers the distance between a node and an obstacle when selecting a path point, considers the distance between the node to be selected and the obstacle when selecting the path point, adopts an 'ant-cycle' pheromone updating mode, limits the size of the pheromone when global pheromone updating, and simulation experiments show that the superiority of the non-uniform grid model compared with that of the uniform grid model, the improved ant colony algorithm has higher convergence speed compared with other basic ant colony algorithms, and can obtain paths which are more in line with the needs of underwater vehicles in complex environment.
Description
Technical field:
the invention relates to the technical field of path planning methods, in particular to an improved ant colony path planning algorithm based on non-uniform modeling, which is capable of obtaining paths more meeting the needs of underwater vehicles in complex environments by improving a basic ant colony algorithm based on a non-uniform grid model in consideration of the defect of modeling the complex environment by a uniform modeling scheme.
The background technology is as follows:
the path planning of the underwater vehicle refers to planning a path which is safe, collision-free and meets an optimization target in an underwater environment after a departure point and a target point are given. The path planning algorithm is often divided into 3 steps, wherein the first step is environment modeling, the second step is to plan and obtain a path by utilizing the algorithm based on the environment model, and the third step is path optimization. In the prior art, the environment modeling often adopts a uniform division modeling scheme, such as a grid method, a polar coordinate modeling method and the like, and the distribution information among obstacles is not considered in the process of dividing the search space by the modeling scheme, so that unnecessary calculation amount exists in the process of planning a path by a search algorithm based on an environment model obtained by the modeling scheme. Algorithms based on these schemes can solve to get solutions when the planning space is a simple environment, but algorithms based on uniform modeling schemes are inefficient or even not to get solutions when the planning space is a complex environment.
Since the underwater vehicle is operated in an underwater space, the ocean current has an effect on the operation of the AUV, and thus the effect of the ocean current must be considered for the path planning of the underwater vehicle. The flow field information may be obtained by remote observation, for example by high frequency radar (HF radar) measurements and satellite observations, or by moored in-situ measurements or numerical prediction models. An example of a numerical forecasting model is the regional ocean model system (Regional Ocean Model System-romas), which is an open-source ocean model program that is widely accepted and supported throughout the oceanographic and simulation communities. Romas was originally used to study ocean changes in the western coast of the united states, with resolutions ranging from 1 km to 15 km. The romas can simulate the flow field by using an analytical equation in addition to the flow field of the specific region. Bluelink is another flow field prediction model that can generate high resolution flow field predictions for the next seven days in Australian surrounding coasts and continental shelf areas. The use of Bluelink is mainly aimed at large areas, e.g. large areas within a few kilometres. Because the rough flow field map is of little use for short range operations in the hundreds of meters range, interpolation is required to generate higher resolution ocean current maps if practical applications are required.
The underwater vehicle works in turbulent and disordered ocean environments, ocean currents influence the movement of the underwater vehicle, forward currents can reduce the energy consumption of the underwater vehicle, reverse currents can increase the energy consumption of the underwater vehicle, the influence of the ocean currents is not considered in many articles in the past on path planning of the underwater vehicle, at the present stage, the battery technology does not make breakthrough progress, and the path energy consumption of the underwater vehicle is a considerable factor.
The path planning problem is a typical NP (Non-Deterministic Polynomial Problems) problem. In recent years, evolutionary algorithms have been widely used to solve the NP problem, and have proven to be an efficient and effective method of dealing with non-deterministic polynomial time problems. The ant colony algorithm is a typical evolutionary algorithm, and has been widely used in path planning.
In a complex environment, such as a U-shaped obstacle, a ring-shaped obstacle, etc. (as shown in fig. 7), ants are liable to fall into a deadlock state when searching for a path. The deadlock state refers to: after the ant reaches a certain grid, there is no alternative grid next, and the neighborhood grid of the grid is either located in the already walked path points or is an obstacle grid.
Ants tend to fall into deadlock during the early stages of searching due to blindness of searching. As shown in the left diagram of fig. 8, the ants, after having passed through grids 1, 2, 3, 4 to grid 5, have no walking grid next, and have fallen into a deadlock, wherein the gray grid is an obstacle grid. As shown in the right-hand diagram of fig. 8, in a non-uniform grid environment, ants get into deadlock after passing through grids 1, 2 to grid 3.
In previous studies, many solutions have been proposed for ant deadlock. Wen Ruchun it is proposed that when modeling an environment, the U-shaped barrier is filled, and this approach compromises the integrity of the environmental expression to avoid ants from getting deadlocked, and the influence on the solution of the optimal solution is not worth referencing. Wang recognizes the deadlocked ants as "ant dead" and stops updating the pheromone between the path nodes it walks through, which reduces the diversity of knowledge and is not conducive to finding the optimal solution. Qu Hong takes the steps of letting the ant trapped in the deadlock fall back to the last node, penalizing the pheromone between the last grid and the deadlock grid, and reselecting the path node at the last node. This reduces the probability of selecting a deadlock grid in the last grid, but if there are only 1 selectable grids (as shown in fig. 8), the approach cannot escape from the deadlock situation.
The invention comprises the following steps:
aiming at the defects and shortcomings in the prior art, the invention provides an improved ant colony path planning algorithm based on non-uniform modeling, which can obtain a path more meeting the needs of an underwater vehicle in a complex environment.
The invention can be achieved by the following measures:
an improved ant colony path planning algorithm based on non-uniform modeling is characterized by comprising the following steps:
step one: dividing a search space by using a quadtree to obtain a non-uniform grid model, and dividing the search space by using the quadtree by using a Python algorithm comprises the following specific steps:
step 1-1: the entire space is stored in a heap as a quadtree object,
step 1-2: if the current iteration number (i) is less than the maximum iteration number (Iterations), steps 1-3 are performed, otherwise steps 1-5 are passed,
step 1-3: the root element in the heap is popped up,
step 1-4: equally dividing the root element in the x and y directions to obtain 4 sub-elements, storing the 4 sub-elements in a pile,
step 1-5: the method of recursively calling the quadtree object to obtain child nodes,
step 1-6: storing quadtree object data;
step two: determining a neighborhood grid of the grid, and storing and obtaining two-dimensional coordinate information of a quadtree object in the quadtree modeling process in the first step, wherein the two-dimensional coordinate information is respectively Y-axis upper and lower limits Y max 、Y min Upper and lower limits X of X axis max 、X min Analysis was performed from 4 directions of up, down, left, and right of the grid: a neighborhood grid that may exist above the grid: y of neighborhood grid min Equal to Y of the grid max One of the following conditions is then satisfied: x of neighborhood grid min Equal to X of the grid max Or X of a neighborhood grid max Equal to X of the grid min Or X of a neighborhood grid min X smaller than grid min X of simultaneous neighborhood grid max X greater than grid min Or X is a neighborhood grid min X greater than grid min X of simultaneous neighborhood grid max X smaller than grid min Neighborhood grid conditions that may exist below the gridSimilar thereto. To the left of the grid, there may be a neighborhood grid: x of neighborhood grid max Equal to X of the grid min One of the following conditions is then satisfied: y of neighborhood grid min Equal to Y of the grid max Or Y of a neighborhood grid max Equal to Y of the grid min Or Y of a neighborhood grid min Y smaller than grid min While Ymax of the neighborhood grid is greater than Y of the grid min Or Y of a neighborhood grid min Y greater than grid min Y of simultaneous neighborhood grid max Y smaller than grid min The situation of a neighborhood grid that may exist above the grid is similar to that;
step three: the viscous Lamb vortics model proposed by Garau is adopted to simulate a flow field, and the speed of the flow field is v c = (u, v) where:
r= (x, y) represents 2D space, R 0 ＝(X 0 ,Y 0 ) Is the center of the vortex, τ and γ are the radius and strength of the parameter control vortex; then, an objective function capable of comprehensively evaluating the path quality is designed, and the energy consumption of the path, the threat degree of the path and the turning times are comprehensively considered when the path is planned in consideration of the movement of the underwater vehicle in turbulent and disordered marine environments, wherein the formula is as follows:
F p ＝(μ 1 .E p -1 +μ 2 .R p -1 +μ 3 .W p -1 )·C p (1-3)
wherein mu 1 、μ 2 、μ 3 、C p Is constant, where mu 1 、μ 2 、μ 3 The sum is 1, and the importance degrees of the items are respectively represented; e (E) p Indicating the energy consumption of the path, R p Indicating the risk level, w, of the route p Indicating the number of turns of the path; r is R p As shown in equations 1-4, the magnitude of which is the inverse of the average of the sum of the distances of the center coordinates of each grid node in the path nearest to the obstacle; the energy consumption formula is to divide the planned path into m pieces, calculate the energy consumption of each piece by the following formula respectively, and then add up to obtain the final energy consumption; wherein v is determined i The key step of obtaining the energy consumption of each path for calculation;
wherein W is i Energy consumption required to overcome ocean current resistance for AUV to travel along a small path, E p The path energy consumption obtained by superposition of the path energy consumption of m sections is that ρ is a constant related to the ocean current of the sea area, c is a constant related to the size of the underwater vehicle, v i The speed of the AUV relative to the current can be obtained by calculation of (equations 1-8) and (equations 1-9),
wherein v is c For the speed of ocean current, the speed and the size of the ocean current can be obtained by a flow field simulation formula, v a Finally along x for AUV i x i-1 Speed of advance, the invention sets v a The size is unchanged, the direction is the advancing direction of the path of the section, E a For the direction vector of the path of the segment,
can be obtained by (formulas 1-10)
Is greater than phi a Small, then combining (equations 1-8), (equations 1-9) can determine v i The size is then determined by (equation 1-5), (equation 1-6) to give w i Finally, the energy consumption E is determined p ；/>
Step four: the method comprises the following steps of improving an ant colony global path planning algorithm, adopting the improved ant colony algorithm to carry out path planning, taking a non-uniform grid model as a basis, taking an objective function as an optimization target, and improving a basic ant colony algorithm, wherein the method specifically comprises the following steps of: an improved path point selection strategy, an improved pheromone updating method and a solution to path deadlock, wherein the improved path point selection strategy comprises the following steps: (1) Reasonable heuristic distance is adopted, heuristic distance design of A algorithm is used for reference, and the heuristic distance is defined as the distance d between the current grid current center coordinate and the grid n center coordinate to be selected cn Distance d from the center coordinates of grid n to be selected to target point target nt And, the improved heuristic information is defined as:
(2) When selecting the path points, considering the distance between the grids to be selected and the barriers, dividing the search space for the quadtree to obtain a non-uniform grid model, improving the basic ant colony algorithm, introducing the concept of safe distance, and when selecting the path points, considering the distance between the grids to be selected and the barriers, designing a new path point selection strategy,
wherein D is n Represents the distance between the grid n and the nearest barrier thereof, and D is shown in the calculation formulas (1-15) 0 Represents the distance length of the grid furthest from the obstacle, A k (c) Representing a grid set that ant k can select in a neighborhood grid of current grid c; if the distance D between the grid n to be selected and its nearest obstacle n Less than the safe distance D safe Taking the distance between the grid and the obstacle into consideration when selecting the path point, adding D into the path point selection strategy n /D 0 At D n /D 0 In (D) n The larger the grid is, the greater the probability of selecting a grid far from the obstacle is, avoiding the selected grid from being too close to the obstacle, noting that the term is always less than 1; if the distance D between the grid n to be selected and its nearest obstacle n Greater than the safety distance D safe The path point selection is performed without considering the distance between the current grid and the obstacle.
The improved pheromone updating method adopts an ant-cycle pheromone updating mode, namely, local pheromones on the passing path are updated after ants reach target points, and when all ants in the population complete path searching, global pheromone updating strategies of MMAS are adopted to limit the size of the pheromones.
The invention solves the problem of path deadlock by adopting an ant rollback strategy, and comprises the following specific steps: if the ants arrive at a certain grid, judging that the ants fall into deadlock and surrounding movable grids are not available, firstly deleting the current grid from the path nodes (PathNode) which have been moved, adding the grid into a tabu table (TaboTable) in the rollback process of the ants, and simultaneously rolling back the ants to the last grid (previousNode), if the ants are not added into the tabu table, according to the path point selection strategy or the grids are the only selectable grids in the neighbor grids of the last grid, the ants still select the next grid as the next passing grid, and still fall into deadlock state. Repeating the above processes until the ants get rid of the deadlock state, namely, the ants have other grids except the path node (PathNode) which has gone through, the barrier grid and the grids in the tabu list in the process of the ant rollback, when the ants finish the process of the ant rollback and are not in the deadlock state, the tabu list does not play any role any more, the scope of action of the tabu list is only in the process of the rollback, the path point selection is carried out after the ants jump out of the deadlock, and the selection of the grids in the process of the path point is carried out; meanwhile, an 'ant-cycle' pheromone updating mode is adopted, namely, after ants arrive at a target point, local pheromones are updated, so that the pheromones are not updated in the ant rollback process, and the change of the pheromones cannot be caused by the process. The ant searches to obtain that the final path does not contain the dead lock, and then the ant rolls back the grid node passing through in the process of leaving the dead lock, and the next dead lock can be avoided by updating the pheromone.
The method comprises the steps of adopting an improved ant colony algorithm to plan to obtain path nodes, then utilizing B spline fitting to obtain a smooth path, adopting the improved ant colony algorithm to divide the path nodes into 2 layers of loops, wherein the first layer of loops is traversing ants in an ant population, ensuring that the ants in the population all reach target points, selecting the path points by the ants after reading a marine environment model, judging whether the path falls into deadlock after the ants reach the next node, adopting an ant rollback strategy to get rid of the deadlock if the path falls into the deadlock, judging whether the end point is reached if the path does not fall into the deadlock, and repeating the processes if the end point is not reached; when the ants reach the end point, updating local pheromones on the walking path according to an ant-cycle pheromone updating mode, judging whether the number of the ants is more than or equal to the number of the ant population at the moment, and if not, repeating the above processes by adopting new ants; if it is greater than, it indicates that the current ant population has all reached the end point, then the next layer cycle is entered.
In the invention, the defect of modeling a complex environment by a uniform modeling scheme is considered, a non-uniform grid model is obtained by using quadtree modeling, and an improved ant colony path planning algorithm based on the non-uniform grid model is provided. The method is characterized in that a basic ant colony algorithm is improved based on a non-uniform grid model, the distance between a node and an obstacle is considered during path point selection, the distance between the node to be selected and the obstacle is considered during path point selection, an 'ant-cycle' pheromone updating mode is adopted, and the size of the pheromone is limited during global pheromone updating. Simulation experiments show that the non-uniform grid model has superiority compared with the uniform grid model, the improved ant colony algorithm has higher convergence speed compared with other basic ant colony algorithms, and the path which meets the requirements of the underwater vehicle in a complex environment can be obtained.
Description of the drawings:
FIG. 1 is a schematic diagram of a quadtree partitioning search space in accordance with the present invention.
FIG. 2 is a flow chart of the quadtree partitioning search space in the present invention.
FIG. 3 is a schematic diagram of neighborhood grid contrast in the present invention.
Fig. 4 is a partial enlarged view of a flow field fitted using a formula in the present invention.
FIG. 5 is a schematic diagram of velocity vector synthesis in the present invention.
Fig. 6 is a schematic diagram of the path point selection in the present invention.
Fig. 7 is a schematic view of an obstacle in the present invention.
Fig. 8 is a schematic diagram of a deadlock in the present invention.
Fig. 9 is a flowchart of an improved ant colony algorithm in the present invention.
Fig. 10 is a graph showing a comparison of a path planned by the improved ant colony algorithm under a non-uniform model and a path corresponding to the uniform model in the present invention.
FIG. 11 is a graph of ACS algorithm for path planning under a non-uniform grid model in accordance with the present invention.
FIG. 12 is a diagram of the MMAS algorithm of the present invention for path planning under a non-uniform grid model.
FIG. 13 is a graph of the path planned under a non-uniform grid model by the EAC10 algorithm of the present invention.
FIG. 14 is a graph of the path planned under a non-uniform grid model by the EAC30 algorithm of the present invention.
Fig. 15 is a graph showing the change of the fitness of the improved algorithm and other ant colony algorithms according to the algebraic evolution in the present invention.
Fig. 16 is a graph showing the comparison of the improved ant colony algorithm of the present invention with a-plan under a non-uniform grid model.
Fig. 17 is a comparison of a smooth pre-path and a smooth post-path in the present invention.
The specific embodiment is as follows:
the invention is further described below with reference to the drawings and examples.
Before the path planning of the underwater vehicle is carried out, modeling of the marine environment is firstly carried out, and the path planning algorithm carries out path planning on the basis of the environment model. The marine environment modeling is divided into 2 parts, and one part is to divide a search space by using a quadtree to obtain a non-uniform grid model, and the 2D underwater space is represented by the non-uniform grid model. Another part is the simulation of the current in the marine environment, i.e. the simulation of the flow field.
Aiming at the defect that a uniform scheme is adopted for modeling in a complex environment, the invention provides a method for dividing a search space by using a quadtree to obtain a non-uniform grid model, and then introduces a non-uniform modeling process, wherein the non-uniform grid model comprises 3 parts, namely chart reading, quadtree division of the search space and node connection relation determination.
(1) Chart reading
And reading the electronic sea chart through Python, converting the read picture into an Image grid matrix by using the Image library and the Image draw library operation of Python, wherein the value of an obstacle area O (x, y) in the matrix is 1, the value of a non-obstacle area O (x, y) in the matrix is 0, and the subsequent algorithm operation is convenient after the electronic sea chart is converted into the grid matrix.
(2) Quadtree partitioning search space
The search space is divided by using a quadtree, and the basic concept of the quadtree is introduced first, and then how the search space is divided by using the quadtree is introduced.
(a) Quadtree basic concept
Quadtrees are a common data structure, and are of many kinds, including some quadtrees (Point Quadtree), linear quadtrees, and PR quadtrees (Point Region Quadtree). The invention adopts linear quadtree to divide the underwater space unevenly.
FIG. 1 is a schematic diagram of a quad-tree partitioning search space. The process of dividing the search space by the quadtree means that the space is divided equally in 2 dimensions to obtain 2 m ×2 n Basic elements. The length and width of each element are not smaller than the minimum dimension r min (r min Is set by man). Each element belongs to one of 3 kinds of free regions, obstacle regions, and regions including obstacles. If the element is a region containing an obstacle, the environment is recursively divided into 4 sub-regions of equal size in a 2-dimensional equal manner until the basic element contained in each region belongs to one of the 2 types of free region and obstacle region.
(b) Four-way tree search space partitioning step
The Python is used for realizing a search space division algorithm by using a quadtree, a data structure of a priority queue is used in the algorithm, and a header module of the Python provides support for heap and priority queue operation. FIG. 2 is a flow chart of a non-uniform grid model obtained by dividing a search space using a quadtree, the algorithm is mainly divided into the following steps:
Step 1: storing the whole space as a quadtree object in a heap; step 2: if the current iteration number (i) is smaller than the maximum iteration number (Iterations), performing step 3, otherwise, turning to step 5; step 3: ejecting the root element in the heap; step 4: equally dividing the root element in the x and y directions to obtain 4 subelements, and storing the 4 subelements into a pile; step 5: a method for recursively calling the quadtree object to obtain child nodes; step 6: the quadtree object data is stored.
(3) Determining a neighborhood grid of grids
The non-uniform grid model does not include the connection relationship between grids, and each grid is not clear about which grids are adjacent to it, and these adjacent grids are called neighbor grids. The invention utilizes ant colony algorithm to carry out path planning, and ants depend on the connection relationship when selecting path points. The neighborhood grid of the grid is thus determined prior to path planning. The lower graph is a neighborhood grid comparison graph of a non-uniform grid model and a uniform grid model, wherein a red grid represents a grid of which the neighborhood connection relation needs to be determined, and a gray grid is a neighborhood grid of the red grid.
As can be seen from fig. 3, the number and direction of the neighbor grids of the uniform grid model are fixed, which is 6 cases of left, upper left, lower left, right, upper right and lower right. The neighbor grid of the non-uniform grid model is more complex and more complex to present.
In the process of quadtree modeling, two-dimensional coordinate information of the quadtree object is stored and obtained, wherein the two-dimensional coordinate information is respectively the upper limit Y and the lower limit Y of the Y axis max 、Y min Upper and lower limits X of X axis max 、X min . We analyzed from 4 directions up, down, left, right of the grid. A neighborhood grid that may exist above the grid: y of neighborhood grid min Equal to Y of the grid max One of the following conditions is then satisfied: x of neighborhood grid min Equal to X of the grid max Or X of a neighborhood grid max Equal to X of the grid min Or X of a neighborhood grid min X smaller than grid min X of simultaneous neighborhood grid max X greater than grid min Or X is a neighborhood grid min X greater than grid min X of simultaneous neighborhood grid max X smaller than grid min . The neighbor grid that may exist below the grid is similar.
To the left of the grid, there may be a neighborhood grid: x of neighborhood grid max Equal to X of the grid min One of the following conditions is then satisfied: y of neighborhood grid min Equal to Y of the grid max Or Y of a neighborhood grid max Equal to Y of the grid min Or Y of a neighborhood grid min Y smaller than grid min While Ymax of the neighborhood grid is greater than Y of the grid min Or Y of a neighborhood grid min Y greater than grid min Y of simultaneous neighborhood grid max Y smaller than grid min . The situation is similar for a neighborhood grid that may exist above the grid.
Flow field simulation:
since the underwater vehicle is operated in an underwater space, the ocean current has an effect on the operation of the AUV, and thus the effect of the ocean current must be considered for the path planning of the underwater vehicle. The flow field information may be obtained by remote observation, for example by high frequency radar (HF radar) measurements and satellite observations, or by moored in-situ measurements or numerical prediction models. An example of a numerical forecasting model is the regional ocean model system (Regional Ocean Model System-romas), which is an open-source ocean model program that is widely accepted and supported throughout the oceanographic and simulation communities. Romas was originally used to study ocean changes in the western coast of the united states, with resolutions ranging from 1 km to 15 km. The romas can simulate the flow field by using an analytical equation in addition to the flow field of the specific region. Bluelink is another flow field prediction model that can generate high resolution flow field predictions for the next seven days in Australian surrounding coasts and continental shelf areas. The use of Bluelink is mainly aimed at large areas, e.g. large areas within a few kilometres. Because the rough flow field map is of little use for short range operations in the hundreds of meters range, interpolation is required to generate higher resolution ocean current maps if practical applications are required.
The present invention uses the viscous Lamb vortics model proposed by Garau to simulate the flow field, which is employed in a number of papers. The velocity of the flow field can be v c = (u, v) where:
r= (x, y) represents 2D space, R 0 ＝(X 0 ,Y 0 ) Is the center of the vortex and τ and γ are the radius and intensity of the parameter-controlled vortex. In a practical application scenario, the number and specific location, radius and intensity of Lamb voids can be measured by horizontal ADCP (H-ADCP) installed in front of AUV.
Fig. 4 is a partial enlarged view of a flow field fitted using the above formula, the flow field comprising 120 Lamb volts with random center coordinates, radius (γ) of 2, and intensity (τ) of 25. In the actual task situation, the environmental size is obtained through chart reading, and the flow field is constructed according to the actual environmental size.
The invention relates to global path planning under the condition of complex distribution of obstacles under the condition of known obstacle information, which comprises the following specific description: and planning by a reasonable algorithm, and obtaining a path which is from a departure point to a target point, meets obstacle avoidance requirements and meets optimization conditions in the underwater space planning. After the non-uniform grid model is obtained, the path joint points are searched by using an improved ant colony algorithm, and then a smooth path is obtained by using B spline fitting. Before path planning, an objective function capable of comprehensively evaluating the path quality is designed at first, so that an optimized path with smoothness, low energy consumption, high safety and high executable performance is planned.
The underwater vehicle works in turbulent and disordered ocean environments, ocean currents influence the movement of the underwater vehicle, forward currents can reduce the energy consumption of the underwater vehicle, reverse currents can increase the energy consumption of the underwater vehicle, the influence of ocean currents is not considered in many articles in the previous research on path planning of the underwater vehicle, and the influence of ocean currents on the forward movement of the underwater vehicle is considered, so that the planned path reduces the energy consumption by utilizing the ocean current 'forward current' effect as much as possible. Because the current battery technology has not made a breakthrough progress, the path energy consumption of underwater vehicles is a considerable factor.
An effective path must be safe and executable in addition to meeting collision avoidance conditions, which requires planning to keep the path a certain distance from the obstacle. A good path also has to take into account the number of turns of the path, which can increase the mechanical loss of the aircraft.
The fitness function is used for evaluating the quality of the path, and considering that the underwater vehicle moves in turbulent and disordered marine environments, the energy consumption of the path, the threat level of the path and the turning times should be comprehensively considered when planning the path. The formula is as follows:
F p ＝(μ 1 .E p -1 +μ 2 .R p -1 +μ 3 .W p -1 )·C p (1-3)
Wherein mu 1 、μ 2 、μ 3 、C p Is constant, where mu 1 、μ 2 、μ 3 The sum is 1, and the importance of each item is shown. E (E) p Indicating the energy consumption of the path, R p Indicating the risk level, w, of the route p Indicating the number of turns of the path. R is R p As shown in (equations 1-4), the magnitude of which is the inverse of the average of the sum of the distances of the center coordinates of each grid node in the path nearest to the obstacle.
The energy consumption formula adopts an algorithm proposed by Alvarez and the like. The specific algorithm is to divide the planned path into m paths, calculate the energy consumption of each path by the following formula respectively, and then add up the energy consumption to obtain the final energy consumption. Wherein v is determined i And (3) obtaining the key step of energy consumption of each path for calculation.
Wherein W is i Energy consumption required to overcome ocean current resistance for AUV to travel along a small path, E p And superposing the energy consumption of the m sections of paths to obtain the energy consumption of the paths, wherein ρ is a constant related to the ocean current of the sea area, and c is a constant related to the size of the underwater vehicle. v i The AUV speed relative to the current can be calculated from (equations 1-8) and (equations 1-9).
Wherein v is c For the velocity of the ocean current, the velocity and the magnitude can be obtained by a flow field simulation formula. v a Finally along x for AUV i x i-1 Speed of advance, the invention sets v a The size is unchanged, and the direction is the advancing direction of the path of the section. E (E) a Is the direction vector of the segment path.
Can be obtained by (formulas 1-10)
Is greater than phi a Small, then combining (equations 1-8), (equations 1-9) can determine v i The size is then determined by (equation 1-5), (equation 1-6) to give w i Finally, the energy consumption E is determined p . Fig. 5 is a schematic diagram of velocity vector synthesis.
The path planning problem is a typical NP (Non-Deterministic Polynomial Problems) problem. In recent years, evolutionary algorithms have been widely used to solve the NP problem, and have proven to be an efficient and effective method of dealing with non-deterministic polynomial time problems. The ant colony algorithm is a typical evolutionary algorithm, and has been widely used in path planning.
After the marine environment model is obtained, an improved ant colony algorithm is adopted for path planning. Based on the non-uniform grid model and the objective function as the optimization target, the basic ant colony algorithm is improved.
ACS algorithm (Ant Colony System ant colony system algorithm), MMAS algorithm (Max Min Ant System maximum minimum ant colony algorithm), path point selection strategy formula of ant colony algorithm adopting elite strategy is as follows:
Wherein:
wherein τ cn And (t) represents the pheromone size.
The representative heuristic information is the reciprocal of the linear distance from the grid n center coordinates to the target point target. Alpha and beta respectively represent the importance degree of pheromone and heuristic information, wherein alpha and beta are less than 0, A k (c) Representing the set of neighbor grids that the current grid c can access by ant k. q is [0,1 ]]Random number, q, generated in interval 0 For artificial setting at [0,1 ]]A constant within the interval. The above formula shows that when q < q 0 When the heuristic factor is the same as the pheromone factor, selecting the grid with the largest product of the heuristic factor and the pheromone factor; if q > q 0 When a roulette type is used, a certain grid in the neighborhood is selected.
The ant colony algorithm is applied to the non-uniform grid model, and the path point selection strategy in the basic ant colony algorithm is improved, wherein the specific improvement measures are as follows:
(1) By reasonable heuristic distance
By means of heuristic distance design based on A algorithm, the heuristic distance is defined as the distance d between the current grid current center coordinate and the grid n center coordinate to be selected cn Distance d from the center coordinates of grid n to be selected to target point target nt And (3) summing.
Compared with ACS, MMAS and ant colony algorithm adopting elite strategy, heuristic distance is equal to Euclidean distance from central coordinate of grid n to be selected to target point, because of characteristic of non-uniform model obtained by dividing search space by quadtree, distance between current grid and selectable grid in adjacent area is probably large. Thus, for a non-uniform model grid model, the distance d between the current grid current center coordinate and the grid n center coordinate to be selected is considered in heuristic distance cn Is reasonable and can better guide ants to advance to target points. The improved heuristic information is thus defined as:
as shown in FIG. 6, the yellow star represents the current grid current, the center coordinates thereof are (30, 30), the green circle and the blue square represent 2 grids to be selected in the current grid neighborhood grid, the center coordinates thereof are (35, 45) and (60, 60), respectively, and the red hexagram represents the target point target, the coordinates thereof are (35, 80). If the blue square is 32.01 from the target location length and the green circle is 35 from the target location length, according to the original heuristic distance, there is a greater likelihood that the grid represented by the blue square will be selected as the next passing grid.
In practice, however, the total length from the yellow star to the blue square and then from the blue square to the target location (30+32.01= 62.01) is much greater than the total length from the yellow star to the green circle and then from the green circle to the target location (15.81+35=50.81), and the new heuristic distance formula can be used to more reasonably express the positions of the nodes and the target location, so that the searching algorithm can be better conducted.
(2) Considering the distance between the grid to be selected and the obstacle
The distance between the grid to be selected and the obstacle is not considered in the path point selection strategy of the basic ant colony algorithm. If the path is closer to the obstacle in the case of unstable AUV control, there is a greater likelihood of collision with the obstacle. The distance between the selection grid and the obstacle should therefore be taken into account when making the waypoint selection.
When the path point selection is performed in the open area, the distance between the selected node and the obstacle is not considered to be great, because the path is 'safe' enough; if a path node is selected in a crowded area of an obstacle, a narrow passage, the selected path node should be moved away from the obstacle to obtain a safe path. Aiming at the concept that the basic ant colony algorithm is improved by obtaining a non-uniform grid model by dividing a search space of a quadtree, introducing a safety distance concept, and designing a new path point selection strategy by considering the distance between a grid to be selected and an obstacle during path point selection.
Wherein D is n The distance between the grid n and the nearest obstacle is represented by the calculation formula (1-15). D (D) 0 Representing the distance length of all grids furthest from the obstacle. A is that k (c) Representing the set of grids that ant k can select from among the neighbor grids of current grid c.
If the distance D between the grid n to be selected and its nearest obstacle n Less than the safe distance D safe In the course of a waypoint selection strategy, the distance between the grid and the obstacle is taken into accountAdding D n /D 0 . At D n /D 0 In (D) n The larger the grid is, the greater the probability of selecting a grid far from the obstacle is, avoiding the selected grid from being too close to the obstacle, noting that the term is always less than 1; if the distance D between the grid n to be selected and its nearest obstacle n Greater than the safety distance D safe The path point selection is performed without considering the distance between the current grid and the obstacle.
The main idea is that if the distance between the grid to be selected and the obstacle is far, the ants move to the open area, the grid size of the open area is larger due to the characteristics of the non-uniform grid environment obtained by dividing the quadtree, and the ants are guided to rapidly pass through the open area/the obstacle-free area mainly by utilizing the improved heuristic distance; if the distance between the grid n to be selected and the obstacle is smaller than the set D safe Indicating that the path point is now very close to the obstacle, the safety problem of the path needs to be considered. And when the path point is selected, the distance between the grid to be selected and the obstacle is considered, so that the safety of the path is ensured.
The improved pheromone updating method of the invention comprises the following steps:
the pheromone updates of ACS algorithms are classified into local pheromone updates and global pheromone updates. The ants update the local pheromone every time they act.
τ t+1 (c,n)＝(1-δ)·τ t (c,n)+δ·τ o (1-16)
Wherein τ t (c, n) represents the pheromone distribution between grid c and grid n at time t. τ t+1 (c, n) represents the pheromone distribution between grid c and grid n at time t+ 1. Delta represents the volatility coefficient of local pheromone when updating, tau o Representing a fixed pheromone delta.
The MMAS algorithm only updates the global pheromone, and updates the pheromone on the path that the optimal ant walks through after all ants in the population complete the path search. MMAS algorithm limits the size of the updated global pheromone to interval [ tau ] min ,τ max ]However, the global pheromone update of the ACS algorithm is not limited to the post-updateIs a size of pheromone.
Wherein:
wherein ρ is the volatility coefficient of the global pheromone when updating, and 0 < ρ < 1.P is p b For the current maximum fitness value path, L b For the length of the maximum path of the current fitness value, τ max 、τ min Respectively representing the maximum and minimum values of the pheromone.
The ant colony algorithm based on elite selection is an earlier improvement on the performance of the ant colony algorithm. The idea is to give additional pheromone amount enhancement to all the optimal solutions that have been found after each cycle. The pheromone updating mode is global pheromone updating, and the formula is as follows:
τ cn (t+1)＝(1-ρ)τ cn (t)+Δτ+Δτ cn * (1-20)
Wherein ρ is the pheromone volatility coefficient,
represents the concentration of pheromone of the kth ant left between the grids c and n in the present cycle, Q is a constant representing the concentration of pheromone, p k Represents the set of path points that ant k walks through in this cycle, sigma represents the number of elite ants, L e Representing the path length, L, corresponding to the optimal solution of ant population in the current cycle k Represents the path length of ant k in this cycle, p e And the set of path nodes which are optimally solved by the ant population in the current cycle is represented.
The invention uses the size limitation of pheromone in basic ant colony algorithm and MMAS algorithm, and adopts an ant-cycle pheromone updating mode, namely updating local pheromone on a passing path after ants reach target points. After all ants in the population complete the path search, the global pheromone updating strategy of MMAS is adopted to limit the pheromone size.
The solution of the path deadlock in the invention is as follows:
in a complex environment, such as a U-shaped obstacle, a ring-shaped obstacle, etc. (as shown in fig. 7), ants are liable to fall into a deadlock state when searching for a path. The deadlock state refers to: after the ant reaches a certain grid, there is no alternative grid next, and the neighborhood grid of the grid is either located in the already walked path points or is an obstacle grid.
Ants tend to fall into deadlock during the early stages of searching due to blindness of searching. As shown in the left diagram of fig. 8, the ants, after having passed through grids 1, 2, 3, 4 to grid 5, have no walking grid next, and have fallen into a deadlock, wherein the gray grid is an obstacle grid. As shown in the right-hand diagram of fig. 8, in a non-uniform grid environment, ants get into deadlock after passing through grids 1, 2 to grid 3.
In previous studies, many solutions have been proposed for ant deadlock. Wen Ruchun it is proposed that when modeling an environment, the U-shaped barrier is filled, and this approach compromises the integrity of the environmental expression to avoid ants from getting deadlocked, and the influence on the solution of the optimal solution is not worth referencing. Wang recognizes the deadlocked ants as "ant dead" and stops updating the pheromone between the path nodes it walks through, which reduces the diversity of knowledge and is not conducive to finding the optimal solution. Qu Hong takes the steps of letting the ant trapped in the deadlock fall back to the last node, penalizing the pheromone between the last grid and the deadlock grid, and reselecting the path node at the last node. This reduces the probability of selecting a deadlock grid in the last grid, but if there are only 1 selectable grids (as shown in fig. 8), the approach cannot escape from the deadlock situation.
The invention provides an ant rollback strategy to solve the problem of path deadlock, which comprises the following specific steps: if the ant reaches a certain grid, judging that the ant falls into deadlock and surrounding walkable grids are not available, deleting the current grid from the walked path node (PathNode), adding the grid into a taboo table (TaboTable) in the ant rollback process, and rollback the ant to the previous grid (previousNode). If the grid is not added to the tabu list, the next time the ant still selects the grid as the next passing grid according to the path point selection strategy or the grid is the only selectable grid in the neighbor grids of the previous grid, the deadlock state still remains. The above process is repeated until the ants get rid of the deadlock state, i.e. other grids except the path node (PathNode) which has been walked, the barrier grid and the grid in the taboo table of the ant rollback process exist in the neighbor grids of the ants and can be accessed. When ants end the process of 'ant rollback', after the ants are not in a deadlock state any more, the tabu list does not play a role any more, and the scope of the tabu list is only that in the rollback process, the selection of the grids in the process of selecting the path points after the ants jump out of the deadlock is not influenced. Meanwhile, an 'ant-cycle' pheromone updating mode is adopted, namely, after ants arrive at a target point, local pheromones are updated, so that the pheromones are not updated in the ant rollback process, and the change of the pheromones cannot be caused by the process. The ant searches to obtain that the final path does not contain the dead lock, and then the ant rolls back the grid node passing through in the process of leaving the dead lock, and the next dead lock can be avoided by updating the pheromone.
And planning by adopting an improved ant colony algorithm to obtain path nodes, and then obtaining a smooth path by using B spline fitting. The improved ant colony algorithm is divided into 2 layers of loops, wherein the first layer of loops is to traverse ants in the ant population, so that the ants in the population can reach target points. After the ocean environment model is read, the ants select the path points, and after the ants reach the next node, whether the path falls into deadlock is judged. An ant rollback policy is employed to break out of the deadlock if it is trapped. If no deadlock is involved, it is determined whether the endpoint is reached. If the endpoint is not reached, the above process is repeated. When the ant reaches the end point, the local pheromone on the walking path is updated according to the pheromone updating mode of the ant-cycle. Judging whether the number of ants is greater than or equal to the number of ant populations at the moment, if not, repeating the above processes by adopting new ants; if it is greater than, it indicates that the current ant population has all reached the end point, then the next layer cycle is entered.
And updating the global pheromone before entering the next cycle, obtaining the data of the paths corresponding to the ants through the fitness function, and judging whether the algorithm is in local convergence.
Local convergence means that if there is a continuous N before the algorithm reaches the set maximum number of loops k In the iteration, the optimal solution of the algorithm is not obviously improved, and ants are considered to be trapped into local optima. The invention adopts the probability of improving the algorithm to accept the random solution to solve the local optimum. (equations 1-24) a strategy is selected for the new waypoints.
Wherein q represents a random number, q r Is a positive number. allowed (n) represents a neighborhood grid A of the current grid c k (c) Is selected from the group consisting of a plurality of grids. rand represents a random function. The above formula means that if q < q r One is randomly selected from the currently selectable grids as the next passing grid node. Otherwise, taking the original pathAnd (5) a radius point selection strategy. By the scheme, the probability of the algorithm to accept random solutions can be increased, the searching capability of the algorithm and the diversity of population are enhanced, and the probability of jumping out of local convergence is increased.
If a local convergence is involved, a new path point selection strategy is employed. And judging whether the current iteration number i is larger than the set iteration number, and if not, repeating the process. If the algorithm is larger than the preset value, the path data is saved, and the algorithm operation is ended. Fig. 9 is a flowchart of an improved ant colony algorithm.
Examples:
on a computer with an operating system of Windows 10, a CPU of Intel Koui 5-5250U and a main frequency of 1.6GHz, matlab R2014b is used as a platform verification algorithm. The values of the parameters in the algorithm are as follows, τ=15, γ=2, α=2, β=1, q 0 ＝0.6，δ＝0.2，ρ＝1，Δτ o ＝0，σ＝5，Q＝200，μ 1 ＝0.3、μ 2 ＝0.3、μ 3 ＝0.4，c p ＝100，Q o ＝100，τ max ＝0.4，τ min ＝0.0001，ρ＝1，c＝3，v＝5m/s，D safe ＝5km，r min =0.01 km. The ant population contains 30 ants, and the maximum number of iterations is set to 60.
(1) Non-uniform grid model and uniform grid model contrast
The invention adopts quadtree modeling to obtain the non-uniform grid model, and aims to verify the effectiveness of the non-uniform grid model. The non-uniform modeling method and the uniform modeling method model the same search space (56.6 km×52.7km sea area), set the minimum grid scale to be the same (shortest length, width > =0.01 km), and then utilize the improved ant colony algorithm to perform path planning in the two grid models. Table 1-1 is a comparison of different environmental models, and it can be seen that the number of free, number of obstacle grids, total number of non-uniform grid models is much smaller than the number of free, number of obstacle grids, total number of uniform grid models, where the total number is about 0.70%, the number of free grids is about 0.50%, the number of obstacle grids is about 1.26%, differing by 2 orders of magnitude. From the comparison data, it can be seen that the non-uniform grid model effectively compresses the environmental space, reducing the storage space compared to the uniform grid model.
TABLE 1-1 comparison of different environmental models
In order to more comprehensively compare the influence of the non-uniform grid model and the uniform grid model on the algorithm performance, a starting point is fixed (17,510), 10 positions are randomly selected from an end point position, an improved algorithm is applied to the non-uniform grid model and the uniform grid model, the improved algorithm is operated for 10 times under the same algorithm parameter setting, and the optimal solution is taken for comparison. Tables 1-2 show the performance parameters of the improved algorithm in the non-uniform model and the uniform model, where F p For adapting degree E p For path energy consumption, R p Is the path risk degree, W p For the number of turns, T p For algorithm run time, data in the table is according to E p Ranging from small to large. It can be seen from tables 1-2 that the paths planned by the improved algorithm in the non-uniform model are superior to the paths planned by the improved algorithm in the uniform model, and the advantages of the improved algorithm are more obvious as the complexity of the search is increased. The non-uniform model was planned for 13.90% of the uniform model in the 1 st planning scenario, and 0.29% of the uniform model in the 10 th planning scenario. This is because the uniform grid model search speed is slower, the number of path nodes required to reach the target position is greater, and the time required for planning is longer, in the case where the minimum grid scale is the same; the non-uniform grid model has the advantages that the searching speed is high in an open area, the number of path nodes required for reaching a target position is small, the planning time is short, and therefore the searching efficiency of an algorithm is greatly improved. With the improvement of the terrain complexity of the planning area, the advantages of the non-uniform model are more obvious, in the 1 st planning situation, the energy consumption, the path danger degree and the turning frequency of the non-uniform model are respectively 90.3%,89.9% and 16.7% of those of the uniform grid model, and in the 10 th planning situation, the energy consumption of the non-uniform model is higher than that of the non-uniform grid model The path danger and the turning times are respectively 64.9%,42.3% and 0.22% of the uniform grid model, so that the non-uniform model effectively reduces the path energy consumption and improves the safety and the executable performance of the algorithm.
FIG. 10 is a graph of path contrast for a non-uniform model versus a uniform model. In the figure, the blue path is a path planned by the improved ant colony algorithm in the uniform grid model, and the red path is a path planned by the improved ant colony algorithm in the non-uniform grid model. It can be seen from the figure that a part of the blue path in the range close to the departure point and the target point is in close contact with the obstacle, while the red path is at a distance from the obstacle. The red path turns much less than the blue path in the figure, it can be seen that if the underwater vehicle is traveling along the red path, the performability is certainly higher than along the blue path. It can be seen from the figure that there is a folding in the blue path, and the folding phenomenon in the red path is less than that in the blue path.
Table 1-2 improved algorithm performance parameter comparison in non-uniform and uniform models
The improved ant colony algorithm is compared with the basic ant colony algorithm:
in order to evaluate and improve the performance of the ant colony algorithm more comprehensively, the improved ant colony algorithm, an ACS algorithm, an MMAS algorithm, an elite strategy ant colony algorithm (hereinafter referred to as EAC 10) with the number of elite ants being 10% of the population number, and an elite strategy ant colony algorithm (hereinafter referred to as EAC 30) with the number of elite ants being 30% of the population number are subjected to the same algorithm parameter setting, 10 times of planning are respectively carried out in a non-uniform grid model environment under the condition of fixed starting points (17,510) and random target positions, and optimal solutions are taken for comparison.
Table 1-3ACS algorithm and MMAS algorithm planning data in non-uniform grid model
Tables 1-4EAC10 algorithm and EAC30 algorithm planning data in non-uniform grid model
Tables 1-3 and tables 1-4 are path parameters that are planned by the basic ant colony algorithm in the non-uniform grid model, respectively. The improved ant colony algorithm data in the tables 1-2 and the basic ant colony algorithm data in the tables 1-3 and the tables 1-4 are compared, so that the improved ant colony algorithm is superior in algorithm fitness, energy consumption, path danger, path turning times and algorithm consumption time. In the 10 th planning scenario, the modified ant colony algorithm is 36.6% of ACS algorithm in algorithm time consumption, 76.4% of MMAS algorithm, 68.9% of EAC10, and 67.2% of EAC30, and it can be seen that the modified algorithm can perform more effective search under the guidance of modified heuristic distance, and takes less time to find the target position. In the 10 th planning situation, the path risk of the improved algorithm is 51.4% of the ACS algorithm, 48.6% of the MMAS algorithm, 48.2% of the EAC10 and 49.3% of the EAC30, which means that after the distance between the grid to be selected and the obstacle is considered in the path point selection strategy, the path planned by the improved algorithm is greatly improved in the safety of the path compared with other basic ant colony algorithms. In the case of planning 1, the number of turns of the improved algorithm is the same as the other several basic ant colony algorithms, but in the case of 10, the number of turns of the improved ant colony algorithm is 28.9% of the ACS algorithm, 20.4% of the MMAS algorithm, 20.7% of the EAC10, 26.8% of the elite strategy of EAC30, and it is seen that the advantage of the improved algorithm in path executability is more obvious with the increase of search complexity. Fig. 11, fig. 12, fig. 13, and fig. 14 are respectively path diagrams of ACS algorithm, MMAS algorithm, EAC10, EAC30 planned under the condition of serial number 10, and it can be seen from the diagrams that many path nodes in the paths planned by these algorithms are close to the obstacle, and the algorithm turning times are more.
Fig. 15 shows the variation of the adaptation of these 5 algorithms with the number of iterations. As can be seen from fig. 15, the improved ant colony algorithm based on the non-uniform grid model converges faster than ACS algorithm and MMAS algorithm. The ECA10 algorithm did not converge and the fitness value diverged, whereas ECA30 had reached the optimum at iteration number 21 and remained stable. The method is characterized in that whether the algorithm converges or not and the algorithm fitness value is influenced by the number of elite ants by adopting an ant colony algorithm of an elite strategy are determined by relying on human experience or after multiple attempts. The MMAS algorithm is still unstable at an iteration number of 34, with small oscillations. Compared with other basic ant colony algorithms, the improved ant colony algorithm has larger path fitness value, faster convergence speed and stability
(2) Improved ant colony algorithm compared with a-algorithm
The improved ant colony algorithm and the traditional heuristic path planning algorithm-A are independently planned for a plurality of times in a non-uniform grid model by defining the same departure position (17,510) and target position (495,65), and average values are obtained for comparison.
It can be seen from tables 1-5 that the running time of the a-algorithm is much smaller than the modified algorithm, because the solution can be obtained by running a1 time, the ant colony algorithm is an evolutionary algorithm, and the optimal solution is found by iterating a plurality of times (specifically how many times the maximum number of iterations is set according to the algorithm). The energy consumption of the path obtained by improving ant colony algorithm planning is slightly larger than that of the path of the A-algorithm, and the energy consumption of the path of the A-algorithm is 104.4%. However, the improved algorithm is superior in terms of the path fitness value, the path risk and the number of turns. The adaptation degree value, the risk degree and the turning frequency of the path are respectively obtained by improving algorithm planning and are 121.4%, 50.7% and 61.2% of the path obtained by A-algorithm planning.
In a complex marine environment, the safety of the path is more important compared with the path energy consumption and the planning time of the algorithm, so that the improved ant colony algorithm is more suitable for the path planning of the underwater vehicle in a marine environment with complex and turbulent working environment compared with the A-type algorithm. If the task requires a planned path as fast as possible or a path with low energy consumption, then the a-algorithm should be chosen.
Fig. 16 is a schematic diagram of path comparison for 2 algorithms. As can be seen from the figure, the a-algorithm is designed to get the path next to the obstacle at some positions, because the a-algorithm does not consider the distance between the selection grid and the obstacle, while the path nodes of the improved ant colony algorithm remain at a distance from the obstacle. Fig. 17 is a graph of the alignment of the path points obtained by improving the ant colony algorithm and a comparison of the smooth paths obtained by fitting the path points using a B-spline curve.
Table 1-5 algorithm data comparison
Claims (4)
1. An improved ant colony path planning algorithm based on non-uniform modeling is characterized by comprising the following steps:
step one: dividing a search space by using a quadtree to obtain a non-uniform grid model, and dividing the search space by using the quadtree by using a Python algorithm comprises the following specific steps:
Step 1-1: the entire space is stored in a heap as a quadtree object,
step 1-2: if the current iteration number i is smaller than the maximum iteration number, step 1-3 is performed, otherwise step 1-5 is passed,
step 1-3: the root element in the heap is popped up,
step 1-4: equally dividing the root element in the x and y directions to obtain 4 sub-elements, storing the 4 sub-elements in a pile,
step 1-5: the method of recursively calling the quadtree object to obtain child nodes,
step 1-6: storing quadtree object data;
step two: determining a neighborhood grid of the grid, and storing to obtain a two-dimensional sitting of the quadtree object in the quadtree modeling process of the step oneThe standard information is the upper limit Y and the lower limit Y of the Y axis respectively max 、Y min Upper and lower limits X of X axis max 、X min Analysis was performed from 4 directions of up, down, left, and right of the grid: a neighborhood grid that may exist above the grid: y of neighborhood grid min Equal to Y of the grid max One of the following conditions is then satisfied: x of neighborhood grid min Equal to X of the grid max Or X of a neighborhood grid max Equal to X of the grid min Or X of a neighborhood grid min X smaller than grid min X of simultaneous neighborhood grid max X greater than grid min Or X is a neighborhood grid min X greater than grid min X of simultaneous neighborhood grid max X smaller than grid min The neighbor grid that may exist below the grid is similar; to the left of the grid, there may be a neighborhood grid: x of neighborhood grid max Equal to X of the grid min One of the following conditions is then satisfied: y of neighborhood grid min Equal to Y of the grid max Or Y of a neighborhood grid max Equal to Y of the grid min Or Y of a neighborhood grid min Y smaller than grid min While Ymax of the neighborhood grid is greater than Y of the grid min Or Y of a neighborhood grid min Y greater than grid min Y of simultaneous neighborhood grid max Y smaller than grid min The situation of a neighborhood grid that may exist above the grid is similar to that;
step three: the viscous Lamb vortics model proposed by Garau is adopted to simulate a flow field, and the speed of the flow field is v c = (u, v) where:
r= (x, y) represents 2D space, R 0 ＝(X 0 ,Y 0 ) Is the center of the vortex, τ and γ are the radius and strength of the parameter control vortex; then, an objective function capable of comprehensively evaluating the path quality is designed, and the energy consumption of the path, the threat degree of the path and the turning times are comprehensively considered when the path is planned in consideration of the movement of the underwater vehicle in turbulent and disordered marine environments, wherein the formula is as follows:
F p ＝(μ 1 ·E p -1 +μ 2 ·R p -1 +μ 3 ·W p -1 )·C p 1-3
wherein mu 1 、μ 2 、μ 3 、C p Is constant, where mu 1 、μ 2 、μ 3 The sum is 1, and the importance degrees of the items are respectively represented; e (E) p Indicating the energy consumption of the path, R p Indicating the risk level, w, of the route p Indicating the number of turns of the path; r is R p As shown in equations 1-4, the magnitude of which is the inverse of the average of the sum of the distances of the center coordinates of each grid node in the path nearest to the obstacle; the energy consumption formula is to divide the planned path into m pieces, calculate the energy consumption of each piece by the following formula respectively, and then add up to obtain the final energy consumption; wherein v is determined i The key step of obtaining the energy consumption of each path for calculation;
wherein W is i Energy consumption required to overcome ocean current resistance for AUV to travel along a small path, E p The path energy consumption obtained by superposition of the path energy consumption of m sections is that ρ is a constant related to the ocean current of the sea area, c is a constant related to the size of the underwater vehicle, v i The speed of AUV relative to ocean current is calculated by equations 1-8 and equations 1-9,
wherein v is c For the speed of ocean current, the speed and the size of the ocean current can be obtained by a flow field simulation formula, v a Finally along x for AUV i x i-1 Speed of advance, the invention sets v a The size is unchanged, the direction is the advancing direction of the path of the section, E a For the direction vector of the path of the segment,
obtained by the formulas 1 to 10 
Is greater than phi a Small, then determine v by combining equations 1-8, equations 1-9 i The size is then calculated by formulas 1-5 and 1-6 to obtain w i Finally, the energy consumption E is determined p ；
Step four: an improved ant colony global path planning algorithm is adopted to carry out path planning, a non-uniform grid model is taken as a base, an objective function is taken as an optimization target, and a basic ant colony algorithm is improved, so that the method has the advantages ofThe body comprises the following contents: an improved path point selection strategy, an improved pheromone updating method and a solution to path deadlock, wherein the improved path point selection strategy comprises the following steps: (1) Reasonable heuristic distance is adopted, heuristic distance design of A algorithm is used for reference, and the heuristic distance is defined as the distance d between the current grid current center coordinate and the grid n center coordinate to be selected cn Distance d from the center coordinates of grid n to be selected to target point target nt And, the improved heuristic information is defined as:
(2) When selecting the path points, considering the distance between the grids to be selected and the barriers, dividing the search space for the quadtree to obtain a non-uniform grid model, improving the basic ant colony algorithm, introducing the concept of safe distance, and when selecting the path points, considering the distance between the grids to be selected and the barriers, designing a new path point selection strategy,
Wherein D is n Representing the distance between the grid n and its nearest obstacle, as shown in calculation formulas 1-15, D 0 Represents the distance length of the grid furthest from the obstacle, A k (c) Representing a grid set that ant k can select in a neighborhood grid of current grid c; if the distance D between the grid n to be selected and its nearest obstacle n Less than the safe distance D safe Taking the distance between the grid and the obstacle into consideration when selecting the path point, adding D into the path point selection strategy n /D 0 At D n /D 0 In (D) n The larger the selectionThe greater the probability of a grid far from the obstacle, the closer the selected grid is to the obstacle, noting that the term is always less than 1; if the distance D between the grid n to be selected and its nearest obstacle n Greater than the safety distance D safe The path point selection is performed without considering the distance between the current grid and the obstacle.
2. The improved ant colony path planning algorithm based on non-uniform modeling according to claim 1, wherein the improved pheromone updating method adopts an "ant-cycle" pheromone updating mode, namely, local pheromones on the passing path are updated after ants reach target points, and when all ants in the population complete path searching, an MMAS global pheromone updating strategy is adopted to limit the size of the pheromones.
3. The improved ant colony path planning algorithm based on non-uniform modeling according to claim 1, wherein the solving the path deadlock problem by adopting an ant rollback strategy comprises the following specific steps: if the ants arrive at a certain grid, judging that the ants fall into deadlock and have no walkable grids around, firstly deleting the current grid from the walked path nodes, adding the grid into a tabu table in the rollback process of the ants, simultaneously rolling back the ants to the last grid, if the grid is not added into the tabu table, selecting the grid as the only selectable grid in the neighborhood grid of the last grid according to a path point selection strategy or the grid, then the ants still select the grid as the next passing grid, still fall into the deadlock state, repeating the above processes until the ants get rid of the deadlock state, namely, other grids except the walked path nodes, the barrier grid and the grids in the tabu table in the rollback process of the ants can be accessed, after the ants finish the rollback process of the ants, the tabu table does not act, the scope of the tabu table is only the tabu table is the tabu table in the rollback process of the time after the deadlock state is not influenced, and the ants select the path points in the process of the tabu table is not influenced again; meanwhile, an ant-cycle pheromone updating mode is adopted, namely, after ants arrive at a target point, local pheromones are updated, so that the pheromones are not updated in the process of rolling back the ants, the change of the pheromones cannot be caused in the process, the ants search to obtain a final path which does not comprise a dead lock, then the ants roll back the grid nodes passing in the process of leaving the dead lock, and the next dead lock is avoided through updating the pheromones.
4. The improved ant colony path planning algorithm based on non-uniform modeling according to claim 1, wherein the improved ant colony algorithm is adopted to plan to obtain path nodes, then B-spline fitting is utilized to obtain smooth paths, the improved ant colony algorithm is divided into 2 layers of loops, the first layer of loops is traversing ants in an ant population to ensure that the ants in the population reach target points, after the ocean environment model is read, the ants perform path point selection, after the next node is reached, whether the path falls into deadlock is judged, if the path falls into deadlock, an ant rollback strategy is adopted to break out the deadlock, if the path does not fall into deadlock, whether a destination is reached, and if the destination is not reached, the above process is repeated; when the ants reach the end point, updating local pheromones on the walking path according to an ant-cycle pheromone updating mode, judging whether the number of the ants is more than or equal to the number of the ant population at the moment, and if not, repeating the above processes by adopting new ants; if it is greater than, it indicates that the current ant population has all reached the end point, then the next layer cycle is entered.
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN2018112207152 | 2018-10-19 | ||
| CN201811220715 | 2018-10-19 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109506655A CN109506655A (en) | 2019-03-22 |
| CN109506655B true CN109506655B (en) | 2023-05-05 |
Family
ID=65750356
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811413526.7A Active CN109506655B (en) | 2018-10-19 | 2018-11-26 | Improved ant colony path planning algorithm based on non-uniform modeling |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109506655B (en) |
Families Citing this family (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110211405B (en) * | 2019-05-22 | 2021-11-09 | 南京理工大学 | Travel path planning method based on traffic information element model |
| CN110244759B (en) * | 2019-06-05 | 2022-06-21 | 哈尔滨工程大学 | Underwater robot time optimal path planning method based on interval optimization |
| CN110319829B (en) * | 2019-07-08 | 2022-11-18 | 河北科技大学 | Unmanned aerial vehicle flight path planning method based on self-adaptive polymorphic fusion ant colony algorithm |
| CN110645991B (en) * | 2019-10-30 | 2020-09-22 | 深圳市银星智能科技股份有限公司 | Path planning method and device based on node adjustment and server |
| CN113081257B (en) * | 2019-12-23 | 2022-06-07 | 四川医枢科技股份有限公司 | Automatic planning method for operation path |
| CN111240326B (en) * | 2020-01-15 | 2023-05-16 | 重庆邮电大学 | Mobile robot path planning method based on heterogeneous double-population ant colony algorithm |
| CN111427378B (en) * | 2020-04-10 | 2023-04-18 | 南宁师范大学 | Method for planning preferential rescue path of unmanned aerial vehicle in mountainous region |
| CN113917912A (en) * | 2020-07-08 | 2022-01-11 | 珠海格力电器股份有限公司 | Global path planning method, device, terminal and readable storage medium |
| CN112710313A (en) * | 2020-12-31 | 2021-04-27 | 广州极飞科技股份有限公司 | Overlay path generation method and device, electronic equipment and storage medium |
| CN112857385B (en) * | 2021-01-18 | 2022-09-27 | 北京理工大学 | Rapid unmanned vehicle local path planning method based on non-uniform grid model |
| CN113219975B (en) * | 2021-05-08 | 2024-04-05 | 珠海一微半导体股份有限公司 | Route optimization method, route planning method, chip and robot |
| CN113341954B (en) * | 2021-05-19 | 2023-03-21 | 华南理工大学 | Unmanned ship energy-saving path planning method based on ant colony algorithm |
| CN113325856B (en) * | 2021-05-31 | 2022-07-08 | 中国船舶工业集团公司第七0八研究所 | UUV optimal operation path planning method based on countercurrent approximation strategy |
| CN113885503B (en) * | 2021-10-11 | 2023-06-16 | 同济大学 | Multi-positioning method in uneven space based on ant colony algorithm |
| CN115994629B (en) * | 2023-03-23 | 2023-06-20 | 南京信息工程大学 | GN-RBF-based air humidity prediction method and system |
| CN116592890B (en) * | 2023-05-17 | 2024-03-26 | 广东海洋大学 | Picking robot path planning method, picking robot path planning system, electronic equipment and medium |
| CN116451507B (en) * | 2023-06-13 | 2023-11-10 | 浙江大学 | Typhoon track path simulation method based on non-uniform grid division |
| CN116520862B (en) * | 2023-06-30 | 2023-09-29 | 青岛哈尔滨工程大学创新发展中心 | AUV task allocation method for multi-objective optimization under influence of ocean currents |
| CN117470247B (en) * | 2023-12-19 | 2024-05-03 | 珠海云洲智能科技股份有限公司 | Path planning method and path planning controller |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105527964A (en) * | 2015-12-28 | 2016-04-27 | 桂林电子科技大学 | Robot path planning method |
| CN108036790A (en) * | 2017-12-03 | 2018-05-15 | 景德镇陶瓷大学 | Robot path planning method and system based on mutillid algorithm under a kind of obstacle environment |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9921068B2 (en) * | 2017-03-09 | 2018-03-20 | Thomas Danaher Harvey | Devices and methods to facilitate escape from a venue with a sudden hazard |
-
2018
- 2018-11-26 CN CN201811413526.7A patent/CN109506655B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105527964A (en) * | 2015-12-28 | 2016-04-27 | 桂林电子科技大学 | Robot path planning method |
| CN108036790A (en) * | 2017-12-03 | 2018-05-15 | 景德镇陶瓷大学 | Robot path planning method and system based on mutillid algorithm under a kind of obstacle environment |
Non-Patent Citations (1)
| Title |
|---|
| 卜新苹 ; 苏虎 ; 邹伟 ; 王鹏 ; 周海 ; .基于复杂环境非均匀建模的蚁群路径规划.机器人.2016,(第03期),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109506655A (en) | 2019-03-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109506655B (en) | Improved ant colony path planning algorithm based on non-uniform modeling | |
| CN111780777B (en) | Unmanned vehicle route planning method based on improved A-star algorithm and deep reinforcement learning | |
| CN108801266B (en) | Flight path planning method for searching uncertain environment by multiple unmanned aerial vehicles | |
| CN108459503B (en) | Unmanned surface vehicle track planning method based on quantum ant colony algorithm | |
| Kratzke et al. | Search and rescue optimal planning system | |
| CN103528585B (en) | A kind of paths planning method of equidistantly not cutting apart the region of can passing through | |
| CN111709571B (en) | Ship collision avoidance route determining method, device, equipment and storage medium | |
| CN112033413B (en) | Path planning method based on improved A-algorithm combined with environment information | |
| CN102880186A (en) | Flight path planning method based on sparse A* algorithm and genetic algorithm | |
| CN111063218A (en) | Ship collision avoidance decision method | |
| CN110908386B (en) | Layered path planning method for unmanned vehicle | |
| CN113671985A (en) | Staged multi-base unmanned aerial vehicle task allocation and flight path planning method | |
| CN111338350A (en) | Unmanned ship path planning method and system based on greedy mechanism particle swarm algorithm | |
| CN113848919A (en) | Ant colony algorithm-based indoor AGV path planning method | |
| CN111750869A (en) | Unmanned aerial vehicle path planning method for reconstructing Voronoi diagram in real time | |
| CN111721296B (en) | Data driving path planning method for underwater unmanned vehicle | |
| CN114625150B (en) | Rapid ant colony unmanned ship dynamic obstacle avoidance method based on danger coefficient and distance function | |
| CN114706400B (en) | Path planning method based on improved A-x algorithm in off-road environment | |
| CN115454067A (en) | Path planning method based on fusion algorithm | |
| Bai et al. | Design and Simulation of a Collision-free Path Planning Algorithm for Mobile Robots Based on Improved Ant Colony Optimization. | |
| Yang et al. | Path planning and collision avoidance methods for distributed multi-robot systems in complex dynamic environments | |
| CN116448134B (en) | Vehicle path planning method and device based on risk field and uncertain analysis | |
| CN115933637A (en) | Path planning method and device for substation equipment inspection robot and storage medium | |
| Sartori et al. | Near-optimal 3d trajectory design in presence of obstacles: a convolutional neural network approach | |
| CN114610027A (en) | Ship navigation path planning method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |