Transfer alignment method and device based on Lie group right invariant error model

Abstract

The invention discloses a transfer alignment method based on a Lie group right invariant error model, and belongs to the field of inertial navigation systems. The method comprises the following steps: firstly, referring to a navigation coordinate system, remodeling a right invariant attitude and a speed error based on the Lie group theory to replace a traditional transfer alignment error, incorporating sensor bias and a lever arm and mounting angle error between master and slave inertial navigation, and fully considering a transfer alignment error source; secondly, a system state equation is deduced again under the Lie group framework, and decoupling of calculation parameters and a system model is achieved; then, based on an attitude + speed measurement matching scheme, a system measurement equation is deduced again under a Lie group framework; and finally, aiming at the nonlinear characteristic of the measurement equation, combining with an unscented Kalman filter to accurately estimate the system state. According to the transfer alignment method provided by the invention, an error source objectively existing between the main inertial navigation system and the sub inertial navigation system is fully considered, system error modeling under a Lie group framework is perfected, and a system model is ensured to accurately reflect actual conditions.

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation systems, and in particular to a transfer alignment method and device based on a Lie group right-invariant error model. Background Art

[0002] Inertial navigation technology autonomously measures and senses the angular velocity and linear acceleration of the vehicle, and based on the initial state information, calculates the position, velocity, and attitude of the carrier in real time, providing a solid foundation for the control and planning of the subsequent vehicle. As a fully autonomous push-to-point navigation technology, it does not rely on external signals or actively transmit signals during operation, and has strong anti-interference capabilities and high concealment. However, the numerical integration process of inertial navigation makes its initial state accuracy directly affect the subsequent navigation accuracy, which in turn affects the subsequent vehicle attitude control and trajectory planning accuracy. Therefore, initial alignment technology is an important guarantee and key factor for the accurate solution of navigation parameters. As a typical dynamic base initial alignment technology, transfer alignment is based on the core idea of ​​using the navigation information provided by the high-precision main inertial navigation system to assist the low-precision sub-inertial navigation system to achieve fast and accurate initial alignment and device error calibration.

[0003] Existing transfer alignment methods, after modeling the error state, utilize the real-time output information of the two inertial navigation systems to achieve real-time state estimation through measurement parameter matching or calculation parameter matching. However, the state transfer matrix of these methods often includes the calculation parameters of the sub-inertial navigation system. In complex and highly maneuverable scenarios, the accuracy of the calculation parameters of the low-precision sub-inertial navigation system cannot be guaranteed, and large errors often exist. As a result, when the state is propagated to the next moment, the calculation error at the current moment is transferred and superimposed, causing the system model at the subsequent moment to no longer be applicable. This causes the reliability of the transfer alignment to decrease over time, ultimately resulting in the sub-inertial navigation system's positioning accuracy failing to meet actual requirements.

[0004] Thanks to the advantages of Lie group theory, Lie group-based transfer alignment technology can largely decouple inertial navigation calculation parameters from the system model. Currently, there is little research on Lie group-based transfer alignment technology, and existing technologies do not consider the actual arm error between the main and sub-insertion navigation systems. The system modeling is relatively simple and does not fully consider the error sources of the actual system, leaving room for improvement in estimation accuracy. In addition, due to the group affine property of Lie groups, existing Lie group transfer alignment methods use the Earth coordinate system as the reference coordinate system. However, state estimation with reference to the Earth coordinate system is actually a practice for polar navigation. Common navigation parameters are all referenced to the local navigation coordinate system. Therefore, existing Lie group transfer alignment methods lack direct application value.

[0005] To address the above problems, it is necessary to invent a Lie group transfer alignment method that fully considers the sources of system errors, has high alignment accuracy and has direct application value. Summary of the Invention

[0006] The purpose of the present invention is to propose a transfer alignment method based on the Lie group right invariant error model to solve the problems raised in the above background technology. Lie group theory is applied to perform right invariant error state modeling in the navigation coordinate system, replacing the traditional transfer alignment error modeling, incorporating the arm and installation angle errors between the master and slave inertial navigation systems, and fully considering the error sources of transfer alignment; based on the newly constructed Lie group right invariant error, the system state equation and measurement equation are re-derived within the Lie group framework to construct a new system state space model; based on the "attitude + velocity" matching scheme, the unscented Kalman filter is applied to achieve accurate estimation of the Lie group right invariant error state, fully considering the nonlinear characteristics of the system model and the random errors of the actual system.

[0007] The present invention provides a transfer alignment method based on a Lie group right invariant error model, comprising:

[0008] Step 1: Define the coordinate system;

[0009] Step 2: Based on the right-invariant error theory under Lie groups, introduce the auxiliary velocity vector and establish the error state model between the main and sub-INS.

[0010] Step 3: Based on the attitude transformation relationship between the real navigation coordinate system and the calculated navigation coordinate system, construct the attitude error differential equation of the sub-INS;

[0011] Step 4: Based on the difference between the auxiliary velocity of the sub-INS in the real and calculated navigation coordinate systems, and considering the arm error caused by the misalignment of the main and sub-INS installations, the velocity error differential equation of the sub-INS system is constructed.

[0012] Step 5: Based on the relationship between the attitude matrices solved by the master and slave INS, construct the attitude measurement equation of the slave INS taking into account the installation error angle;

[0013] Step 6: Based on the relationship between the velocities calculated by the master and slave INS systems, construct the speed measurement equation of the slave INS system.

[0014] Step 7: First, construct the system's error state vector based on attitude error, velocity error, gyroscope bias, accelerometer bias, lever arm, and mounting angle error. Simultaneously, construct the system noise vector based on the measurement errors of the sub-INS gyroscope and accelerometer. Next, construct the state differential equation of the sub-INS system based on the differential equations for each error state and combining the system state vector and the noise vector. Furthermore, construct the error state space equation of the sub-INS system based on the attitude and velocity measurement equations of the sub-INS. Finally, organize and discretize the system to obtain a complete discrete-time error state space model.

[0015] Step 8: Based on the discrete-time error state space model, the unscented Kalman filter algorithm is used to achieve accurate estimation of the error state, thereby completing the transfer alignment of the main and sub-INS.

[0016] Furthermore, in step 1, the coordinate system is defined as:

[0017] I system: Earth-centered inertial coordinate system, referred to as inertial coordinate system;

[0018] e system: Earth-centered Earth-fixed coordinate system, referred to as the Earth coordinate system;

[0019] n system: the "East-North-Sky" geographic coordinate system is used as the navigation coordinate system; the navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving navigation parameters;

[0020] M-frame: Main inertial navigation coordinate system, with the main inertial navigation geometric center as the origin, and the three axes xyz pointing to the right, front and top directions of the vehicle respectively;

[0021] S system: Sub-INS coordinate system, with the sub-INS geometric center as the origin, and the three axes xyz pointing to the right, front and top directions of the carrier respectively.

[0022] Furthermore, the establishment of the right invariant error state model between the master and slave inertial navigation systems is specifically as follows:

[0023] Step 2.1: Build Auxiliary Speed

[0024]

[0025] Among them, the symbol (·×) is is the projection of the Earth's rotation angular velocity in the real navigation coordinate system; is the projection of the velocity of the sub-INS relative to the earth coordinate system in the real navigation coordinate system; is the projection of the position of the sub-INS relative to the earth coordinate system in the real navigation coordinate system;

[0026] Step 2.2: The state χ∈SE(3) in the Lie group space is:

[0027]

[0028] in, The rotation matrix from the real navigation coordinate system to the sub-INS coordinate system, that is, the sub-INS attitude;

[0029] Step 2.3: Based on the calculation status The deviation from the true state χ, the right invariant error is defined as:

[0030]

[0031] in, The rotation matrix from the sub-INS coordinate system to the computational navigation coordinate system; is the estimated value of the auxiliary speed; is the attitude error of the right invariant error, is the velocity error of the right invariant error.

[0032] Furthermore, in step 3, the attitude error differential equation of the sub-INS is specifically:

[0033] Step 3.1: Based on the relationship between the equivalent rotation vector and the rotation matrix, the attitude error is defined as:

[0034]

[0035] Where φ is the equivalent rotation vector from the real navigation coordinate system n to the sub-INS calculation navigation coordinate system n′, that is, the misalignment angle error; I 3×3 It is a unit matrix with 3 rows and 3 columns;

[0036] Step 3.2: Sub-INS attitude differential equation:

[0037]

[0038] in, is the projection of the angular velocity of the sub-INS relative to the inertial coordinate system in the sub-INS coordinate system; is the projection of the angular velocity of the real navigation coordinate system relative to the inertial coordinate system in the real navigation coordinate system;

[0039] Step 3.3: The attitude error differential equation of the sub-INS is:

[0040]

[0041] in, is the measurement error of the sub-INS gyroscope; Since it is not considered when modeling the system state, it can be regarded as state noise.

[0042] Furthermore, in step 4, the sub-inertial navigation velocity error differential equation is specifically:

[0043] Step 4.1: Calculate the velocity error:

[0044]

[0045] in, is the projection of the auxiliary velocity of the sub-INS relative to the earth coordinate system in the calculated navigation coordinate system;

[0046] Step 4.2: Considering the influence of the lever arm between the master and slave INS, the positions of the master and slave INS can be modeled as:

[0047]

[0048] in, is the projection of the position of the sub-INS coordinate system relative to the Earth coordinate system in the Earth coordinate system; The projection of the position of the main inertial navigation coordinate system relative to the earth coordinate system in the earth coordinate system; is the projection of the position of the sub-INS coordinate system relative to the main INS coordinate system in the Earth coordinate system; is the rotation matrix from the earth coordinate system to the master inertial navigation coordinate system; is the projection of the position of the sub-INS coordinate system relative to the main INS coordinate system in the main INS coordinate system, i.e., the lever arm;

[0049] Step 4.3: Based on the auxiliary velocity vector and the main and sub-INS positions of the combined lever arm, the relationship between the auxiliary velocity of the main and sub-INS and the lever arm is obtained as follows:

[0050]

[0051] in, The projection of the angular velocity of the main inertial navigation coordinate system relative to the inertial coordinate system in the main inertial navigation coordinate system; is the rotation matrix from the real navigation coordinate system to the main inertial navigation coordinate system;

[0052] Step 4.4: The differential equation for calculating the auxiliary velocity is:

[0053]

[0054] Among them, G n is the projection of gravitational acceleration in the real navigation system; g n is the projection of gravitational acceleration in the real navigation system; For centripetal force; The projection of the position of the main inertial navigation coordinate system relative to the earth coordinate system in the real navigation coordinate system;

[0055] Step 4.5: Calculate the velocity error differential equation as:

[0056]

[0057] in, is the accelerometer measurement error; is the estimated value of the Earth's rotational angular velocity projected in the main inertial navigation coordinate system; is the estimated value of gravitational acceleration;

[0058] Furthermore, in step 5, the attitude measurement equation of the sub-INS is:

[0059] z obφ =eul(Z obφ )=g1(φ,μ) (13)

[0060]

[0061] in, The attitude matrix of the main inertial navigation; Z is the attitude matrix calculated by the sub-INS; obφ is the attitude measurement matrix; g1(φ,μ) is a nonlinear function of the misalignment angle error φ and the installation error angle μ; eul(·) is the conversion of the rotation matrix to the Euler angle. The specific input and output relationship is:

[0062]

[0063] Furthermore, in step 6, the sub-INS velocity measurement equation is:

[0064]

[0065] Furthermore, in step 7, specifically:

[0066] Step 7.1: Construct the state vector of the sub-INS;

[0067] Step 7.1.1: Use constant value modeling to construct the lever arm vector and installation error angle;

[0068]

[0069] Step 7.1.2: Change the state variables ε, ▽, and μ are expanded to the right invariant error state, and the 18-dimensional state quantity x of the filter model is constructed as follows:

[0070]

[0071] Where φ is the misalignment angle of the sub-INS, dv R is the velocity error, ε is the gyroscope bias, ▽ is the accelerometer bias, The arm error between the main and sub-INS, μ is the installation error angle between the main and sub-INS;

[0072] Step 7.2: Construct the noise vector of the sub-INS;

[0073] Step 7.2.1: Use constant bias and white noise modeling to construct the measurement error of the sub-INS gyroscope in equation (6) The measurement error of the neutron inertial navigation accelerometer is

[0074]

[0075] Among them, w g and w a is a zero-mean Gaussian white noise; the gyroscope bias ε and accelerometer bias ▽ are modeled as constant biases.

[0076] Step 7.2.2: The 6-dimensional state noise w of the filtering model is:

[0077] w=[w g w a ] T (twenty one)

[0078] Step 7.3: Construct the state space equation of the sub-INS;

[0079] Step 7.3.1: According to the attitude error differential equation (6) and the velocity error differential equation (12), determine the transfer matrix F and the noise matrix G as:

[0080]

[0081] Step 7.3.2: Based on the attitude measurement equation (13) and the velocity measurement equation (17), determine the measurement generation function h(x) as:

[0082]

[0083] Step 7.3.3: Generate the function h(x) and combine it with the actual system noise to obtain the state space equation of the sub-INS:

[0084]

[0085] Where, v is the measurement noise;

[0086] Step 7.4: Discretize the state space equation to obtain:

[0087]

[0088]

[0089] Where Δt is the discrete time; Φ k,k-1 is the discrete transfer matrix; G k-1 is the discrete noise matrix; v k is the discrete measurement noise.

[0090] Furthermore, in step 8, the nonlinear state estimation method - unscented Kalman filtering is used to achieve state estimation, specifically:

[0091] Step 8.1: Construct 2n+1 Sigma points for time update:

[0092]

[0093] Step 8.2: The weight of each Sigma point is calculated as follows:

[0094]

[0095] Among them, ω (m) is the weight when calculating the mean, ω (c) is the weight when calculating the covariance; the parameter β is used to introduce the prior information of non-Gaussian distribution, and the parameter λ=α 2 (n+κ)-n is the proportional coefficient. The distribution of Sigma points around the mean can be determined by the parameters α and κ, κ = 3-n;

[0096] Step 8.3: Bring the Sigma point into state propagation:

[0097]

[0098] in, is the predicted Sigma point;

[0099] Step 8.4: Calculate the predicted mean according to steps 8.1 to 8.3 and predicted P k|k-1 :

[0100]

[0101] Step 8.5: Reconstruct 2n+1 Sigma points for measurement update:

[0102]

[0103] Step 8.6: Substitute the newly constructed Sigma point into the system measurement equation:

[0104] Z i,k|k-1 =h(χ i,k|k-1 ),i=0,...,2n (33)

[0105] Among them, Z i,k|k-1 is the predicted measurement Sigma point;

[0106] Step 8.7: Calculate the predicted mean of the quantity measure and the predicted covariance and the cross-covariance between state and quantity measurements

[0107]

[0108] Step 8.8: Calculate the filter gain K k , binding amount measurement z k , obtain the filter state estimate and variance P k|k :

[0109]

[0110] Step 8.9: After obtaining an estimate of the error state vector x through the unscented Kalman filter, feedback correction is performed on the lever arm and installation angle errors to improve the transfer alignment accuracy of the main and sub-INS.

[0111] The present invention also provides a computer device / equipment / system, comprising a memory, a processor, and a computer program stored in the memory, wherein when the processor executes the computer program, the steps of the transfer alignment method based on the Lie group right invariant error model described above are implemented.

[0112] The present invention also provides a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of any of the above-mentioned transfer alignment methods based on the Lie group right invariant error model.

[0113] The present invention also provides a computer program product, comprising a computer program / instruction, which, when executed by a processor, implements the steps of any of the above-mentioned transfer alignment methods based on the Lie group right invariant error model.

[0114] The beneficial effects of the present invention are:

[0115] The present invention proposes a transfer alignment technology based on right-invariant errors in Lie group space, which decouples the system state equation from the inertial navigation calculation parameters, avoiding the accumulation of model errors caused by the coupling of calculation errors. At the same time, the state modeling of the present invention takes into account the arm error, sensor bias and master-slave inertial navigation installation angle error, fully considering the common error sources in transfer alignment, and avoiding the reduction in estimation accuracy caused by the simple modeling of existing Lie group methods. At the same time, the relationship between state variables is re-derived with reference to the navigation coordinate system in Lie group space, and the estimation results of the present invention are more meaningful for practical engineering applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0116] Figure 1 A flow chart of a Lie group right-invariant transfer alignment method provided by an embodiment of the present invention;

[0117] Figure 2 Schematic diagram of two inertial navigation system coordinate systems and their placement methods provided in an embodiment of the present invention;

[0118] Figure 3 A diagram showing the estimation results of the gyroscope bias using the Lie group right-invariant transfer alignment method provided by an embodiment of the present invention;

[0119] Figure 4 A diagram showing the estimation results of the accelerometer bias using the Lie group right-invariant transfer alignment method provided by an embodiment of the present invention;

[0120] Figure 5 This is a diagram showing the estimation results of the main and sub-inertial navigation system installation error angles using the Lie group right-invariant transfer alignment method provided by an embodiment of the present invention;

[0121] Figure 6 This is a diagram showing the estimation results of the main and sub-inertial navigation rod arms using the Lie group right-invariant transfer alignment method provided in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0123] The present invention discloses a transfer alignment method based on a Lie group right invariant error model, such as Figure 1 The flowchart of the master-slave inertial navigation transfer alignment method, defined by Lie group right-invariant errors, is shown. First, the Lie group right-invariant errors are constructed. Second, each navigation computer performs an inertial navigation solution based on sensor data. Next, the error sources in transfer alignment are fully considered in the navigation coordinate system, and differential equations between states are constructed to establish a transfer alignment model based on Lie group descriptions. Finally, an unscented Kalman filter algorithm is used to accurately estimate the modeled states, completing transfer alignment.

[0124] Step 1: The variables of the present invention are defined as follows:

[0125] Geocentric Inertial Coordinate System (I-Frame): The origin of the I-Frame is the Earth's center. The x- and y-axes lie in the Earth's equatorial plane, with the x-axis pointing to the vernal equinox (one of the intersection points of the equatorial and ecliptic planes with the celestial sphere). The z-axis is the Earth's rotation axis and points to the North Pole. The output of inertial sensors is referenced to this coordinate system.

[0126] Earth-centered Earth-fixed coordinate system (E-system): The Earth-centered Earth-fixed coordinate system is also called the Earth coordinate system, abbreviated as E-system. The Earth coordinate system is fixed to the Earth, with its origin being the Earth's center of mass. The x-axis and y-axis are both in the equatorial plane, with the x-axis pointing to the prime meridian. The z-axis coincides with the Earth's rotation axis and points to the North Pole. The x-axis, y-axis, and z-axis form a right-handed rectangular coordinate system. The angular motion of the E-system relative to the I-system is the Earth's rotation angular rate ω. ie .

[0127] Navigation Coordinate System (N System): The navigation coordinate system is referred to as the N System. The navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving navigation parameters. Any coordinate system can be used as the navigation coordinate system. This invention selects the "Northeastern Sky (ENU)" geographic coordinate system as the navigation coordinate system.

[0128] Main Inertial Navigation Coordinate System (M-system): The main inertial navigation coordinate system is referred to as the M-system. It is a three-axis orthogonal coordinate system obtained after the high-precision inertial navigation system is calibrated. The origin is at the geometric center of the inertial navigation system and along the inertial navigation housing, it meets the "right-front-up" three-axis definition of the carrier coordinate system. At the same time, the main inertial navigation data is recorded as

[0129] Sub-INS coordinate system (S system): Sub-INS coordinate system is referred to as S system for short. It is a three-axis orthogonal coordinate system obtained after calibration of low-precision INS system. Its origin is at the geometric center of INS and it satisfies the “right-front-up” three-axis definition of the carrier coordinate system along the INS shell. At the same time, sub-INS data is recorded as

[0130] Step 2: Based on Lie group theory, establish the right invariant error state. First, in order to satisfy the group affine property, the velocity is converted to Redefine as auxiliary speed The conversion relationship between the two is:

[0131]

[0132] in, is the projection of the velocity of the sub-INS relative to the earth coordinate system in the real navigation coordinate system; is the projection of the position of the sub-INS relative to the earth coordinate system in the real navigation coordinate system; is the projection of the Earth's rotational angular velocity in the real navigation coordinate system; the symbol (·×) indicates:

[0133]

[0134] Based on this, the state χ∈SE(3) in the Lie group space can be expressed as:

[0135]

[0136] in, is the rotation matrix from the real navigation coordinate system to the sub-INS coordinate system, that is, the sub-INS attitude. Since there is a deviation between the attitude and velocity calculated in the navigation computer and the true value, the transfer alignment right invariant error state is defined as the calculation state in combination with the Lie group right invariant error definition. The deviation from the true state χ is:

[0137]

[0138] in, The rotation matrix from the sub-INS coordinate system to the computational navigation coordinate system; The attitude error defined as the right invariant error, Velocity error defined for the right invariant error.

[0139] Step 3: To describe the attitude error propagation process, establish the attitude differential equation. For attitude error, take the n-frame as the reference coordinate system and denote the equivalent rotation vector from the n-frame to the n′-frame as φ, which is often called the misalignment angle error. Based on the relationship between the equivalent rotation vector and the direction cosine matrix, we have:

[0140]

[0141] Taking the time derivative of both sides of equation (5) we have:

[0142]

[0143] According to the attitude differential equation Formula (6) can be expanded as:

[0144]

[0145] Applying the formula (V1×)(V2×)-(V2×)(V1×)=[(V1×V2)×] to Equation (7) and ignoring the second-order small quantities, the attitude differential equation can be simplified to:

[0146]

[0147] Step 4: To describe the velocity error propagation process, establish the velocity differential equation. From formula (4), we can see that the velocity error is defined as Simplifying this formula we can get:

[0148]

[0149] In particular, considering that it is difficult to ensure complete overlap between the installation of the main and sub-INS in the actual system, there is a position deviation between the two sets of INS. The positions of the main and sub-INS are modeled in combination with the lever arm as follows:

[0150]

[0151] Take the time derivative of both sides of equation (10) and multiply them together We can get:

[0152]

[0153] Since the actual modeling state is the redefined auxiliary velocity in order to satisfy the group affine property, substituting formula (1) into formula (11) yields:

[0154]

[0155] From formula (12), the relationship between the main and sub-INS auxiliary speeds is:

[0156]

[0157] Combining the relationship of formula (13), we can get the formula (9) The differential equation is:

[0158]

[0159] At the same time, G n As the projection of gravitational acceleration in the navigation system, it can be expressed as:

[0160]

[0161] According to this, the differential equation of the auxiliary speed Expanding formula (14) yields:

[0162]

[0163] Based on this, we can take the time derivative of both sides of equation (9) and substitute them into equation (16) to obtain:

[0164]

[0165] Arrange the velocity differential equation:

[0166]

[0167] Step 5: Construct the attitude measurement equation. When the navigation system is running, the attitude matrix obtained by the sub-inertial navigation solution can be obtained. The attitude matrix obtained by the main inertial navigation solution Since the accuracy of the main inertial navigation system is often very high, its calculated value can be used as the reference true value of the sub-inertial navigation system. Therefore, by multiplying the two attitude matrices, the Euler angle corresponding to the direction cosine matrix obtained is used as the attitude measurement, and the following attitude measurement equation can be established:

[0168]

[0169] Among them, for the master and slave inertial navigation systems, in addition to the position deviation described by the lever arm, there is also an angular error described by the installation error angle. For the installation error angle, taking the m system as the reference coordinate system and denoting the equivalent rotation vector from the m system to the s system as μ, Equation (19) can be expanded as:

[0170]

[0171] but:

[0172] z obφ =eul(Z obφ )=g1(φ,μ) (21)

[0173] Where g(φ,μ) is a nonlinear function of the misalignment angle error φ and the installation error angle μ, and the relationship between its input and output is:

[0174]

[0175] Step 6: Construct velocity measurement equation. Similar to attitude measurement, when the navigation system is running, the velocity calculated by the sub-INS can be obtained. The speed calculated by the main inertial navigation Subtract the two speeds and use the speed difference as the speed measurement. The following speed measurement equation can be established:

[0176]

[0177] Combining equation (1) with equation (13), expanding equation (23) yields:

[0178]

[0179] Step 7: Construct the state space equation under the right invariant error definition. First, construct the 18-dimensional state With 6-dimensional state noise w=[w g w a ] T In the present invention, the gyroscope drift and accelerometer drift are modeled by constant zero bias and white noise, and and μ are modeled as constants, and the corresponding equations are:

[0180]

[0181] For the state propagation equation, according to the corresponding coefficients of the states in the attitude differential equation (8) and the velocity differential equation (18), the state transfer matrix F and the noise driving matrix G can be obtained.

[0182]

[0183]

[0184] Secondly, according to the attitude measurement equation (21) and the velocity measurement equation (25), the measurement generation function h(x) can be obtained.

[0185]

[0186] The state space equation of the sub-INS is obtained:

[0187]

[0188] Where, v is the measurement noise;

[0189] Finally, the noise v is measured by combining the actual system matching k , sorting and discretizing the state space equation is:

[0190]

[0191] Where Δt is the discrete time; Φ k,k-1 is the discrete transfer matrix; G k-1 is the discrete noise matrix; v k is the discrete measurement noise.

[0192] Step 8: Due to the nonlinear transformation in Equation (30), the state probability density after nonlinear transformation cannot maintain Gaussian properties, which violates the assumption of Kalman filtering. Therefore, the nonlinear state estimation method - unscented Kalman filtering is used to achieve state estimation. First, construct 2n+1 Sigma points for time update:

[0193]

[0194] The weight of each Sigma point is calculated as follows:

[0195]

[0196] Among them, ω (m) is the weight when calculating the mean, ω (c) is the weight when calculating the covariance. The parameter β is used to introduce the prior information of non-Gaussian distribution, and the parameter λ=α 2 (n+κ)-n is the proportional coefficient. The distribution of Sigma points around the mean can be determined by the parameters α and κ, κ = 3-n. Next, bring the Sigma points into state propagation:

[0197]

[0198] Calculate the predicted mean and predicted P k|k-1 :

[0199]

[0200] Next, construct a new Sigma point for measurement update:

[0201]

[0202] Substitute the newly constructed Sigma point into the system measurement equation:

[0203] Zi,k|k-1 =h(χ i,k|k-1 ),i=0,...,2n(38)

[0204] Calculate the amount of measurement prediction mean and the predicted covariance and the cross-covariance between state and quantity measurements

[0205]

[0206] Finally, calculate the filter gain K k , binding amount measurement z k , obtain the filter state estimate and variance P k|k :

[0207]

[0208] Example 1

[0209] Compared with the traditional transfer alignment method which requires three-axis angular velocity motion or linear velocity motion, in order to verify the superiority of the present invention, the simulation trajectory only has two-axis angular motion of X and Y. When simulating the sliding of the carrier-based aircraft on the deck, the main and sub-inertial navigation systems rotate relative to each other due to the deck motion. The motion of the pitch angle θ and the roll angle γ conforms to a composite sinusoidal function form, such as:

[0210]

[0211] Among them, the shaking amplitude θ m , γ m Both are 15°, initial angle θ I , γ I All are 0°, shaking period T θ 、T γ The discrete time T is 0.01s, and the entire simulation movement lasts for 60s.

[0212] In step 1, the initial geographical location is 126.6877° east longitude and 45.7817° north latitude;

[0213] In step 1, the sensor output frequency is 100Hz;

[0214] In step 1, gyroscope drift: constant bias of 10° / h, random walk Accelerometer drift: constant bias of 1 mg, random walk

[0215] In step 2, the installation error angle between the main and sub inertial navigation systems is set to [1°1°1°];

[0216] In step 2, the lever arm between the master and slave inertial navigation systems is set to [10m 10m 10m];

[0217] In step 7, the noise v is measured k The covariance matrix is ​​a 6×6 diagonal matrix, where the first three elements are all 1.0e -8 Represents attitude measurement noise, and the remaining elements are all 1.0e -4 represents the speed measurement noise;

[0218] In step 8, the parameter α in the unscented Kalman filter is 0.01, the parameter β is 2, and the parameter κ is 0.

[0219] The results of 20 Monte Carlo simulations are as follows:

[0220] Taking the root mean square error RMSE as the measurement indicator, the simulation results are as follows Figure 3 、 Figure 4 As shown in the figure. It can be seen from the figure that the estimation results of the gyroscope bias, installation error angle and lever arm are stable and basically converged in about 10 seconds, and the maximum RMSE averages of the three states in the X, Y and Z axes in the last 5 seconds are 0.2666° / h, 0.0088° and 0.0368m respectively; the estimation result of the accelerometer bias is stable and basically converged in about 15 seconds, and the maximum RMSE value in the X, Y and Z axes in the last 5 seconds is 320.0231ug. It can be seen from the simulation results that the present invention can quickly and effectively complete the transfer alignment of the main and sub-inertial navigation systems even when there is only two-axis motion.

[0221] In summary, the proposed transfer alignment method fully accounts for the objective error sources between the master and slave inertial navigation systems, improves system error modeling within the Lie group framework, and ensures that the system model accurately reflects the actual situation. Furthermore, the modeling approach, which uses the local navigation coordinate system as the reference coordinate system, closely aligns with the application requirements of practical engineering projects, has broad application prospects, and provides strong support for the development of downstream technologies.

[0222] In particular, in some preferred embodiments of the present invention, a computer device is also provided, including a memory and a processor and a computer program stored on the memory. When the processor executes the computer program, the steps of the transfer alignment method based on the Lie group right invariant error model in any of the above embodiments are implemented.

[0223] In other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instructions are stored. When the computer program is executed by a processor, the steps of the transfer alignment method based on the Lie group right invariant error model described in any of the above embodiments are implemented.

[0224] Those skilled in the art will understand that all or part of the processes in the above-mentioned embodiment method can be implemented by instructing related hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the process of the above-mentioned transfer alignment method embodiment based on the Lie group right-invariant error model, which will not be repeated here.

[0225] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or N embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and the features of different embodiments or examples without contradiction.

[0226] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of technical features indicated. Thus, a feature specified as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present invention, "N" means at least two, such as two, three, etc., unless otherwise specifically defined.

[0227] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing a custom logical function or step of a process, and the scope of the preferred embodiments of the invention includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of the invention pertain.

[0228] The logic and / or steps represented in the flowcharts or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing the logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (e.g., a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device). For purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include the following: an electrical connection with one or N wires (electronic devices), a portable computer disk cartridge (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and programmable read-only memory (EPROM or flash memory), fiber optic devices, and a portable compact disc read-only memory (CDROM). Furthermore, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing it in another suitable manner if necessary, and then storing it in a computer memory.

[0229] It should be understood that various parts of the present invention can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiment, the N steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having logic gate circuits for implementing logic functions on data signals, an application-specific integrated circuit having suitable combinational logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0230] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0231] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing module, or each unit may exist physically separately, or two or more units may be integrated into a single module. The aforementioned integrated modules may be implemented in the form of hardware or in the form of software functional modules. If the integrated modules are implemented in the form of software functional modules and sold or used as independent products, they may also be stored in a computer-readable storage medium.

[0232] The storage medium mentioned above may be a read-only memory, a magnetic disk, or an optical disk, etc. Although the embodiments of the present invention have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Persons skilled in the art may make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A transfer alignment method based on Lie group right invariant error model, characterized in that: include: Step 1: Define the coordinate system; Step 2: Based on the right-invariant error theory under Lie groups, introduce the auxiliary velocity vector and establish the error state model between the main and sub-INS. Step 3: Based on the attitude transformation relationship between the real navigation coordinate system and the calculated navigation coordinate system, construct the attitude error differential equation of the sub-INS; Step 4: Based on the difference between the auxiliary velocity of the sub-INS in the real and calculated navigation coordinate systems, and considering the arm error caused by the misalignment of the main and sub-INS installations, the velocity error differential equation of the sub-INS system is constructed. Step 5: Based on the relationship between the attitude matrices solved by the master and slave INS, construct the attitude measurement equation of the slave INS taking into account the installation error angle; Step 6: Based on the relationship between the velocities calculated by the master and slave INS systems, construct the speed measurement equation of the slave INS system. Step 7: First, construct the system's error state vector based on attitude error, velocity error, gyroscope bias, accelerometer bias, lever arm, and mounting angle error. Simultaneously, construct the system noise vector based on the measurement errors of the sub-INS gyroscope and accelerometer. Next, construct the state differential equation of the sub-INS system based on the differential equations for each error state and combining the system state vector and the noise vector. Furthermore, construct the error state space equation of the sub-INS system based on the attitude and velocity measurement equations of the sub-INS. Finally, organize and discretize the system to obtain a complete discrete-time error state space model. Step 8: Based on the discrete-time error state space model, the unscented Kalman filter algorithm is used to achieve accurate estimation of the error state, thereby completing the transfer alignment of the main and sub-INS.

2. The transfer alignment method based on the Lie group right invariant error model according to claim 1, characterized in that: In step 1, the coordinate system is defined as: I system: Earth-centered inertial coordinate system, referred to as inertial coordinate system; e system: Earth-centered Earth-fixed coordinate system, referred to as the Earth coordinate system; n system: "East-North-Sky" geographic coordinate system is used as the navigation coordinate system; the navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving navigation parameters; M system: Main inertial navigation coordinate system, with the main inertial navigation geometric center as the origin, and the three axes xyz pointing to the right, front and top directions of the vehicle respectively; S system: Sub-INS coordinate system, with the sub-INS geometric center as the origin, and the three axes xyz pointing to the right, front and top directions of the carrier respectively.

3. The transfer alignment method based on the Lie group right invariant error model according to claim 2, characterized in that: In step 2, the establishment of the right invariant error state model between the master and slave inertial navigation systems is specifically as follows: Step 2.1: Build Auxiliary Speed Among them, the symbol (·×) is is the projection of the Earth's rotation angular velocity in the real navigation coordinate system; is the projection of the velocity of the sub-INS relative to the earth coordinate system in the real navigation coordinate system; is the projection of the position of the sub-INS relative to the earth coordinate system in the real navigation coordinate system; Step 2.2: The state χ∈SE(3) in the Lie group space is: in, The rotation matrix from the real navigation coordinate system to the sub-INS coordinate system, that is, the sub-INS attitude; Step 2.3: Based on the calculation status The deviation from the true state χ, the right invariant error is defined as: in, The rotation matrix from the sub-INS coordinate system to the computational navigation coordinate system; is the estimated value of the auxiliary speed; is the attitude error of the right invariant error, is the velocity error of the right invariant error.

4. The transfer alignment method based on the Lie group right invariant error model according to claim 3, characterized in that: In step 3, the attitude error differential equation of the sub-INS is specifically: Step 3.1: Based on the relationship between the equivalent rotation vector and the rotation matrix, the attitude error is defined as: Where φ is the equivalent rotation vector from the real navigation coordinate system n to the sub-INS calculation navigation coordinate system n′, that is, the misalignment angle error; I 3×3 It is a unit matrix with 3 rows and 3 columns; Step 3.2: Sub-INS attitude differential equation: in, is the projection of the angular velocity of the sub-INS relative to the inertial coordinate system in the sub-INS coordinate system; is the projection of the angular velocity of the real navigation coordinate system relative to the inertial coordinate system in the real navigation coordinate system; Step 3.3: The attitude error differential equation of the sub-INS is: in, is the measurement error of the sub-INS gyroscope; Since it is not considered when modeling the system state, it can be regarded as state noise.

5. The transfer alignment method based on the Lie group right invariant error model according to claim 4, characterized in that: In step 4, the velocity error differential equation of the sub-INS is specifically: Step 4.1: Calculate the velocity error: in, is the projection of the auxiliary velocity of the sub-INS relative to the earth coordinate system in the calculated navigation coordinate system; Step 4.2: Considering the influence of the lever arm between the master and slave INS, the positions of the master and slave INS can be modeled as: in, is the projection of the position of the sub-INS coordinate system relative to the Earth coordinate system in the Earth coordinate system; The projection of the position of the main inertial navigation coordinate system relative to the earth coordinate system in the earth coordinate system; is the projection of the position of the sub-INS coordinate system relative to the main INS coordinate system in the Earth coordinate system; is the rotation matrix from the earth coordinate system to the master inertial navigation coordinate system; is the projection of the position of the sub-INS coordinate system relative to the main INS coordinate system in the main INS coordinate system, i.e., the lever arm; Step 4.3: Based on the auxiliary velocity vector and the main and sub-INS positions of the combined lever arm, the relationship between the auxiliary velocity of the main and sub-INS and the lever arm is obtained as follows: in, The projection of the angular velocity of the main inertial navigation coordinate system relative to the inertial coordinate system in the main inertial navigation coordinate system; is the rotation matrix from the real navigation coordinate system to the main inertial navigation coordinate system; Step 4.4: The differential equation for calculating the auxiliary velocity is: Among them, G n is the projection of gravitational acceleration in the real navigation system; g n is the projection of gravitational acceleration in the real navigation system; For centripetal force; The projection of the position of the main inertial navigation coordinate system relative to the earth coordinate system in the real navigation coordinate system; Step 4.5: Calculate the velocity error differential equation as: in, is the accelerometer measurement error; is the estimated value of the Earth's rotational angular velocity projected in the main inertial navigation coordinate system; is an estimate of the gravitational acceleration.

6. The transfer alignment method based on the Lie group right invariant error model according to claim 5, characterized in that: In step 5, the attitude measurement equation of the sub-INS is: With obφ =eul(Z obφ )=g1(φ,μ)(13) in, The attitude matrix of the main inertial navigation; Z is the attitude matrix calculated by the sub-INS; obφ is the attitude measurement matrix; g1(φ,μ) is a nonlinear function of the misalignment angle error φ and the installation error angle μ; eul(·) is the conversion of the rotation matrix to the Euler angle. The specific input and output relationship is: With obφ (1)=arcsin(Z obφ (3,2)) With obφ (2)=arctan2(-Z obφ (3,1) / Z obφ (3,3)) (15) With obφ (3)=arctan2(-Z obφ (1,2) / Z obφ (2,2))。 7. The transfer alignment method based on the Lie group right invariant error model according to claim 6, characterized in that: In step 6, the velocity measurement equation of the sub-INS is:

8. The transfer alignment method based on the Lie group right invariant error model according to claim 7, characterized in that: In step 7, specifically: Step 7.1: Construct the state vector of the sub-INS; Step 7.1.1: Use constant value modeling to construct the lever arm vector and installation error angle; Step 7.1.2: Change the state variables ε, ▽, and μ are expanded to the right invariant error state, and the 18-dimensional state quantity x of the filter model is constructed as follows: Where φ is the misalignment angle of the sub-INS, dv R is the velocity error, ε is the gyroscope bias, ▽ is the accelerometer bias, The arm error between the main and sub-INS, μ is the installation error angle between the main and sub-INS; Step 7.2: Construct the noise vector of the sub-INS; Step 7.2.1: Use constant bias and white noise modeling to construct the measurement error of the sub-INS gyroscope in equation (6) The measurement error of the neutron inertial navigation accelerometer is Among them, w g and w a is a zero-mean Gaussian white noise; the gyroscope bias ε and accelerometer bias ▽ are modeled as constant biases. Step 7.2.2: The 6-dimensional state noise w of the filtering model is: in=[in g In a ] T (21) Step 7.3: Construct the state space equation of the sub-INS; Step 7.3.1: According to the attitude error differential equation (6) and the velocity error differential equation (12), determine the transfer matrix F and the noise matrix G as: Step 7.3.2: Based on the attitude measurement equation (13) and the velocity measurement equation (17), determine the measurement generation function h(x) as: Step 7.3.3: Generate the function h(x) and combine it with the actual system noise to obtain the state space equation of the sub-INS: Where, v is the measurement noise; Step 7.4: Discretize the state space equation to obtain: Where Δt is the discrete time; Φ k,k-1 is the discrete transfer matrix; G k-1 is the discrete noise matrix; v k is the discrete measurement noise.

9. The transfer alignment method based on the Lie group right invariant error model according to claim 8, characterized in that: In step 8, the nonlinear state estimation method - unscented Kalman filter is used to achieve state estimation, specifically: Step 8.1: Construct 2n+1 Sigma points for time update: Step 8.2: The weight of each Sigma point is calculated as follows: Among them, ω (m) is the weight when calculating the mean, ω (c) is the weight when calculating the covariance; the parameter β is used to introduce the prior information of non-Gaussian distribution, and the parameter λ=α 2 (n+κ)-n is the proportional coefficient. The distribution of Sigma points around the mean can be determined by the parameters α and κ, κ = 3-n; Step 8.3: Bring the Sigma point into state propagation: in, is the predicted Sigma point; Step 8.4: Calculate the predicted mean according to steps 8.1 to 8.3 and predicted P k|k-1 : Step 8.5: Reconstruct 2n+1 Sigma points for measurement update: Step 8.6: Substitute the newly constructed Sigma point into the system measurement equation: Z i,k|k-1 =h(χ i,k|k-1 ),i=0,...,2n(33) Among them, Z i,k|k-1 is the predicted measurement Sigma point; Step 8.7: Calculate the predicted mean of the quantity measure and the predicted covariance and the cross-covariance between state and quantity measurements Step 8.8: Calculate the filter gain K k , binding amount measurement z k , obtain the filter state estimate and variance P k|k : Step 8.9: After obtaining an estimate of the error state vector x through the unscented Kalman filter, feedback correction is performed on the lever arm and installation angle errors to improve the transfer alignment accuracy of the main and sub-INS.

10. A computer device / apparatus / system comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 9.

Images