CN113252039A - 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
CN113252039A
CN113252039A CN202110492424.4A CN202110492424A CN113252039A CN 113252039 A CN113252039 A CN 113252039A CN 202110492424 A CN202110492424 A CN 202110492424A CN 113252039 A CN113252039 A CN 113252039A
Authority
CN
China
Prior art keywords
terrain
particle
value
algorithm
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.)
Granted
Application number
CN202110492424.4A
Other languages
Chinese (zh)
Other versions
CN113252039B (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]

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. The method comprises the steps of firstly establishing an environment model of an unmanned aerial vehicle task interval, using a sparse A-x algorithm to carry out flight path planning, adopting an optimized particle swarm algorithm to carry out terrain height matching on the basis, introducing a distribution estimation idea on the basis of the particle swarm algorithm, enhancing the global optimization capability of the particle swarm optimization algorithm, and improving the real-time performance of TERCOM. 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 error of the SINS 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 Optimization algorithm of Particle Swarm Optimization-based aircraft terrain matching new algorithm (Hazelnese, Liu Xin, Chua Yanping, Liyaxiong, Zhu Yi, Infrared and laser engineering, volume 45 of 2016 No. 5) is introduced 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 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 BDA0003052975260000021
in the formula, hiRepresenting elevation values; m is the total number of elevation points of the region;
Figure BDA0003052975260000022
representing an average elevation value; ciRepresenting the elevation difference value; piRepresenting 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 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 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 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; 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 xi＝(xi1,xi2,…,xiD) The variation of the velocity of the particle at the corresponding position is vi＝(vi1,vi2,…,viD) 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. C1，c2Taking a non-negative value as an acceleration factor; pbestThe best solution found for the particle itself; gbestThe best solution currently found for the whole population; r is1，r2Is a random number, obeys an interval of [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 D0；
(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, mu1，μ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,y0) Let mu in the probability model1＝x0，μ2＝y0，
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 ═ a1,a2,…an}，B＝{b1,b2,…bnIs two sequences; p is the number of elements in A; | | · | is a euclidean norm defined between sets a and B; a isiIs the position of the ith point in A; bjIs 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 xjCorresponding to a Gaussian distribution mean and variance, respectively
Figure BDA0003052975260000046
Figure BDA0003052975260000047
yjCorresponding to a Gaussian distribution mean and variance, respectively
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 populationjA value;
Figure BDA0003052975260000055
respectively representing the y corresponding to the optimal individual, the suboptimal individual and the worst individual in the populationjA value; k is the number of the selected better population; x is the number ofjkThe x of the k-1 individual of the population is sorted from small to large according to the fitnessjA value; y isjkThe y of the k-1 individual of the population is sorted from small to large according to the fitnessjA value;
Figure BDA0003052975260000056
x as the K preferred populations selectedjkThe mean value of (a);
Figure BDA0003052975260000057
for y of the K preferred populations selectedjkThe 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 optimizationbestAnd Pbest(ii) a Comparing the current fitness value of each particle with the individual extremum PbestAnd global extreme value GbestIf the current fitness value of the particle is less than PbestThen P will bebestUpdating the current fitness value of the particle; if the current fitness value is less than GbestThen G will bebestUpdating 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 BDA0003052975260000059
(8) Establishing a new population
Figure BDA00030529752600000510
(9) Judging whether an algorithm termination condition is met, if not, turning to the step (3) to carry out t +1 times of iteration, otherwise, outputting a global extreme value GbestAnd 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.
In step 2, R is 1.2km, and Ψ is 48 °.
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
The invention provides a terrain-assisted navigation-oriented particle swarm fast matching method aiming at the problem of poor real-time performance of search by adopting a traversal algorithm in traditional terrain-assisted navigation. And after the TERCOM optimal matching position is obtained, the difference value of 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 are only for purposes of illustrating particular embodiments and are not to be construed as limiting the invention, wherein like reference numerals are used to designate like parts throughout.
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 non-adaptive area need to be distinguished, so that the efficient navigation of the unmanned aerial vehicle is ensured;
before the unmanned aerial vehicle carries out flight path planning, the environment information of an area where a task is executed needs to be acquired, 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, hiRepresenting elevation values, M is the total number of elevation points of the area,
Figure BDA0003052975260000072
representing the mean elevation, CiRepresenting elevation difference value, PiIndicating 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 30 m; 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 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 terrain aided 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 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 step 3: terrain height matching based on optimized particle swarm optimization algorithm
TERCOM obtains the position information of the aircraft by comparing the terrain altitude directly 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 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 reference subgraph parallel to the real-mapping graph is a particle, and the dimension of the particle is D if the real-mapping graph has D high-level points. The position of the ith particle is denoted xi＝(xi1,xi2,…,xiD) The variation of the velocity of the particle at the corresponding position is vi＝(vi1,vi2,…,viD) The velocity and position of the particle are updated by the formula:
Figure BDA0003052975260000091
in the formula, ω ═ 0.9 is an inertia 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. C1＝2，c22 is an acceleration factor; pbestThe optimal solution found for the particle itself; gbestThe best solution currently found for the whole population; r is1，r2Is a random number, obeys an interval of [0,1 ]]Are uniformly distributed.
The procedure for performing TERCOM by the PSO-EDA algorithm is as follows:
(1) initializing particles, and randomly generating 200 reference subgraphs in a search area to obtain a population D0；
(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, mu1，μ2，
Figure BDA0003052975260000094
Mean and variance of variables x, y, respectively, ρ is a constant. Let the center coordinate of the search range be (x)0,y0) Let mu in the probability model1＝x0，μ2＝y0，
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 ═ a1,a2,…an}，B＝{b1,b2,…bnIs two sequences; p is the number of elements in A; is defined in Europe between sets A and BA norm; a isiIs the position of the ith point in A; bjIs 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 xjCorresponding to a Gaussian distribution mean and variance, respectively
Figure BDA0003052975260000102
Figure BDA0003052975260000103
yjCorresponding 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 populationjA value;
Figure BDA00030529752600001011
respectively representing the y corresponding to the optimal individual, the suboptimal individual and the worst individual in the populationjA value; k is the number of the selected better population; x is the number ofjkThe x of the k-1 individual of the population is sorted from small to large according to the fitnessjA value; y isjkThe y of the k-1 individual of the population is sorted from small to large according to the fitnessjA value;
Figure BDA00030529752600001012
x as the K preferred populations selectedjkThe mean value of (a);
Figure BDA00030529752600001013
for y of the K preferred populations selectedjkIs measured.
(5) Generating a t +1 generation EDA sub population according to the updated probability model
Figure BDA00030529752600001014
(6) Updating G of each particle in particle swarm optimizationbestAnd Pbest(ii) a Comparing the current fitness value of each particle with the individual extremum PbestAnd global extreme value GbestIf the current fitness value of the particle is less than PbestThen P will bebestUpdating the current fitness value of the particle; if the current fitness value is less than GbestThen G will bebestUpdating 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 GbestAnd itThe corresponding position, i.e. the best match position.
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, phiE、φNAnd phiUAn east misalignment angle, a north misalignment angle, and an azimuth misalignment angle, respectively; delta VE、δVNAnd δ VUEast, north and sky speed errors, respectively; δ L, δ λ and δ h are longitude, latitude and altitude errors, respectively; epsilonbx、εbyAnd εbzGyroscope 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, A15×15The non-zero elements of (d) are:
Figure BDA0003052975260000116
A1,10＝-(cosγcosΨ+sinΨsinθ),A1,11＝cosγsinΨ-sinγcosΨsinθ,A1,12＝sinγcosθ
Figure BDA0003052975260000117
Figure BDA0003052975260000118
Figure BDA0003052975260000121
Figure BDA0003052975260000122
Figure BDA0003052975260000123
Figure BDA0003052975260000124
Figure BDA0003052975260000125
Figure BDA0003052975260000126
Figure BDA0003052975260000127
A6,13＝sinγcosΨ-cosγsinΨsinθ,A6,15＝cosγcosθ,A6,14＝-sinγsinΨ-cosγcosΨsinθ,
Figure BDA0003052975260000128
A9,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; omegaieIs the earth rotation angular rate; vE、VNAnd VURespectively calculating the latest speed values of the vehicle along east, north and sky directions obtained by navigation calculation; f. ofE、fNAnd fUAccelerometer measurements for east, north, and sky; rMAnd RNRespectively the curvature radius of the meridian and the unitary mortise at the point of the carrier; 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 isgTime associated with the Markov process;
Figure BDA00030529752600001210
distributing an array for system noise; the white noise vector of the system is W6×1＝[ωgx,ωgy,ωgz,ωax,ωay,ωaz]T，ωgx、ωgyAnd ωgzIs a gyro random white noise drift; omegaax、ωayAnd ωazWhite 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, LSINS，λSINSLongitude and latitude measured for SINS; l and lambda represent the true longitude and latitude of the carrier; delta LSINS、δλSINSThe 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, LTER、λTERLongitude and latitude measured for TERCOM; delta LTER、δλTERThe 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 H2×15＝[02×6 I2×2 02×7](ii) a Measuring noise vector V2×1＝[NL Nλ]T；NL、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 (5)

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, 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 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 FDA0003052975250000011
in the formula, hiRepresenting elevation values; m is the total number of elevation points of the region;
Figure FDA0003052975250000012
representing an average elevation value; ciRepresenting the elevation difference value; piRepresenting 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 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 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 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; 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 xi＝(xi1,xi2,…,xiD) The variation of the velocity of the particle at the corresponding position is vi＝(vi1,vi2,…,viD) The velocity and position of the particle are updated by the formula:
Figure FDA0003052975250000021
in the formula, omega is inertia weight and can moveAdjusting the particle movement speed;
Figure FDA0003052975250000022
the speed and the position of the d-dimensional component of the ith particle in the t iteration; c. C1，c2Taking a non-negative value as an acceleration factor; pbestThe best solution found for the particle itself; gbestThe best solution currently found for the whole population; r is1，r2Is a random number, obeys an interval of [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 D0；
(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 FDA0003052975250000031
in the formula, mu1，μ2，
Figure FDA0003052975250000032
Mean and variance of variables x, y, respectively; rho is a constant; let the center coordinate of the search range be (x)0,y0) Let mu in the probability model1＝x0，μ2＝y0，
Figure FDA0003052975250000033
Figure FDA0003052975250000034
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 FDA0003052975250000035
wherein A ═ a1,a2,…an}，B＝{b1,b2,…bnIs two sequences; p is the number of elements in A; | | · | is a euclidean norm defined between sets a and B; a isiIs the position of the ith point in A; bjIs 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 xjCorresponding to a Gaussian distribution mean and variance, respectively
Figure FDA0003052975250000036
Figure FDA0003052975250000037
yjCorresponding to a Gaussian distribution mean and variance, respectively
Figure FDA0003052975250000038
Then there is
Figure FDA0003052975250000039
Figure FDA00030529752500000310
Figure FDA00030529752500000311
Figure FDA0003052975250000041
Wherein, alpha is a learning factor;
Figure FDA0003052975250000042
and
Figure FDA0003052975250000043
respectively representing x corresponding to the optimal individual, the suboptimal individual and the worst individual in the populationjA value;
Figure FDA0003052975250000044
respectively representing the y corresponding to the optimal individual, the suboptimal individual and the worst individual in the populationjA value; k is the number of the selected better population; x is the number ofjkThe x of the k-1 individual of the population is sorted from small to large according to the fitnessjA value; y isjkThe y of the k-1 individual of the population is sorted from small to large according to the fitnessjA value;
Figure FDA0003052975250000045
x as the K preferred populations selectedjkThe mean value of (a);
Figure FDA0003052975250000046
for y of the K preferred populations selectedjkThe mean value of (a);
(5) generating a t +1 generation EDA sub population according to the updated probability model
Figure FDA0003052975250000047
(6) Updating G of each particle in particle swarm optimizationbestAnd Pbest(ii) a Comparing the current fitness value of each particle with the individual extremum PbestAnd global extreme value GbestIf the particle is presentFitness value less than PbestThen P will bebestUpdating the current fitness value of the particle; if the current fitness value is less than GbestThen G will bebestUpdating 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 FDA0003052975250000048
(8) Establishing a new population
Figure FDA0003052975250000049
(9) Judging whether an algorithm termination condition is met, if not, turning to the step (3) to carry out t +1 times of iteration, otherwise, outputting a global extreme value GbestAnd 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.
2. The terrain-assisted navigation-oriented particle swarm fast matching method according to claim 1, wherein R is 1.2km and Ψ is 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.
5. A computer program comprising computer executable instructions which when executed perform 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 true CN113252039A (en) 2021-08-13
CN113252039B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114216465A (en) * 2021-12-08 2022-03-22 哈尔滨工程大学 Submarine topography navigation parallel matching method

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120022784A1 (en) * 2010-06-25 2012-01-26 Thales Navigation filter for a navigation system using terrain correlation
CN102426018A (en) * 2011-08-19 2012-04-25 北京航空航天大学 Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering
US20160047657A1 (en) * 2013-03-25 2016-02-18 Raytheon Company Autonomous range-only terrain aided navigation
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
GB2555805A (en) * 2016-11-09 2018-05-16 Atlantic Inertial Systems Ltd A navigation system
CN108871351A (en) * 2018-08-02 2018-11-23 哈尔滨工程大学 A kind of matched dynamic path planning method of AUV sea-floor relief
CN110617819A (en) * 2019-10-17 2019-12-27 国营芜湖机械厂 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning
CN111486845A (en) * 2020-04-27 2020-08-04 中国海洋大学 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
CN112230678A (en) * 2020-10-29 2021-01-15 皖江工学院 Three-dimensional unmanned aerial vehicle path planning method and planning system based on particle swarm optimization

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120022784A1 (en) * 2010-06-25 2012-01-26 Thales Navigation filter for a navigation system using terrain correlation
CN102426018A (en) * 2011-08-19 2012-04-25 北京航空航天大学 Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering
US20160047657A1 (en) * 2013-03-25 2016-02-18 Raytheon Company Autonomous range-only terrain aided navigation
CN105787489A (en) * 2016-03-04 2016-07-20 哈尔滨工程大学 Matching navigation algorithm based on underwater landform
GB2555805A (en) * 2016-11-09 2018-05-16 Atlantic Inertial Systems Ltd A navigation system
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
CN110617819A (en) * 2019-10-17 2019-12-27 国营芜湖机械厂 Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning
CN111486845A (en) * 2020-04-27 2020-08-04 中国海洋大学 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
CN112230678A (en) * 2020-10-29 2021-01-15 皖江工学院 Three-dimensional unmanned aerial vehicle path planning method and planning system based on particle swarm optimization

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
WANG, GUANGMIN AND MA, LINMAO: "The Estimation of Particle Swarm Distribution Algorithm With Sensitivity Analysis for Solving Nonlinear Bilevel Programming Problems", 《IEEE ACCESS》 *
程向红等: "基于改进粒子群优化的水下地形辅助导航方法", 《中国惯性技术学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114216465A (en) * 2021-12-08 2022-03-22 哈尔滨工程大学 Submarine topography navigation parallel matching method
CN114216465B (en) * 2021-12-08 2022-11-01 哈尔滨工程大学 Submarine topography navigation parallel matching method

Also Published As

Publication number Publication date
CN113252039B (en) 2022-10-28

Similar Documents

Publication Publication Date Title
CN109211276B (en) SINS initial alignment method based on GPR and improved SRCKF
CN112347840B (en) Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method
CN110487301B (en) Initial alignment method of radar-assisted airborne strapdown inertial navigation system
CN107314768B (en) Underwater terrain matching auxiliary inertial navigation positioning method and positioning system thereof
CN113252038B (en) Course planning terrain auxiliary navigation method based on particle swarm optimization
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
Han et al. An improved TERCOM-based algorithm for gravity-aided navigation
Han et al. A combined matching algorithm for underwater gravity-aided navigation
CN111366148B (en) Target positioning method suitable for multiple observations of airborne photoelectric observing and sighting system
CN112697138B (en) Bionic polarization synchronous positioning and composition method based on factor graph optimization
Wang et al. A GNSS/INS integrated navigation algorithm based on Kalman filter
CN111397599A (en) Improved ICCP (Integrated Circuit chip) underwater geomagnetic matching method based on triangular matching algorithm
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
CN110243377A (en) A kind of cluster aircraft collaborative navigation method based on hierachical structure
CN111024072A (en) Satellite map aided navigation positioning method based on deep learning
CN115683141A (en) Local reference path generation method for automatic driving in unknown environment
CN107525502B (en) Method for improving inertial terrain matching navigation average precision of underwater vehicle
CN116642482A (en) Positioning method, equipment and medium based on solid-state laser radar and inertial navigation
CN113252039B (en) Terrain-assisted navigation-oriented particle swarm fast matching method
US11465782B2 (en) Systems and methods for autonomous deorbiting of a spacecraft
CN114088098A (en) Auxiliary navigation path planning method for polar region underwater vehicle database
Wang et al. Land vehicle navigation using odometry/INS/vision integrated system
CN110873577B (en) Underwater rapid-acting base alignment method and device
CN116222551A (en) Underwater navigation method and device integrating multiple data

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