CN110617819A - Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning - Google Patents

Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning Download PDF

Info

Publication number
CN110617819A
CN110617819A CN201910988280.4A CN201910988280A CN110617819A CN 110617819 A CN110617819 A CN 110617819A CN 201910988280 A CN201910988280 A CN 201910988280A CN 110617819 A CN110617819 A CN 110617819A
Authority
CN
China
Prior art keywords
terrain
grid
path
ants
ant
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.)
Granted
Application number
CN201910988280.4A
Other languages
Chinese (zh)
Other versions
CN110617819B (en
Inventor
汤郡郡
单奕萌
李金猛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
State Run Wuhu Machinery Factory
Original Assignee
State Run Wuhu Machinery Factory
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by State Run Wuhu Machinery Factory filed Critical State Run Wuhu Machinery Factory
Priority to CN201910988280.4A priority Critical patent/CN110617819B/en
Publication of CN110617819A publication Critical patent/CN110617819A/en
Application granted granted Critical
Publication of CN110617819B publication Critical patent/CN110617819B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

The invention relates to the field of unmanned aerial vehicle terrain aided navigation methods, in particular to an unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning, which comprises the following specific steps: s1: loading terrain elevation data; s2: judging whether the landforms can be matched or not according to the landform information quantity; s3: importing an adaptive map and setting initial parameters; s4: planning a path according to an ant colony algorithm; s5: correcting the position information output by the inertial navigation system by using the position information obtained by ICCP matching; aiming at the problem of inertial navigation error accumulation along with time, a terrain-assisted navigation method based on an ICCP algorithm is adopted to correct the inertial navigation error so as to meet the high-precision positioning requirement of the unmanned aerial vehicle during long-term navigation; aiming at the problem that the inertial navigation accumulated error can not be effectively corrected in an area with unobvious terrain change by terrain aided navigation, the terrain is divided into a terrain adaptation area and a terrain non-adaptation area by calculating the terrain information quantity of a navigation area and utilizing an entropy method to give a weight grey correlation decision.

Description

Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning
Technical Field
The invention relates to the field of unmanned aerial vehicle terrain auxiliary navigation methods, in particular to an unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning.
Background
The inertial navigation system does not need any external information or radiate any information outwards, can continuously position and navigate in the global range and any medium environment in all weather conditions only by depending on the inertial navigation system, and has the unique advantages of autonomy, concealment and carrier complete information acquisition, which are incomparable with other navigation systems. However, the inertial navigation system has a principle defect that system errors are accumulated over time, and in order to achieve a high-precision navigation target when the unmanned aerial vehicle is long-endurance, it is necessary to periodically adjust and correct the navigation target by using external position information.
The terrain aided navigation is a method for carrying out aided positioning by using the terrain elevation characteristics, has the advantages of autonomous, concealed, continuous and all-weather work, no accumulated navigation positioning errors along with time and the like, and is an ideal aided navigation positioning means. However, terrain assisted navigation requires significant changes in terrain elevation, and it is not feasible to reduce the positioning error of the inertial navigation system by using a terrain assisted navigation method for an area with too smooth terrain changes and insignificant terrain features.
The main characteristic parameters for measuring the terrain information quantity comprise terrain standard deviation, terrain correlation coefficient, terrain roughness, terrain entropy and the like. Based on the characteristic parameters, the terrain can be effectively divided into a terrain adaptive area and a terrain non-adaptive area.
Modern intelligent algorithms applied to path planning mainly include genetic algorithms, particle swarm algorithms, ant colony algorithms and the like. Compared with the particle swarm algorithm, the ant colony algorithm has stronger capability of searching global optimum; the ant colony algorithm adopts a positive feedback mechanism and a heuristic greedy strategy to ensure that the search time is obviously shorter than that of the genetic algorithm; meanwhile, the ant colony algorithm is simple in environment modeling and implementation, and does not need a complex encoding mechanism of a genetic algorithm and a particle swarm algorithm. At present, the research and the application of the ant colony algorithm are more mature and extensive, and the selection and the determination of the parameters are supported by more documents and theories.
In conclusion, according to the terrain division condition, the path is planned by combining the ant colony algorithm, so that the unmanned aerial vehicle flies through the terrain adaptation area at intervals, the inertial navigation error is corrected, and the method has very important practical significance for really realizing high-precision navigation and positioning in long-endurance.
Disclosure of Invention
In order to solve the problems, the invention provides an unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning.
An unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning comprises the following specific steps:
s1: loading terrain elevation data;
s2: judging whether the terrain can be matched according to the terrain information quantity:
s2.1: dividing a terrain area in path planning into L candidate areas, wherein the value of L is determined according to matching positioning precision and the storage capacity of a carrier computer;
s2.2: determining a terrain adaptation area and a terrain non-adaptation area according to the positioning precision requirement of the unmanned aerial vehicle;
s2.3: and (3) combining the adaptation zone division results, and planning a navigation path based on an ant colony algorithm:
(1) if the terrain where the grid is located is a terrain adaptation area, setting 0, and setting a non-adaptation area;
(2) a reachability matrix, wherein if grids i to j are reachable, a distance is recorded by LEN (i, j), and if not reachable, LEN (i, j) is 0; the method comprises the following steps of (1) defining the grids which can be reached by each grid as eight grids which are adjacent around the grid, and eight grids with the directions of up, down, left, right, left up, left down, right up and right down, wherein if the adaptive matrix is an N-dimensional matrix, the reachability matrix is an N multiplied by N square matrix;
s3: importing an adaptive map, and setting initial parameters: setting a starting point grid of path planning as S, a terminal point grid as E, a wave number of the moving ant colony as K, and the number of ants moving per wave as M; setting an pheromone elicitation factor as alpha, a self-elicitation factor as beta, an pheromone evaporation coefficient as rho, an pheromone enhancement coefficient as Q, an initial pheromone concentration of an pheromone matrix corresponding to the reachability matrix as c, and c being a constant, recording the traveling path of each ant in each wave of ants by using ROUTES, and recording the traveling path length of each ant in each wave of ants by using PL;
s4: planning the path according to the ant colony algorithm:
a: firstly, judging whether the current grid is a terminal grid, if so, terminating the path searching of the ant, and starting the next ant to search the path;
b: if not, selecting the next grid which can go forward according to a rotation betting method, and solving the probability of each grid which can be moved by the ants in the next step through the rotation betting method;
c: suppose that the next step ant can enter the grid is g1，g2，g3]Calculating the probability of entering each grid as [ xi ] according to a formula1，ξ2，ξ3](0≤ξ1，ξ2，ξ31 or less and xi1+ξ2+ξ31), the betting round is performed as follows: firstly, making cumulative probability statistics for each grid to obtain [ xi1ξ1+ξ2ξ1+ξ2+ξ3]＝[ξ1ξ1+ξ2]Then, a random number between 0 and 1 is generated, if the generated random number is between 0 and xi1While ants go to the grid g1Go forward if located in xi1And xi1+ξ2While ants go to the grid g2Go forward if located in xi1+ξ2And between 1, ants go to grid g3Advancing;
d: updating the path and the path length;
e: repeating the steps a and d until the ants reach the end point or sink into the dead zone, and if the ants do not reach the end point, the path length is 0;
f: repeating the steps a to e until all the ants of the wave reach the end point or fall into a dead zone;
g: updating the pheromone matrix: after all ants in the wave complete path selection, if the ants reach the end point, the pheromone is updated;
h: if all the wave-time ants have already carried out path searching, outputting the shortest paths and path lengths in all the paths, and finishing path searching, otherwise, returning to execute the step a;
i: according to the path planned in the step S2.3, completing terrain matching by adopting an isoline closest point Iteration (ICCP) algorithm, and correcting the inertial navigation position error;
j: acquiring an inertial navigation indication sequence and carrying out initial transformation:
performing initial transformation by random rotation and translation, wherein the rotation and translation values are randomly selected within 3 times of the error variance of the inertial navigation system, and the rotation offset is taken as thetarandTranslation in the latitudinal direction of tL_randRandom translation in the longitudinal direction of tλ_randObtaining an initially transformed sequence Pirand；
k: extracting an elevation contour line, searching rigid transformation to obtain a closest point:
obtaining the position sequence P after initial transformation through an airborne barometric altimeter and a radio altimeterirandCorresponding elevation value HiAnd extracting the corresponding elevation contour C from the known digital mapi(ii) a Suppose PirandThe closest point to the corresponding contour is YiSearching a rigid transformation T containing a rotation matrix R and a translation vector T to obtain a minimum objective function d;
l: the rigid transformation is iteratively performed until convergence:
from the rigid transformation T obtained in step 3.2, for PirandObtaining P by applying rigid transformationirand＝T·Pirand(ii) a At this time, if the iteration number k is greater than the maximum iteration number kmaxIf the convergence rate is too low, discarding the iterative result and returning to execute the step j; if the iteration number k is less than the maximum iteration number kmaxAnd | dk-dk-1|>τ, returning to step 3.2; if the iteration number k is less than the maximum iteration number kmaxAnd | dk-dk-1If L is less than or equal to tau, the iteration is terminated, and the final matching result is determined to be LICCPλICCP]T；
S5: and correcting the output position information of the inertial navigation system by using the position information obtained by ICCP matching:
position information L output by inertial navigation systemSINS、λSINSLocation information obtained by matching with ICCPMessage LICCP、λICCPDifference value L ofSINS-LICCP、λSINS-λICCPAnd performing Kalman filtering as observed quantity, and feeding back position information obtained by filtering to an inertial navigation system to correct the position of inertial navigation output.
The terrain elevations in step S1 are stored in a grid matrix manner.
In S2.2 of step S2, it is assumed that the longitude and latitude span of a certain terrain is an m × n grid, the terrain elevation value at the grid point coordinate (i, j) is h (i, j), and i is 1, 2, …, m, j is 1, 2, …, n; the main characteristic parameters capable of calculating the terrain information quantity comprise terrain standard deviation sigma, terrain correlation coefficient R, terrain roughness R and terrain height entropy HfThey are specifically defined as follows:
wherein the content of the first and second substances,
the terrain elevation average value is taken;
is a longitude direction correlation coefficient;
is weftDegree direction correlation coefficient;
roughness in the longitudinal direction;
the roughness in the latitudinal direction;
is a normalized elevation value.
And S2.2 of the step S2, dividing the terrain by an entropy weighted gray correlation decision method.
Step S3 is to place ants on the starting grid S and add the starting grid to the TABU table, wherein the TABU table is the TABU table, and the TABU table is 1 row N when the terrain grid is N-dimensional square2The column matrix is used for indicating whether a certain grid has passed through, if the certain grid has passed through, the column corresponding to the grid serial number is set to be 0, and in order to prevent the ant from backtracking, the taboo table needs to be dynamically adjusted according to the path passed by the ant.
In the step S4, the ant selects the next marching grid to calculate by using the formula (5):
wherein allowed represents the grid without tabu table passing, taui，jFor the pheromone concentration, η, on the path from grid i to grid ji，gBeing a self-inspiring function, ηi，g＝1/dig，digRepresenting the distance from grid i to the target grid;
the g pheromone in the step S4 is updated according to the formula (6), and the pheromone concentration on the path gradually evaporates with the passage of time:
τi，j←(1-ρ)·τi，j+Δτi，j (6)
wherein: delta taui，jFor the added pheromone portion, Q is the pheromone enhancement factor, and L is the path length of the ant to the end point.
The j pairs of inertial navigation system indication track sequences P in the step S4iRandom rotation and translation according to the formula (8) to obtain the initially transformed sequence Pirand：
K in step S4 is expressed by equation (11) to obtain a minimum objective function d:
where k denotes the number of iterations, D (P)irand，Yi) Represents PirandAnd distance between Y, DmaxRepresents PirandAnd the maximum value of the distance between Y.
The invention has the beneficial effects that: aiming at the problem of inertial navigation error accumulation along with time, a terrain-assisted navigation method based on an ICCP algorithm is adopted to correct the inertial navigation error so as to meet the high-precision positioning requirement of the unmanned aerial vehicle during long-term navigation; aiming at the problem that inertial navigation accumulated errors cannot be effectively corrected in an area with unobvious terrain change by terrain-assisted navigation, the terrain is divided into a terrain adaptation area and a terrain non-adaptation area by calculating terrain information quantity of a navigation area and utilizing an entropy method weighted gray correlation decision; aiming at the problem that the unmanned aerial vehicle cannot accurately pass through the terrain adaptation area, the path planning method based on the ant colony algorithm is adopted to plan the traveling path based on the terrain adaptation area, and the effective correction of the terrain-assisted navigation to the inertial navigation error in the whole process is ensured.
Drawings
The invention is further illustrated with reference to the following figures and examples.
FIG. 1 is a schematic view of the flow structure of the present invention;
FIG. 2 is a schematic diagram of a regular grid topographic structure of the present invention;
FIG. 3 is a schematic diagram of an adaptive matrix structure according to the present invention;
FIG. 4 is a schematic diagram of a reachability schematic of the present invention;
FIG. 5 is a schematic structural diagram of a three-dimensional topographic map of the present invention;
FIG. 6 is a schematic view of a terrain adaptation zone of the present invention;
FIG. 7 shows the optimal path for the 1 st, 2 nd, 3 rd, 4 th and 5 th wave ants according to the present invention;
fig. 8 shows the optimal path for 10 th, 20 th, 50 th, 100 th, 300 th and 500 th wave ants in accordance with the present invention;
fig. 9 is a diagram of the planned path matching result of the present invention.
Detailed Description
In order to make the technical means, the creation characteristics, the achievement purposes and the effects of the invention easy to understand, the invention is further explained below.
As shown in fig. 1 to 9, an unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning includes the following specific steps:
s1: loading terrain elevation data: as shown in fig. 5, the maximum elevation value of the area is 251.3940m, the minimum elevation value is 182.0500m, the average elevation value is 223.5656m, the longitude and latitude start from (118 ° E, 38 ° N), the grid number is 44 × 44, the grid spacing is 0.00125 °, and 139 m;
s2: judging whether the terrain can be matched according to the terrain information quantity:
s2.1: dividing a terrain area in path planning into L candidate areas, wherein the value of L is determined according to matching positioning precision and the storage capacity of a carrier computer;
s2.2: determining a terrain adaptation area and a terrain non-adaptation area according to the positioning precision requirement of the unmanned aerial vehicle;
s2.3: and planning a navigation path based on an ant colony algorithm by combining with the adaptation zone division result, wherein a terrain adaptation zone schematic diagram is shown in fig. 6, and a shadow part is a terrain non-adaptation zone:
(1) an adaptive matrix, which is set to 0 if the terrain where the grid is located is a terrain adaptive area, and set to a non-adaptive area, as shown in fig. 2, the terrain is a regular grid terrain, the shaded part represents the terrain non-adaptive area, and fig. 4 is the adaptive matrix of the area;
(2) a reachability matrix, wherein if grids i to j are reachable, a distance is recorded by LEN (i, j), and if not reachable, LEN (i, j) is 0; defining the grids which can be reached by each grid as eight grids adjacent to the periphery of the grid, and eight grids in the directions of upper, lower, left, right, upper left, lower left, upper right and lower right, as shown in fig. 3, the reachability diagram corresponds to each grid in fig. 1, the arrows represent the grids which can be advanced, and if the adaptive matrix is an N-dimensional square matrix, the reachability matrix is an N × N square matrix;
s3: importing an adaptive map, and setting initial parameters: setting a starting point grid of path planning, namely an initial point (118.01 degrees E, 38.00875 degrees N), a terminal point (118.0475 degrees E, 38.04625 degrees N), setting the wave number K of the ant colony to be 500 times, and setting the number M of ants in each wave to be 50; setting pheromone elicitation factor alpha to be 1, self-elicitation factor beta to be 7, pheromone evaporation coefficient rho to be 0.3 and pheromone enhancement coefficient Q to be 1, setting initial pheromone concentration c of the pheromone matrix corresponding to the reachability matrix to be 1, c to be a constant, recording the traveling path of each ant in each wave of ants by using ROUTES, and recording the traveling path length of each ant in each wave of ants by using PL;
according to the parameter setting, the optimal paths found by the 1 st, 2 nd, 3 th, 4 th and 5 th wave ant ants are shown in fig. 7, and the optimal paths calculated by the 10 th, 20 th, 50 th, 100 th, 300 th and 500 th wave ant colony algorithm are shown in fig. 8;
the path planned by the 500 th generation of ants in fig. 8 is used as the real track of the unmanned aerial vehicle, the track matching is performed by using the ICCP algorithm, and the matching result is shown in fig. 8:
as shown in fig. 9, the statistical results of the matching errors are shown in table 1:
TABLE 1 match error statistics
As can be seen from the matching results: the unmanned aerial vehicle terrain auxiliary navigation based on ant colony algorithm path planning is effective in unmanned aerial vehicle terrain auxiliary navigation application, when the unmanned aerial vehicle navigates along the planned path, higher positioning precision can be obtained by using the terrain auxiliary navigation based on ICCP algorithm, and high-precision navigation positioning of the unmanned aerial vehicle during long-term navigation is realized;
s4: planning the path according to the ant colony algorithm:
a: firstly, judging whether the current grid is a terminal grid, if so, terminating the path searching of the ant, and starting the next ant to search the path;
b: if not, selecting the next grid which can go forward according to a rotation betting method, and solving the probability of each grid which can be moved by the ants in the next step through the rotation betting method;
c: suppose that the next step ant can enter the grid is g1，g2，g3]Calculating the probability of entering each grid as [ xi ] according to a formula1，ξ2，ξ3](0≤ξ1，ξ2，ξ31 or less and xi1+ξ2+ξ31), the betting round is performed as follows: firstly, making cumulative probability statistics for each grid to obtain [ xi1ξ1+ξ2ξ1+ξ2+ξ3]＝[ξ1ξ1+ξ2]Then, a random number between 0 and 1 is generated, if the generated random number is between 0 and xi1While ants go to the grid g1Go forward if located in xi1And xi1+ξ2While ants go to the grid g2Go forward if located in xi1+ξ2And between 1, ants go to grid g3Advancing;
d: updating the path and the path length;
e: repeating the steps a and d until the ants reach the end point or sink into the dead zone, and if the ants do not reach the end point, the path length is 0;
f: repeating the steps a to e until all the ants of the wave reach the end point or fall into a dead zone;
g: updating the pheromone matrix: after all ants in the wave complete path selection, if the ants reach the end point, the pheromone is updated;
h: if all the wave-time ants have already carried out path searching, outputting the shortest paths and path lengths in all the paths, and finishing path searching, otherwise, returning to execute the step a;
i: according to the path planned in the step S2.3, completing terrain matching by adopting an isoline closest point Iteration (ICCP) algorithm, and correcting the inertial navigation position error;
j: acquiring an inertial navigation indication sequence and carrying out initial transformation:
performing initial transformation by random rotation and translation, wherein the rotation and translation values are randomly selected within 3 times of the error variance of the inertial navigation system, and the rotation offset is taken as thetarandTranslation in the latitudinal direction of tL_randRandom translation in the longitudinal direction of tλ_randObtaining an initially transformed sequence Pirand；
k: extracting an elevation contour line, searching rigid transformation to obtain a closest point:
obtaining the position sequence P after initial transformation through an airborne barometric altimeter and a radio altimeterirandCorresponding elevation value HiAnd extracting the corresponding elevation contour C from the known digital mapi(ii) a Suppose PirandThe closest point to the corresponding contour is YiFinding a rigid transformation T comprising a rotation matrix R and a translation vector T to obtainA minimum objective function d;
l: the rigid transformation is iteratively performed until convergence:
from the rigid transformation T obtained in step 3.2, for PirandObtaining P by applying rigid transformationirand＝T·Pirand(ii) a At this time, if the iteration number k is greater than the maximum iteration number kmaxIf the convergence rate is too low, discarding the iterative result and returning to execute the step j; if the iteration number k is less than the maximum iteration number kmaxAnd | dk-dk-1|>τ, returning to step 3.2; if the iteration number k is less than the maximum iteration number kmaxAnd | dk-dk-1If L is less than or equal to tau, the iteration is terminated, and the final matching result is determined to be LICCPλICCP]T；
S5: and correcting the output position information of the inertial navigation system by using the position information obtained by ICCP matching:
position information L output by inertial navigation systemSINS、λSINSPosition information L obtained by matching with ICCPICCP、λICCPDifference value L ofSINS-LICCP、λSINS-λICCPAnd performing Kalman filtering as observed quantity, and feeding back position information obtained by filtering to an inertial navigation system to correct the position of inertial navigation output.
The terrain elevations in step S1 are stored in a grid matrix manner.
In S2.2 of step S2, it is assumed that the longitude and latitude span of a certain terrain is an m × n grid, the terrain elevation value at the grid point coordinate (i, j) is h (i, j), and i is 1, 2, …, m, j is 1, 2, …, n; the main characteristic parameters capable of calculating the terrain information quantity comprise terrain standard deviation sigma, terrain correlation coefficient R, terrain roughness R and terrain height entropy HfThey are specifically defined as follows:
wherein the content of the first and second substances,
the terrain elevation average value is taken;
is a longitude direction correlation coefficient;
is a latitude direction correlation coefficient;
roughness in the longitudinal direction;
the roughness in the latitudinal direction;
is a normalized elevation value.
Aiming at the problem of inertial navigation error accumulation along with time, a terrain-assisted navigation method based on an ICCP algorithm is adopted to correct the inertial navigation error so as to meet the high-precision positioning requirement of the unmanned aerial vehicle during long-term navigation; aiming at the problem that inertial navigation accumulated errors cannot be effectively corrected in an area with unobvious terrain change by terrain-assisted navigation, the terrain is divided into a terrain adaptation area and a terrain non-adaptation area by calculating terrain information quantity of a navigation area and utilizing an entropy method weighted gray correlation decision; aiming at the problem that the unmanned aerial vehicle cannot accurately pass through the terrain adaptation area, the path planning method based on the ant colony algorithm is adopted to plan the traveling path based on the terrain adaptation area, and the effective correction of the terrain-assisted navigation to the inertial navigation error in the whole process is ensured.
In step S2, S2.2, the terrain is divided according to the entropy weighted gray-related decision method in the "terrain-aided navigation adaptive area selection based on entropy weighted gray-related decision" in the reference, as shown in fig. 6, which is a schematic diagram of a terrain adaptive area, where a shaded portion is a terrain non-adaptive area.
Step S3 is to place ants on the starting grid S and add the starting grid to the TABU table, wherein the TABU table is the TABU table, and the TABU table is 1 row N when the terrain grid is N-dimensional square2The column matrix is used for indicating whether a certain grid has passed through, if the certain grid has passed through, the column corresponding to the grid serial number is set to be 0, and in order to prevent the ant from backtracking, the taboo table needs to be dynamically adjusted according to the path passed by the ant.
In the step S4, the ant selects the next marching grid to calculate by using the formula (5):
wherein allowed represents the grid without tabu table passing, taui，jFor the pheromone concentration, η, on the path from grid i to grid ji，gBeing a self-inspiring function, ηi，g＝1/dig，digRepresenting the distance from grid i to the target grid;
the g pheromone in the step S4 is updated according to the formula (6), and the pheromone concentration on the path gradually evaporates with the passage of time:
τi，j←(1-ρ)·τi，j+Δτi，j (6)
wherein: delta taui，jFor the added pheromone portion, Q is the pheromone enhancement factor, and L is the path length of the ant to the end point.
The j pairs of inertial navigation system indication track sequences P in the step S4iRandom rotation and translation according to the formula (8) to obtain the initially transformed sequence Pirand：
K in step S4 is expressed by equation (11) to obtain a minimum objective function d:
where k denotes the number of iterations, D (P)irand，Yi) Represents PirandAnd distance between Y, DmaxRepresents PirandAnd the maximum value of the distance between Y.
The foregoing shows and describes the general principles, essential features, and advantages of the invention. It will be understood by those skilled in the art that the present invention is not limited to the embodiments described above, which are merely illustrative of the principles of the invention, but that various changes and modifications may be made without departing from the spirit and scope of the invention, which fall within the scope of the invention as claimed. The scope of the invention is defined by the appended claims and equivalents thereof.

Claims (9)

1. An unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning is characterized in that: the method comprises the following specific steps:
s1: loading terrain elevation data;
s2: judging whether the terrain can be matched according to the terrain information quantity:
s2.1: dividing a terrain area in path planning into L candidate areas, wherein the value of L is determined according to matching positioning precision and the storage capacity of a carrier computer;
s2.2: determining a terrain adaptation area and a terrain non-adaptation area according to the positioning precision requirement of the unmanned aerial vehicle;
s2.3: and (3) combining the adaptation zone division results, and planning a navigation path based on an ant colony algorithm:
(1) if the terrain where the grid is located is a terrain adaptation area, setting 0, and setting a non-adaptation area;
(2) a reachability matrix, wherein if grids i to j are reachable, a distance is recorded by LEN (i, j), and if not reachable, LEN (i, j) is 0; the method comprises the following steps of (1) defining the grids which can be reached by each grid as eight grids which are adjacent around the grid, and eight grids with the directions of up, down, left, right, left up, left down, right up and right down, wherein if the adaptive matrix is an N-dimensional matrix, the reachability matrix is an N multiplied by N square matrix;
s3: importing an adaptive map, and setting initial parameters: setting a starting point grid of path planning as S, a terminal point grid as E, a wave number of the moving ant colony as K, and the number of ants moving per wave as M; setting an pheromone elicitation factor as alpha, a self-elicitation factor as beta, an pheromone evaporation coefficient as rho, an pheromone enhancement coefficient as Q, an initial pheromone concentration of an pheromone matrix corresponding to the reachability matrix as c, and c being a constant, recording the traveling path of each ant in each wave of ants by using ROUTES, and recording the traveling path length of each ant in each wave of ants by using PL;
s4: planning the path according to the ant colony algorithm:
a: firstly, judging whether the current grid is a terminal grid, if so, terminating the path searching of the ant, and starting the next ant to search the path;
b: if not, selecting the next grid which can go forward according to a rotation betting method, and solving the probability of each grid which can be moved by the ants in the next step through the rotation betting method;
c: suppose that the next step ant can enter the grid is g1，g2，g3]Calculating the probability of entering each grid as [ xi ] according to a formula1，ξ2，ξ3](0≤ξ1，ξ2，ξ31 or less and xi1+ξ2+ξ31), the betting round is performed as follows: firstly, making cumulative probability statistics for each grid to obtain [ xi1ξ1+ξ2ξ1+ξ2+ξ3]＝[ξ1ξ1+ξ2]Then, a random number between 0 and 1 is generated, if the generated random number is between 0 and xi1While ants go to the grid g1Go forward if located in xi1And xi1+ξ2While ants go to the grid g2Go forward if located in xi1+ξ2And between 1, ants go to grid g3Advancing;
d: updating the path and the path length;
e: repeating the steps a and d until the ants reach the end point or sink into the dead zone, and if the ants do not reach the end point, the path length is 0;
f: repeating the steps a to e until all the ants of the wave reach the end point or fall into a dead zone;
g: updating the pheromone matrix: after all ants in the wave complete path selection, if the ants reach the end point, the pheromone is updated;
h: if all the wave-time ants have already carried out path searching, outputting the shortest paths and path lengths in all the paths, and finishing path searching, otherwise, returning to execute the step a;
i: according to the path planned in the step S2.3, completing terrain matching by adopting an isoline closest point iterative ICCP algorithm, and correcting the inertial navigation position error;
j: acquiring an inertial navigation indication sequence and carrying out initial transformation:
performing initial transformation by random rotation and translation, wherein the rotation and translation values are randomly selected within 3 times of the error variance of the inertial navigation system, and the rotation offset is taken as thetarandTranslation in the latitudinal direction of tL_randRandom translation in the longitudinal direction of tλ_randObtaining an initially transformed sequence Pirand；
k: extracting an elevation contour line, searching rigid transformation to obtain a closest point:
obtaining the position sequence P after initial transformation through an airborne barometric altimeter and a radio altimeterirandCorresponding elevation value HiAnd extracting the corresponding elevation contour C from the known digital mapi(ii) a Suppose PirandThe closest point to the corresponding contour is YiSearching a rigid transformation T containing a rotation matrix R and a translation vector T to obtain a minimum objective function d;
l: the rigid transformation is iteratively performed until convergence:
from the rigid transformation T obtained in step 3.2, for PirandObtaining P by applying rigid transformationirand＝T·Pirand(ii) a At this time, if the iteration number k is greater than the maximum iteration number kmaxIf the convergence rate is too low, discarding the iterative result and returning to execute the step j; if the iteration number k is less than the maximum iteration number kmaxAnd | dk-dk-1|>τ, returning to step 3.2; if the iteration number k is less than the maximum iteration number kmaxAnd | dk-dk-1If L is less than or equal to tau, the iteration is terminated, and the final matching result is determined to be LICCP λICCP]T；
S5: and correcting the output position information of the inertial navigation system by using the position information obtained by ICCP matching:
position information L output by inertial navigation systemSINS、λSINSPosition information L obtained by matching with ICCPICCP、λICCPDifference value L ofSINS-LICCP、λSINS-λICCPPerforming Kalman filtering as observed quantity, and usingAnd feeding back the position information obtained by filtering to an inertial navigation system to correct the position of inertial navigation output.
2. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: the terrain elevations in step S1 are stored in a grid matrix manner.
3. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: in S2.2 of step S2, it is assumed that the longitude and latitude span of a certain terrain is an m × n grid, the terrain elevation value at the grid point coordinate (i, j) is h (i, j), and i is 1, 2, …, m, j is 1, 2, …, n; the main characteristic parameters capable of calculating the terrain information quantity comprise terrain standard deviation sigma, terrain correlation coefficient R, terrain roughness R and terrain height entropy HfThey are specifically defined as follows:
wherein the content of the first and second substances,
the terrain elevation average value is taken;
is a longitude direction correlation coefficient;
is a latitude direction correlation coefficient;
roughness in the longitudinal direction;
the roughness in the latitudinal direction;
is a normalized elevation value.
4. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: and S2.2 of the step S2, dividing the terrain by an entropy weighted gray correlation decision method.
5. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: step S3 is to place ants on the starting grid S and add the starting grid to the TABU table, wherein the TABU table is the TABU table, and the TABU table is 1 row N when the terrain grid is N-dimensional square2The column matrix is used for indicating whether a certain grid has passed through, if the certain grid has passed through, the column corresponding to the grid serial number is set to be 0, and in order to prevent the ant from backtracking, the taboo table needs to be dynamically adjusted according to the path passed by the ant.
6. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: in the step S4, the ant selects the next marching grid to calculate by using the formula (5):
wherein allowed represents the grid without tabu table passing, taui，jFor the pheromone concentration, η, on the path from grid i to grid ji，gBeing a self-inspiring function, ηi，g＝1/dig，digRepresenting the distance of grid i from the target grid.
7. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: the g pheromone in the step S4 is updated according to the formula (6), and the pheromone concentration on the path gradually evaporates with the passage of time:
τi，j←(1-ρ)·τi，j+Δτi，j (6)
wherein: delta taui，jFor the added pheromone portion, Q is the pheromone enhancement factor, and L is the path length of the ant to the end point.
8. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: the j pairs of inertial navigation system indication track sequences P in the step S4iRandom rotation and translation according to the formula (8) to obtain the initially transformed sequence Pirand：
9. The unmanned aerial vehicle terrain aided navigation method based on ant colony algorithm path planning as claimed in claim 1, wherein: k in step S4 is expressed by equation (11) to obtain a minimum objective function d:
where k denotes the number of iterations, D (P)irand，Yi) Represents PirandAnd distance between Y, DmaxRepresents PirandAnd the maximum value of the distance between Y.
CN201910988280.4A 2019-10-17 2019-10-17 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning Active CN110617819B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910988280.4A CN110617819B (en) 2019-10-17 2019-10-17 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910988280.4A CN110617819B (en) 2019-10-17 2019-10-17 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning

Publications (2)

Publication Number Publication Date
CN110617819A true CN110617819A (en) 2019-12-27
CN110617819B CN110617819B (en) 2022-09-30

Family

ID=68925933

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910988280.4A Active CN110617819B (en) 2019-10-17 2019-10-17 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning

Country Status (1)

Country Link
CN (1) CN110617819B (en)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111252212A (en) * 2020-01-15 2020-06-09 大连海事大学 Automatic rescue method and system for multiple drowning people by cooperation of navigable lifesaving device and unmanned aerial vehicle
CN111896002A (en) * 2020-07-22 2020-11-06 北京航空航天大学 Method and system for online planning and adaptability analysis of terrain-assisted navigation matching area
CN113252039A (en) * 2021-05-06 2021-08-13 西北工业大学 Terrain-assisted navigation-oriented particle swarm fast matching method
CN113252038A (en) * 2021-05-06 2021-08-13 西北工业大学 Course planning terrain auxiliary navigation method based on particle swarm optimization
CN113532438A (en) * 2021-07-23 2021-10-22 东南大学 Improved ICCP terrain matching method under large initial positioning error
CN114812568A (en) * 2022-03-01 2022-07-29 航天科工智能运筹与信息安全研究院(武汉)有限公司 Route matching area selection method based on rule scoring

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0237714A1 (en) * 1986-02-14 1987-09-23 Messerschmitt-Bölkow-Blohm Gesellschaft mit beschränkter Haftung Low altitude flying method for automatically finding the flight path
CN101339036A (en) * 2008-08-20 2009-01-07 北京航空航天大学 Terrain auxiliary navigation method and apparatus
CN103743402A (en) * 2014-01-03 2014-04-23 东南大学 Underwater intelligent self-adapted terrain matching method based on terrain information amount
CN103822634A (en) * 2014-02-24 2014-05-28 北京理工大学 ICCP algorithm-based gravity matching aided inertial navigation method
CN104075715A (en) * 2014-07-07 2014-10-01 东南大学 Underwater navigation and positioning method capable of combining terrain and environment characteristics
US9104201B1 (en) * 2012-02-13 2015-08-11 C&P Technologies, Inc. Method and apparatus for dynamic swarming of airborne drones for a reconfigurable array
US9146557B1 (en) * 2014-04-23 2015-09-29 King Fahd University Of Petroleum And Minerals Adaptive control method for unmanned vehicle with slung load
CN106323293A (en) * 2016-10-14 2017-01-11 淮安信息职业技术学院 Multi-target searching-based two-colony multidirectional route planning method for robot
CN106643714A (en) * 2017-03-15 2017-05-10 北京航空航天大学 Autonomous real-time airborne terrain-aided inertial navigation method and system
US20180172821A1 (en) * 2016-12-16 2018-06-21 The Government Of The United States Of America, As Represented By The Secretary Of The Navy Millimeter-Wave Terrain Aided Navigation System
CN109000656A (en) * 2018-06-15 2018-12-14 淮海工学院 Underwater terrain matching navigation adaptation area's selection method based on space clustering
CN109165891A (en) * 2018-07-28 2019-01-08 国营芜湖机械厂 A kind of aeronautical maintenance production scene material management system and its application method
CN110108284A (en) * 2019-05-24 2019-08-09 西南交通大学 A kind of quick planing method of no-manned plane three-dimensional track for taking complex environment constraint into account

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0237714A1 (en) * 1986-02-14 1987-09-23 Messerschmitt-Bölkow-Blohm Gesellschaft mit beschränkter Haftung Low altitude flying method for automatically finding the flight path
CN101339036A (en) * 2008-08-20 2009-01-07 北京航空航天大学 Terrain auxiliary navigation method and apparatus
US9104201B1 (en) * 2012-02-13 2015-08-11 C&P Technologies, Inc. Method and apparatus for dynamic swarming of airborne drones for a reconfigurable array
CN103743402A (en) * 2014-01-03 2014-04-23 东南大学 Underwater intelligent self-adapted terrain matching method based on terrain information amount
CN103822634A (en) * 2014-02-24 2014-05-28 北京理工大学 ICCP algorithm-based gravity matching aided inertial navigation method
US9146557B1 (en) * 2014-04-23 2015-09-29 King Fahd University Of Petroleum And Minerals Adaptive control method for unmanned vehicle with slung load
CN104075715A (en) * 2014-07-07 2014-10-01 东南大学 Underwater navigation and positioning method capable of combining terrain and environment characteristics
CN106323293A (en) * 2016-10-14 2017-01-11 淮安信息职业技术学院 Multi-target searching-based two-colony multidirectional route planning method for robot
US20180172821A1 (en) * 2016-12-16 2018-06-21 The Government Of The United States Of America, As Represented By The Secretary Of The Navy Millimeter-Wave Terrain Aided Navigation System
CN106643714A (en) * 2017-03-15 2017-05-10 北京航空航天大学 Autonomous real-time airborne terrain-aided inertial navigation method and system
CN109000656A (en) * 2018-06-15 2018-12-14 淮海工学院 Underwater terrain matching navigation adaptation area's selection method based on space clustering
CN109165891A (en) * 2018-07-28 2019-01-08 国营芜湖机械厂 A kind of aeronautical maintenance production scene material management system and its application method
CN110108284A (en) * 2019-05-24 2019-08-09 西南交通大学 A kind of quick planing method of no-manned plane three-dimensional track for taking complex environment constraint into account

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
ZEFANG HE; LONG ZHAO: "The Comparison of Four UAV Path Planning Algorithms Based on Geometry Search Algorithm", 《2017 9TH INTERNATIONAL CONFERENCE ON INTELLIGENT HUMAN-MACHINE SYSTEMS AND CYBERNETICS》 *
张毅, 高永琪, 牛兴江: "基于蚁群优化算法的水下航路规划", 《鱼雷技术》 *
赵丹等: "基于蚁群-粒子群融合算法的无人机三维航迹规划研究", 《吉林化工学院学报》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111252212A (en) * 2020-01-15 2020-06-09 大连海事大学 Automatic rescue method and system for multiple drowning people by cooperation of navigable lifesaving device and unmanned aerial vehicle
CN111252212B (en) * 2020-01-15 2022-03-22 大连海事大学 Automatic rescue method and system for multiple drowning people by cooperation of navigable lifesaving device and unmanned aerial vehicle
CN111896002A (en) * 2020-07-22 2020-11-06 北京航空航天大学 Method and system for online planning and adaptability analysis of terrain-assisted navigation matching area
CN111896002B (en) * 2020-07-22 2023-05-12 北京航空航天大学 Terrain-assisted navigation matching area online planning and suitability analysis method and system
CN113252039A (en) * 2021-05-06 2021-08-13 西北工业大学 Terrain-assisted navigation-oriented particle swarm fast matching method
CN113252038A (en) * 2021-05-06 2021-08-13 西北工业大学 Course planning terrain auxiliary navigation method based on particle swarm optimization
CN113252038B (en) * 2021-05-06 2022-10-28 西北工业大学 Course planning terrain auxiliary navigation method based on particle swarm optimization
CN113532438A (en) * 2021-07-23 2021-10-22 东南大学 Improved ICCP terrain matching method under large initial positioning error
CN113532438B (en) * 2021-07-23 2023-12-05 东南大学 Improved ICCP terrain matching method under large initial positioning error
CN114812568A (en) * 2022-03-01 2022-07-29 航天科工智能运筹与信息安全研究院(武汉)有限公司 Route matching area selection method based on rule scoring
CN114812568B (en) * 2022-03-01 2024-05-28 航天科工智能运筹与信息安全研究院(武汉)有限公司 Track matching area selection method based on rule grading

Also Published As

Publication number Publication date
CN110617819B (en) 2022-09-30

Similar Documents

Publication Publication Date Title
CN110617819B (en) Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning
WO2018018994A1 (en) Method and system for indoor positioning
CN111024092B (en) Method for rapidly planning tracks of intelligent aircraft under multi-constraint conditions
Han et al. An improved TERCOM-based algorithm for gravity-aided navigation
US6259988B1 (en) Real-time mission adaptable route planner
CN111176807A (en) Multi-satellite cooperative task planning method
CN113252038B (en) Course planning terrain auxiliary navigation method based on particle swarm optimization
CN113961004A (en) Pirate area ship route planning method and system, electronic equipment and storage medium
CN111256696B (en) Aircraft autonomous navigation method with multi-feature and multi-level scene matching
CN112432649A (en) Heuristic unmanned aerial vehicle swarm flight path planning method introducing threat factors
CN110220510B (en) Underwater robot submarine topography matching navigation path planning method considering map accuracy
CN111307143B (en) Bionic navigation algorithm for multi-target evolution search based on geomagnetic gradient assistance
CN113048981B (en) DEM-oriented method for road-free area path planning algorithm
CN114563003A (en) Path planning method and device, electronic equipment and storage medium
CN111895995B (en) PSO-based aircraft formation multidimensional geomagnetic matching navigation method and system
CN110849355A (en) Bionic navigation method for geomagnetic multi-parameter multi-target rapid convergence
CN107966718B (en) Improved integer ambiguity searching method
CN110322061B (en) Multi-target observation track intelligent sensing method suitable for load staring imaging
CN115437386A (en) Unmanned vehicle route planning method based on air-ground information fusion
CN110763234B (en) Submarine topography matching navigation path planning method for underwater robot
CN113532438B (en) Improved ICCP terrain matching method under large initial positioning error
CN111121778A (en) Navigation system initialization method
CN114088098A (en) Auxiliary navigation path planning method for polar region underwater vehicle database
CN113325856B (en) UUV optimal operation path planning method based on countercurrent approximation strategy
CN109307513A (en) A kind of Real-time Road matching process and system based on driving recording

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