Rotation modulation inertial navigation system backtracking alignment method based on group affine property

Abstract

The invention relates to the technical field of inertial navigation, in particular to a group affine property-based rotation modulation inertial navigation system backtracking alignment method, which is characterized in that on the basis of rotation inertial navigation multi-position alignment, forward and reverse Kalman filtering can be respectively combined with an alignment state equation meeting group affine property based on the Lie group theory, so that the rotation inertial navigation multi-position alignment can be realized. Carrying out repeated backtracking initial alignment on any large misalignment angle, and obtaining an alignment result with relatively high precision in a relatively short time; the method has the capability of quickly realizing initial alignment in a maximum misalignment angle environment, and the speed of the method is far higher than that of a traditional alignment technology; within the same alignment time, the precision performance of the method is more excellent; compared with a current Lie group alignment method, the method has the advantages that the speed and the precision are obviously improved under the condition that the calculation performance is relatively excellent; within limited alignment time, the higher the computing power of the equipment is, the better the alignment effect is, and the method is particularly suitable for the equipment needing to be quickly started.

Description

Technical Field

[0001] The invention relates to the technical field of inertial navigation, and in particular to a rotation modulation inertial navigation system backtracking alignment method based on group affine properties. Background Art

[0002] Inertial navigation is an autonomous navigation that relies solely on its own inertial sensors to achieve real-time navigation and positioning. The Inertial Measurement Unit (IMU) is the core of the inertial navigation system, which includes three gyroscopes and three accelerometers, which measure the angular velocity and acceleration of the carrier relative to the inertial space respectively. By calculating and processing the data of the inertial sensors, the carrier's attitude, position, speed and other information can be obtained, thereby achieving real-time navigation. However, the error of inertial navigation will increase over time. At present, in most cases, an economical and efficient combined navigation system is used to suppress the inertial navigation error by using other navigation information, but other navigation methods cannot be used normally in some harsh environments and special environments. Inertial navigation has advantages such as autonomy and anti-interference that other navigation cannot match, and can still achieve real-time navigation, but due to the accumulation of errors, the navigation accuracy gradually decreases. Therefore, pure inertial navigation requires a higher-precision inertial navigation system for navigation. Long-duration, high-precision inertial navigation generally uses a rotational modulation inertial navigation system, which rotates the IMU through a rotating mechanism to offset the errors caused by gyro constant drift and accelerometer constant zero bias on navigation and positioning, thereby improving the system accuracy of navigation and positioning.

[0003] Initial alignment is one of the key technologies of rotational modulation inertial navigation. There are two important indicators for initial alignment: the time required for initial alignment and the accuracy of initial alignment, which will directly affect the performance of the entire rotational modulation inertial navigation system. The alignment time determines the rapid startup performance of the inertial navigation system, and the alignment result determines the accuracy of subsequent navigation. However, there is a certain contradiction between alignment time and alignment accuracy. The longer the alignment time, the more accurate the alignment result and the higher the navigation accuracy. The initial alignment of traditional rotational modulation inertial navigation usually includes: coarse alignment stage and fine alignment stage, which not only leads to the problem of alignment time allocation, but also causes the alignment time to be too long.

[0004] The Chinese invention patent application "A fast alignment method for a rotational modulation inertial navigation system based on group affine properties (application number: 2023112393937, publication date: 2023-12-29)" proposes a fast alignment method based on group affine properties. This method designs an alignment state model that satisfies the group affine properties based on Lie group theory. Combined with Kalman filtering, it can achieve initial alignment of arbitrarily large initial misalignment angles and converge quickly in a short time. However, fast alignment often comes at the expense of accuracy. Finding the best balance between alignment speed and accuracy can be a challenge, especially in some applications that require high-precision navigation. The invention cannot achieve the best balance between alignment speed and accuracy. Under the same alignment time and the same state, the present invention can use the high-speed computing and processing performance of the computer to store the navigation information within the alignment time, and perform multiple backtracking processes on this information, so that the alignment converges faster and the alignment accuracy reaches a higher accuracy. Summary of the invention

[0005] In order to solve the problem of long initial alignment time and low precision of rotation modulation inertial navigation system, the present invention proposes a rotation modulation inertial navigation system backtracking alignment method based on group affine property. Based on the multi-position alignment of rotation inertial navigation, the method can combine forward and reverse Kalman filtering with the alignment state equation that satisfies the group affine property based on Lie group theory, repeatedly backtrack the initial alignment for any large misalignment angle, and obtain a high-precision alignment result in a short time.

[0006] The technical solution adopted by the present invention is as follows: a rotation modulation inertial navigation system backtracking alignment method based on group affine properties, which is divided into the following steps:

[0007] S1: Define the coordinate system, power on the inertial navigation system, the indexing mechanism of the inertial navigation system executes a specific indexing sequence, and the inertial measurement unit (IMU) collects gyro and accelerometer data and transmits it to the navigation computer in real time;

[0008] S1.1 Define coordinate system:

[0009] i system: the Earth-centered inertial coordinate system is used as the inertial reference coordinate system;

[0010] e-frame: Earth-centered Earth-fixed coordinate system (ECEF);

[0011] n system: The "East-North-Sky" geographic coordinate system is used as the navigation coordinate system;

[0012] b system: carrier coordinate system, the three axes xyz point to the right, front and top directions of the carrier respectively;

[0013] s system: IMU coordinate system, the three axes xyz correspond to the sensitive axes of the three gyroscopes xyz respectively;

[0014] S1.2 Turn on the inertial navigation system and set the initial alignment time T a , the number of backtracking M (generally set to 0, 1, 2..., the number of backtracking can be selected according to the alignment effect), the number of alignment N = 0 (the total number of forward alignment and reverse alignment in this mission), start the navigation computer timer, and the initialization time k = 0;

[0015] S1.3 IMU collects gyro output angular velocity at time k Comparison with accelerometer output and will and Transmit to the navigation computer in real time and store;

[0016] After the S1.4 inertial navigation system is turned on, the indexing mechanism will synchronously execute the double-position indexing sequence:

[0017] 1) The indexing mechanism is stationary T a / 2-180 / ω-0.5×ω / a second;

[0018] 2) The indexing mechanism rotates 180° around the celestial axis at an angular velocity of ω;

[0019] 3) The indexing mechanism is stationary T a / 2-180 / ω-0.5×ω / a second;

[0020] Among them, the initial alignment time T a The unit is seconds, which is usually set to 300 seconds in engineering applications; ω is the angular velocity of the indexing mechanism, in degrees / s; a is the angular acceleration of the indexing mechanism, in degrees / s 2 ;

[0021] S2: The navigation computer receives the gyro output angular velocity in real time Comparison with accelerometer output After that, perform the inertial navigation solution:

[0022] S2.1 Initialize navigation parameters, as follows:

[0023] S2.1.1 Define the navigation parameters at time k: The attitude matrix of the IMU is The velocity vector is v e (k), the position vector is p e (k), the auxiliary velocity vector is

[0024] The superscript “e” indicates that this parameter is a parameter in the Earth-centered Earth-fixed coordinate system (e system), and the same applies below.

[0025] S2.1.2 The attitude matrix at time k = 0 To initialize:

[0026]

[0027] S2.1.3 The velocity vector v at time k = 0 e (0) Initialization:

[0028] v e (0) =

[000] T (2)

[0029] The superscript “T” indicates matrix transpose;

[0030] S2.1.4 Set the position vector p at time k = 0 e (0) Initialization:

[0031] p e (0) = [L0λ0 h0] T (3)

[0032] Where L0 represents the latitude of the inertial navigation system at the initial position, λ0 represents the longitude of the inertial navigation system at the initial position, and h0 represents the altitude of the inertial navigation system at the initial position;

[0033] S2.1.5 The auxiliary velocity vector at time k = 0 To initialize:

[0034]

[0035] is the projection of the rotational angular velocity of the e system relative to the i system in the e system, Earth's rotation rate ω ie =15° / h; Represents the calculation vector The antisymmetric matrix of , the same below;

[0036] S3 performs forward inertial navigation solution in the Earth-centered Earth-fixed coordinate system (e system) (forward solution recursively deduces the attitude, velocity and position information at the next moment, and calculates the time from 1 to k), as follows:

[0037] S3.1 First update the navigation computer timer time: k = k + 1, then update the attitude:

[0038]

[0039] Where I 3×3 is a 3×3 dimensional unit matrix; φ(k) is the equivalent rotation vector at time k, and the angular velocity output by the gyroscope It is obtained by calculation. The calculation process can be referred to the literature [Strapdown Inertial Navigation Algorithm and Integrated Navigation Principle / Yan Gongmin, Weng Jun ed.—Xi’an: Northwestern Polytechnical University Press, 2019.8]; |φ(k)| represents the modulus of φ(k); (φ(k)×) represents the antisymmetric matrix of the calculation vector φ(k).

[0040] S3.2 performs speed update:

[0041]

[0042] Where T s is the sampling time interval of IMU data; is the gravitational vector at time (k-1); the auxiliary velocity vector at time (k-1) and the velocity vector v at time (k-1) e (k-1) relationship, the gravitational vector at (k-1) time and the gravity vector g at time (k-1) e The relationships between (k-1) are:

[0043]

[0044] S3.3 performs location update:

[0045]

[0046] S4: Constructing a state model for forward alignment of a rotationally modulated inertial navigation system that satisfies the group affine property

[0047] Based on Lie group theory, if the state quantity of a dynamic system is defined in the Lie group space and satisfies the condition of global state quantity independence, then the corresponding state model satisfies the group affine property. The group affine model has the following characteristics: 1) the left / right invariant error is independent of the trajectory; 2) the linearized model in the Euclidean space is independent of the global state estimation; 3) the nonlinear error in the Lie group description can be accurately inferred from the linearized model. From the attitude, velocity, and position update equations constructed by S3, it can be seen that by converting the differential equations of the Rotational Inertial Navigation System (RINS) to the e system and introducing the auxiliary velocity vector, the attitude update is decoupled from the velocity and position errors, as well as the decoupling between the state vectors, so that the constructed state model can satisfy the group affine property.

[0048] Based on the group affine property, a linear state model that satisfies the group affine property is established to solve the nonlinear state model (for details on the Lie group theory and group affine properties, please refer to the literature [Barrau A. Non-linear state error based extended Kalman filters with applications to navigation [D]. Mines Paristech, 2015.]). The details are as follows:

[0049] S4.1 constructs a linear state model as follows:

[0050]

[0051] In the formula, X k is the 18-dimensional state vector at time k, Z k is the three-dimensional measurement vector at time k; Φ k|k-1 is the state one-step transfer matrix from time (k-1) to time k; G k|k-1 is the system noise distribution matrix from time (k-1) to time k, H k is the measurement matrix at time k; w k-1 represents the system noise matrix at time (k-1); v k represents the observation noise matrix at time k, the noise matrix w k-1 The setting of can refer to the literature [Strapdown Inertial Navigation Algorithm and Integrated Navigation Principle / Yan Gongmin, Weng Jun ed.—Xi’an: Northwestern Polytechnical University Press, 2019.8];

[0052] S4.2 Construct the 18-dimensional state vector X at time k k :

[0053]

[0054] In the formula, represents the attitude error of the IMU at time k, and the subscript "lg" indicates that the type of this error is a left invariant error that satisfies the group affine property (Note: the group affine property has left and right invariant error models, and the present invention adopts the left invariant error model); Represents the velocity error of the IMU at time k; represents the position error of IMU at time k; ε s (k) represents the gyro drift of the IMU at time k, Indicates the accelerometer zero bias of the IMU at time k, L s(k) represents the inner lever arm error of the IMU at time k; the definition of the left invariant error can be found in the literature [Barrau A. Non-linear state error based extended Kalman filters with applications to navigation [D]. Mines Paristech, 2015.]. S4.3 Construct the three-dimensional measurement vector Z at time k k :

[0055]

[0056] To measure the speed, the rotation modulation inertial navigation system is aligned while the vehicle remains in place.

[0057] S4.4 Construct the state one-step transition matrix Φ from time (k-1) to time k k|k-1 :

[0058]

[0059] Where I 18×18 represents the 18×18 identity matrix, I 3×3 represents the 3×3 identity matrix, 0 3×3 represents a 3×3 zero matrix;

[0060] S4.5 Construct the system noise allocation matrix G from time (k-1) to time k k|k-1 :

[0061]

[0062] S4.6 Construct the measurement matrix H at time k k :

[0063]

[0064] S5: Based on the rotation modulation inertial navigation system constructed in S4, the state quantity is estimated by tracing back the alignment state forward model:

[0065] S5.1 Calculate the posterior estimate of the state vector from time (k-1) to time k

[0066]

[0067] In the formula, Represents the 18-dimensional state vector X output at time (k-1) k-1 The posterior estimate of ; Initial value of Denotes the state vector X k One-step prediction from time (k-1) to time k, i.e., the prior estimate at time k;

[0068] S5.2 Calculate the one-step prediction mean square error matrix of the state from time (k-1) to time k:

[0069]

[0070] Where P k-1 represents the mean square error matrix of the state posterior estimate at time (k-1), P kk-1 represents the one-step prediction mean square error matrix of the state from time (k-1) to time k; Q k-1 represents the system noise covariance matrix at time (k-1);

[0071] Among them, P k-1 Initial value of Represents the initial value of the mean square error matrix under n-system projection:

[0072]

[0073] T represents the mean square error coordinate system transformation matrix:

[0074]

[0075] S5.3 Calculate the filter gain K at time k k :

[0076]

[0077] Where, the measurement noise covariance matrix R is:

[0078]

[0079] S5.4 Calculate the posterior estimate of the state at time k

[0080]

[0081] S5.5 Calculate the error estimate of each navigation parameter at time k:

[0082] Attitude error estimate for The first to third dimensions of Right now:

[0083]

[0084] Auxiliary velocity error estimate for The 4th to 6th dimensions of Right now:

[0085]

[0086] Position error estimate for The 7th to 9th dimensions of Right now:

[0087]

[0088] S5.6 Calculate the mean square error matrix P of the posterior estimate of the state at time k k :

[0089] P k =P kk-1 -K k H k P kk -1; (26)

[0090] S6: Use the error estimate obtained in S5.5 to correct the navigation solution result in S3, as follows:

[0091] S6.1 Corrected attitude matrix Get the corrected attitude matrix

[0092]

[0093] In the formula, Express Perform matrix exponential operation, the calculation formula is:

[0094]

[0095] S6.2 Corrected auxiliary velocity vector Get the corrected auxiliary velocity vector

[0096]

[0097] S6.3 Corrected position vector p e (k), get the corrected position vector

[0098]

[0099] S7: Determine whether to continue the forward alignment. a When k = T a When S7.2 is executed, the corrected high-precision attitude matrix is ​​output. End alignment; details are as follows:

[0100] S7.1 When k<T a , perform the following steps:

[0101] S7.1.1 The navigation parameters corrected by S6 and Overwrite the uncorrected data in the navigation computer's memory p e (k), namely:

[0102]

[0103] S7.1.2 pe(k), P k Stored in the memory of the navigation computer, return to S3 and continue to execute to S7, and make the next condition judgment of S7;

[0104] S7.2 When k = T a When , the corrected attitude matrix is ​​output Complete the forward initial alignment, the number of alignments N = N + 1;

[0105] S8 performs the reverse inertial navigation solution in the Earth-centered Earth-fixed coordinate system (e system) (the forward solution of S3 recursively calculates the attitude, velocity, and position information of the IMU at the next moment, and the calculation time increases from 1 to k. The reverse solution is to recursively calculate the attitude, velocity, and position information of the IMU at the previous moment, and the calculation time decreases from k-1 to 1), as follows:

[0106] S8.1 first updates the navigation computer timer: k = k-1; then, based on the obtained k-time information (the initial value is the forward navigation solution result), the information of the (k-1)-time is reversely deduced. The attitude update formula at the (k-1)-time can be derived from the forward attitude update formula of S3.1 as follows:

[0107]

[0108] S8.2 speed update:

[0109]

[0110] S8.3 performs location update:

[0111]

[0112] S9: Constructing a state model for reverse alignment of a rotationally modulated inertial navigation system that satisfies the group affine property

[0113] The S9.1 state model is as follows

[0114]

[0115] In the formula, X k-1 is the 18-dimensional state vector at time (k-1), Z k-1 is the 3D measurement vector at time (k-1); Φ k|k-1 is the state one-step transfer matrix from time (k-1) to time k, Φ k-1|k is the state one-step transfer matrix from time k to time (k-1), G k|k-1 is the system noise distribution matrix from time (k-1) to time k, H k-1 is the measurement matrix at time (k-1); w k-1 represents the system noise matrix at time (k-1); v k-1 represents the observation noise matrix at time (k-1).

[0116] S9.2 Construct the 18-dimensional state vector X at time (k-1) k-1 :

[0117]

[0118] In the formula, Represents the attitude error of the IMU at time (k-1). The subscript "lg" indicates that the type of this error is a left-invariant error that satisfies the group affine property. Represents the velocity error of the IMU at time (k-1); represents the position error of the IMU at time (k-1); ε s (k-1) represents the gyro drift of the IMU at time (k-1), Indicates the accelerometer zero bias of the IMU at time (k-1), L s (k-1) represents the inner lever arm error of the IMU at time (k-1);

[0119] S9.3 Construct the 3D measurement vector Z at time (k-1) k-1 :

[0120]

[0121] S9.4 Construct the state one-step transition matrix Φ from time k to time (k-1) k-1|k :

[0122]

[0123] Where I 18×18 represents the 18×18 identity matrix, I 3×3 represents the 3×3 identity matrix, 0 3×3 represents a 3×3 zero matrix;

[0124] S9.5 Construct the system noise allocation matrix G from time k to time (k-1) k-1|k :

[0125]

[0126] S9.6 Construct the measurement matrix H at time (k-1) k-1 :

[0127]

[0128] S10: Based on the rotation modulation inertial navigation system constructed in S9, the state quantity is estimated by tracing back the alignment state inverse model:

[0129] S10.1 Calculate the posterior estimate of the state vector from time k to time (k-1)

[0130]

[0131] In the formula, Represents the 18-dimensional state vector X output at time k k The posterior estimate of ; Denotes the state vector X k One-step prediction from time k to time (k-1), i.e., the prior estimate at time (k-1);

[0132] S10.2 Calculate the one-step prediction mean square error matrix of the state from time (k-1) to time k:

[0133]

[0134] Where P kk-1 represents the one-step prediction mean square error matrix of the state from time (k-1) to time k; P k represents the mean square error matrix of the state posterior estimate at time k, Q k represents the system noise covariance matrix at time k;

[0135] S10.3 Calculate the filter gain K at time (k-1) k-1 :

[0136]

[0137] Where, the measurement noise covariance matrix R is the same as the forward direction;

[0138] S10.4 Calculate the posterior estimate of the state at time (k-1)

[0139]

[0140] S10.5 Calculate the error estimate of each navigation parameter at time (k-1):

[0141] Attitude error estimate for The first to third dimensions of Right now:

[0142]

[0143] Auxiliary velocity error estimate for The 4th to 6th dimensions of Right now:

[0144]

[0145] Position error estimate for The 7th to 9th dimensions of Right now:

[0146]

[0147] S10.6 Calculate the mean square error matrix P of the posterior estimate of the state at time (k-1) k-1 :

[0148]

[0149] S11: Use the error estimate obtained in S10.5 to correct the navigation solution result of S7, as follows:

[0150] S11.1 Corrected attitude matrix Get the corrected attitude matrix

[0151]

[0152] In the formula, Express Perform matrix exponential operation, the calculation formula is:

[0153]

[0154] S11.2 Correction of auxiliary velocity vector Get the corrected auxiliary velocity vector

[0155]

[0156] S11.3 Corrected position vector p e (k-1), get the corrected position vector

[0157]

[0158] S12: Determine whether to continue the reverse alignment. When k>0, execute S12.1; when k=0, execute S12.2 and output the corrected high-precision attitude matrix End alignment;

[0159] S12.1 When k>0, perform the following steps:

[0160] S12.1.1 The corrected navigation parameters and Overwrite the uncorrected data in the navigation computer's memory p e (k-1), that is:

[0161]

[0162] S12.1.2 pe(k-1), P k-1 Stored in the memory of the navigation computer, return to S8 and continue to S11.3, and then make the next conditional judgment of S12;

[0163] S12.2 When k = 0, output the corrected attitude matrix The reverse alignment is completed, and the number of alignments N=N+1.

[0164] S13: Execution and end judgment of backtracking alignment (S1 to S2.2 for inertial navigation initialization, S3 to S7.2 for forward alignment, and S8 to S12.2 for reverse alignment)

[0165] After completing the inertial navigation initialization in S13.1, perform forward alignment (S3 to S7.2), and determine whether the initial alignment is completed according to the number of backtracking times M set in S1.2: if the number of backtracking times M = 0, execute S13.2; if the number of backtracking times M> 0, execute S13.3;

[0166] S13.2 Alignment is completed and the alignment result is output;

[0167] S13.3 If the number of alignments N = 2M + 1, execute S13.5; if the number of alignments N < 2M + 1, execute S13.4;

[0168] S13.4 performs a backtracking alignment process (performs a reverse alignment (S8 to S12.2), and then performs a forward alignment (S3 to S7.2)) until N=2M+1;

[0169] S13.5 The backtracking alignment is completed and the alignment result is output.

[0170] In order to simplify the expression and algorithm implementation, the alignment time in the backtracking alignment process is a fixed value T a , as attached Figure 2 As shown, the second forward alignment time T can be changed in actual navigation a +ΔT, because the navigation process is carried out in real time, the backtracking alignment takes a period of time ΔT, ΔT is the time from T a The time required to trace back from the moment to catch up with the real time, so the alignment time can be increased during the second forward alignment to catch up with the real-time navigation state, thereby achieving real-time alignment, as shown in the attached Figure 3 shown.

[0171] Compared with the prior art, the present invention has the following beneficial effects:

[0172] 1. The present invention has the ability to quickly achieve initial alignment in a large misalignment angle environment, and its speed is much faster than traditional alignment technology. In the same alignment time, the accuracy of the present invention is more outstanding.

[0173] 2. Under the condition of superior computing performance, the present invention has significantly improved the speed and accuracy compared with the current Lie group alignment method. Within the limited alignment time, the stronger the computing power of the device, the better the alignment effect of the present invention, which is especially suitable for devices that need to start quickly. BRIEF DESCRIPTION OF THE DRAWINGS

[0174] Figure 1 Invention flow chart;

[0175] Figure 2 Schematic diagram of the backtracking alignment of the present invention (this figure shows that after the inertial navigation is turned on, the forward alignment is performed and the data during this period is recorded. a Always use this data (from T a To 0 time), perform reverse navigation, and then perform forward alignment after reaching 0 time);

[0176] Figure 3 Schematic diagram of the backtracking real-time alignment of the present invention (this figure is a schematic diagram of the alignment time being increased to catch up with the real-time process after the second forward alignment);

[0177] Figure 4 Navigation radial error of different schemes. DETAILED DESCRIPTION

[0178] In order to provide a detailed technical description of the present invention, the following examples will be given and further described in conjunction with the accompanying drawings. (The specific examples described here are only used to explain the present application and are not intended to limit the present application)

[0179] The present invention provides a backtracking alignment method for a rotating inertial navigation system based on the group affine property, establishes a backtracking alignment model that satisfies the group affine property, and derives a reverse error model based on the reverse navigation recursive equation; then, based on the reverse navigation solution and filtering algorithm, a backtracking alignment method suitable for rotating inertial navigation is established. The process of the present invention is as follows Figure 1 As shown, specifically including:

[0180] S1: Define the coordinate system, power on the inertial navigation system, the indexing mechanism of the inertial navigation system executes a specific indexing sequence, and the inertial measurement unit (IMU) collects gyro and accelerometer data and transmits it to the navigation computer in real time;

[0181] Set the initial alignment time T a = 300 seconds, the number of backtracking times M is set to 1, the number of alignment times N = 0, and the time k = 0;

[0182] S2: The navigation computer receives the gyro output angular velocity in real time Comparison with accelerometer output After that, perform inertial navigation solution;

[0183] Initialize the navigation parameters and set the attitude matrix at time k = 0 To initialize:

[0184]

[0185] The velocity vector v at time k = 0 e (0) Initialization:

[0186] v e (0) =

[000] T (2)

[0187] The position vector p at time k = 0 e (0) Initialization:

[0188] p e (0) = [L0λ0 h0] T (3)

[0189] The auxiliary velocity vector at time k = 0 To initialize:

[0190]

[0191] in, Earth's rotation rate ω ie =15° / h;

[0192] Navigation computer timer time update: k = k + 1;

[0193] S3 performs forward inertial navigation solution in the Earth-centered Earth-fixed coordinate system (e-system)

[0194] Calculation time k = k + 1

[0195] Perform posture update:

[0196]

[0197] Where I 3×3 is a 3×3 dimensional identity matrix;

[0198] To update the speed:

[0199]

[0200] S3.3 performs location update:

[0201]

[0202] S4: Constructing a state model for forward alignment of a rotationally modulated inertial navigation system that satisfies the group affine property

[0203] Construct a linear state model as follows:

[0204]

[0205] In formula (10), X k , Z k They are

[0206]

[0207] Z k In

[0208] In formula (10), Φ k|k-1 for

[0209]

[0210] In formula (13), I 18×18 is the 18×18 identity matrix, I 3×3 is a 3×3 identity matrix, 0 3×3 is a 3×3 zero matrix, is the angular velocity output by the gyroscope in the IMU, Output specific force of the accelerometer in the IMU;

[0211] In formula (10), G k|k-1 for

[0212]

[0213] In formula (14), I 3×3is a 3×3 identity matrix, 0 3×3 is a 3×3 zero matrix, 0 12×3 is a 12×3 zero matrix.

[0214] In formula (10), H k for

[0215]

[0216] In formula (15) For this moment, the posture matrix is ω ie =15° / h;

[0217] S5: Based on the rotation modulation inertial navigation system constructed in S4, the state quantity is estimated by tracing back the alignment state forward model;

[0218] Calculate the posterior estimate of the state vector from time (k-1) to time k

[0219]

[0220] In the formula Initial value of

[0221] Calculate the one-step prediction mean square error matrix of the state from time (k-1) to time k:

[0222]

[0223] Where P k-1 Initial value of

[0224]

[0225] Where R e =6378137m;

[0226] Calculate the filter gain K at time k k :

[0227]

[0228] Where R is:

[0229]

[0230] Calculate the posterior estimate of the state at time k

[0231]

[0232] Calculate the error estimate of each navigation parameter at time k:

[0233] Attitude error estimate for The first to third dimensions of Right now:

[0234]

[0235] Auxiliary velocity error estimate for The 4th to 6th dimensions of Right now:

[0236]

[0237] Position error estimate for The 7th to 9th dimensions of Right now:

[0238]

[0239] Calculate the mean square error matrix P of the posterior estimate of the state at time k k :

[0240] P k =P k|k-1 -K k H k P k|k-1 ; (26)

[0241] S6: Correcting the navigation solution result using the error estimate;

[0242] Corrected attitude matrix Get the corrected attitude matrix

[0243]

[0244] In the formula, Express Perform matrix exponential operation, the calculation formula is:

[0245]

[0246] Corrected auxiliary velocity vector Get the corrected auxiliary velocity vector

[0247]

[0248] Corrected position vector p e (k), get the corrected position vector

[0249]

[0250] S7: Determine whether the forward alignment is to continue;

[0251] When k<T a When the navigation parameters are corrected by S6 and Overwrite the uncorrected data in the navigation computer's memory p e (k), namely:

[0252]

[0253]

[0254] Will pe(k), P k Stored in the memory of the navigation computer; then return to the forward alignment start step S3, repeat the forward alignment operation until k = T a ;

[0255] When k = T a When , the corrected attitude matrix is ​​output The forward initial alignment is completed, and the number of alignments N=N+1.

[0256] S8 performs inverse inertial navigation solution in the Earth-centered Earth-fixed coordinate system (e-system)

[0257] Calculation time k = k-1

[0258] According to the information at time k, the information at time (k-1) is reversely deduced, and the posture at time (k-1) is updated as follows:

[0259]

[0260] The initial value This is the attitude result output for the forward alignment.

[0261] To update the speed:

[0262]

[0263] The initial value is the velocity at the end of the positive alignment.

[0264] To perform a location update:

[0265]

[0266] The initial value p e (k) is the position at the time of positive alignment output.

[0267] S9: Construct a state model for reverse alignment of a rotationally modulated inertial navigation system that satisfies the group affine property;

[0268]

[0269] In formula (37), X k-1 for

[0270]

[0271] In formula (37), Z k-1 for

[0272]

[0273] Z k-1 In

[0274] In formula (37), Φ k-1|k for

[0275]

[0276] Where I 18×18 is the 18×18 identity matrix, I 3×3 is a 3×3 identity matrix, 0 3×3 is a 3×3 zero matrix, is the angular velocity output by the gyroscope (k-1) in the IMU, is the specific force output by the accelerometer in the IMU at time (k-1);

[0277] In formula (37), G k-1|k for

[0278]

[0279] Where I 3×3 is a 3×3 identity matrix, 0 3×3 is a 3×3 zero matrix, 0 12×3 is a 12×3 zero matrix;

[0280] In formula (37), H k-1 for

[0281]

[0282] In formula (42), ω ie =15° / h;

[0283] S10: Estimating state quantities based on the inverse model of the rotation modulation inertial navigation system backtracking alignment state;

[0284] Calculate the posterior estimate of the state vector from time k to time (k-1)

[0285]

[0286] Calculate the one-step prediction mean square error matrix of the state from time (k-1) to time k:

[0287]

[0288] The initial value P k is the P at the end time k of the forward alignment k ;

[0289] Calculate the filter gain K at time (k-1) k-1 :

[0290]

[0291] Where, the measurement noise covariance matrix R is the same as the forward direction;

[0292] Calculate the posterior estimate of the state at time (k-1)

[0293]

[0294] Calculate the error estimate of each navigation parameter at time (k-1):

[0295] Attitude error estimate for

[0296]

[0297] Auxiliary velocity error estimate for

[0298]

[0299] Position error estimate for

[0300]

[0301] Calculate the mean square error matrix P of the posterior estimate of the state at time (k-1) k-1

[0302]

[0303] S11: Correct the navigation solution using the error estimate obtained

[0304] Corrected attitude matrix for

[0305]

[0306] In the formula, for

[0307]

[0308] Corrected auxiliary velocity vector for

[0309]

[0310] Corrected position vector for

[0311]

[0312] S12: Determine whether the reverse alignment is to continue

[0313] When k>0, the corrected navigation parameters and Overwrite the uncorrected data in the navigation computer's memory p e (k-1)

[0314]

[0315] Will p e (k-1), P k-1 Store in the memory of the navigation computer, return to step S8, and repeat the reverse alignment operation until k=0;

[0316] When k = 0, output the corrected attitude matrix The reverse alignment is completed, and the number of alignments N=N+1.

[0317] S13: Execution and termination judgment of backtracking alignment

[0318] First, perform inertial navigation initialization and forward alignment (S1 to S7.2), and then finish the alignment and save the alignment results;

[0319] If the number of backtracking times M = 0, the alignment is terminated and the alignment result is output;

[0320] If the number of backtracking times M>0, perform the backtracking alignment operation (first perform the reverse alignment (S8 to S12.2), and finally perform the forward alignment (S3 to S7.2));

[0321] If the number of alignments N is less than 2M+1, the backtracking alignment operation is continued until N=2M+1;

[0322] If the alignment times N=2M+1, the backtracking alignment is completed and the alignment result is output.

[0323] Example 1

[0324] The alignment performance test of the present invention and other alignment methods was carried out on a real-time rotation modulation inertial navigation system. The performance parameters of the inertial navigation system in the test are shown in Table 1:

[0325] Table 1 IMU performance parameters

[0326]

[0327] The heading references at the four positions of the turntable are shown in Table 2:

[0328] Table 2 Heading reference

[0329]

[0330] The initial alignment time is 300 seconds, and tests are performed at different orientations to compare the alignment results.

[0331] Three schemes will be adopted to compare the alignment results. The first is the traditional alignment method (inertia method + KF, coarse alignment 120 + fine alignment 180 seconds), the second is group affine fast alignment, and the third is group affine backtracking alignment.

[0332] Table 3 Heading angle alignment results and errors of different schemes

[0333]

[0334]

[0335] It can be seen from the experimental results that the present invention has the highest accuracy of heading angle alignment under different benchmarks compared with the other two solutions, which can reflect the obvious advantages of the present invention over other solutions.

[0336] Example 2

[0337] Carry out vehicle navigation experiment verification to further compare the advantages of the present invention over other alignment methods.

[0338] Three schemes will be adopted to compare the alignment results. The first is the traditional alignment method (inertia method + KF, coarse alignment 120 seconds + fine alignment 180 seconds), the second is group affine fast alignment, and the third is group affine backtracking alignment.

[0339] Table 4 Comparison of navigation errors of different schemes for 2.5 hours

[0340]

[0341] It can be seen from the experimental results that the navigation error of the present invention is the smallest within 2.5 hours, and the maximum value of the navigation error is 60% higher than the traditional alignment method (inertial method + KF), and the navigation accuracy is 7.2% higher than the group affine fast alignment method. The mean value of the navigation error is 67% higher than the traditional alignment method (inertial method + KF), and the navigation accuracy is 24% higher than the group affine fast alignment.

Claims

1. A rotation modulation inertial navigation system backtracking alignment method based on group affine properties, characterized in that: The method is divided into the following steps: S1: Define the coordinate system, power on the inertial navigation system, the indexing mechanism of the inertial navigation system executes a specific indexing sequence, and the inertial measurement unit (IMU) collects gyro and accelerometer data and transmits it to the navigation computer in real time; S1.1 Define coordinate system: i system: the Earth-centered inertial coordinate system is used as the inertial reference coordinate system; e system: Earth-centered Earth-fixed coordinate system; n system: "East-North-Sky" geographic coordinate system as navigation coordinate system; b system: carrier coordinate system, the three axes xyz point to the right, front and top directions of the carrier respectively; s system: IMU coordinate system, the three axes xyz correspond to the sensitive axes of the three gyroscopes xyz respectively; S1.2 Turn on the inertial navigation system and set the initial alignment time T a , backtracking times M, alignment times N = 0, start the navigation computer timer, initialization time k = 0; S1.3 IMU collects gyro output angular velocity at time k Comparison with accelerometer output and will and Transmit to the navigation computer in real time and store; After the S1.4 inertial navigation system is turned on, the indexing mechanism will synchronously execute the double-position indexing sequence: 1) The indexing mechanism is stationary T a / 2-180 / ω-0.5×ω / a second; 2) The indexing mechanism rotates 180° around the celestial axis at an angular velocity of ω; 3) The indexing mechanism is stationary T a / 2-180 / ω-0.5×ω / a second; Among them, the initial alignment time T a The unit is seconds, ω is the angular velocity of the indexing mechanism, the unit is ° / s; a is the angular acceleration of the indexing mechanism, the unit is ° / s 2 ; S2: The navigation computer receives the gyro output angular velocity in real time Comparison with accelerometer output After that, perform the inertial navigation solution: S2.1 Initialize navigation parameters, as follows: S2.1.1 Define the navigation parameters at time k: The attitude matrix of the IMU is The velocity vector is v e (k), the position vector is p e (k), the auxiliary velocity vector is The superscript "e" indicates that this parameter is in the Earth-centered Earth-fixed coordinate system, and the same applies below; S2.1.2 The attitude matrix at time k = 0 To initialize: S2.1.3 The velocity vector v at time k = 0 e (0) Initialization: v e (0)＝[000] T (2) The superscript "T" indicates matrix transpose; S2.1.4 Set the position vector p at time k = 0 e (0) Initialization: p e (0)=[L0λ0 h0] T (3) Where L0 represents the latitude of the inertial navigation system at the initial position, λ0 represents the longitude of the inertial navigation system at the initial position, and h0 represents the altitude of the inertial navigation system at the initial position; S2.1.5 The auxiliary velocity vector at time k = 0 To initialize: is the projection of the rotational angular velocity of the e system relative to the i system in the e system, Earth's rotation rate ω ie =15° / h; Represents the calculation vector The antisymmetric matrix of , the same below; S3 performs forward inertial navigation solution in the Earth-centered Earth-fixed coordinate system, and forward solution recursively deduces the attitude, velocity and position information at the next moment, and calculates the time from 1 to k; the details are as follows: S3.1 First update the navigation computer timer time: k = k + 1, then update the attitude: Where I 3×3 is a 3×3 dimensional unit matrix; φ(k) is the equivalent rotation vector at time k, and the angular velocity output by the gyroscope It is calculated that |φ(k)| represents the modulus of φ(k); (φ(k)×) represents the antisymmetric matrix of the calculated vector φ(k); S3.2 performs speed update: Where T s is the sampling time interval of IMU data; is the gravitational vector at time (k-1); the auxiliary velocity vector at time (k-1) and the velocity vector v at time (k-1) e (k-1) relationship, the gravitational vector at (k-1) time and the gravity vector g at time (k-1) e The relationships between (k-1) are: S3.3 performs location update: S4: Constructing a state model for forward alignment of a rotationally modulated inertial navigation system that satisfies the group affine property Based on the group affine property, a linear state model that satisfies the group affine property is established to solve the nonlinear state model; the details are as follows: S4.1 constructs a linear state model as follows: Where, X k is the 18-dimensional state vector at time k, Z k is the three-dimensional measurement vector at time k; Φ k|k-1 is the state one-step transfer matrix from time (k-1) to time k; G k|k-1 is the system noise distribution matrix from time (k-1) to time k, H k is the measurement matrix at time k; w k-1 represents the system noise matrix at time (k-1); v k represents the observation noise matrix at time k; S4.2 Construct the 18-dimensional state vector X at time k k : In the formula, represents the attitude error of the IMU at time k. The subscript "lg" indicates that the type of this error is the left invariant error that satisfies the group affine property. Represents the velocity error of the IMU at time k; represents the position error of IMU at time k; ε s (k) represents the gyro drift of the IMU at time k, Indicates the accelerometer zero bias of the IMU at time k, L s (k) represents the inner lever arm error of the IMU at time k; S4.3 Constructing the three-dimensional measurement vector Z at time k k : To measure the speed, the rotation modulation inertial navigation system is aligned while the vehicle remains in place. S4.4 Construct the state one-step transition matrix Φ from time (k-1) to time k k|k-1 : Where I 18×18 represents the 18×18 identity matrix, I 3×3 represents the 3×3 identity matrix, 0 3×3 represents a 3×3 zero matrix; S4.5 Construct the system noise allocation matrix G from time (k-1) to time k k|k-1 : S4.6 Construct the measurement matrix H at time k k : S5: Based on the rotation modulation inertial navigation system constructed in S4, the state quantity is estimated by tracing back the alignment state forward model: S5.1 Calculate the posterior estimate of the state vector from time (k-1) to time k In the formula, Represents the 18-dimensional state vector X output at time (k-1) k-1 The posterior estimate of ; Initial value of Denotes the state vector X k One-step prediction from time (k-1) to time k, i.e., the prior estimate at time k; S5.2 Calculate the one-step prediction mean square error matrix of the state from time (k-1) to time k: Where P k-1 represents the mean square error matrix of the state posterior estimate at time (k-1), P kk-1 represents the one-step prediction mean square error matrix of the state from time (k-1) to time k; Q k-1 represents the system noise covariance matrix at time (k-1); Among them, P k-1 Initial value of Represents the initial value of the mean square error matrix under n-system projection: T represents the mean square error coordinate system transformation matrix: S5.3 Calculate the filter gain K at time k k : Where, the measurement noise covariance matrix R is: S5.4 Calculate the posterior estimate of the state at time k S5.5 Calculate the error estimate of each navigation parameter at time k: Attitude error estimate for The first to third dimensions of Right now: Auxiliary velocity error estimate for The 4th to 6th dimensions of Right now: Position error estimate for The 7th to 9th dimensions of Right now: S5.6 Calculate the mean square error matrix P of the posterior estimate of the state at time k k : P k =P k|k-1 -K k H k P k|k-1 ; (26) S6: Use the error estimate obtained in S5.5 to correct the navigation solution result in S3, as follows: S6.1 Corrected attitude matrix Get the corrected attitude matrix In the formula, Express Perform matrix exponential operation, the calculation formula is: S6.2 Corrected auxiliary velocity vector Get the corrected auxiliary velocity vector S6.3 Corrected position vector p e (k), get the corrected position vector S7: Determine whether to continue the forward alignment. a When k = T a When S7.2 is executed, the corrected high-precision attitude matrix is ​​output. End alignment; details are as follows: S7.1 When k<T a , perform the following steps: S7.1.1 The navigation parameters corrected by S6 and Overwrite the navigation computer's memory with uncorrected p e (k), namely: S7.1.2 pe(k), P k Stored in the memory of the navigation computer, return to S3 and continue to execute to S7, and make the next condition judgment of S7; S7.2 When k = T a When , the corrected attitude matrix is ​​output Complete the forward initial alignment, the number of alignments N = N + 1; S8 performs the reverse inertial navigation solution in the Earth-centered Earth-fixed coordinate system. The reverse solution is to recursively calculate the attitude, velocity, and position information of the IMU at the previous moment. The calculation time decreases from k-1 to 1. The details are as follows: S8.1 first updates the navigation computer timer: k = k-1; then, based on the information at time k, the information at time (k-1) is reversely deduced. The attitude update formula at time (k-1) can be derived from the forward attitude update formula in S3.1 as follows: S8.2 speed update: S8.3 performs location update: S9: Constructing a state model for reverse alignment of a rotationally modulated inertial navigation system that satisfies the group affine property The S9.1 state model is as follows Where, X k-1 is the 18-dimensional state vector at time (k-1), Z k-1 is the 3D measurement vector at time (k-1); Φ k|k-1 is the state one-step transfer matrix from time (k-1) to time k, Φ k-1|k is the state one-step transfer matrix from time k to time (k-1), G k|k-1 is the system noise distribution matrix from time (k-1) to time k, H k-1 is the measurement matrix at time (k-1); w k-1 represents the system noise matrix at time (k-1); v k-1 represents the observation noise matrix at time (k-1); S9.2 Construct the 18-dimensional state vector X at time (k-1) k-1 : In the formula, Represents the attitude error of the IMU at time (k-1). The subscript "lg" indicates that the type of this error is a left-invariant error that satisfies the group affine property. Represents the velocity error of the IMU at time (k-1); represents the position error of the IMU at time (k-1); ε s (k-1) represents the gyro drift of the IMU at time (k-1), Indicates the accelerometer zero bias of the IMU at time (k-1), L s (k-1) represents the inner lever arm error of the IMU at time (k-1); S9.3 Construct the 3D measurement vector Z at time (k-1) k-1 : S9.4 Construct the state one-step transition matrix Φ from time k to time (k-1) k-1|k : Where I 18×18 represents the 18×18 identity matrix, I 3×3 represents the 3×3 identity matrix, 0 3×3 represents a 3×3 zero matrix; S9.5 Construct the system noise allocation matrix G from time k to time (k-1) k-1|k : S9.6 Construct the measurement matrix H at time (k-1) k-1 : S10: Based on the rotation modulation inertial navigation system constructed in S9, the state quantity is estimated by tracing back the alignment state inverse model: S10.1 Calculate the posterior estimate of the state vector from time k to time (k-1) In the formula, Represents the 18-dimensional state vector X output at time k k The posterior estimate of ; Denotes the state vector X k One-step prediction from time k to time (k-1), i.e., the prior estimate at time (k-1); S10.2 Calculate the one-step prediction mean square error matrix of the state from time (k-1) to time k: Where P kk-1 represents the one-step prediction mean square error matrix of the state from time (k-1) to time k; P k represents the mean square error matrix of the state posterior estimate at time k, Q k represents the system noise covariance matrix at time k; S10.3 Calculate the filter gain K at time (k-1) k-1 : Where, the measurement noise covariance matrix R is the same as the forward direction; S10.4 Calculate the posterior estimate of the state at time (k-1) S10.5 Calculate the error estimate of each navigation parameter at time (k-1): Attitude error estimate for The first to third dimensions of Right now: Auxiliary velocity error estimate for The 4th to 6th dimensions of Right now: Position error estimate for The 7th to 9th dimensions of Right now: S10.6 Calculate the mean square error matrix P of the posterior estimate of the state at time (k-1) k-1 : P k-1 =P k-1k -K k-1 H k-1 P k-1k ; (50) S11: Use the error estimate obtained in S10.5 to correct the navigation solution result of S7, as follows: S11.1 Corrected attitude matrix Get the corrected attitude matrix In the formula, Express Perform matrix exponential operation, the calculation formula is: S11.2 Correction of auxiliary velocity vector Get the corrected auxiliary velocity vector S11.3 Corrected position vector p e (k-1), get the corrected position vector S12: Determine whether to continue the reverse alignment. When k>0, execute S12.1; when k=0, execute S12.2 and output the corrected high-precision attitude matrix End alignment; S12.1 When k>0, perform the following steps: S12.1.1 The corrected navigation parameters and Overwrite the navigation computer's memory with uncorrected p e (k-1), that is: S12.1.2 pe(k-1), P k-1 Stored in the memory of the navigation computer, return to S8 and continue to S11.3, and then make the next conditional judgment of S12; S12.2 When k = 0, output the corrected attitude matrix Complete reverse alignment, alignment times N = N + 1; S13: Execution and termination judgment of backtracking alignment After completing the inertial navigation initialization in S13.1, perform forward alignment and determine whether the initial alignment is completed according to the number of backtracking times M set in S1.2: if the number of backtracking times M = 0, execute S13.2; if the number of backtracking times M> 0, execute S13.3; S13.2 Alignment is completed and the alignment result is output; S13.3 If the number of alignments N = 2M + 1, execute S13.5; if the number of alignments N < 2M + 1, execute S13.4; S13.4 performs a backtracking alignment process, and then performs a forward alignment until N=2M+1; S13.5 The backtracking alignment is completed and the alignment result is output.

2. The rotation modulation inertial navigation system backtracking alignment method based on group affine property according to claim 1 is characterized in that: In S1.2, the number of backtracking times M can be selected according to the alignment effect.

3. The rotation modulation inertial navigation system backtracking alignment method based on group affine property according to claim 1 is characterized in that: Initial alignment time T a Set to 300 seconds.

4. The rotation modulation inertial navigation system backtracking alignment method based on group affine property according to any one of claims 1 to 3, characterized in that: The second forward alignment time T can be changed in actual navigation a +ΔT, ΔT is the time from T a The time required for the moment to go back to catch up with the real time.

Images