Adaptive search particle swarm terrain matching positioning and integrated navigation method

Abstract

The invention discloses an adaptive search particle swarm terrain matching positioning and integrated navigation method, and belongs to the field of aircraft navigation. The weight is dynamically adjusted according to the population state during searching, the particle swarm algorithm is adaptively improved, and the success rate and the convergence rate are comprehensively optimized, so that when the particle swarm algorithm is applied to terrain matching, the overall performance of a matching system can be improved, and algorithm comprehensive optimization with the matching success rate and the convergence rate as indexes is realized.

Description

Technical Field

[0001] The present invention relates to the field of aircraft navigation, and particularly to an adaptive search particle swarm terrain matching positioning and integrated navigation method. Background Art

[0002] Terrain matching navigation is a navigation method that detects terrain height by the difference between terrain radar height and barometric height, searches for the point on a pre-made terrain feature map that best matches the features detected by the sensor, determines the position of the detection point on the map, and realizes the positioning of the vehicle. Terrain matching navigation has the advantages of strong autonomy and the navigation characteristics of the navigation information source being not easily changed. Terrain features do not change significantly unless there are major changes in the terrain and landforms of the region. Terrain has long been used as a navigation information source in the field of navigation and has been widely applied to some long-endurance cruise aircraft.

[0003] Existing terrain matching relies heavily on the prior information of the detection point and it is difficult to carry out matching positioning under the conditions of vague prior information and a large matching range. Its core problem is the multi-extremum problem of the map. The Particle Swarm Optimization (PSO) algorithm is a search algorithm that can achieve a global optimal matching point search over a large range, but it has poor timeliness and a long matching time each time. The main search parameters of the particle swarm algorithm include the inertia weight, the individual weight, and the swarm weight. The parameter configuration will affect the convergence speed and the matching success rate. Among them, the inertia weight enables the particle to maintain its current motion state, making the particle behavior exhibit some randomness, making the search more sufficient and affecting the convergence speed. The swarm weight can make the particle converge to the optimal point that has been found, but it may converge too fast, resulting in insufficient search and falling into a local extremum. The individual weight provides a large amount of search history information for adjusting the particle behavior and searching for the possible optimal point near the current sub-optimal point. Summary of the Invention

[0004] In view of the above deficiencies in the prior art, the present invention provides an adaptive search particle swarm terrain matching positioning and integrated navigation method.

[0005] In order to achieve the above-mentioned invention purpose, the technical solution adopted by the present invention is as follows: An adaptive search particle swarm terrain matching positioning and integrated navigation method, comprising the following steps: S1. Define the matching area and establish a matching map; S2. Calculate the terrain height within the matching area through the barometric height and the radar height to obtain the elevation information of multiple points within the matching area; S3. Extract matching parameters from a section of data in the elevation information and use the adaptive particle swarm algorithm for matching positioning; S4. Output the matching positioning result.

[0006] Further, the matching parameter in S2 is expressed as:

[0007] In the formula, is the matching parameter, is the point coordinate, is the sequence rotating around the point by the rotation angle.

[0008] Further, the specific method of using the adaptive particle swarm algorithm for matching and positioning in S3 is as follows: S31. Initialize the particle swarm search, divide the particle search range, and assign particle position, velocity, and quantity attributes; S32. Randomly assign initial values to the particle position and velocity, establish a fitness function as the error metric, and calculate the matching error of each particle at the search position according to the position; S33. Establish a particle motion model, update the velocity and position according to the particle motion model, calculate the fitness of the current position, compare it with the historical best fitness of the particle, record the individual best position so far, and select the global best position from all individual historical best positions; S35. Judge whether the individual best position is less than the error threshold compared with the global best position. If so, stop the search and output the result. If not, perform step S36; S36. Extract feature parameters from the particle individual best position and the global best position, judge the evolution state from the feature parameters, and adjust the weight parameters of the particle motion and return to step S33.

[0009] Further, the error metric in S32 is expressed as:

[0010] In the formula, is the coordinate of the j-th grid point in the grid point sequence, is the coordinate of the i-th grid point, is the total number of grid points.

[0011] Further, the specific method of updating the velocity of the particle according to the particle motion model in S33 is as follows:

[0012] In the formula, is the particle velocity at the current i-th moment, is the particle velocity at the previous moment, is the inertia weight; is the individual historical best position, is the individual iThe current position of is to track the individual experience weight; gbest is the group historical optimal position, is to track the group experience weight.

[0013] Furthermore, the individual optimal position in S33 is expressed as:

[0014] In the formula, is the individual historical optimal position, is the optimal position of the individual numbered m; The group optimal position is expressed as:

[0015] In the formula, is the group historical optimal position, is the group optimal position, where represents the numbers of the individuals in the group that have reached the group optimal position.

[0016] Furthermore, the specific method for judging the evolution state from the characteristic parameters and adjusting the weight parameters of the particle movement in S36 is: S361. Perform search iteration using the basic particle swarm algorithm; S362. When one generation of search is completed, enter parameter adaptive adjustment, and calculate the evolution factor according to the map average gradient; S363. Calculate the control factor according to the evolution factor, and adaptively adjust the tracking individual experience weight and the tracking group experience weight according to the constraints of the design strategy and return to step S33.

[0017] Furthermore, the calculation method of the evolution factor in S362 is:

[0018] In the formula, is the evolution factor, is the global optimal matching point error, is the ground average map gradient, is the distribution of the top 10% individuals in the group, is the statistical threshold.

[0019] Furthermore, the specific method for adaptively adjusting the tracking individual experience weight and the tracking group experience weight in S363 is:

[0020]

[0021] In the formula, is the tracking individual experience weight, For tracking the group experience weight, and: , is the inertia weight; is the control factor and:

[0022] In the formula, is the evolution factor.

[0023] The present invention has the following beneficial effects: The present invention proposes an adaptive particle swarm terrain matching integrated navigation method. By extracting the evolution state parameters from the particle swarm and adaptively calculating the weights, the comprehensive performance of the algorithm's matching success rate and matching time is improved. The comprehensive energy efficiency of the integrated navigation algorithm is enhanced. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 is a schematic diagram of the adaptive particle swarm terrain matching positioning process of the present invention.

[0025] Figure 2 is a schematic diagram of the adaptive particle swarm matching method of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0026] The following describes the specific embodiments of the present invention to facilitate those skilled in the art of the present technology to understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific embodiments. For those of ordinary skill in the art of the present technology, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions created using the concept of the present invention are within the scope of protection.

[0027] An adaptive search particle swarm terrain matching positioning and integrated navigation method, as Figure 1 shown, includes the following steps: S1. Define the matching area and establish a matching map; S2. Calculate the terrain height in the matching area through the barometric altitude and the radar altitude to obtain the elevation information of multiple points in the matching area; This method is applicable to inertial / terrain matching integrated navigation, and corrects the inertial navigation system with the terrain matching result to achieve the purpose of integrated navigation. The inertial navigation model and the inertial navigation state equation adopt the conventional local horizontal coordinate system navigation model. The integrated navigation system model is: (1) The integrated navigation system requires the terrain matching system to provide the longitude and latitude information of the carrier. First, the terrain matching system needs to load the terrain data into the navigation computer. Secondly, it uses the altimetry system to measure the elevation information of n points, constructs a matching sequence from the relative positions of N points output by the inertial navigation system, and the inertial navigation system gives the longitude and latitude of the i th sample as , which is converted into grid points and denoted as , obtaining the grid point sequence , then the sequence contains N grid points as: (2) Extract the matching parameters for terrain matching from this sequence: (3) Among them, is the angle of rotation of the sequence around the point . The above matching parameters are used as particles of the adaptive particle swarm optimization algorithm to participate in the matching.

[0028] At the same time, establish a measure of the sequence matching error: (4) The above measure is used as the error function of the adaptive particle swarm optimization algorithm to evaluate the quality of particles.

[0029] After the matching is completed, the matching system outputs the matching positioning result in the form of longitude and latitude .

[0030] S3. Extract the matching parameters from a section of data in the elevation information and use the adaptive particle swarm optimization algorithm for matching positioning; As Figure 2 shown, use the particle swarm optimization algorithm for matching positioning to search for the best matching point. The method first establishes a matching search framework for the particle swarm optimization algorithm. Denote the particle position as p, the velocity as v, and the search range as x×y. The particles randomly generate the initial position p and the initial velocity v.

[0031] There are n particles in the population. Denote m as the particle number, ; i is the number of iterations. Usually, a limit is placed on the maximum number of iterations during the search. In this method, the search stops when i increases to 200.

[0032] Each search, the particle records the point with the smallest matching error among the positions it has passed through. The algorithm updates its own velocity by tracking the optimal position and fitness of a single particle record, as well as the optimal position and fitness of the particle swarm.

[0033] The particle first updates (5) Among them, is the particle velocity at the previous moment, is the inertia weight; is the individual historical optimal position, that is, the optimal solution found by the particle itself, is the tracking individual experience weight; gbest is the global historical optimal position, that is, the optimal solution currently found by the entire population, is the tracking global experience weight.

[0034] The individual historical optimal position refers to the position passed through at the k-th time among the positions passed through in the previous i search iterations when the particle has performed i search iterations, and its matching error reaches the minimum among all the positions passed through by the particle, that is, The individual optimal position pbest takes the value of:

[0035] where the of the m-th individual satisfies: (6) The global optimal position gbest takes the value of:

[0036] where satisfies: (7) In the formula, when the number of iterations is i, the fitness of the global historical optimal position is denoted as .

[0037] The above design can make the search point gradually move towards the matching point with small error, and at the same time fully search the possible area where the matching point may appear during the particle movement process.

[0038] Set the algorithm optimization stop condition. The estimated matching error limit is , which is the total error combining the system error, map error, and sensor measurement error. When the matching is correct, it is 3 times the total error between the gravitational force of the matching point and the measured value, as the algorithm stop condition.

[0039] (8) When the above conditions are met, it is judged that the iteration ends. Among them, is the matching result of the first ten individuals.

[0040] During the search process, according to the search progress, an adaptive weight strategy is adopted. In the particle swarm algorithm, the inertia weight , the individual experience weight , the global experience weight The three weights always satisfy (9) The characteristic of the terrain matching problem is that within a selected search range large enough, there is an optimal solution, and at the same time there are several sub-optimal solutions. The sub-optimal solutions reach a minimum within a certain neighborhood near them, but it is not the global minimum. The optimal matching point can reduce the matching error to the error threshold. If the measurement conditions are normal, the measurement error will not exceed the threshold. The fitness function of the particle search is constructed by choosing the matching error. The position with the smallest individual matching error is the individual optimal point, and the position with the smallest matching error for the entire population is the population optimal point. Denote the matching error as: This method determines the three motion weights of the particle swarm algorithm through two population parameters. The two population parameters are: 1. The matching error of the population optimum, denote the error of the global optimum matching point as . 2. The concentration degree of the optimal matching individuals. The distribution of the top 10% individuals in the population is 1 is . Furthermore, there are three situations: The population individuals are dispersed and the optimal matching error is large, that is is large and is large, and the population does not show a convergence trend; The differences among the population individuals are small and the optimal matching error is large, that is is small but is large, and it may fall into a local extreme value; The differences among the population individuals are large and the optimal matching error is small, that is is large but is small, and the optimal result may appear.

[0041] The parameter adaptive strategy is designed as follows. First, let the particle swarm algorithm run according to the basic particle swarm algorithm. After searching for one generation, enter the parameter adaptive adjustment module. The parameter adaptive adjustment module first calculates the average gradient of the map , and calculates the evolution factor according to the following formula: (10) where is the statistical threshold of the measurement. Calculate the control factor according to the evolution factor: (11) The calculation formula for the particle swarm adaptive control coefficient is: The basic individual optimal weight is 0.5, the upper limit is 0.5, and the lower limit is 0.2. The adjustment strategy is: (12) The basic global optimal weight is 0.2, the lower limit is 0.2, and the upper limit is 0.8. The adjustment strategy is: (13) wherein is the matching error limit.

[0042] The constraint of the design strategy is that the sum of the three weights is 1, that is, in Equation (5), there is: (14) Determine the inertia weight accordingly.

[0043] S4. Output the matching positioning result.

[0044] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, and the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0045] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including instruction means, and the instruction means implements the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0046] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0047] Specific embodiments are applied in the present invention to elaborate on the principles and implementation manners of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation to the present invention.

[0048] Those of ordinary skill in the art will realize that the embodiments described herein are for helping the reader understand the principles of the present invention, and it should be understood that the scope of protection of the present invention is not limited to such specific statements and embodiments. Those of ordinary skill in the art can make various other specific deformations and combinations that do not depart from the essence of the present invention based on these technical revelations disclosed by the present invention, and these deformations and combinations are still within the scope of protection of the present invention.

Claims

1. An adaptive search particle swarm terrain matching positioning and integrated navigation method, characterized in that: The steps include: S1. Define the matching area and establish a matching map; S2. Calculate the terrain height in the matching area by using the pressure altitude and the radar altitude to obtain the elevation information of multiple points in the matching area; S3, extracting matching parameters from a segment of data in the elevation information, and using an adaptive particle swarm algorithm for matching and positioning; S4. Output the matching positioning result.

2. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 1 is characterized in that: The matching parameters in S2 are expressed as: In the formula, To match the parameters, is the point coordinate, Wrap the sequence around The angle of rotation.

3. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 1 is characterized in that: The specific method of using the adaptive particle swarm algorithm for matching and positioning in S3 is: S31, particle swarm search initialization, dividing the particle search range and assigning particle position, speed and quantity attributes; S32, randomly assigning initial values ​​to the particle positions and velocities, establishing a fitness function as an error metric, and calculating the matching error of each particle when it is at the search position according to the position; S33, establish a particle motion model, update the speed and position according to the particle motion model, calculate the fitness of the current position, and compare it with the historical optimal fitness of the particle, record the individual optimal position so far, and select the group optimal position from all individual historical optimal positions; S34, determine whether the individual optimal position and the group optimal position are less than the error threshold, if so, stop searching and output the result, if not, proceed to step S35; S35, extracting characteristic parameters from the optimal position of the individual particles and the optimal position of the group, judging the evolution state from the characteristic parameters and adjusting the weight parameters of the particle motion, and returning to step S33.

4. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 3 is characterized in that: The error metric in S32 is expressed as: In the formula, is the coordinate of the jth grid point in the grid point sequence, is the coordinate of the ith grid point, is the total number of grid points.

5. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 3 is characterized in that: The specific method of updating the velocity of the particle according to the particle motion model in S33 is: In the formula, is the particle velocity at the current moment i, is the particle velocity at the previous moment, is the inertia weight; is the individual's best historical position, For individuals i Current location, To track individual experience weights; gbest is the historical optimal position of the group, To track group experience weights.

6. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 3 is characterized in that: The individual optimal position in S33 is expressed as: In the formula, is the individual's best historical position, is the optimal position of the individual numbered m; The optimal position of the group is expressed as: In the formula, is the historical optimal position of the group, is the optimal position of the group, where Indicates the number of the individual in the group that has reached the optimal position of the group.

7. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 3 is characterized in that: The specific method of judging the evolution state from the characteristic parameters and adjusting the weight parameters of the particle motion in S36 is: S361, basic particle swarm algorithm for search iteration; S362, after searching for one generation, enter the parameter adaptive adjustment, and calculate the evolution factor according to the average gradient of the map; S363. Calculate the control factor according to the evolution factor, and adaptively adjust the tracking individual experience weight and the tracking group experience weight according to the constraints of the design strategy and return to step S33.

8. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 7, characterized in that: The calculation method of the evolution factor in S362 is: In the formula, is the evolution factor, is the global optimal matching point error, is the ground average map gradient, is the distribution of the top 10% of individuals in the group, Statistical threshold.

9. The adaptive search particle swarm terrain matching positioning and integrated navigation method according to claim 7, characterized in that: The specific method of adaptively adjusting the tracking individual experience weight and the tracking group experience weight in S363 is: In the formula, To keep track of individual experience weights, To track group experience weights, and: , is the inertia weight; is the control factor and: In the formula, The evolution factor.

Images