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 PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000006978 adaptation Effects 0.000 claims abstract description 27
- 230000003044 adaptive effect Effects 0.000 claims abstract description 18
- 241000257303 Hymenoptera Species 0.000 claims description 46
- 239000011159 matrix material Substances 0.000 claims description 36
- 239000003016 pheromone Substances 0.000 claims description 36
- 230000009466 transformation Effects 0.000 claims description 21
- 238000013519 translation Methods 0.000 claims description 18
- 238000001914 filtration Methods 0.000 claims description 6
- IYLGZMTXKJYONK-ACLXAEORSA-N (12s,15r)-15-hydroxy-11,16-dioxo-15,20-dihydrosenecionan-12-yl acetate Chemical compound O1C(=O)[C@](CC)(O)C[C@@H](C)[C@](C)(OC(C)=O)C(=O)OCC2=CCN3[C@H]2[C@H]1CC3 IYLGZMTXKJYONK-ACLXAEORSA-N 0.000 claims description 3
- YTAHJIFKAKIKAV-XNMGPUDCSA-N [(1R)-3-morpholin-4-yl-1-phenylpropyl] N-[(3S)-2-oxo-5-phenyl-1,3-dihydro-1,4-benzodiazepin-3-yl]carbamate Chemical compound O=C1[C@H](N=C(C2=C(N1)C=CC=C2)C1=CC=CC=C1)NC(O[C@H](CCN1CCOCC1)C1=CC=CC=C1)=O YTAHJIFKAKIKAV-XNMGPUDCSA-N 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 238000001704 evaporation Methods 0.000 claims description 3
- 230000008020 evaporation Effects 0.000 claims description 3
- IYLGZMTXKJYONK-UHFFFAOYSA-N ruwenine Natural products O1C(=O)C(CC)(O)CC(C)C(C)(OC(C)=O)C(=O)OCC2=CCN3C2C1CC3 IYLGZMTXKJYONK-UHFFFAOYSA-N 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 230000007774 longterm Effects 0.000 abstract description 4
- 238000009825 accumulation Methods 0.000 abstract description 3
- 230000008859 change Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 8
- 230000002068 genetic effect Effects 0.000 description 3
- 239000002245 particle Substances 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009024 positive feedback mechanism Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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
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.
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)
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 |
CN113252038A (en) * | 2021-05-06 | 2021-08-13 | 西北工业大学 | Course planning terrain auxiliary navigation method based on particle swarm optimization |
CN113252039A (en) * | 2021-05-06 | 2021-08-13 | 西北工业大学 | Terrain-assisted navigation-oriented particle swarm fast matching method |
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)
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 |
-
2019
- 2019-10-17 CN CN201910988280.4A patent/CN110617819B/en active Active
Patent Citations (13)
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)
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)
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 |
CN113252038A (en) * | 2021-05-06 | 2021-08-13 | 西北工业大学 | Course planning terrain auxiliary navigation method based on particle swarm optimization |
CN113252039A (en) * | 2021-05-06 | 2021-08-13 | 西北工业大学 | Terrain-assisted navigation-oriented particle swarm fast matching method |
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 | |
CN111024092B (en) | Method for rapidly planning tracks of intelligent aircraft under multi-constraint conditions | |
Hunter et al. | The path inference filter: model-based low-latency map matching of probe vehicle data | |
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 | |
CN113916243B (en) | Vehicle positioning method, device, equipment and storage medium for target scene area | |
CN113252038B (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
CN112432649A (en) | Heuristic unmanned aerial vehicle swarm flight path planning method introducing threat factors | |
CN111256696B (en) | Aircraft autonomous navigation method with multi-feature and multi-level scene matching | |
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 | |
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 | |
CN113532438B (en) | Improved ICCP terrain matching method under large initial positioning error | |
CN111121778A (en) | Navigation system initialization method | |
CN113325856B (en) | UUV optimal operation path planning method based on countercurrent approximation strategy | |
CN112484733B (en) | Reinforced learning indoor navigation method based on topological graph | |
CN109307513A (en) | A kind of Real-time Road matching process and system based on driving recording | |
CN110763234B (en) | Submarine topography matching navigation path planning method for underwater robot | |
CN116576868A (en) | Multi-sensor fusion accurate positioning and autonomous navigation 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 |