Abnormal information processing method for inertial satellite navigation integrated navigation

Abstract

The invention discloses an abnormal information processing method for inertial satellite navigation integrated navigation, which comprises the following steps of: S1, forming an inertial navigation system by using three gyroscopes and three accelerators, and performing inertial navigation calculation; s2, estimating and correcting an attitude misalignment angle by combining satellite navigation information on the basis of pure inertial navigation solution, and readjusting the position and the speed; and S3, a measurement weight factor determination method. According to the method, a Kalman filter is used for carrying out combined filtering on navigation information of an inertial navigation system and navigation information of a satellite navigation system to obtain optimal navigation information, and when the positioning result of the satellite navigation system is abnormal under the condition that the satellite navigation system is interfered, a measurement weight determination method is used for properly processing the abnormal information, so that the positioning accuracy of the satellite navigation system is improved. And stable navigation information output of the integrated navigation system is ensured.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to an abnormal information processing method for inertial satellite integrated navigation. Background Art

[0002] The inertial navigation system has the advantages of high autonomy, strong anti-interference ability, and high real-time output, but it has the disadvantages of error accumulation over time and high price. The satellite navigation system has the advantages of high positioning accuracy, no error divergence over time, and low price, but it also has the disadvantages of poor real-time output and susceptibility to external interference. The combined navigation system composed of the two can combine their respective advantages to achieve long-term, high-precision real-time output of navigation information at a lower cost.

[0003] In a complex working environment, the satellite navigation system is susceptible to various environmental interferences, which may reduce the navigation information accuracy of the satellite navigation system or cause sudden changes and other faults. At this time, if the fault information is directly used for combined filtering without identification, the navigation accuracy of the combined navigation system will be greatly reduced. Summary of the invention

[0004] The present invention aims to solve at least one of the technical problems existing in the prior art. To this end, one purpose of the present invention is to provide an abnormal information processing method for inertial satellite navigation integrated navigation, which uses a Kalman filter to perform combined filtering on the navigation information of the inertial navigation system and the navigation information of the satellite navigation system to obtain the optimal navigation information. When the positioning result of the satellite navigation system is abnormal under interference, a measurement weight determination method is used to appropriately process the abnormal information to ensure the stability of the navigation information output of the integrated navigation system.

[0005] In order to solve the above problems, the present invention provides an abnormal information processing method for inertial satellite integrated navigation, comprising the following steps:

[0006] S1..Use three gyroscopes and three accelerometers to form an inertial navigation system and perform inertial navigation calculation;

[0007] S2. Estimate and correct the attitude misalignment angle based on pure inertial navigation solution and satellite navigation information, and readjust the position and speed;

[0008] S3. Method for determining measurement weight factor.

[0009] Preferably, the inertial navigation solution method of S1 includes:

[0010] The direction cosine matrix corresponding to the initial attitude angle is The corresponding attitude quaternion is recorded as Will Convert to attitude quaternion form:

[0011]

[0012] When k = 0, 1, 2, ..., the quaternion update algorithm is used to calculate t k+1 The quaternion of the attitude at the moment

[0013]

[0014] in:

[0015]

[0016] Δθ=[Δθ x Δθ y Δθ z ] T is the gyro output angle increment within the sampling period (tk, tk+1], unit: rad;

[0017] The angle that the n system rotates relative to the i system during the update period (tk, tk+1):

[0018]

[0019] is the projection of the rotation angular velocity of the e system relative to the i system in the n system, is the projection of the angular velocity of the n system relative to the e system on the n system, which is calculated by the following two formulas:

[0020]

[0021] The speed, latitude and curvature radius in the formula are all the results of the previous navigation solution update cycle; finally, quaternion normalization is performed

[0022]

[0023] Get the updated attitude quaternion

[0024] The velocity increment Δv output by the accelerometer is transformed into the velocity component in the geographic coordinate system Where Δv=[Δv x Δv y Δv z ] T is the velocity increment output by the accelerometer;

[0025] The acceleration component in the geographic coordinate system is f n =Δv n / h, where f n =[f E fN f U ] T ;

[0026] Initial time of velocity update, v n (0) =

[000] T ;

[0027] When k=0, 1, 2, ..., in a speed update cycle (tk, tk+1], the speed is updated to

[0028]

[0029] Where δA is the harmful acceleration, which is calculated as follows:

[0030]

[0031] At the initial moment, L(0), λ(0) and S(0) are the bound latitude, longitude and altitude;

[0032] When k = 0, 1, 2, ..., within a speed update cycle (tk, tk+1], the position is updated as follows:

[0033]

[0034] Preferably, the estimation and correction method of S2 includes:

[0035] Kalman filter one-step prediction is divided into state transition matrix Φ k,k-1 Calculation of the input noise variance matrix Calculation, state prediction and the error variance prediction P k,k-1 There are three stages of calculation:

[0036] 1) State transition matrix Φ k,k-1 Calculation

[0037] Let (tk-1, tk] be a prediction period, h = tk-tk-1; the prediction period h is generally short, and the state transition matrix is ​​calculated as follows

[0038]

[0039] in,

[0040]

[0041]

[0042] 2) Input noise variance matrix Calculation

[0043] The system noise of the continuous system, i.e., the covariance matrix of the three gyroscopes and three accelerometer vectors W(t), is Q(t), and the variance matrix of the input noise is

[0044] Q q =G(t)Q(t)G T (t);

[0045] Where Q(t) is a constant and G(t) is the noise input matrix, which can be rewritten as follows:

[0046]

[0047] Get the input noise variance Q of the continuous system q Then calculate the input noise variance of the Kalman filter as follows:

[0048]

[0049] 3) Status prediction and the error variance prediction P k,k-1 Calculation:

[0050] At the initial moment when k = 0, and P 0 Initialize;

[0051] When k=0, 1, 2, ..., recursive calculation:

[0052]

[0053] When the filter update cycle has not arrived, the filter update is not performed and the prediction continues.

[0054]

[0055] P k =P k,k-1 ;

[0056] When the filter update period arrives, P k Follow the filter update calculations in the next section.

[0057] Preferably, when the filtering time is up, the filtering is updated, and the filtering update period is the external reference information update period, and the filtering update period is set to 1s, which is divided into four steps of calculation:

[0058] 1) Measurement and calculation

[0059] Calculate the measured value as follows:

[0060]

[0061] The subscript s represents the strapdown inertial navigation system solution output, and the subscript r represents the reference datum output;

[0062] v sE 、v sN 、v sU To solve the eastward velocity, northward velocity and vertical velocity, the unit is m / s;

[0063] L s , s To solve the latitude and longitude, the unit is rad;

[0064] S s To calculate the height, unit: m;

[0065] v rE 、v rN 、v rU is the reference speed, unit: m / s;

[0066] L r , r To bind latitude and longitude, the unit is rad;

[0067] S r is the reference height, unit: m;

[0068] 2) Filter gain calculation

[0069] Calculate the filter gain K as follows: k :

[0070]

[0071] Among them, P k,k-1 is the error variance prediction calculation; H k =[0 6×3 I 6×6 0 6×6 ];

[0072] R k =diag[(R ve ) 2 (R vn ) 2 (R vu ) 2 (R lat ) 2 (R lon ) 2 (R h ) 2 ]

[0073] 3) State estimation update

[0074] The state estimate is calculated as follows:

[0075]

[0076] in, Calculated for state prediction;

[0077] 4) Error variance update

[0078] The error variance is calculated as follows:

[0079] Preferably, the method for determining the measurement weight factor in S3 is:

[0080]

[0081] w i is the weight coefficient of the i-th component in the measurement vector;

[0082] c h is the adjustment factor, which can generally be taken as 1.345;

[0083] u i is the standardized absolute residual;

[0084] S is the residual scale.

[0085] The advantages of the present invention compared with the prior art are:

[0086] The invention discloses an abnormal information processing method for inertial satellite navigation integrated navigation. In an inertial satellite navigation integrated navigation system, the new information of the Kalman filter is weighted, and the size of the measurement information is adjusted by different weight factors. On the one hand, normal and reasonable useful information is fully utilized, and on the other hand, abnormal information is timely identified and the influence of the abnormal information on the integrated navigation system is weakened by using the weight factors, so as to realize continuous, smooth and accurate combined positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or technical descriptions will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0088] Figure 1 : It is a schematic diagram of the navigation solution process of S1 of the present invention;

[0089] Figure 2 : is a schematic diagram of the inertial satellite navigation integrated navigation process of the present invention. DETAILED DESCRIPTION

[0090] The embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be understood as limiting the present application.

[0091] In the description of this application, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0092] The present invention is further described in detail below in conjunction with the accompanying drawings.

[0093] In order to solve the above problems, the present invention provides an abnormal information processing method for inertial satellite integrated navigation, such as Figure 1 As shown:

[0094] S1..Use three gyroscopes and three accelerometers to form an inertial navigation system and perform inertial navigation calculation;

[0095] The direction cosine matrix corresponding to the initial attitude angle is The corresponding attitude quaternion is recorded as Will Convert to attitude quaternion form:

[0096]

[0097] When k = 0, 1, 2, ..., the quaternion update algorithm is used to calculate t k+1 The quaternion of the attitude at the moment

[0098]

[0099] in

[0100]

[0101] Δθ=[Δθ x Δθ y Δθ z ] T is the gyro output angle increment within the sampling period (tk, tk+1], unit: rad;

[0102] The angle that the n system rotates relative to the i system during the update period (tk, tk+1)

[0103]

[0104] is the projection of the rotation angular velocity of the e system relative to the i system in the n system, is the projection of the angular velocity of the n system relative to the e system on the n system, which is calculated by the following two formulas:

[0105]

[0106] The speed, latitude and curvature radius in the formula are all the results of the previous navigation solution update cycle. Finally, the quaternion normalization is performed:

[0107]

[0108] Get the updated attitude quaternion The velocity increment Δv output by the accelerometer is transformed into the velocity component in the geographic coordinate system Where Δv=[Δv x Δv y Δv z ] T It is the velocity increment output by the accelerometer.

[0109] The acceleration component in the geographic coordinate system is f n =Δv n / h, where f n =[f E f N f U ] T .

[0110] Initial time of velocity update, v n (0) =

[000] T ;

[0111] When k=0, 1, 2, ..., in a speed update cycle (tk, tk+1], the speed is updated to

[0112]

[0113] in,

[0114] δA is the harmful acceleration, calculated as follows:

[0115]

[0116] At the initial moment, L(0), λ(0) and S(0) are the bound latitude, longitude and altitude;

[0117] When k=0, 1, 2, ..., within a speed update cycle (tk, tk+1], the position is updated as

[0118]

[0119] S2. Estimate and correct the attitude misalignment angle based on pure inertial navigation solution and satellite navigation information, and readjust the position and speed;

[0120] The inertial satellite navigation system is a process that uses external information (satellite navigation) to estimate and correct the attitude misalignment angle based on pure inertial navigation solution, and readjusts the position and speed. In the integrated navigation state, the equipment can maintain the optimal accuracy. The integrated navigation uses the position of the satellite navigation system as the measurement information of the Kalman filter. The process is as follows: Figure 2 shown.

[0121] Kalman filter one-step prediction is divided into state transition matrix Φ k,k-1 Calculation of the input noise variance matrix Calculation, state prediction and the error variance prediction P k,k-1 There are three stages of calculation:

[0122] 4) State transition matrix Φ k,k-1 Calculation

[0123] Let (tk-1, tk] be a prediction period, h = tk-tk-1. The prediction period h is generally short, and the state transition matrix is ​​calculated as follows

[0124] in,

[0125]

[0126]

[0127] The variables of each element in the matrix can be calculated by specific force coordinate transformation, velocity update, and position update, among which,

[0128] L is latitude, unit: rad;

[0129] v E is the eastward speed, unit: m / s;

[0130] v N is the north velocity, unit: m / s;

[0131] v U is the vertical velocity, unit: m / s;

[0132] R E is the radius of curvature of the normal plane perpendicular to the meridian plane, unit: m;

[0133] R N is the radius of curvature on the meridian plane, unit: m;

[0134] f E is the eastward acceleration of the geographic coordinate system, unit: m / s2;

[0135] f N is the north acceleration of the geographic coordinate system, unit: m / s2;

[0136] f U is the vertical acceleration in the geographic coordinate system, unit: m / s2;

[0137] ω ie is the angular velocity of the Earth’s rotation, ω ie =7.292115×10-5rad / s.

[0138] 5) Input noise variance matrix Calculation

[0139] The system noise of the continuous system, i.e., the covariance matrix of the three gyroscopes and three accelerometer vectors W(t), is Q(t), and the variance matrix of the input noise is

[0140] Q q =G(t)Q(t)G T (t);

[0141] Where Q(t) is a constant and G(t) is the noise input matrix, which can be rewritten as follows:

[0142]

[0143] Get the input noise variance Q of the continuous system q Then calculate the input noise variance of the Kalman filter as follows:

[0144]

[0145] 6) Status prediction and the error variance prediction P k,k-1 Calculation

[0146] At the initial moment when k = 0, and P 0 Initialize;

[0147] When k=0, 1, 2, ..., recursive calculation

[0148]

[0149] When the filter update cycle has not arrived, the filter update is not performed and the prediction continues.

[0150]

[0151] P k =P k,k-1 ;

[0152] When the filter update period arrives, P k Follow the filter update calculations in the next section.

[0153] When the filtering time is up, the filter is updated. The filter update period is the external reference information update period. The filter update period is set to 1s and is calculated in four steps:

[0154] 5) Measurement and calculation

[0155] Calculate the measured value as follows:

[0156]

[0157] The subscript s represents the strapdown inertial navigation system solution output, and the subscript r represents the reference datum output.

[0158] v sE 、v sN 、v sU To solve the eastward velocity, northward velocity and vertical velocity, the unit is m / s;

[0159] L s , s To solve the latitude and longitude, the unit is rad;

[0160] S s To calculate the height, unit: m;

[0161] v rE 、v rN 、v rU is the reference speed, unit: m / s;

[0162] L r , r To bind latitude and longitude, the unit is rad;

[0163] S r is the reference height, unit: m.

[0164] 6) Calculation of filter gain The filter gain K is calculated as follows: k :

[0165]

[0166] Among them, P k,k-1 is the error variance prediction calculation; H k =[0 6×3 I6×6 0 6×6 ];

[0167] R k =diag[(R ve ) 2 (R vn ) 2 (R vu ) 2 (R lat ) 2 (R lon ) 2 (R h ) 2 ];

[0168] 7) State estimation update

[0169] The state estimate is calculated as follows:

[0170]

[0171] in, Calculated for state prediction.

[0172] 8) Error variance update

[0173] The error variance is calculated as follows: S3. Method for determining measurement weight factor:

[0174]

[0175] w i is the weight coefficient of the i-th component in the measurement vector;

[0176] c h is the adjustment factor, which can generally be taken as 1.345;

[0177] u i is the standardized absolute residual;

[0178] S is the residual scale.

[0179] Finally, all the parts not described in the present invention adopt mature products and mature technical means of the existing technology.

[0180] The present invention and its embodiments are described above, and such description is not restrictive. The drawings show only one embodiment of the present invention, and the actual structure is not limited thereto. In short, if ordinary technicians in the field are inspired by it, without departing from the purpose of the invention, they can design a structure and embodiment similar to the technical solution without creativity, which should belong to the protection scope of the present invention.

Claims

1. A method for processing abnormal information for inertial satellite integrated navigation, characterized in that: The following steps are involved: S1..Use three gyroscopes and three accelerometers to form an inertial navigation system and perform inertial navigation calculation; S2. Estimate and correct the attitude misalignment angle based on pure inertial navigation solution and satellite navigation information, and readjust the position and speed; S3. Method for determining measurement weight factor.

2. The abnormal information processing method for inertial satellite integrated navigation according to claim 1 is characterized in that: The inertial navigation solution method of S1 includes: Remember the direction cosine matrix corresponding to the initial attitude angle The corresponding attitude quaternion is recorded as Will Convert to attitude quaternion form: When k = 0, 1, 2, ..., the quaternion update algorithm is used to calculate t k+1 The quaternion of the attitude at the moment in: Δθ=[Δθ x Δθ y Δθ z ] T is the gyro output angle increment within the sampling period (tk, tk+1], unit: rad; The angle that the n system rotates relative to the i system during the update period (tk, tk+1): is the projection of the rotation angular velocity of the e system relative to the i system in the n system, is the projection of the angular velocity of the n system relative to the e system on the n system, which is calculated by the following two formulas: The speed, latitude and curvature radius in the formula are all the results of the previous navigation solution update cycle; finally, quaternion normalization is performed Get the updated attitude quaternion The velocity increment Δv output by the accelerometer is transformed into the velocity component in the geographic coordinate system Where Δv=[Δv x Δv y Δv z ] T is the velocity increment output by the accelerometer; The acceleration component in the geographic coordinate system is f n =Δv n / h, where f n =[f E f N f U ] T ; Initial time of velocity update, v n (0) = [000] T ; When k=0, 1, 2, ..., in a speed update cycle (tk, tk+1], the speed is updated to Where δA is the harmful acceleration, which is calculated as follows: At the initial moment, L(0), λ(0) and S(0) are the bound latitude, longitude and altitude; When k = 0, 1, 2, ..., within a speed update cycle (tk, tk+1], the position is updated as follows:

3. The abnormal information processing method for inertial satellite integrated navigation according to claim 1 is characterized in that: The estimation and correction method of S2 includes: Kalman filter one-step prediction is divided into state transition matrix Φ k,k-1 Calculation of the input noise variance matrix Calculation, state prediction and the error variance prediction P k,k-1 There are three stages of calculation: 1) State transition matrix Φ k,k-1 Calculation Let (tk-1, tk] be a prediction period, h = tk-tk-1; the prediction period h is generally short, and the state transition matrix is ​​calculated as follows in, 2) Input noise variance matrix Calculation The system noise of the continuous system, i.e., the covariance matrix of the three gyroscopes and three accelerometer vectors W(t), is Q(t), and the variance matrix of the input noise is Q q =G(t)Q(t)G T (t); Where Q(t) is a constant and G(t) is the noise input matrix, which can be rewritten as follows: Get the input noise variance Q of the continuous system q Then calculate the input noise variance of the Kalman filter as follows: 3) Status prediction and the error variance prediction P k,k-1 Calculation: At the initial moment when k = 0, Initialize with P0; When k=0, 1, 2, ..., recursive calculation: When the filter update cycle has not arrived, the filter update is not performed and the prediction continues. P k =P k,k-1 ; When the filter update period arrives, P k Follow the filter update calculations in the next section.

4. The abnormal information processing method for inertial satellite integrated navigation according to claim 3 is characterized in that: When the filtering time is up, the filter is updated. The filter update period is the external reference information update period. The filter update period is set to 1s and is calculated in four steps: 1) Measurement and calculation Calculate the measured value as follows: The subscript s represents the strapdown inertial navigation system solution output, and the subscript r represents the reference datum output; v sE 、v sN 、v sU To solve the eastward velocity, northward velocity and vertical velocity, the unit is m / s; L s , s To solve the latitude and longitude, the unit is rad; S s To calculate the height, unit: m; v rE 、v rN 、v rU is the reference speed, unit: m / s; L r , r To bind latitude and longitude, the unit is rad; S r is the reference height, unit: m; 2) Filter gain calculation Calculate the filter gain K as follows: k : Among them, P k,k-1 is the error variance prediction calculation; H k =[0 6×3 I 6×6 0 6×6 ]; R k =diag[(R ve ) 2 (R vn ) 2 (R vu ) 2 (R lat ) 2 (R lon ) 2 (R h ) 2 ] 3) State estimation update The state estimate is calculated as follows: in, Calculated for state prediction; 4) Error variance update The error variance is calculated as follows:

5. The abnormal information processing method for inertial satellite integrated navigation according to claim 1 is characterized in that: The method for determining the measurement weight factor in S3 is: w i is the weight coefficient of the i-th component in the measurement vector; c h is the adjustment factor, which can generally be taken as 1.345; u i is the standardized absolute residual; S is the residual scale.

Images