CN113252039B - Terrain-assisted navigation-oriented particle swarm fast matching method - Google Patents

Terrain-assisted navigation-oriented particle swarm fast matching method Download PDF

Info

Publication number
CN113252039B
CN113252039B CN202110492424.4A CN202110492424A CN113252039B CN 113252039 B CN113252039 B CN 113252039B CN 202110492424 A CN202110492424 A CN 202110492424A CN 113252039 B CN113252039 B CN 113252039B
Authority
CN
China
Prior art keywords
terrain
algorithm
value
particle
sins
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110492424.4A
Other languages
Chinese (zh)
Other versions
CN113252039A (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.)
Northwestern Polytechnical University
AVIC Chengdu Aircraft Design and Research Institute
Original Assignee
Northwestern Polytechnical University
AVIC Chengdu Aircraft Design and Research Institute
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 Northwestern Polytechnical University, AVIC Chengdu Aircraft Design and Research Institute filed Critical Northwestern Polytechnical University
Priority to CN202110492424.4A priority Critical patent/CN113252039B/en
Publication of CN113252039A publication Critical patent/CN113252039A/en
Application granted granted Critical
Publication of CN113252039B publication Critical patent/CN113252039B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • General Health & Medical Sciences (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Biophysics (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Computational Linguistics (AREA)
  • Biomedical Technology (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention relates to a terrain-assisted navigation-oriented particle swarm fast matching method, which is used for solving the technical problem of poor real-time performance of the existing terrain-assisted navigation method. According to the method, firstly, an environment model of an unmanned aerial vehicle task interval is established, a sparse A-x algorithm is used for flight path planning, an optimized particle swarm algorithm is adopted for terrain height matching on the basis, the distribution estimation idea is introduced into the algorithm on the basis of the particle swarm algorithm, the global optimization capability of the particle swarm optimization algorithm is enhanced, and the real-time performance of TERCOM is improved. And after obtaining the optimal TERCOM position, taking the difference value between the optimal TERCOM matching position and the position information output by the SINS as a measurement value, estimating the SINS error by using Kalman filtering, and correcting the SINS. The improved particle swarm algorithm is used for terrain height matching in terrain assisted navigation, the problem of poor real-time performance caused by the traversal mode of the traditional TERCOM is solved, and the real-time performance of the terrain assisted navigation is improved.

Description

Terrain-assisted navigation-oriented particle swarm fast matching method
Technical Field
The invention relates to a terrain aided navigation method, in particular to a terrain aided navigation-oriented particle swarm fast matching method, and belongs to the field of navigation, guidance and control.
Background
The Terrain Aided Navigation TAN (Terrain-Aided Navigation) technology is a combined Navigation system which is widely regarded and successfully used in recent years, and the Terrain Aided Navigation is a method for carrying out Aided positioning by using Terrain elevation features, 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 improvement of the matching efficiency is the key of terrain-assisted navigation, and the Particle Swarm Optimization-based new aircraft terrain matching algorithm (He Yanping, liu Xinxue, cai Yanping, li Yaxiong, zhu Yu, infrared and laser engineering, 2016, volume 45 in 5) introduces a Particle Swarm Optimization (PSO) Optimization algorithm into the terrain-assisted navigation problem so as to solve the problems of low precision, poor real-time performance and the like of the traditional terrain matching algorithm. However, the particle swarm optimization algorithm has the problem of premature convergence, and is easy to fall into a local extreme point, so that the particle swarm optimization algorithm cannot quickly converge to a global extreme point.
Disclosure of Invention
Technical problem to be solved
Aiming at the problem that terrain assisted navigation real-time performance is poor due to premature convergence of a particle swarm algorithm, the invention designs a rapid particle swarm matching method for terrain assisted navigation.
Technical scheme
A terrain-assisted navigation-oriented particle swarm fast matching method is characterized by comprising the following steps:
step 1: terrain adaptation area selection based on terrain difference entropy
Before the unmanned aerial vehicle carries out flight path planning, acquiring environmental information of an area where a task is executed, and distinguishing a terrain adaptation area from a non-adaptation area to ensure that flight paths are all located in the terrain adaptation area; the terrain auxiliary navigation utilizes terrain elevation information, and terrain difference entropy can be used for describing the complexity of the fluctuation of the terrain, and is as follows:
Figure BDA0003052975260000021
in the formula, h i Representing elevation values; m is the total number of elevation points of the region;
Figure BDA0003052975260000022
representing an average elevation value; c i Representing the elevation difference value; p is i Representing the probability of occurrence of a certain high-range difference value; the sharper the terrain elevation change is, the smaller the calculated entropy value is, and the better the matching calculation is;
step 2: sparse A-algorithm-based track planning
On the basis of modeling of a planning environment, planning an optimal flight path between a task starting point and a task finishing point by using a sparse A-star algorithm; according to the algorithm, the flight constraint condition of the unmanned aerial vehicle is added in the search, so that invalid nodes in the search space can be reduced, and the search efficiency is improved; because the terrain auxiliary navigation track planning is carried out in a two-dimensional plane, only the maximum turning angle constraint and the minimum track segment constraint are considered;
taking the projection of the current route on a horizontal plane as a symmetry axis, taking the length R of a minimum track segment as a radius, taking two times of a maximum turning angle psi as a central angle to construct a fan-shaped area as a to-be-searched area, taking the intersection point of a fan-shaped arc line and a digital map grid as a to-be-expanded node, expanding the intersection point not on the digital map grid into the vertex position of the grid, if the to-be-expanded node is positioned in a non-adaptive area or the distance between the to-be-expanded node and the current node is less than the distance of the minimum track segment, excluding the node from the to-be-expanded node, and stopping searching when the distance between the current node and a target point is less than the distance of the minimum track segment;
and 3, step 3: terrain height matching based on optimized particle swarm optimization algorithm
The terrain altitude matching TERCOM compares the terrain altitude right below the flight path of the aircraft with a stored reference elevation map to obtain the position information of the aircraft; the method comprises the following steps:
1. elevation information acquisition
The SINS provides a horizontal position, the barometric altimeter can obtain the altitude of the carrier, the radar altimeter measures the ground clearance of the carrier, and the difference between the altitude of the carrier and the ground clearance is the terrain height;
2. determining search area
The size of the search range is related to SINS precision, and if the drift amount of the SINS is sigma, the search range on the reference graph takes SINS dead reckoning as the center, and traversal search is performed on the reference graph by taking a height point within the range of +/-5 sigma as a starting point of matching search;
3. match search
After the elevation of the terrain right below the airplane is measured by the airplane, the airplane is matched with the elevation data of the digital map; the matching search adopts an optimized particle swarm algorithm, the global optimization capability of the PSO algorithm is considered to be insufficient, an EDA thought is introduced on the basis of the PSO algorithm, namely, the particles are updated simultaneously through the PSO algorithm and the EDA algorithm, R is set as the total number of the reference subgraphs in the search range, M reference subgraphs selected from each generation are moved to the next position through the particle swarm algorithm, and N reference subgraphs are selected to be evolved to the next generation in the EDA; because the population in the EDA is sorted from optimal to worst according to the fitness value of the reference subgraph, the PSO-EDA combination method can avoid the search direction from rapidly converging to local optimal;
in the terrain height matching search, each reference subgraph parallel to the actual mapping graph is a particle, and the dimension of the particle is D if the actual mapping graph has D elevation points; the position of the ith particle is denoted as x i ＝(x i1 ,x i2 ,…,x iD ) The variation of the velocity of the particle at the corresponding position is v i ＝(v i1 ,v i2 ,…,v iD ) The velocity and position of the particle are updated by the formula:
Figure BDA0003052975260000031
in the formula, omega is an inertia weight, and the motion speed of particles can be dynamically adjusted;
Figure BDA0003052975260000032
the speed and the position of the d-dimensional component of the ith particle in the t iteration; c. C 1 ，c 2 Taking a non-negative value as an acceleration factor; p best The best solution found for the particle itself; g best The best solution currently found for the whole population; r is 1 ，r 2 Is a random number, obedient interval [0,1]Uniform distribution of the components;
the procedure for using the PSO-EDA algorithm TERCOM is as follows:
(1) Initializing particles, and randomly generating R reference subgraphs in a search area to obtain a population D 0 ；
(2) Initializing a probability model of an EDA algorithm; in TERCOM, the position distribution of the reference subgraph obeys two-dimensional Gaussian distribution, x represents the longitude of a node in the reference subgraph, y represents the latitude of the node in the reference subgraph, and then the position distribution of the reference subgraph is represented as follows:
Figure BDA0003052975260000041
in the formula, mu 1 ，μ 2 ，
Figure BDA0003052975260000042
Mean and variance of variables x, y, respectively; rho is a constant; let the center coordinate of the search range be (x) 0 ,y 0 ) Let mu in the probability model 1 ＝x 0 ，μ 2 ＝y 0 ，
Figure BDA0003052975260000043
Figure BDA0003052975260000044
In the formula, X [ i ]][j]，Y[i][j]Respectively the longitude and latitude of the jth node in the ith reference subgraph; n is the number of reference subgraphs;
(3) Determining a fitness value of the particle; taking the average Hausdorff distance MHD between the reference subgraph and the actual graph as the fitness value of the particles, and sequencing the particles from small to large according to the fitness value; MHD is defined as:
Figure BDA0003052975260000045
wherein A = { a = 1 ,a 2 ,…a n }，B＝{b 1 ,b 2 ,…b n Is two sequences; p is the number of elements in A; | | · | is a euclidean norm defined between sets a and B; a is i Is the position of the ith point in A; b j Is the position of the jth point in B;
(4) Updating the mean and variance of the Gaussian probability model in the EDA algorithm; selecting the top 30% reference subgraph with the optimal fitness value from all the reference subgraphs, and setting the reference subgraph at the t-th generation and x j Corresponding to a Gaussian distribution mean and variance, respectively
Figure BDA0003052975260000046
Figure BDA0003052975260000047
y j Corresponding to a Gaussian distribution mean and variance of
Figure BDA0003052975260000048
Then there is
Figure BDA0003052975260000049
Figure BDA00030529752600000410
Figure BDA0003052975260000051
Figure BDA0003052975260000052
Wherein, alpha is a learning factor;
Figure BDA0003052975260000053
and
Figure BDA0003052975260000054
respectively representing x corresponding to the optimal individual, the suboptimal individual and the worst individual in the population j A value;
Figure BDA0003052975260000055
respectively representing the y corresponding to the optimal individual, the suboptimal individual and the worst individual in the population j A value; k is the number of the selected better population; x is the number of jk The x of the k-1 individual of the population is sorted from small to large according to the fitness j A value; y is jk The y of the (k-1) th individual is sorted according to the fitness j A value;
Figure BDA0003052975260000056
x as the K preferred populations selected jk The mean value of (a);
Figure BDA0003052975260000057
for the selected y of the K better populations jk The mean value of (a);
(5) Generating a t +1 generation EDA sub population according to the updated probability model
Figure BDA0003052975260000058
(6) Updating G of each particle in particle swarm optimization best And P best (ii) a Comparing the current fitness value of each particle with the individual extremum P best And global extreme value G best If the current fitness value of the particle is less than P best Then P will be best Updating the current fitness value of the particle; if the current fitness value is less than G best Then G will be best Updating the current fitness value of the particle;
(7) Updating the position of each particle according to the formula (1) to obtain t +1 generation particle swarmPopulation
Figure BDA0003052975260000059
(8) Establishing a new population
Figure BDA00030529752600000510
(9) Judging whether an algorithm termination condition is met, if not, turning to the step (3) for t +1 iteration, otherwise, outputting a global extreme value G best And its corresponding position, i.e. the best matching position;
and 4, step 4: SINS/TERCOM combined navigation
After the optimal matching position provided by TERCOM is obtained, selecting the SINS error as the state variable of the SINS/TERCOM combined navigation system to construct a state equation; selecting a difference value between the TERCOM optimal matching position and the position information output by the SINS as a measurement value; and filtering and updating the established state equation and the measurement equation by using a Kalman filter, and feeding back and correcting the SINS to obtain navigation parameters after the integrated navigation.
R =1.2km, Ψ =48 ° as described in step 2.
A computer system, comprising: one or more processors, a computer readable storage medium, for storing one or more programs, which when executed by the one or more processors, cause the one or more processors to implement the above-described method.
A computer-readable storage medium having stored thereon computer-executable instructions for performing the above-described method when executed.
A computer program comprising computer executable instructions which when executed perform the method described above.
Advantageous effects
Aiming at the problem that the traditional terrain assisted navigation adopts a traversal algorithm to search for poor real-time performance, the invention provides a terrain assisted navigation-oriented particle swarm fast matching method. After obtaining the TERCOM optimal matching position, the difference value between the TERCOM optimal matching position and the position information output by the SINS is used as a measurement value, kalman filtering is used for estimating the error of the SINS, and the SINS is corrected. The improved particle swarm optimization is adopted to realize TERCOM, the global optimization capability of the particle swarm optimization is enhanced, and the real-time performance of terrain-assisted navigation is improved.
Drawings
The drawings, in which like reference numerals refer to like parts throughout, are for the purpose of illustrating particular embodiments only and are not to be considered limiting of the invention.
Fig. 1 is a schematic diagram of a region to be searched.
FIG. 2 is a flow chart of the method of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
Referring to fig. 2, the terrain-assisted navigation-oriented particle swarm fast matching method specifically comprises the following steps:
step 1: terrain adaptation area selection based on terrain difference entropy
Since the terrain-assisted navigation cannot be used in a terrain flat area, before the unmanned aerial vehicle carries out flight path planning, the environmental information of an area where a task is executed needs to be acquired, and a terrain adaptive area and a terrain non-adaptive area need to be distinguished, so that the unmanned aerial vehicle can navigate efficiently;
before the unmanned aerial vehicle carries out flight path planning, the environment information of an area where a task is executed needs to be obtained, and a terrain adaptation area is distinguished from a non-adaptation area, so that the flight path is ensured to be located in the terrain adaptation area. The terrain elevation information is utilized by the terrain aided navigation, and as the precision of the terrain aided navigation is influenced by the terrain, the terrain difference entropy can be used for describing the fluctuation complexity of the terrain, and is as follows:
Figure BDA0003052975260000071
in the formula, h i Representing elevation values, M is the total number of elevation points of the area,
Figure BDA0003052975260000072
denotes the mean elevation value, C i Representing elevation difference value, P i Indicating the probability of occurrence of a certain high range difference value. The sharper the terrain elevation change is, the smaller the calculated entropy value is, and the better the matching calculation is.
The relevant parameters are defined as follows:
the unmanned aerial vehicle flight task is a rectangular area from the east longitude 109 degrees to the north latitude 34 degrees to the east longitude 110 degrees, namely the task interval is from the east longitude 109 degrees to the east longitude 110 degrees and from the north latitude 34 degrees to the north latitude 35 degrees; the precision of the digital elevation map is 30m multiplied by 30m; the task interval is divided into the grid area with the precision of 900m multiplied by 900m, and then the grid number of the task interval is 14400.
Step 2: sparse A-algorithm-based track planning
And on the basis of modeling of a planning environment, planning an optimal flight path between a task starting point and a task finishing point by using a sparse A-star algorithm. According to the algorithm, the flight constraint condition of the unmanned aerial vehicle is added in the search, so that invalid nodes in the search space can be reduced, and the search efficiency is improved. Because the planning of the terrain auxiliary navigation track is carried out in a two-dimensional plane, only the maximum turning angle constraint and the minimum track segment constraint are considered.
Taking the projection of the current route on a horizontal plane as a symmetry axis, taking the length R of the minimum route segment as a radius, taking the two times of the maximum turning angle psi as a central angle to construct a fan-shaped area as a to-be-searched area, taking the intersection point of a fan-shaped arc line and a digital map grid as a node to be expanded, and expanding the intersection point which is not on the digital map grid as the vertex position of the grid, as shown in fig. 1. And if the node to be expanded is positioned in the non-adaptive area or the distance between the node to be expanded and the current node is less than the minimum track segment distance, excluding the node from the node to be expanded, and if the distance between the current node and the target point is less than the minimum track segment, terminating the search.
The values of the relevant parameters are as follows: r =1.2km, Ψ =48 °.
And 3, step 3: terrain height matching based on optimized particle swarm optimization algorithm
TERCOM obtains the position information of the aircraft by comparing the altitude of the terrain right below the flight path of the aircraft with a stored reference elevation map. The method comprises the following steps:
1. elevation information acquisition
The inertial navigation system provides horizontal position, and the altitude of the vehicle is high to the baro-altimeter, and the terrain clearance of vehicle is measured to the radar altimeter, and the terrain clearance is promptly subtracted for the two.
2. Determining a search area
If the drift amount of the SINS is 500m, the search range on the reference map is centered on SINS estimated positioning, and traversal search is performed on the reference map by taking a height point within the range of +/-2500 m as a starting point of matching search.
3. Match search
After the elevation of the terrain right below the airplane is measured by the airplane, the airplane can be matched with the elevation data of the digital map, the matching search adopts an optimization particle swarm algorithm, the global optimization capability of the PSO algorithm is considered to be insufficient, and an EDA thought is introduced on the basis of the optimization particle swarm algorithm, namely, the particles in the algorithm are updated simultaneously through the PSO algorithm and the EDA algorithm. And setting R as the total number of the reference subgraphs in the search range, selecting M reference subgraphs from each generation, moving the M reference subgraphs to the next position by a particle swarm algorithm, and selecting N reference subgraphs to evolve to the next generation in the EDA. As the population in the EDA is sorted from the best to the worst according to the fitness value of the reference subgraph, the PSO-EDA combination method can avoid the search direction from rapidly converging to the local optimum.
In the terrain height matching search, each timeAnd a reference subgraph parallel to the actual graph is a particle, and the dimension of the particle is D if the actual graph has D high-distance points. The position of the ith particle is denoted as x i ＝(x i1 ,x i2 ,…,x iD ) The variation of the velocity of the particle at the corresponding position is v i ＝(v i1 ,v i2 ,…,v iD ) The velocity and position update formula of the particle is:
Figure BDA0003052975260000091
in the formula, ω =0.9 is an inertial weight, and the particle motion speed can be dynamically adjusted;
Figure BDA0003052975260000092
the velocity and position of the d-dimensional component of the ith particle in the t iteration; c. C 1 ＝2，c 2 =2 is acceleration factor; p best The optimal solution found for the particle itself; g best The best solution currently found for the whole population; r is a radical of hydrogen 1 ，r 2 Is a random number, obedient interval [0,1]Are uniformly distributed.
The procedure for TERCOM by the PSO-EDA algorithm is as follows:
(1) Initializing particles, randomly generating 200 reference subgraphs in a search area to obtain a population D 0 ；
(2) The probabilistic model of the EDA algorithm is initialized. In the terrain height matching, the position distribution of the reference subgraph obeys two-dimensional Gaussian distribution, x represents the longitude of a node in the reference subgraph, y represents the latitude of the node in the reference subgraph, and the distribution is represented as;
Figure BDA0003052975260000093
in the formula, mu 1 ，μ 2 ，
Figure BDA0003052975260000094
The mean and variance of the variables x, y,ρ is a constant. Let the center coordinate of the search range be (x) 0 ,y 0 ) Let mu in the probability model 1 ＝x 0 ，μ 2 ＝y 0 ，
Figure BDA0003052975260000095
Figure BDA0003052975260000096
In the formula, X [ i ]][j]，Y[i][j]Respectively the longitude and latitude of the jth node in the ith reference subgraph, and N is the number of the reference subgraphs.
(3) A fitness value of the particle is determined. And taking the average Hausdorff distance between the reference subgraph and the actually measured terrain elevation as the fitness value of the particles, and sequencing the particles from small to large according to the fitness value. MHD is defined as:
Figure BDA0003052975260000101
wherein A = { a = 1 ,a 2 ,…a n }，B＝{b 1 ,b 2 ,…b n Is two sequences; p is the number of elements in A; | | · | is a euclidean norm defined between sets a and B; a is i Is the position of the ith point in A; b j Is the position of the jth point in B.
(4) And updating the mean and the variance of the Gaussian probability model in the EDA algorithm. Selecting the top 30% reference subgraph with the optimal fitness value from all the reference subgraphs, and setting the reference subgraph at the t-th generation and x j Corresponding to a Gaussian distribution mean and variance of
Figure BDA0003052975260000102
Figure BDA0003052975260000103
y j Corresponding to a Gaussian distribution mean and variance, respectively
Figure BDA0003052975260000104
Then there is
Figure BDA0003052975260000105
Figure BDA0003052975260000106
Figure BDA0003052975260000107
Figure BDA0003052975260000108
Wherein α =0.5 is a learning factor;
Figure BDA0003052975260000109
and
Figure BDA00030529752600001010
respectively representing x corresponding to the optimal individual, the suboptimal individual and the worst individual in the population j A value;
Figure BDA00030529752600001011
respectively representing the y corresponding to the optimal individual, the suboptimal individual and the worst individual in the population j A value; k is the number of the selected better population; x is a radical of a fluorine atom jk The x of the k-1 individual of the population is sorted from small to large according to the fitness j A value; y is jk The y of the k-1 individual of the population is sorted from small to large according to the fitness j A value;
Figure BDA00030529752600001012
x as the K preferred populations selected jk The mean value of (a);
Figure BDA00030529752600001013
for the selected y of the K better populations jk Is measured.
(5) According to the updated probabilityModel generation t +1 generation EDA sub population
Figure BDA00030529752600001014
(6) Updating G of each particle in particle swarm optimization best And P best (ii) a Comparing the current fitness value of each particle with the individual extremum P best And global extreme value G best If the current fitness value of the particle is less than P best Then P will be best Updating the current fitness value of the particle; if the current fitness value is less than G best Then G will be best Updating the current fitness value of the particle;
(7) Updating the position of each particle according to the formula (1) to obtain t +1 generation particle swarm
Figure BDA00030529752600001015
(8) Establishing a new population
Figure BDA0003052975260000111
(9) Judging whether the algorithm termination condition is met, if not, turning to the step (2) for next iteration, otherwise, outputting a global extreme value G best And its corresponding location, i.e., the best match location.
And 4, step 4: SINS/TERCOM combined navigation
And after the TERCOM optimal matching position is obtained, taking the difference value between the TERCOM optimal matching position and the position information output by the SINS as a measurement value, estimating the error of the inertial navigation system by using Kalman filtering, and correcting the SINS.
Selecting the position error, the speed error, the attitude error, the gyro drift and the accelerometer zero offset of the SINS as state quantities:
Figure BDA0003052975260000112
in the formula, phi E 、φ N And phi U An east misalignment angle, a north misalignment angle, and an azimuth misalignment angle, respectively; delta V E 、δV N And δ V U East, north and sky speed errors, respectively; δ L, δ λ and δ h are longitude, latitude and altitude errors, respectively; epsilon bx 、ε by And epsilon bz Gyroscope drift errors of an x axis, a y axis and a z axis under a coordinate system of the body are respectively obtained;
Figure BDA0003052975260000113
and
Figure BDA0003052975260000114
the zero offset of the accelerometer is respectively an x axis, a y axis and a z axis under the coordinate system of the body.
Selecting SINS error as state variable of SINS/TERCOM combined navigation system, and obtaining system state equation as
Figure BDA0003052975260000115
In the formula, A 15×15 The non-zero elements of (d) are:
Figure BDA0003052975260000116
A 1,10 ＝-(cosγcosΨ+sinΨsinθ),A 1,11 ＝cosγsinΨ-sinγcosΨsinθ,A 1,12 ＝sinγcosθ
Figure BDA0003052975260000117
Figure BDA0003052975260000118
Figure BDA0003052975260000121
Figure BDA0003052975260000122
Figure BDA0003052975260000123
Figure BDA0003052975260000124
Figure BDA0003052975260000125
Figure BDA0003052975260000126
Figure BDA0003052975260000127
A 6,13 ＝sinγcosΨ-cosγsinΨsinθ,A 6,15 ＝cosγcosθ,A 6,14 ＝-sinγsinΨ-cosγcosΨsinθ,
Figure BDA0003052975260000128
A 9,3 ＝1，
Figure BDA0003052975260000129
in the formula, psi is a vehicle heading angle; gamma is the transverse rolling angle of the carrier; theta is the carrier pitch angle; omega ie Is the earth rotation angular rate; v E 、V N And V U Respectively calculating the latest speed values of the vehicle along east, north and sky directions obtained by navigation calculation; f. of E 、f N And f U Accelerometer measurements for east, north, and sky; r M And R N Curvature halves of meridian and unitary mortise respectivelyDiameter; l is the latitude of the point where the carrier is located; h is the height of the point where the carrier is located; tau. g Time associated with the Markov process;
Figure BDA00030529752600001210
distributing an array for system noise; the white noise vector of the system is W 6×1 ＝[ω gx ,ω gy ,ω gz ,ω ax ,ω ay ,ω az ] T ，ω gx 、ω gy And ω gz Is a gyro random white noise drift; omega ax 、ω ay And ω az White noise is driven for accelerometer markov.
Selecting longitude and latitude as measurement quantities, the longitude and latitude of the SINS system can be expressed as:
Figure BDA0003052975260000131
in the formula, L SINS ，λ SINS Longitude and latitude measured for SINS; l and lambda represent the true longitude and latitude of the carrier; delta L SINS 、δλ SINS The measurement error of the longitude and latitude of the SINS is shown.
The longitude and latitude of the TERCOM system can be expressed as:
Figure BDA0003052975260000132
in the formula, L TER 、λ TER Longitude and latitude measured for TERCOM; delta L TER 、δλ TER The error of TERCOM longitude and latitude is measured.
The combined observation vector of the SINS/TERCOM combined navigation system is as follows:
Figure BDA0003052975260000133
in the formula, a measuring array H 2×15 ＝[0 2×6 I 2×2 0 2×7 ](ii) a Measuring noise vector V 2×1 ＝[N L N λ ] T ；N L 、N λ And the error of longitude and latitude output after coordinate transformation of TERCOM.
And filtering and updating the established state equation and the measurement equation by using a Kalman filter, and feeding back and correcting the SINS to obtain the navigation parameters after the integrated navigation.
While the invention has been described with reference to specific embodiments, the invention is not limited thereto, and various equivalent modifications or substitutions can be easily made by those skilled in the art within the technical scope of the present disclosure.

Claims (4)

1. A terrain-assisted navigation-oriented particle swarm fast matching method is characterized by comprising the following steps:
step 1: terrain adaptation area selection based on terrain difference entropy
Before the unmanned aerial vehicle carries out flight path planning, environment information of an area where a task is executed needs to be obtained, and a terrain adaptation area is distinguished from a non-adaptation area, so that the flight path is ensured to be located in the terrain adaptation area; the terrain auxiliary navigation utilizes terrain elevation information, and a terrain difference entropy can be used for describing the complexity of the relief of the terrain, wherein the terrain difference entropy is as follows:
Figure FDA0003802079630000011
in the formula, h i Representing an elevation value; m is the total number of elevation points of the region;
Figure FDA0003802079630000012
representing an average elevation value; c i Representing the elevation difference value; p i Representing the probability of occurrence of a certain high-range difference value; the sharper the terrain elevation change is, the smaller the calculated entropy value is, and the better the matching calculation is;
step 2: sparse A-algorithm-based flight path planning
On the basis of planning environment modeling, an optimal flight path is planned between a task starting point and a task finishing point by using a sparse A-star algorithm; according to the algorithm, the flight constraint condition of the unmanned aerial vehicle is added in the search, so that invalid nodes in the search space can be reduced, and the search efficiency is improved; because the planning of the terrain auxiliary navigation track is carried out in a two-dimensional plane, only the maximum turning angle constraint and the minimum track segment constraint are considered;
taking the projection of the current route on a horizontal plane as a symmetry axis, taking the length R of a minimum track segment as a radius, taking two times of a maximum turning angle psi as a central angle to construct a fan-shaped area as a to-be-searched area, taking the intersection point of a fan-shaped arc line and a digital map grid as a to-be-expanded node, expanding the intersection point not on the digital map grid into the vertex position of the grid, if the to-be-expanded node is positioned in a non-adaptive area or the distance between the to-be-expanded node and the current node is less than the distance of the minimum track segment, excluding the node from the to-be-expanded node, and stopping searching when the distance between the current node and a target point is less than the distance of the minimum track segment;
and step 3: terrain height matching based on optimized particle swarm optimization algorithm
The terrain altitude matching TERCOM compares the terrain altitude right below the flight path of the aircraft with a stored reference elevation map to obtain the position information of the aircraft; the method comprises the following steps:
A. elevation information acquisition
The SINS provides a horizontal position, the barometric altimeter can obtain the altitude of the carrier, the radar altimeter measures the ground clearance of the carrier, and the difference between the altitude of the carrier and the ground clearance is the terrain height;
B. determining search area
The size of the search range is related to SINS precision, and if the drift amount of the SINS is sigma, the search range on the reference graph takes SINS dead reckoning as the center, and traversal search is performed on the reference graph by taking a height point within the range of +/-5 sigma as a starting point of matching search;
C. matching search
After the elevation of the terrain right below the airplane is measured by the airplane, the airplane is matched with the elevation data of the digital map; the matching search adopts an optimized particle swarm algorithm, the global optimization capability of the PSO algorithm is considered to be insufficient, an EDA thought is introduced on the basis of the PSO algorithm, namely, the particles are updated simultaneously through the PSO algorithm and the EDA algorithm, R is set as the total number of the reference subgraphs in the search range, M reference subgraphs selected from each generation are moved to the next position through the particle swarm algorithm, and N reference subgraphs are selected to be evolved to the next generation in the EDA; as the population in the EDA is sorted from the optimal to the worst according to the fitness value of the reference subgraph, the PSO-EDA combination method can avoid the search direction from rapidly converging to the local optimal;
in the terrain height matching search, each reference subgraph parallel to the actual mapping graph is a particle, and the dimension of the particle is D if the actual mapping graph has D elevation points; the position of the ith particle is denoted x i ＝(x i1 ,x i2 ,…,x iD ) The variation of the velocity of the particle at the corresponding position is v i ＝(v i1 ,v i2 ,…,v iD ) The velocity and position of the particle are updated by the formula:
Figure FDA0003802079630000021
in the formula, omega is an inertia weight, and the motion speed of particles can be dynamically adjusted;
Figure FDA0003802079630000022
the speed and the position of the d-dimensional component of the ith particle in the t iteration; c. C 1 ，c 2 Taking a non-negative value as an acceleration factor; p best The best solution found for the particle itself; g best The best solution currently found for the whole population; r is 1 ，r 2 Is a random number, obedient interval [0,1]Uniform distribution of the components;
the procedure for using the PSO-EDA algorithm TERCOM is as follows:
(1) Initializing particles, and randomly generating R reference subgraphs in a search area to obtain a population D 0 ；
(2) Initializing a probability model of an EDA algorithm; in TERCOM, the position distribution of the reference subgraph obeys two-dimensional Gaussian distribution, x represents the longitude of a node in the reference subgraph, y represents the latitude of the node in the reference subgraph, and then the position distribution of the reference subgraph is represented as follows:
Figure FDA0003802079630000031
in the formula, mu 1 ，μ 2 ，
Figure FDA0003802079630000032
Mean and variance of variables x, y, respectively; rho is a constant; let the center coordinate of the search range be (x) 0 ,y 0 ) Let mu in the probability model 1 ＝x 0 ，μ 2 ＝y 0 ，
Figure FDA0003802079630000033
Figure FDA0003802079630000034
In the formula, X [ i ]][j]，Y[i][j]Respectively the longitude and latitude of the jth node in the ith reference subgraph; n is the number of reference subgraphs;
(3) Determining a fitness value of the particle; taking the average Hausdorff distance MHD between the reference subgraph and the actual graph as the fitness value of the particles, and sequencing the particles from small to large according to the fitness value; MHD is defined as:
Figure FDA0003802079630000035
wherein A = { a = 1 ,a 2 ,…a n }，B＝{b 1 ,b 2 ,…b n Is two sequences; p is the number of elements in A; i | · | | is a euclidean norm defined between sets a and B; a is a i Is the position of the ith point in A; b j Is the position of the jth point in B;
(4) Updating the mean value and the variance of the Gaussian probability model in the EDA algorithm; selecting from all reference subgraphsSelecting the top 30% reference subgraph with the optimal fitness value, and setting the subgraph at the t generation, x j Corresponding to a Gaussian distribution mean and variance, respectively
Figure FDA0003802079630000036
Figure FDA0003802079630000037
y j Corresponding to a Gaussian distribution mean and variance, respectively
Figure FDA0003802079630000038
Then there is
Figure FDA0003802079630000039
Figure FDA00038020796300000310
Figure FDA0003802079630000041
Figure FDA0003802079630000042
In the formula, alpha is a learning factor;
Figure FDA0003802079630000043
and
Figure FDA0003802079630000044
respectively representing x corresponding to the optimal individual, the suboptimal individual and the worst individual in the population j A value;
Figure FDA0003802079630000045
respectively representing the y corresponding to the optimal individual, the suboptimal individual and the worst individual in the population j A value; k is the number of the selected better population; x is the number of jk The x of the k-1 individual of the population is sorted from small to large according to the fitness j A value; y is jk The y of the k-1 individual of the population is sorted from small to large according to the fitness j A value;
Figure FDA0003802079630000049
x as the K preferred populations selected jk The mean value of (a);
Figure FDA00038020796300000410
for y of the K preferred populations selected jk The mean value of (a);
(5) Generating a t +1 generation EDA sub population according to the updated probability model
Figure FDA0003802079630000046
(6) Updating G of each particle in particle swarm optimization best And P best (ii) a Comparing the current fitness value of each particle with the individual extremum P best And global extreme value G best If the current fitness value of the particle is less than P best Then P will be best Updating the current fitness value of the particle; if the current fitness value is less than G best Then G will be best Updating the current fitness value of the particle;
(7) Updating the position of each particle according to the formula (2) to obtain t +1 generation particle swarm
Figure FDA0003802079630000047
(8) Establishing a new population
Figure FDA0003802079630000048
(9) Judging whether an algorithm termination condition is met, if not, turning to the step (3) for t +1 iteration, otherwise, outputting a global extreme value G best And their corresponding positions, i.e. best matchesA location;
and 4, step 4: SINS/TERCOM combined navigation
After the optimal matching position provided by TERCOM is obtained, selecting the SINS error as the state variable of the SINS/TERCOM combined navigation system to construct a state equation; selecting a difference value between the TERCOM optimal matching position and the position information output by the SINS as a measurement value; and filtering and updating the established state equation and the measurement equation by using a Kalman filter, and feeding back and correcting the SINS to obtain navigation parameters after the integrated navigation.
2. The terrain-assisted navigation-oriented particle swarm fast matching method according to claim 1, wherein R =1.2km and Ψ =48 ° in step 2.
3. A computer system, comprising: one or more processors, a computer readable storage medium, for storing one or more programs, wherein the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of claim 1.
4. A computer-readable storage medium having stored thereon computer-executable instructions for, when executed, implementing the method of claim 1.
CN202110492424.4A 2021-05-06 2021-05-06 Terrain-assisted navigation-oriented particle swarm fast matching method Active CN113252039B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110492424.4A CN113252039B (en) 2021-05-06 2021-05-06 Terrain-assisted navigation-oriented particle swarm fast matching method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110492424.4A CN113252039B (en) 2021-05-06 2021-05-06 Terrain-assisted navigation-oriented particle swarm fast matching method

Publications (2)

Publication Number Publication Date
CN113252039A CN113252039A (en) 2021-08-13
CN113252039B true CN113252039B (en) 2022-10-28

Family

ID=77223797

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110492424.4A Active CN113252039B (en) 2021-05-06 2021-05-06 Terrain-assisted navigation-oriented particle swarm fast matching method

Country Status (1)

Country Link
CN (1) CN113252039B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114216465B (en) * 2021-12-08 2022-11-01 哈尔滨工程大学 Submarine topography navigation parallel matching method
CN114727217A (en) * 2022-03-02 2022-07-08 西北工业大学 Low-cost double-collar heterogeneous unmanned aerial vehicle formation cooperative positioning method based on data chain communication

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105787489A (en) * 2016-03-04 2016-07-20 哈尔滨工程大学 Matching navigation algorithm based on underwater landform
CN107356254A (en) * 2017-06-07 2017-11-17 东南大学 Suitable for the particle group optimizing method of geomagnetic auxiliary navigation trajectory planning
CN108871351A (en) * 2018-08-02 2018-11-23 哈尔滨工程大学 A kind of matched dynamic path planning method of AUV sea-floor relief

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2961897B1 (en) * 2010-06-25 2012-07-13 Thales Sa NAVIGATION FILTER FOR A FIELD CORRELATION NAVIGATION SYSTEM
CN102426018B (en) * 2011-08-19 2014-01-01 北京航空航天大学 Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering
US9404754B2 (en) * 2013-03-25 2016-08-02 Raytheon Company Autonomous range-only terrain aided navigation
GB2555805A (en) * 2016-11-09 2018-05-16 Atlantic Inertial Systems Ltd A navigation system
CN110617819B (en) * 2019-10-17 2022-09-30 国营芜湖机械厂 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning
CN111486845B (en) * 2020-04-27 2022-02-11 中国海洋大学 AUV multi-strategy navigation method based on submarine topography matching
CN112082552A (en) * 2020-07-11 2020-12-15 西北工业大学太仓长三角研究院 Unmanned aerial vehicle flight path planning method based on improved hybrid particle swarm optimization algorithm
CN112230678B (en) * 2020-10-29 2023-07-14 皖江工学院 Three-dimensional unmanned aerial vehicle path planning method and system based on particle swarm optimization

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105787489A (en) * 2016-03-04 2016-07-20 哈尔滨工程大学 Matching navigation algorithm based on underwater landform
CN107356254A (en) * 2017-06-07 2017-11-17 东南大学 Suitable for the particle group optimizing method of geomagnetic auxiliary navigation trajectory planning
CN108871351A (en) * 2018-08-02 2018-11-23 哈尔滨工程大学 A kind of matched dynamic path planning method of AUV sea-floor relief

Also Published As

Publication number Publication date
CN113252039A (en) 2021-08-13

Similar Documents

Publication Publication Date Title
CN109211276B (en) SINS initial alignment method based on GPR and improved SRCKF
CN110487301B (en) Initial alignment method of radar-assisted airborne strapdown inertial navigation system
CN108226980B (en) Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit
CN113252038B (en) Course planning terrain auxiliary navigation method based on particle swarm optimization
CN111366148B (en) Target positioning method suitable for multiple observations of airborne photoelectric observing and sighting system
CN113252039B (en) Terrain-assisted navigation-oriented particle swarm fast matching method
CN110567455B (en) Tightly-combined navigation method for quadrature updating volume Kalman filtering
CN111156986B (en) Spectrum red shift autonomous integrated navigation method based on robust adaptive UKF
CN110132269A (en) A kind of guided missile high-precision Vertical Launch initial attitude acquisition methods
CN111397599A (en) Improved ICCP (Integrated Circuit chip) underwater geomagnetic matching method based on triangular matching algorithm
CN112697138A (en) Factor graph optimization-based bionic polarization synchronous positioning and composition method
RU2318188C1 (en) Method for autonomous navigation and orientation of spacecrafts
CN107525502B (en) Method for improving inertial terrain matching navigation average precision of underwater vehicle
CN110595503A (en) Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation
CN115683141A (en) Local reference path generation method for automatic driving in unknown environment
CN109855623A (en) Geomagnetic model online approximating method based on Legendre multinomial and BP neural network
CN116642482A (en) Positioning method, equipment and medium based on solid-state laser radar and inertial navigation
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN106643726B (en) Unified inertial navigation resolving method
CN115265532A (en) Auxiliary filtering method for marine integrated navigation
CN111207773A (en) Attitude unconstrained optimization solving method for bionic polarized light navigation
CN114088098A (en) Auxiliary navigation path planning method for polar region underwater vehicle database
CN110873577B (en) Underwater rapid-acting base alignment method and device
CN112229421A (en) Strapdown inertial navigation shaking base rough alignment method based on lie group optimal estimation
CN116295342A (en) Multi-sensing state estimator for aircraft survey

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