CN116952224A - Adaptive inertia/geomagnetic integrated navigation method based on geomagnetic chart suitability evaluation - Google Patents

Abstract

The invention discloses an adaptive inertial/geomagnetic combined navigation method based on geomagnetic map suitability evaluation. According to the suitability evaluation algorithm of the geomagnetic reference map, the geomagnetic matching suitability in the matching area is evaluated. According to the evaluation As a result, the parameters or periods of the integrated navigation filter are adaptively changed to improve the accuracy of the integrated navigation system. It solves the problem that the geomagnetic matching algorithm is sensitive to changes in the geomagnetic field in the matching area. In areas with low adaptability, the accuracy of the geomagnetic matching algorithm may decrease, or even fail to match, seriously affecting the accuracy of the integrated navigation system. question.

Description

Adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability assessment

Technical field

The present invention relates to the field of geomagnetic navigation, and in particular to an adaptive inertial/geomagnetic combined navigation method based on geomagnetic map suitability evaluation.

Background technique

The geomagnetic navigation system is a completely autonomous navigation method. It has the advantage that errors do not accumulate over time. It can enhance the inertial navigation system. The inertial/geomagnetic combined navigation system can improve the overall performance of the navigation system. The integrated navigation system uses two or more navigation systems to measure and solve the same navigation information to form quantitative measurements. From these quantitative measurements, the navigation system errors are calculated and corrected. The workflow of the traditional inertial/geomagnetic integrated navigation system is as follows: first, select appropriate geomagnetic components based on the geomagnetic information of the planned track area to make a geomagnetic reference map, and store the drawn geomagnetic reference map in the computer of the carrier aircraft. The geomagnetic sensor on the aircraft obtains the value of the matching characteristic quantity in the area in real time. At the same time, according to the reference track output by the inertial navigation system (INS) on the carrier, a matching area of appropriate size is selected, geomagnetic matching is performed, and the result is filtered and fused with the output of the INS to obtain the output of the inertial/geomagnetic integrated navigation system. Since the geomagnetic matching algorithm is more sensitive to changes in the geomagnetic field within the matching area, for example, in areas where the degree of self-similarity of the geomagnetic map is high or the map value changes relatively slowly, the accuracy of the geomagnetic matching algorithm may decrease or even fail to match. situation, seriously affecting the accuracy of the integrated navigation system.

Contents of the invention

In view of the above-mentioned deficiencies in the prior art, the present invention provides an adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability assessment.

In order to achieve the above-mentioned object of the invention, the technical solutions adopted by the present invention are:

An adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability assessment, including the following steps:

S1. Given the Kalman filter initial state equation X ₀ , initial covariance matrix P ₀ , variance matrix Q ₀ of the initial system noise sequence and variance matrix R ₀ of the initial measurement noise sequence;

S2. When the Kalman filter has the same frequency as the inertial navigation solution cycle, the time is updated to obtain the state estimate X _k and covariance matrix P _k at time k;

S3. When the measurement update cycle arrives, obtain the geomagnetic sensor measurement value, extract the geomagnetic map in the matching area based on the inertial navigation reference position at the current moment, and calculate the rough variance ratio based on the obtained geomagnetic map;

S4. Adaptively update the Kalman filter parameters according to the calculated rough variance ratio. If the rough variance ratio is less than the set threshold, no geomagnetic matching will be performed in this period. If the rough variance ratio is greater than or equal to the set threshold, the geomagnetic map will be passed. Adaptively correct the variance matrix of the measurement noise sequence;

S5. Obtain the observation at time k based on the positioning information output by geomagnetic matching and the reference position output by inertial navigation, update the Kalman filter measurement equation, and correct the inertial navigation error based on the measurement update results.

Further, the state equation of the Kalman filter in S1 is expressed as:

Among them, X(t) is the system error state variable established using the geographical coordinate system as the navigation coordinate system at time t, and

In the formula, φ _E , φ _N , φ _U are the attitude error angular components of the strapdown inertial navigation mathematical platform, δv _E , δv _N , δv _U are the northeast direction velocity error components, are the longitude, latitude, and altitude errors in sequence, ε _bx , ε _by , and ε _bz are once the three-axis component of the gyro constant drift error, /> is the accelerometer zero bias;

F(t) is the system state transition matrix:

In the formula, F _N is the corresponding 9-dimensional basic navigation parameter system array, F _S is the state transfer matrix between the inertial device error and the attitude speed position error, F _M is the state transfer matrix of the inertial device error itself, and

F _M =[0 _6×6 ]

is the coordinate transformation matrix from the missile coordinate system b to the navigation coordinate system n;

G(t) is the system noise transfer matrix and

W(t) is the system white noise error matrix and

In the formula, is the three-axis component of the gyroscope’s random error,/> is the three-axis component of the accelerometer’s random error.

Further, the specific calculation method of the rough variance ratio in S3 is:

ξ＝R/σ _m

Among them, ξ is the roughness variance ratio, R is the geomagnetic variation roughness and

R＝(R _x +R _y )/2

In the formula, R _x and R _y are the components in the corresponding directions of the geomagnetic variation roughness, m and n are the geomagnetic map size of the evaluated area, which is m×n, and M(i, j) is the geomagnetic map numbered (i, j) The geomagnetic intensity value of the grid;

σ _m is the standard deviation of geomagnetic intensity values and

In the formula, E _m is the average value of geomagnetic intensity in the area and

Further, the specific method of adaptively updating the Kalman filter parameters in S4 is:

Among them, R _k is the variance matrix of the measurement noise sequence at time k, and ξ is the rough variance ratio.

Further, in S5, the observation quantity at time k obtained based on the positioning information output by geomagnetic matching and the reference position output by inertial navigation is expressed as:

Among them, Z is the quantity measurement, Errors in longitude, latitude and altitude, in order,/> The longitude, latitude and altitude information output by strapdown inertial navigation are in sequence, λ _m ,/> is the longitude and latitude information output by geomagnetic matching, h _m is the altitude of the nearby space hypersonic aircraft output by the barometric altimeter.

Further, the specific method for updating the Kalman filter measurement equation in S5 is:

S51. Discretize the state equation and perform one-step prediction of the state based on the initial state.

in, is the convergence value of the state vector at time k-1;

S52. Use one-step prediction value for state estimation:

Among them, K _k is the filter gain matrix;

Among them, R _k is the covariance matrix of the measurement noise at time k, P _k/k-1 is the one-step prediction mean square error matrix, and its diagonal elements are the variances of each state estimate;

Among them, Q _k-1 is the covariance matrix of the system noise at time k-1, P _k-1 is the estimated mean square error matrix at time k-1, and the estimated mean square error matrix at time k P _k is

Among them, I is the unit matrix;

S53. Given the initial values X ₀ and P ₀ , according to the position measurement vector Z _k at time k, recursively obtain the state estimate at time k.

S54. After repeated calculation n times, we get convergence value.

The invention has the following beneficial effects:

According to the suitability evaluation algorithm of the geomagnetic reference map, the geomagnetic matching suitability in the matching area is evaluated, and the parameters or periods of the integrated navigation filter are adaptively changed according to the evaluation results to improve the accuracy of the integrated navigation system.

Description of the drawings

Figure 1 is a schematic flowchart of an adaptive inertial/geomagnetic combined navigation method based on geomagnetic map suitability assessment according to the present invention.

Detailed ways

The specific embodiments of the present invention are described below to facilitate those skilled in the art 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 technical field, as long as various changes These changes are obvious within the spirit and scope of the invention as defined and determined by the appended claims, and all inventions and creations utilizing the concept of the invention are protected.

An adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability assessment, as shown in Figure 1, includes the following steps:

S1. Given the Kalman filter initial state equation X ₀ , initial covariance matrix P ₀ , variance matrix Q ₀ of the initial system noise sequence and variance matrix R ₀ of the initial measurement noise sequence;

The state equation of the Kalman filter is expressed as:

Among them, X(t) is the system error state variable established using the geographical coordinate system as the navigation coordinate system at time t, and

In the formula, φ _E , φ _N , φ _U are the attitude error angular components of the strapdown inertial navigation mathematical platform, δv _E , δv _N , δv _U are the northeast direction velocity error components, are the longitude, latitude, and altitude errors in sequence, ε _bx , ε _by , ε _bz is once the three-axis component of the gyro constant drift error, /> is the accelerometer zero bias;

F(t) is the system state transition matrix:

In the formula, F _N is the corresponding 9-dimensional basic navigation parameter system array, F _S is the state transfer matrix between the inertial device error and the attitude speed position error, F _M is the state transfer moment of the inertial device error itself, and

F _M =[0 _6×6 ] (5)

G(t) is the system noise transfer matrix and

is the coordinate transformation matrix from the missile coordinate system b to the navigation coordinate system n;

W(t) is the system white noise error matrix and

In the formula, is the three-axis component of the gyroscope’s random error,/> is the three-axis component of the accelerometer’s random error.

The observation equation of the system can be expressed as:

Z(t)＝H(t)X(t)+V(t) (8)

Since the geomagnetic matching output position is a horizontal position and does not contain altitude information, the altitude error of the INS cannot be effectively corrected. Therefore, a barometric altimeter is introduced into the altitude channel to output altitude information to correct the altitude error of the SINS system.

S2. When the Kalman filter has the same frequency as the inertial navigation solution cycle, the time is updated to obtain the state estimate X _k and covariance matrix P _k at time k;

S3. When the measurement update cycle arrives, obtain the geomagnetic sensor measurement value, extract the geomagnetic map in the matching area based on the inertial navigation reference position at the current moment, and calculate the rough variance ratio based on the obtained geomagnetic map;

There are several indicators for evaluating the suitability of geomagnetic maps, among which the more representative one is the rough variance ratio. The standard deviation of geomagnetic intensity values can reflect the fluctuations of the geomagnetic background field. The greater the variance, the greater the fluctuations of the geomagnetic map, and the stronger the matching reliability. On the contrary, the worse the matching reliability. The standard deviation of geomagnetic intensity values is as follows:

In the formula, m and n represent that the size of the geomagnetic map in the assessed area is m×n, M(i,j) represents the geomagnetic intensity value of the grid numbered (i,j) in the geomagnetic map, and E _m is the geomagnetic intensity in the area. Average intensity value:

Roughness refers to the microscopic geometric shape characteristics composed of small spacing and tiny peaks and valleys on the surface of the geomagnetic background field. The greater the roughness of the geomagnetic change, the more obvious the surface features are, and the more conducive it is to navigation. The calculation of roughness R is given by equation (13):

R＝(R _x +R _y )/2 (13)

The rough variance ratio represents the change ratio of adjacent sampling points, and is defined as:

ξ＝R/σ _m (14)

A small ratio means that the changes between sampling points are small, but the entire area may have large and slow fluctuations, and the geomagnetic value changes relatively smoothly. A large ratio means that the changes between adjacent sampling points are relatively larger than the fluctuations of the entire area, so it is easier to achieve geomagnetic matching. This method uses the rough variance ξ ratio as the basis for evaluating the suitability of the geomagnetic map, and uses ξ to The R _k matrix in the integrated navigation system is corrected.

S4. Adaptively update the Kalman filter parameters according to the calculated rough variance ratio. If the rough variance ratio is less than the set threshold, no geomagnetic matching will be performed in this period. If the rough variance ratio is greater than or equal to the set threshold, the geomagnetic map will be passed. Adaptively correct the variance matrix of the measurement noise sequence;

The quantity measurement Z of the Kalman filter observation equation consists of the difference Z ₁ between the horizontal position information output by the INS and the geomagnetic matching output, and the difference Z ₂ between the altitude information output by the SINS and the altitude information output by the altimeter. Z ₁ and Z ₂ are defined as follows:

Z ₂ =[h _s -h _m ] (16)

In formulas (15) and (16), They are the longitude, latitude and height information output by SINS;/> is the latitude and longitude information output by geomagnetic matching; h _m is the altitude of the nearby space hypersonic aircraft output by the barometric altimeter. Therefore

In formula (8), H() is the observation matrix

H(t)＝[H ₁₁ H ₁₂ H ₁₃ H ₁₄ H ₁₅ ] ^T (18)

The expressions of each component of the matrix in the above formula are

H ₁₁ ＝H ₁₃ ＝H ₁₄ ＝H ₁₅ ＝0 _3×3 (19)

V() is the observation noise matrix of geomagnetic matching. because

therefore,

S5. Obtain the observation at time k based on the positioning information output by geomagnetic matching and the reference position output by inertial navigation, update the Kalman filter measurement equation, and correct the inertial navigation error based on the measurement update results.

After discretizing the state equation (1), assume that the estimated state X _k at time t _k is driven by the system noise W _k-1 . The driving mechanism is described by the following state equation.

X _k ＝Φ _k，k-1 X _k-1 +Γ _k-1 W _K-1 (22)

The measurement equation is

_{Zk ＝ HkXk} + _Vk (23)

Among them, Φ _{k, k-1} is the one-step transfer matrix at time t _k-1 , Γ _k-1 is the system noise driving matrix, W _k is the system noise, generally considered to be white noise; H _k is the measurement matrix, V _k is the measurement noise; at the same time, W _k and V _k satisfy

Among them, Q _k is the variance matrix of the system noise sequence; R _k is the variance matrix of the measurement noise sequence.

_{If the} estimator X _k satisfies equation ₍ 22) _, the measurement Z _k of Definite, the variance matrix R _k of the measurement noise sequence is positive definite, and is corrected through the geomagnetic map suitability evaluation ξ:

The specific way to update the Kalman filter measurement equation is:

S51. Discretize the state equation and perform one-step prediction of the state based on the initial state.

in, is the convergence value of the state vector at time k-1;

S52. Use one-step prediction value for state estimation:

Among them, K _k is the filter gain matrix;

Among them, R _k is the covariance matrix of the measurement noise at time k, P _k/k-1 is the one-step prediction mean square error matrix, and its diagonal elements are the variances of each state estimate;

Among them, Q _k-1 is the covariance matrix of the system noise at time k-1, P _k-1 is the estimated mean square error matrix at time k-1, and the estimated mean square error matrix at time k P _k is

Among them, I is the unit matrix;

S53. Given the initial values X ₀ and P ₀ , according to the position measurement vector Z _k at time k, recursively obtain the state estimate at time k.

S54. After repeated calculation n times, we get convergence value.

The present invention uses specific embodiments to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for those of ordinary skill in the art, based on this The idea of the invention will be subject to change in the specific implementation and scope of application. In summary, the contents of this description should not be understood as limiting the invention.

Those of ordinary skill in the art will appreciate that the embodiments described here are provided to help readers understand the principles of the present invention, and it should be understood that the scope 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 modifications and combinations based on the technical teachings disclosed in the present invention without departing from the essence of the present invention, and these modifications and combinations are still within the protection scope of the present invention.

Claims (6)

1. The adaptive inertial/geomagnetic integrated navigation method based on geomagnetic chart suitability evaluation is characterized by comprising the following steps of:

s1, giving an initial state equation X of a Kalman filter ₀ Initial covariance matrix P ₀ Variance matrix Q of initial system noise sequence ₀ And variance matrix R of initial measurement noise sequence ₀ ；

S2, updating time when the Kalman filter and the inertial navigation resolving period are in the same frequency, and obtaining a state estimation value X of k moment _k And covariance matrix P _k ；

S3, acquiring a geomagnetic sensor measurement value when a measurement update period comes, extracting a geomagnetic chart in a matching area according to an inertial navigation reference position at the current moment, and calculating a rough variance ratio according to the acquired geomagnetic chart;

s4, carrying out Kalman filter parameter self-adaptive updating according to the calculated rough variance ratio, if the rough variance ratio is smaller than a set threshold, not carrying out geomagnetic matching in the period, and if the rough variance ratio is larger than or equal to the set threshold, adaptively correcting a variance array of a measurement noise sequence through a geomagnetic map;

s5, obtaining the observed quantity at the moment k according to the positioning information output by geomagnetic matching and the reference position output by inertial navigation, updating a Kalman filter measurement equation, and correcting inertial navigation errors according to the measurement updating result.

2. The adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability evaluation as set forth in claim 1, wherein the state equation of the kalman filter in S1 is expressed as:

wherein X (t) is a system error state variable established by taking a geographic coordinate system as a navigation coordinate system at the moment t and

in the formula ,φ_E ,φ _M ,φ _U For strapdown inertial navigation mathematical platform attitude error angle component δv _E ,δv _N ,δv _U For the northeast tangential velocity error component, δλ,δh is the longitude, latitude and altitude errors in turn, ε _bx ,ε _by ,ε _bz Once is the constant drift error triaxial component of the gyro, < >>Zero offset for the accelerometer;

f (t) is a system state transition matrix:

in the formula ,F_N For corresponding 9-dimensional basic navigation parameter system array, F _S Is a state transition matrix between inertial device errors and attitude speed position errors, F _M State transition matrix for inertial device error itself and

F _M ＝[0 _6×6 ]

the coordinate transformation matrix from the projectile coordinate system b to the navigation coordinate system n;

g (t) is a system noise transfer matrix and

w (t) is a system white noise error matrix and

in the formula ,is the three-axis component of the random error of the gyroscope, +.>Is the three-axis component of the random error of the accelerometer.

3. The adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability evaluation as set forth in claim 1, wherein the specific calculation mode of the rough variance ratio in S3 is as follows:

ξ＝R/σ _m

wherein, xi is the rough variance ratio, R is the geomagnetic variation roughness and

R＝(R _x +R _y )/2

in the formula ,R_x 、R _y For the components in the direction corresponding to the geomagnetic variation roughness, M and n are geomagnetic map sizes of the evaluated area are M multiplied by n, and M (i, j) is geomagnetic intensity value of a grid numbered (i, j) in the geomagnetic map;

σ _m is the standard deviation of geomagnetic intensity value

in the formula ,E_m Is the average value of geomagnetic intensity values in the area and

4. the adaptive inertial/geomagnetic integrated navigation method based on geomagnetic chart suitability evaluation as set forth in claim 1, wherein the specific way of adaptive update of the kalman filter parameters in S4 is:

wherein ,R_k Method for measuring noise sequence at k momentAnd a difference matrix, wherein xi is a rough variance ratio.

5. The adaptive inertial/geomagnetic integrated navigation method based on geomagnetic map suitability evaluation as set forth in claim 1, wherein the obtaining of the observed quantity at the time k from the positioning information output by geomagnetic matching and the reference position of inertial navigation output in S5 is expressed as:

where Z is the number of measurements, δλ,δh is the error of longitude, latitude and altitude in turn, λ _s ,/>h _s Longitude, latitude and altitude information, lambda which are sequentially output by strapdown inertial navigation _m 、/>Longitude and latitude information output for geomagnetic matching, h _m Is the altitude of the hypersonic aircraft in the near space output by the barometric altimeter.

6. The adaptive inertial/geomagnetic integrated navigation method based on geomagnetic chart suitability evaluation as set forth in claim 1, wherein the specific way of updating the kalman filter measurement equation in S5 is:

s51, discretizing a state equation, and predicting the state in one step according to the initial state

wherein ,a convergence value of the state vector at the moment k-1;

s52, performing state estimation by using the one-step predicted value:

wherein ,K_k Is a filtering gain matrix;

wherein ,R_k For measuring the covariance matrix of the noise at the moment k, P _k/k-1 A mean square error matrix is predicted in one step, and diagonal elements of the mean square error matrix are variances of all state estimates;

wherein ,Q_k-1 Is the covariance matrix of the system noise at the moment k-1, P _k-1 Estimating a mean square error matrix for time k-1, and estimating a mean square error matrix P for time k _k Is that

Wherein I is a unit array;

s53, giving initial value X ₀ and P₀ Based on the position measurement vector Z at time k _k Recursively obtaining state estimation at k time

S54, repeating the calculation for n times to obtainIs a convergence value of (a).