A low-frequency reverse filtering-based inertial navigation quick alignment method

Abstract

The application relates to a low-frequency reverse filtering-based inertial navigation rapid alignment method, which realizes rapid alignment of a strapdown inertial navigation system in an airborne vibration environment based on low-frequency reverse filtering. A low-frequency data compression method is designed, high-frequency data in a short time is compressed into 1Hz data in coarse alignment, and the occupied space is less than 3KB. Then, the stored 1Hz data is extracted for data restoration after coarse alignment is completed. Meanwhile, based on the stored data, low-frequency (1Hz) forward and reverse undamped calculation and forward and reverse filtering are carried out in a short time. The data is indirectly used to prolong the alignment time, the problem that a filter is difficult to converge in a short time due to a vibration environment is solved, and the operation resource and storage space required by the low-frequency backtracking process are very small. Therefore, the alignment time can be shortened and the alignment precision can be improved without changing the existing hardware level.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of fast initial alignment of strapdown inertial navigation system, and particularly relates to a fast inertial navigation alignment method based on low-frequency reverse filtering. BACKGROUND

[0002] Before being put into use, the navigation system carried by vehicle-mounted and airborne military equipment needs to be prepared for initial use, including initialization of attitude angle, velocity, position information and initial alignment process. The traditional initial alignment generally includes coarse alignment and fine alignment processes, and the alignment time is generally not less than 5 minutes. The alignment precision is directly related to the alignment environment and the precision and noise level of the inertial elements such as accelerometers and gyroscopes of the inertial navigation system equipment. In order to meet the requirements of alignment precision and time, the alignment needs to be performed under a static base. If there is disturbance and the noise of the inertial elements increases, the alignment precision will be reduced or even the alignment will fail.

[0003] In order to realize the function of fast alignment in a short time, many documents adopt the algorithm of reverse navigation, store the inertial element data to realize data reuse. Since the storage frequency of the inertial element is generally 100Hz-1kHz, the storage space is large. At the same time, the reverse navigation needs to perform normal navigation calculation process, which puts forward higher requirements for the processing capacity of the navigation computer. Therefore, some documents adopt the scheme of storing at 1Hz and then backtracking the navigation filter. However, this scheme only prolongs the fine alignment filter time by saving the coarse alignment time, and cannot fully play the advantage of data reuse in the case of poor coarse alignment result or short total alignment time (less than or equal to 3 minutes). SUMMARY

[0004] The application aims at overcoming the deficiencies of the prior art, and provides a fast inertial navigation alignment method based on low-frequency reverse filtering. The fast initial alignment algorithm of low-frequency storage combined with low-frequency navigation calculation and forward and reverse filters can effectively prolong the fine alignment time in the alignment algorithm, so that the fast alignment algorithm is realized.

[0005] The application solves the technical problem by adopting the following technical scheme:

[0006] A fast inertial navigation alignment method based on low-frequency reverse filtering, comprising the following steps:

[0007] Step 1: at t0, the device is powered on, and a forward coarse alignment process is started. The forward coarse alignment calculates the initial attitude matrix of the inertial navigation system at t0 with a 200Hz gyro and accelerometer sampling frequency

[0008] Step 2: While performing Step 1, compress the 200Hz information from the gyroscope and accelerometer and store it in the navigation computer's storage space at a storage rate of 7 float / s.

[0009] Step 3: At the 180s alignment time, latch the initial attitude matrix at time t0 in Step 1. Simultaneously, read and restore the compressed data stored in step 2, and then start the backtracking forward and reverse undamped inertial navigation calculations;

[0010] Step 4: Simultaneously with initiating the backtracking forward and reverse undamped inertial navigation calculations in Step 3, activate the backtracking Kalman filter to process the results calculated in Step 1. Perform error estimation;

[0011] Step 5: After completing the backtracking filter, perform... The estimated error is corrected, and the data stored in step 1 from t=181s to 184s is used for forward navigation and tracing to complete the entire initial alignment process.

[0012] Furthermore, the specific method of step 1 is as follows: using the inertial coordinate system as a reference, initially fix two inertial coordinate systems, then update the quaternion of the angular motion of the carrier relative to the inertial coordinate system, and use the transformation relationship between gravity and the linear motion of the carrier in the two inertial coordinate systems to obtain the transformation matrix of the two solidified inertial coordinate systems, thereby obtaining...

[0013] Moreover, the aforementioned The specific calculation method is as follows:

[0014] Calculate using the pose update algorithm

[0015]

[0016] in, For the inertial navigation system, align the navigation coordinate system n at time t with the navigation inertial frame i at the initial time. n0 The attitude matrix of the system, at the initial time t0, Let λ be a 3x3 identity matrix. t λt represents the longitude of the inertial navigation system's position at time t; λ0 represents the longitude of the inertial navigation system's position at time t0; ωt ie L is the Earth's rotation angular rate; t is the inertial navigation system alignment time; L t Let be the latitude of the position of the inertial navigation system at time t;

[0017] Attitude Quaternion Update Calculation Method

[0018]

[0019] where: [a x] denotes the anti-symmetric matrix of vector a; is the attitude matrix of the inertial navigation system i b0 with respect to the initial time inertial system i is the angular velocity of the carrier sensed by the gyroscopes, at the initial time i b0 the b coordinate system coincides with the n coordinate system, 3×3 is a 3x3 unit matrix;

[0020] The specific force in the b coordinate system is projected into the n coordinate system as follows:

[0021]

[0022]

[0023] where: f b is the accelerometer output in the moving base; g b is the projection of the gravity in the b coordinate system; is the linear motion acceleration of the carrier; is the projection of the specific force of the inertial navigation accelerometer in the i b0 coordinate system;

[0024] Let Taking the integral of both sides of the above equation, we get:

[0025]

[0026] where: is the projection of the integral of the specific force vector in the i b0 coordinate system; t k is the time at the alignment time k; is the attitude matrix of the initial time inertial system i b0 with respect to the initial time navigation inertial system i n0 ; is the attitude matrix of the inertial navigation system i b0 with respect to the initial time inertial system i n0 ; is the projection of the local gravity acceleration vector in the initial time navigation inertial system i n0 ;

[0027] Under the condition that the carrier is in uniform motion or equal amplitude swing motion, we get:

[0028]

[0029] where; is the projection of the local gravitational acceleration vector in the initial navigation inertial frame i n0 ; is the transpose matrix of ; g n is the projection of the local gravitational acceleration in the navigation frame n

[0030] According to the velocity basic equation of the inertial navigation, we have

[0031]

[0032] wherein is the velocity increment of the inertial navigation system in the navigation frame n n is the velocity of the inertial navigation system in the navigation frame n is the projection of the body angular rate in the navigation frame n is the projection of the earth rotation angular rate in the navigation frame n n is the projection of the body sensitive acceleration in the navigation frame n

[0033] Integrating the above equation and multiplying by we have

[0034]

[0035] wherein is the required attitude matrix in step 1 is the projection of the acceleration in the navigation inertial frame i n0 is the projection of the acceleration in the navigation inertial frame i is the external reference earth-fixed velocity, under the condition of static base

[0036] Let

[0037]

[0038]

[0039] we have

[0040]

[0041] arbitrarily take two time points in the alignment process we have

[0042]

[0043] Moreover, the step 2 uses the low-frequency storage data processing algorithm to compress the storage data.

[0044] And the specific implementation method of the low-frequency storage data processing algorithm is: storing the acceleration output in i b0 The sum of the specific forces projected on the coordinate system and the specific force at time t The corresponding quaternion q:

[0045]

[0046]

[0047] Wherein: The specific force stored in the kth second; The quaternion stored in the kth second; Δt is the storage period 1s, and the data to be stored is converted into a floating point type, wherein 3 floating point type spaces are occupied, and q occupies 4 floating point type spaces.

[0048] And the specific implementation method of step 3 is: first, the low-frequency forward navigation algorithm is performed, the acceleration and gyroscope attitude update data stored in the navigation computer are read every sampling period, the speed and position information under the geographic coordinate system are calculated through the mechanical arrangement of the geographic coordinate system, and when the data reading ends, the speed information output by the forward navigation is inverted, and the earth rotation angular velocity is also inverted:

[0049]

[0050]

[0051] Wherein, The speed calculated by the inverse navigation, The speed calculated by the forward navigation, Represent the earth rotation angular velocity under the navigation system; wherein the local geographic system mechanical arrangement process based on 1Hz data is:

[0052] Speed update:

[0053]

[0054] Wherein, The speed calculated by the undamped solution at time k; The acceleration read in the sampling period; T s The calculation period is Ts=1s;

[0055] Position update:

[0056]

[0057] Wherein: p k The position vector at time k, L, λ, h are the latitude, longitude and altitude of the carrier respectively; M pv is the position vector matrix, R M , R N are the principal radii of curvature of the meridian and prime vertical respectively;

[0058] Inertial attitude update: the inertial attitude update is to update the initial attitude matrix at time i n0 between the inertial system and the geographic coordinate system n The initial alignment matrix at time i is is the identity matrix, and the initial quaternion at time i is The iterative calculation of the quaternion process is:

[0059] Calculate the quaternion of each storage time interval

[0060]

[0061]

[0062]

[0063] where: represents the rotation matrix of the navigation system relative to the inertial system, which includes two parts: the rotation of the navigation system caused by the earth rotation, and the rotation of the navigation system caused by the earth surface bending due to the movement of the system near the earth surface; v N , v E is the inertial velocity, obtained by velocity update in the last section; calculate and then update

[0064] Let the quaternion is obtained:

[0065]

[0066] where, Taylor expansion of the above formula is obtained, and the fourth order approximation is:

[0067]

[0068] According to the time t-1 , the time t

[0069]

[0070] Let convert the quaternion obtained from the above formula into an attitude matrix to obtain ​

[0071]

[0072] And the step 4 backtracking Kalman filter specific implementation method is:

[0073] The state variable of the backtracking Kalman filter in the fine alignment is selected as:

[0074]

[0075] The selected state variable includes attitude angle error amount φ E , φ N , φ U , eastward, northward and vertical velocity error δv E , δv N , δv U of the navigation channel, position coordinate point latitude, longitude and height error δL, δλ, δh, gyro constant drift error ε x , ε y , ε z , and accelerometer measurement constant bias

[0076] The state equation of the backtracking Kalman filter is:

[0077]

[0078]

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085]

[0086] Wherein, and are the gyro drift and accelerometer bias noise variance; flag is the value of the forward and reverse navigation flag bit, flag = 1 when forward navigation, and flag = -1 when reverse navigation;

[0087] When the zero speed or Beidou speed is selected as the observation quantity in the fast initial alignment, the measurement equation of the backtracking Kalman filter is:

[0088]

[0089] wherein H=[0 3×3 flag*I 3×3 0 3×9 ]; V is a velocity observation white noise; flag is a forward and reverse navigation flag value;

[0090] At this time, the state equation of the backtracking Kalman filter is:

[0091]

[0092] The observation equation is:

[0093] Z p =HX+V

[0094] Wherein F, G state transition matrix and noise matrix adopt the general mathematical model of inertial navigation algorithm.

[0095] The advantages and positive effects of the present application are:

[0096] The present application constructs a low-frequency reverse filtering based on the realization of the fast alignment of the strapdown inertial navigation in the airborne vibration environment, through the design of the low-frequency data compression method, the high-frequency data in a short time is compressed into 1Hz data in the forward coarse alignment process, the occupied space is less than 3KB, then after the coarse alignment is completed, the stored 1Hz data is extracted for data restoration, and at the same time, based on the stored data, the low-frequency (1Hz) forward and reverse undamped solution and forward and reverse filtering are carried out in a short time, the alignment time is indirectly 'extended' by means of multiplexing data, the problem that the filter is difficult to converge in a short time due to the vibration environment is solved, and since the operation resources and storage space required by the low-frequency backtracking process are very small, the alignment time can be shortened and the alignment precision can be improved without changing the existing hardware level. BRIEF DESCRIPTION OF DRAWINGS

[0097] Figure 1 The flowchart of the present application;

[0098] Figure 2 The algorithm flowchart of the present application;

[0099] Figure 3 The forward coarse alignment flowchart of the present application;

[0100] Figure 4 The low-frequency forward and reverse navigation algorithm schematic diagram of the present application;

[0101] Figure 5 The backtracking Kalman filter basic principle schematic diagram of the present application. DETAILED DESCRIPTION

[0102] The present invention will be further described in detail below with reference to the accompanying drawings.

[0103] A fast inertial navigation alignment method based on low-frequency inverse filtering, such as Figure 1 As shown, it includes four parts: ① forward coarse alignment, ② low-frequency data storage algorithm, ③ forward and reverse navigation mechanical arrangement, and ④ forward and reverse Kalman filtering algorithm.

[0104] Its specific implementation method, such as Figure 2 As shown, it includes the following steps:

[0105] Step 1: Power on the device at time t0 and start the forward coarse alignment process. The forward coarse alignment calculates the initial attitude matrix of the inertial navigation system at time t0 using the 200Hz gyroscope and accelerometer sampling frequency. Step 1 as follows Figure 1 Thread 1 is shown.

[0106] The specific implementation method of this step is as follows: using the inertial coordinate system as a reference, initially fix two inertial coordinate systems, then update the quaternion of the angular motion of the carrier relative to the inertial coordinate system, and use the transformation relationship between gravity and the linear motion of the carrier in the two inertial coordinate systems to obtain the transformation matrix of the two solidified inertial coordinate systems, thereby obtaining... like Figure 3 The diagram shows the coarse alignment process based on the inertial solidification coordinate system.

[0107] The specific calculation method is as follows:

[0108] Calculate using the pose update algorithm

[0109]

[0110] in, For the inertial navigation system, align the navigation coordinate system n at time t with the navigation inertial frame i at the initial time. n0 The attitude matrix of the system, at the initial time t0, Let λ be a 3x3 identity matrix. t λt represents the longitude of the inertial navigation system's position at time t; λ0 represents the longitude of the inertial navigation system's position at time t0; ωt ie L is the Earth's rotation angular rate; t is the inertial navigation system alignment time; L t Let be the latitude of the position of the inertial navigation system at time t;

[0111] Attitude Quaternion Update Calculation Method

[0112]

[0113] Wherein: [a x] symbol represents the anti-symmetric matrix of vector a; is the attitude matrix of the carrier inertial system b relative to the initial time carrier inertial system (i b0 ); is the carrier angular velocity sensitive to the gyroscope, the alignment starting time i b0 The coordinate system coincides with the b coordinate system, 3×3 is a 3x3 unit matrix;

[0114] The specific force under the carrier coordinate system b is projected into the navigation coordinate system n as follows:

[0115]

[0116]

[0117] Wherein: f b is the accelerometer output under the moving base; g b is the projection of gravity on the b coordinate system; is the linear motion acceleration of the carrier; is the projection of the inertial navigation accelerometer output specific force on i b0 ;

[0118] Let Take the integral of both sides of the above formula to get:

[0119]

[0120] Wherein: is the projection of the integral of the specific force vector on i b0 ;t k is the time at the alignment k time; is the attitude matrix of the initial time carrier inertial system i b0 relative to the initial time navigation inertial system i n0 ; is the attitude matrix of the inertial navigation carrier coordinate system b relative to the initial time carrier inertial system i b0 ; is the projection of the local gravity acceleration vector on the initial time navigation inertial system i n0 ;

[0121] Under the condition that the carrier does uniform motion or equal amplitude swing motion, get:

[0122]

[0123] Wherein; g n = [0 0 -g] T , is the projection of the local gravitational acceleration vector in the initial navigation inertial frame i n0 ; is the transpose matrix of ; g n is the projection of the local gravitational acceleration in the navigation frame n; g is the magnitude of the local gravitational acceleration;

[0124] According to the velocity basic equation of inertial navigation, we have

[0125]

[0126] wherein is the velocity increment of the inertial navigation system in the navigation frame n; v n is the velocity of the inertial navigation system in the navigation frame n; is the projection of the body angular rate in the navigation frame n; is the projection of the earth rotation angular rate in the navigation frame n; f n is the projection of the body sensitive acceleration in the navigation frame n;

[0127] Integrating the above equation and multiplying by we have

[0128]

[0129] wherein is the required attitude matrix in step 1, is the projection of the acceleration in the navigation inertial frame i n0 , and is the external reference earth-fixed velocity, and in the static base condition,

[0130] respectively set

[0131]

[0132]

[0133] we have

[0134]

[0135] arbitrarily take two time points in the alignment process we have

[0136]

[0137] Step 2, at the same time as step 1, compress the information of the gyro and accelerometer at 200Hz and store it in the storage space of the navigation computer, with a storage rate of 7float / s. Step 2 is shown in the data flow buffer register storage process. Figure 2

[0138] In order to indirectly extend the initial alignment time, considering the limited hardware and software storage and processing capacity, if the original pulse of the inertial element is stored, the memory occupation and the time for processing the pulse do not meet the requirements of fast initial alignment. Therefore, a low-frequency storage data processing algorithm is designed, that is, the pulses output by the gyro and accelerometer are projected on the i b0 frame and accumulated to 1s storage algorithm. While not losing the carrier motion information, the storage space occupation is small, and the memory of the general embedded computer is enough to use. At the same time, the software calculates the 1Hz data stored in each 200Hz cycle, which is equivalent to a 200-1000 times increase in calculation speed (related to the sampling frequency of the navigation computer). The specific algorithm is as follows:

[0139] The sum of the specific forces in the i b0 frame and the corresponding quaternion q of the t time are stored every 1s.

[0140]

[0141]

[0142] Wherein: is the specific force sum stored in the kth second; is the quaternion size stored in the kth second; Δt is the storage period 1s, and the data to be stored is converted into a floating-point type, wherein occupies 3 floating-point type spaces, and q occupies 4 floating-point type spaces.

[0143] Step 3, at the time of alignment 180s, latch the initial attitude matrix of t0 time in step 1 , and read the compressed data stored in step 2 and restore it, and then start the backtracking forward and reverse undamped inertial navigation solution. Step 3 is shown in thread 2. Figure 2

[0144] As shown in Figure 4 , the compressed data is read through the buffer register, and then the read data is used to realize the specific implementation method of the forward and reverse navigation:

[0145] ​​​After the completion of the positive coarse alignment, first, the low-frequency positive navigation algorithm is carried out, the data of the accelerometer and the gyroscope attitude update stored in the navigation computer is read every sampling period, the speed and position information in the geographic system is calculated through the mechanical arrangement of the geographic coordinate system, when the data reading is finished, the speed information output by the positive navigation is inverted, and the earth rotation angular velocity is also inverted:

[0146]

[0147]

[0148] wherein, is the speed of the inverse navigation solution, is the speed of the positive navigation solution, represents the earth rotation angular velocity in the navigation system; wherein the local geographic system mechanical arrangement process based on 1Hz data is as follows:

[0149] Speed update:

[0150]

[0151] wherein, is the undamped solution speed at time k; is the read stored acceleration magnitude in the sampling period; T s is the calculation period, Ts=1s;

[0152] Position update:

[0153]

[0154] wherein: p k is the position vector at time k, L, λ, h are the latitude, longitude and height of the carrier respectively; M pv is the position vector matrix, R M , R N are the principal radii of curvature of the earth's meridian and the principal radii of curvature of the prime vertical respectively;

[0155] Inertial system attitude update: the inertial system attitude update is to update the initial attitude matrix between the inertial system i n0 and the geographic coordinate system n The initial attitude matrix at the alignment initial time is is the unit matrix, and the initial quaternion is The iterative calculation process of the quaternion is as follows:

[0156] The calculation of each storage time interval

[0157] ​

[0158]

[0159]

[0160] in: The rotation matrix representing the navigation frame relative to the inertial frame consists of two parts: the rotation of the navigation frame caused by the Earth's rotation, and the rotation of the navigation frame caused by the curvature of the Earth's surface as the system moves near the Earth's surface; v N v E The inertial navigation velocity is obtained from the velocity update in the previous section; the calculation yields... Then, a quaternion update algorithm was used to update.

[0161] make To find the quaternion, we have:

[0162]

[0163] in, Expanding the above equation using Taylor series, we obtain a fourth-order approximation:

[0164]

[0165] Based on time t-1 Calculate time t

[0166]

[0167] set up The quaternion obtained from the above formula is converted into an attitude matrix to obtain the following result.

[0168] Meanwhile, all calculations are based on information stored at 1Hz, with a calculation time interval T. s =1s.

[0169] Step 4: Simultaneously with initiating the backtracking forward and reverse undamped inertial navigation calculations in Step 3, activate the backtracking Kalman filter to process the results calculated in Step 1. Error estimation is performed. Low-frequency backtracking filtering utilizes the forward and reverse navigation mechanical arrangement from step 3 to establish system state error equations based on forward and reverse navigation. Zero-velocity or GPS information is used to estimate the attitude error of low-frequency navigation, achieving initial alignment. This allows for multiple uses of past time information, overcoming the problem of insufficient convergence time in Kalman filters due to carrier disturbances or inertial component noise. Its basic principle block diagram is shown below. Figure 5 As shown:

[0170] Backward Kalman filter has the same basic principle as general Kalman filter. Kalman filter is widely used as an iterative linear least square estimator. Gyro drift and accelerometer bias can be regarded as random constant process, and the following equation is obtained:

[0171]

[0172] The state variable of the backward Kalman filter in fine alignment is selected as follows:

[0173]

[0174] The selected state variable includes attitude angle error φ E , φ N , φ U , eastward, northward and vertical velocity error δv E , δv N , δv U of the navigation channel, latitude, longitude and height error δL, δλ, δh of the position coordinate point, gyro constant drift error ε x , ε y , ε z , and accelerometer measurement constant bias

[0175] The state equation of the backward Kalman filter is as follows:

[0176]

[0177]

[0178]

[0179]

[0180]

[0181]

[0182]

[0183]

[0184]

[0185] wherein, and are the gyro drift and accelerometer bias noise variance; flag is the value of the forward and reverse navigation flag bit, flag = 1 when forward navigation, and flag = -1 when reverse navigation;

[0186] If zero velocity or BeiDou velocity is selected as the observation for rapid initial alignment, then the measurement equation for backtracking Kalman filtering is:

[0187]

[0188] Where, H = [0 3×3 flag·I 3×3 0 3×9 V represents the white noise from the velocity observation; flag represents the value of the forward and reverse navigation flags.

[0189] The state equation of the backtracking Kalman filter is now:

[0190]

[0191] The observation equation is:

[0192] Z p =HX+V

[0193] The F and G state transition matrices and the noise matrix adopt the mathematical model commonly used in inertial navigation algorithms.

[0194] Step 5: After completing the backtracking filter, perform... The estimated error is corrected, and the data stored in step 1 from t=181s to 184s is used for forward navigation and tracing to complete the entire initial alignment process.

[0195] The above establishes the error model for a continuous system. Discretization is required for its implementation in the program. Discretization essentially involves calculating the transition matrix of the discrete system from the system matrix of the continuous system, and calculating the noise variance matrix of the discrete system from the system process variance intensity matrix of the continuous system.

[0196] The state equation model for a continuous system is as follows:

[0197]

[0198] Where: X is the n-dimensional state vector of the system; F is the n×n-dimensional system matrix; W is the p-dimensional system process noise; G is the n×p-dimensional noise input matrix; Z is the m-dimensional observation vector of the system; V is the m-dimensional observation noise; and H is the m×n-dimensional observation matrix.

[0199] Calculate Φ k,k-1 :

[0200] Using the calculation method for steady systems, the state transition matrix Φ k,k-1 The system matrix F is related to the following:

[0201]

[0202] Among them, (t) kt k+1 is the prediction period, let h = t k+1 -t k .

[0203] The prediction period h is usually short, and the Taylor expansion is performed to obtain:

[0204]

[0205] The calculation is

[0206] The covariance matrix of the system noise vector W(t) of the continuous system is Q(t), and the variance matrix of the input noise is:

[0207] Q q = G(t)Q(t)G T (t)

[0208] The input noise variance of the Kalman filter and the input noise variance Q q of the continuous system have the following relationship:

[0209]

[0210] It should be emphasized that the embodiments described in the present application are illustrative rather than restrictive, and therefore the present application includes but is not limited to the embodiments described in the specific embodiments, and any other embodiments derived by those skilled in the art according to the technical solutions of the present application also belong to the scope of protection of the present application.

Claims

1. A fast inertial navigation alignment method based on low-frequency inverse filtering, characterized in that: Includes the following steps: Step 1: Power on the device at time t0 and start the forward coarse alignment process. The forward coarse alignment calculates the initial attitude matrix of the inertial navigation system at time t0 using the 200Hz gyroscope and accelerometer sampling frequency. Step 2: While performing Step 1, compress the 200Hz information from the gyroscope and accelerometer and store it in the navigation computer's storage space at a storage rate of 7 float / s. Step 3: At the 180s alignment time, latch the initial attitude matrix at time t0 in Step 1. Simultaneously, read and restore the compressed data stored in step 2, and then start the backtracking forward and reverse undamped inertial navigation calculations; First, a low-frequency forward navigation algorithm is performed. In each sampling cycle, the accelerometer and gyroscope attitude update data stored in the navigation computer are read. The velocity and position information in the geographic coordinate system are calculated through mechanical arrangement of the geographic coordinate system. When the data is finished, the velocity information output by the forward navigation is inverted, and the Earth's rotation angular rate is also inverted. in, For the speed of reverse navigation solution, For the speed of forward navigation solution, Represents the Earth's rotation angular rate in the navigation system; the mechanical arrangement process for the local geographic system based on 1Hz data is as follows: Speed ​​updates: in, The velocity calculated without damping at time k; T represents the magnitude of the acceleration read from the storage during the sampling period. s For the calculation period, Ts = 1s; Location update: Where: p k Let k be the position vector at time k. L, λ, h represent the latitude, longitude, and altitude of the carrier, respectively; M pv It is a position vector matrix. R M ,R N These are the principal curvature radii of the Earth's meridian and the principal curvature radii of the zonal and tropospheric axes, respectively. Inertial frame attitude update: Inertial frame attitude update is to update the navigation inertial frame i at the initial moment. n0 attitude matrix between the system and the geographic coordinate system n The alignment initial time matrix is For an identity matrix, the initial quaternion is... Iterative computation The quaternion process is as follows: Calculate each storage time interval in: The rotation matrix representing the navigation frame relative to the inertial frame consists of two parts: the rotation of the navigation frame caused by the Earth's rotation, and the rotation of the navigation frame caused by the curvature of the Earth's surface as the system moves near the Earth's surface; v N v E The inertial navigation velocity is obtained from the velocity update in the previous section; the calculation yields... Then, a quaternion update algorithm was used to update. make To find the quaternion, we have: in, Expanding the above equation using Taylor series, we obtain a fourth-order approximation: Based on time t-1 Calculate time t set up The quaternion obtained from the above formula is converted into an attitude matrix to obtain the following result. Step 4: Simultaneously with initiating the backtracking forward and reverse undamped inertial navigation calculations in Step 3, activate the backtracking Kalman filter to process the results calculated in Step 1. Perform error estimation; Step 5: After completing the backtracking filter, perform... The estimated error is corrected, and the data stored in step 1 from t=181s to 184s is used for forward navigation and tracing to complete the entire initial alignment process.

2. The inertial navigation fast alignment method based on low-frequency inverse filtering according to claim 1, characterized in that: The specific method of step 1 is as follows: using the inertial coordinate system as a reference, initially fix two inertial coordinate systems, then update the quaternion of the angular motion of the carrier relative to the inertial coordinate system, and use the transformation relationship between gravity and the linear motion of the carrier in the two inertial coordinate systems to obtain the transformation matrix of the two solidified inertial coordinate systems, thereby obtaining...

3. The inertial navigation fast alignment method based on low-frequency inverse filtering according to claim 2, characterized in that: The The specific calculation method is as follows: Calculate using the pose update algorithm in, For the inertial navigation system, align the navigation coordinate system n at time t with the navigation inertial frame i at the initial time. n0 The attitude matrix of the system, at the initial time t0, Let λ be a 3x3 identity matrix. t λt represents the longitude of the inertial navigation system's position at time t; λ0 represents the longitude of the inertial navigation system's position at time t0; ωt ie L is the Earth's rotation angular rate; t is the inertial navigation system alignment time; L t Let be the latitude of the position of the inertial navigation system at time t; Attitude Quaternion Update Calculation Method Where: [a×] symbol represents the antisymmetric matrix of vector a; For the inertial navigation system b relative to the carrier inertial frame at the initial moment (i... b0 The attitude matrix; For the carrier angular velocity sensitive to the gyroscope, align with the start time i b0 The coordinate system coincides with the b coordinate system. I 3×3 It is a 3x3 identity matrix; Projecting the relative forces in the b-frame of the vehicle system onto the n-frame of the navigation coordinate system, we get: Where: f b For the accelerometer output under the moving base; g b This represents the projection of gravity onto the b-coordinate system. This refers to the acceleration of the carrier's linear motion. For the inertial accelerometer output specific force in i b0 Projection on; set up Integrating both sides of the above equation, we get: in: The integral of the force vector in i b0 The vector of projection under the system; t k For the time at time k; For the initial moment, the carrier inertial frame i b0 The navigation inertial frame i is relative to the initial moment. n0 The attitude matrix of the system; For the inertial navigation system b relative to the carrier inertial system i at the initial moment b0 The attitude matrix of the system; The local gravitational acceleration vector is the initial value of the navigation inertial frame i. n0 The projection below; Under the condition that the carrier is in uniform motion or constant amplitude swaying motion. get: in; g n =[00-g] T , The local gravitational acceleration vector is the initial value of the navigation inertial frame i. n0 Projection under the system; for The transpose of the matrix, g n is the projection of the local gravitational acceleration onto the navigation coordinate system n; g is the magnitude of the local gravitational acceleration; Based on the fundamental velocity equations obtained from inertial navigation systems: in: The velocity increment of the inertial navigation system's navigation coordinate system n; v n The velocity of the inertial navigation system in the n-frame of the navigation coordinate system; This is the projection of the carrier's angular velocity onto the navigation coordinate system n. The projection of the Earth's rotation angular rate in the navigation coordinate system n; f n The projection of the carrier-sensitive acceleration onto the navigation coordinate system n; Integral on both sides of the above equation and multiply by get: in, The pose matrix required in step 1, For acceleration in the navigation inertial frame i n0 The projection under the system has As an external reference velocity relative to the ground, under static base conditions, Let them be: get: Choose any two time points during the alignment process get 4. The inertial navigation fast alignment method based on low-frequency inverse filtering according to claim 1, characterized in that: Step 2 uses a low-frequency storage data processing algorithm to compress and store data.

5. The inertial navigation fast alignment method based on low-frequency inverse filtering according to claim 4, characterized in that: The specific implementation method of the low-frequency storage data processing algorithm is as follows: Acceleration output is stored at intervals of 1 second in i... b0 The sum of the projections of the coordinate system and time t The corresponding quaternion q: in: The ratio of force stored at the k-th second; Let be the size of the quaternion stored in the k-th second; Δt is the storage period of 1 second, and the data to be stored is converted into floating-point type, where... It occupies 3 floating-point spaces, and q occupies 4 floating-point spaces.

6. The inertial navigation fast alignment method based on low-frequency inverse filtering according to claim 1, characterized in that: The specific implementation method of the backtracking Kalman filter in step 4 is as follows: The state variables of the precise alignment backtracking Kalman filter are selected as follows: The selected state variables include the attitude angle error φ. E φ N φ U The eastward, northward, and vertical velocity errors δv of the navigation channel E δv N δv U The position coordinates latitude, longitude, and altitude errors δL, δλ, and δh, and the gyroscope constant drift error ε. x ε y ε z Accelerometer measurement constant deviation ▽ x 、▽ y 、▽ z , The state equation for a backtracking Kalman filter is: in, and This represents the variance of gyroscope drift and accelerometer bias noise; flag is the value of the forward and reverse navigation flag, flag = 1 for forward navigation and flag = -1 for reverse navigation; If zero velocity or BeiDou velocity is selected as the observation for rapid initial alignment, then the measurement equation for backtracking Kalman filtering is: Where, H = [0 3×3 flag·I 3×3 0 3×9 V represents the white noise from the velocity observation; flag represents the value of the forward and reverse navigation flags. The state equation of the backtracking Kalman filter is now: The observation equation is: Z p =HX+V The F and G state transition matrices and the noise matrix adopt the mathematical model commonly used in inertial navigation algorithms.

Images