A transfer alignment method and device based on a right-invariant error model of a Lie group

Abstract

This invention discloses a transfer alignment method based on a right-invariant error model using Lie groups, belonging to the field of inertial navigation systems. The method includes: first, referencing the navigation coordinate system, remodeling the right-invariant attitude and velocity errors based on Lie group theory to replace traditional transfer alignment errors, and incorporating sensor bias, lever arm and installation angle errors between the master and sub-inertial navigation systems to fully consider transfer alignment error sources; second, re-deriving the system state equations within the Lie group framework to decouple computational parameters from the system model; then, based on an "attitude + velocity" measurement matching scheme, re-deriving the system measurement equations within the Lie group framework; finally, addressing the nonlinear characteristics of the measurement equations, combining an unscented Kalman filter to accurately estimate the system state. The transfer alignment method proposed in this invention fully considers the objectively existing error sources between the master and sub-inertial navigation systems, improves system error modeling within the Lie group framework, and ensures that the system model accurately reflects the actual situation.

Description

Technical Field

[0001] This 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 Technology

[0002] Inertial navigation technology autonomously measures and senses the angular velocity and linear acceleration of a launch vehicle. Based on initial state information, it calculates the vehicle's position, velocity, and attitude in real time, providing a solid foundation for subsequent launch vehicle control and planning. As a fully autonomous positioning and navigation technology, it does not rely on external signals or actively transmit signals during operation, exhibiting strong anti-interference capabilities and high stealth. However, the numerical integration process of inertial navigation means that the accuracy of its initial state directly affects the subsequent navigation accuracy, which in turn affects the accuracy of subsequent launch vehicle attitude control and trajectory planning. Therefore, initial alignment technology is an important guarantee and key factor for the accurate calculation of navigation parameters. Transfer alignment, as a typical moving-base initial alignment technology, utilizes navigation information provided by a high-precision main inertial navigation system to assist a low-precision sub-inertial navigation system in achieving rapid 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 two inertial navigation systems (INS) to achieve real-time state estimation through measurement parameter matching or computational parameter matching. However, the state transition matrix of these methods often includes sub-INS computational parameters. In complex, highly maneuverable scenarios, the accuracy of the low-precision sub-INS computational parameters cannot be guaranteed, often resulting in significant errors. Consequently, during state propagation to the next time step, the computational error of the current time step is propagated and superimposed, rendering the system model inapplicable in subsequent time steps. This leads to a decrease in the reliability of transfer alignment over time, ultimately causing the sub-INS positioning accuracy to fail to meet practical requirements.

[0004] Benefiting from the advantages of Lie group theory, Lie group-based transfer alignment techniques can largely decouple inertial navigation calculation parameters from the system model. Currently, research on Lie group-based transfer alignment techniques is limited, and existing techniques do not consider the link arm error problem between the master and sub-inertial navigation systems in practice. System modeling is relatively simple, failing to fully account for the error sources of real-world systems, leaving room for improvement in estimation accuracy. Furthermore, 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, using the Earth coordinate system for state estimation is actually a practice for polar navigation, where commonly used navigation parameters reference the local navigation coordinate system. Therefore, existing Lie group transfer alignment methods lack direct application value.

[0005] To address the aforementioned issues, there is a need to invent a Lie group transfer alignment method that fully considers the sources of systematic errors, achieves high alignment accuracy, and has direct application value. Summary of the Invention

[0006] The purpose of this invention is to propose a transfer alignment method based on a Lie group right-invariant error model to solve the problems mentioned in the background. It applies Lie group theory to model the right-invariant error state in the navigation coordinate system, replacing the traditional transfer alignment error modeling, and incorporates the link arm and installation angle errors between the master and sub-inertial navigation systems, 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, constructing a new system state space model. Based on the "attitude + velocity" matching scheme, unscented Kalman filtering 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] This 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, an auxiliary velocity vector is introduced to establish an error state model between the master and sub-inertial navigation systems;

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

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

[0012] Step 5: Based on the relationship between the attitude matrices obtained from the main and sub-inertial navigation systems, construct the attitude measurement equations of the sub-inertial navigation system that take into account the installation error angle;

[0013] Step 6: Based on the relationship between the velocities calculated by the main and sub-inertial navigation systems, construct the velocity measurement equation for the sub-inertial navigation system;

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

[0015] Step 8: For the discrete-time error state-space model, the unscented Kalman filter algorithm is used to accurately estimate the error state, thereby completing the master and sub-inertial navigation transfer alignment.

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

[0017] i-frame: Geocentric inertial coordinate system, or simply inertial coordinate system;

[0018] e-system: Earth-centered, Earth-fixed coordinate system, or simply 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 for navigation parameters;

[0020] m-frame: The main inertial navigation coordinate system, with the geometric center of the main inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively;

[0021] S-frame: Sub-inertial navigation coordinate system, with the geometric center of the sub-inertial navigation as the origin, and the three axes x, y, and z 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 sub-inertial navigation systems specifically involves:

[0023] Step 2.1: Build auxiliary speed

[0024]

[0025] Wherein, the symbol (·×) is This is the projection of the Earth's rotational angular velocity onto the actual navigation coordinate system. The projection of the velocity of the inertial navigation system relative to the Earth coordinate system onto the actual navigation coordinate system; The projection of the sub-inertial navigation system's position relative to the Earth coordinate system onto the actual navigation coordinate system;

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

[0027]

[0028] in, This is the rotation matrix from the real navigation coordinate system to the sub-inertial navigation coordinate system, i.e., the sub-inertial navigation attitude;

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

[0030]

[0031] in, This is the rotation matrix from the sub-inertial navigation coordinate system to the computational navigation coordinate system; This is an estimate of the auxiliary speed; The attitude error is the right-invariant error. The velocity error is a right-invariant error.

[0032] Furthermore, in step 3, the differential equation for the attitude error of the sub-inertial navigation system is specifically as follows:

[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-inertial navigation computational coordinate system n′, i.e., the misalignment angle error; I 3×3 It is a 3x3 identity matrix;

[0036] Step 3.2: Attitude differential equations of the sub-inertial navigation system:

[0037]

[0038] in, The projection of the sub-inertial navigation system's angular velocity relative to the inertial coordinate system onto the sub-inertial navigation system's coordinate system; This is the projection of the angular velocity of the real navigation coordinate system relative to the inertial coordinate system onto the real navigation coordinate system.

[0039] Step 3.3: The differential equation for the attitude error of the sub-inertial navigation system is:

[0040]

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

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

[0043] Step 4.1: Calculate the speed error:

[0044]

[0045] in, The projection of the auxiliary velocity of the sub-inertial navigation system relative to the Earth coordinate system onto the navigation coordinate system;

[0046] Step 4.2: Considering the influence of the lever arms between the master and sub-inertial navigation systems, the positions of the master and sub-inertial navigation systems can be modeled as follows:

[0047]

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

[0049] Step 4.3: Based on the auxiliary velocity vector and the main and sub-inertial navigation positions of the linkage arm, the relationship between the auxiliary velocities of the main and sub-inertial navigation systems and the linkage 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 onto the main inertial navigation coordinate system; This is the rotation matrix from the real navigation coordinate system to the master inertial navigation coordinate system;

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

[0053]

[0054] Among them, G n g is the projection of gravitational acceleration in the real navigation frame; n This is the projection of gravitational acceleration onto the actual navigation frame. Centripetal force; The projection of the position of the main inertial navigation coordinate system relative to the Earth coordinate system onto the actual navigation coordinate system;

[0055] Step 4.5: The differential equation for the velocity error is calculated as follows:

[0056]

[0057] in, This is due to accelerometer measurement error; This is an estimated value of the Earth's rotational angular velocity projected onto the principal inertial navigation coordinate system. This is an estimate of gravitational acceleration;

[0058] Further, in step 5, the attitude measurement equation of the sub-inertial navigation system is:

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

[0060]

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

[0062]

[0063] Further, in step 6, the velocity measurement equation of the sub-inertial navigation system is:

[0064]

[0065] Furthermore, in step 7, specifically:

[0066] Step 7.1: Construct the state vector of the sub-inertial navigation system;

[0067] Step 7.1.1: Construct the lever arm vector and installation error angle using constant value modeling;

[0068]

[0069] Step 7.1.2: Set the state variables ε, ▽, Extending μ to the right-invariant error state, the 18-dimensional state variable x of the filtering model is:

[0070]

[0071] Where φ is the misalignment angle of the sub-inertial navigation system, dv R Here, ε represents the velocity error, ▽ represents the gyroscope bias, and ▽ represents the accelerometer bias. The lever arm error between the master and sub-inertial navigation systems, μ is the installation error angle between the master and sub-inertial navigation systems;

[0072] Step 7.2: Construct the noise vector of the sub-inertial navigation system;

[0073] Step 7.2.1: Construct the measurement error of the sub-inertial gyroscope in Equation (6) using constant zero bias and white noise modeling. Measurement error of neutron inertial accelerometer in equation (12)

[0074]

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

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

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

[0078] Step 7.3: Construct the state-space equations of the sub-inertial navigation system;

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

[0080]

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

[0082]

[0083] Step 7.3.3: Combining the generating function h(x) with the noise level of the actual system, the state-space equation of the sub-inertial navigation system is obtained:

[0084]

[0085] Where v is the measurement noise;

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

[0087]

[0088]

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

[0090] Furthermore, in step 8, the state estimation is achieved using a nonlinear state estimation method—unscented Kalman filtering—specifically as follows:

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

[0092]

[0093] Step 8.2: The weights of each Sigma point are calculated as follows:

[0094]

[0095] Where, ω (m) ω is the weight used when calculating the mean. (c) The weights are used to calculate the covariance; the parameter β is used to introduce prior information about the non-Gaussian distribution, and the parameter λ = α. 2 (n+κ)-n is the proportionality coefficient. The distribution of the Sigma point near the mean can be determined by the parameters α and κ, where κ = 3-n.

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

[0097]

[0098] in, For the predicted Sigma point;

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

[0100]

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

[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 For the predicted measurement Sigma point;

[0106] Step 8.7: Calculate the predicted mean. and predicted covariance and the cross-covariance between state and measurement

[0107]

[0108] Step 8.8: Calculate the filter gain K k Combined measurement z k Obtain the filtered state estimate and variance P k|k :

[0109]

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

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

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

[0113] The present invention also provides a computer program product, including a computer program / instruction that, when executed by a processor, implements the steps of the transit alignment method based on the right-invariant error model of the Lie group as described above.

[0114] The beneficial effects of this invention are as follows:

[0115] This invention proposes a transfer alignment technique based on right-invariant error in Lie group space, which decouples the system state equation from the inertial navigation calculation parameters, avoiding the accumulation of model errors caused by computational error coupling. Simultaneously, the state modeling of this invention considers lever arm errors, sensor offsets, and master inertial navigation system installation angle errors, fully taking into account common error sources in transfer alignment, thus avoiding the decrease in estimation accuracy caused by the simplistic modeling of existing Lie group methods. Furthermore, the relationships between state variables are re-derived in the reference navigation coordinate system in Lie group space, making the estimation results of this invention more relevant to practical engineering applications. Attached Figure Description

[0116] Figure 1 A flowchart of the Lie group right-invariant transfer alignment method provided in an embodiment of the present invention;

[0117] Figure 2 This is a schematic diagram of the coordinate systems and placement of two inertial navigation systems provided in an embodiment of the present invention;

[0118] Figure 3 The figure shows the estimation result of gyroscope bias by the Lie group right-invariant transfer alignment method provided in the embodiment of the present invention.

[0119] Figure 4 The figure shows the estimation result of accelerometer bias by the Lie group right-invariant transfer alignment method provided in the embodiment of the present invention;

[0120] Figure 5 The figure shows the estimation results of the installation error angle of the main and sub-inertial navigation systems using the Lie group right-invariant transfer alignment method provided in the embodiments of the present invention.

[0121] Figure 6 The figure shows the estimation results of the master and slave inertial guide arms using the Lie group right-invariant transfer alignment method provided in the embodiments of the present invention. Detailed Implementation

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

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

[0124] Step 1: The variables in this invention are defined as follows:

[0125] Geocentric inertial coordinate system (i-frame): The origin of the geocentric inertial coordinate system is the center of the Earth. The x-axis and y-axis lie in the Earth's equatorial plane, with the x-axis pointing to the vernal equinox (one of the intersections of the equatorial plane and the ecliptic plane with the celestial sphere). The z-axis is the Earth's rotation axis and points to the North Pole. The output of the inertial sensor is based on this coordinate system.

[0126] Geocentric-fixed coordinate system (e-frame): The geocentric-fixed coordinate system, also known as the Earth coordinate system, is simply called the e-frame. The Earth coordinate system is fixed to the Earth, with its origin at the Earth's center of mass. The x-axis and y-axis lie in the equatorial plane, with the x-axis pointing towards the Prime Meridian. The z-axis coincides with the Earth's rotation axis and points towards the North Pole. The x, y, and z axes form a right-handed rectangular coordinate system. The angular motion of the e-frame relative to the i-frame is equal to the Earth's rotational angular rate ω. ie .

[0127] Navigation coordinate system (n-system): The navigation coordinate system is simply referred to as the n-system. The navigation coordinate system is the reference coordinate system used by the inertial navigation system when solving for navigation parameters; any coordinate system can be used as the navigation coordinate system. This invention selects the "Northeast Sky (ENU)" geographic coordinate system as the navigation coordinate system.

[0128] Master Inertial Navigation Coordinate System (m-system): The master inertial navigation coordinate system, abbreviated as m-system, is a three-axis orthogonal coordinate system obtained after calibration of a high-precision inertial navigation system. Its origin is at the geometric center of the inertial navigation system, and it satisfies the "right-front-up" three-axis definition of the carrier coordinate system along the inertial navigation shell. Simultaneously, the master inertial navigation data is denoted as...

[0129] Sub-inertial navigation coordinate system (S-frame): The sub-inertial navigation coordinate system, abbreviated as S-frame, is a three-axis orthogonal coordinate system obtained after calibration of a low-precision inertial navigation system. Its origin is at the geometric center of the inertial navigation system, and it satisfies the "right-front-up" three-axis definition of the carrier coordinate system along the inertial navigation shell. Simultaneously, the sub-inertial navigation data is denoted as...

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

[0131]

[0132] in, The projection of the velocity of the inertial navigation system relative to the Earth coordinate system onto the actual navigation coordinate system; The projection of the sub-inertial navigation system's position relative to the Earth coordinate system onto the actual navigation coordinate system; This represents the projection of the Earth's rotational angular velocity onto the actual 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, This is the rotation matrix from the true navigation coordinate system to the sub-INS coordinate system, i.e., the sub-INS attitude. Since there are discrepancies between the attitude and velocity calculated by the navigation computer and the true values, the right-invariant error state is defined as the calculated state, based on the Lie group's definition of right-invariant error. The deviation from the true state χ is:

[0137]

[0138] in, This is the rotation matrix from the sub-inertial navigation coordinate system to the computational navigation coordinate system; The attitude error is defined as the right-invariant error. The velocity error is defined as the right-invariant error.

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

[0140]

[0141] Differentiating both sides of equation (5) with respect to time, we have:

[0142]

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

[0144]

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

[0146]

[0147] Step 4: To describe the propagation process of velocity error, establish the velocity differential equation. From equation (4), it can be seen that the velocity error is defined as... Simplifying the expression, we get:

[0148]

[0149] Specifically, considering that in actual systems it is difficult to ensure that the installation of the main and sub-inertial navigation systems is completely overlapping, and that there is a positional deviation between the two sets of inertial navigation systems, the positions of the main and sub-inertial navigation systems are modeled as follows, taking into account the lever arms:

[0150]

[0151] Differentiate both sides of equation (10) with respect to time and multiply them by the same time. We can obtain:

[0152]

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

[0154]

[0155] From equation (12), the relationship between the main and sub-inertial navigation auxiliary velocities can be obtained as follows:

[0156]

[0157] Combining the relationship in equation (13), we can obtain the relationship in equation (9). The differential equation is:

[0158]

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

[0160]

[0161] Therefore, based on the differential equation of the auxiliary velocity Expanding equation (14) yields:

[0162]

[0163] Based on this, by differentiating both sides of equation (9) with respect to time and substituting them into equation (16), we can obtain:

[0164]

[0165] The velocity differential equation is obtained by rearranging:

[0166]

[0167] Step 5: Construct the attitude measurement equations. During navigation system operation, the attitude matrix obtained from the sub-inertial navigation system calculations can be obtained. The attitude matrix obtained by the main inertial navigation system Since the accuracy of the primary inertial navigation system (INS) is often very high, its calculated value can be used as the reference true value for the secondary INS. Therefore, by multiplying the two attitude matrices, the Euler angles corresponding to the direction cosine matrix can be used as attitude measurements, 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 positional 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-frame as the reference coordinate system, and denoting the equivalent rotation vector from the m-frame to the s-frame as μ, equation (19) can be expanded as follows:

[0170]

[0171] but:

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

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

[0174]

[0175] Step 6: Construct the velocity measurement equation. Similar to attitude measurement, the velocity calculated by the sub-inertial navigation system can be obtained during navigation system operation. The velocity obtained from the main inertial navigation system Subtracting the two velocities and obtaining the velocity difference as the velocity measurement, the following velocity measurement equation can be established:

[0176]

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

[0178]

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

[0180]

[0181] Based on the corresponding coefficients of the states in the attitude differential equation (8) and the velocity differential equation (18) for the state propagation equation, the state transition matrix F and the noise driving matrix G can be obtained.

[0182]

[0183]

[0184] Secondly, based on 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 equations of the sub-inertial navigation system are obtained as follows:

[0187]

[0188] Where v is the measurement noise;

[0189] Finally, the measurement noise v is matched to the actual system. k After rearranging and discretizing, the state-space equations are obtained as follows:

[0190]

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

[0192] Step 8: Due to the nonlinear transformation in equation (30), the state probability density after the nonlinear transformation cannot maintain Gaussianity, violating the assumption of Kalman filtering. Therefore, the nonlinear state estimation method - unscented Kalman filtering - is used to achieve state estimation. First, 2n+1 Sigma points are constructed for time updates:

[0193]

[0194] The weights of each Sigma point are calculated as follows:

[0195]

[0196] Where, ω (m) ω is the weight used when calculating the mean. (c) The weights are used to calculate the covariance. The parameter β is used to incorporate prior information about the non-Gaussian distribution, and the parameter λ = α. 2 (n+κ)-n is the proportionality coefficient. The distribution of the Sigma points near the mean can be determined by parameters α and κ, where κ = 3-n. Next, the Sigma points are introduced into state propagation:

[0197]

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

[0199]

[0200] Next, construct new Sigma points for measurement updates:

[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] Computational quantity measurement prediction mean and predicted covariance and the cross-covariance between state and measurement

[0205]

[0206] Finally, calculate the filter gain K. k Combined measurement z k Obtain the filtered state estimate and variance P k|k :

[0207]

[0208] Example 1

[0209] Compared to traditional transfer alignment methods that require three-axis angular velocity or linear velocity motion, to verify the superiority of this invention, the simulated trajectory only involves angular motion along the X and Y axes. Furthermore, when simulating a carrier-based aircraft sliding on the deck, the deck motion causes relative rotation between the main and sub-inertial navigation systems. The pitch angle θ and roll angle γ conform to a composite sine function form, such as:

[0210]

[0211] Among them, the sway amplitude θ m γ m Both are 15°, with an initial angle θ. I γ I All are 0°, shaking period T θ T γ All times are 5s, the discrete time T is 0.01s, and the entire simulation 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 primary and secondary 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 This represents attitude measurement noise; all other elements are 1.0e. -4 Represents velocity measurement noise;

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

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

[0220] Using the root mean square error (RMSE) as a metric, the simulation results are as follows: Figure 3 , Figure 4 As shown in the figure, the estimated results for gyroscope bias, installation error angle, and lever arm stabilize and converge around 10 seconds. In the last 5 seconds, the average maximum RMSE values ​​for the three states along the X, Y, and Z axes are 0.2666° / h, 0.0088°, and 0.0368m, respectively. The estimated result for accelerometer bias stabilizes and converges around 15 seconds, with the maximum RMSE value for the X, Y, and Z axes in the last 5 seconds being 320.0231ug. The simulation results demonstrate that this invention can quickly and effectively complete the transfer and alignment of the main and sub-inertial navigation systems even with only two-axis motion.

[0221] In summary, the transfer alignment method proposed in this invention fully considers the objectively existing error sources between the master and sub-inertial navigation systems, improves the system error modeling under the Lie group framework, and ensures that the system model can accurately reflect the actual situation. Furthermore, the modeling method using the local navigation coordinate system as the reference coordinate system closely aligns with the application needs of practical engineering, 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 in the memory, wherein the processor executes the computer program to implement the steps of the transfer alignment method based on the right-invariant error model of the Lie group described in any of the above embodiments.

[0223] In some other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instruction is stored, wherein when the computer program is executed by a processor, the steps of the transfer alignment method based on the right-invariant error model of the Lie group 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 embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above embodiments of the transfer alignment method based on the right-invariant error model of the Lie group, which will not be repeated here.

[0225] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

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

[0227] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more N executable instructions for implementing custom logic functions or processes, and the scope of preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously 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 flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing 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 (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs 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: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

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

[0230] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0231] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

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

Claims

1. A method of transfer alignment 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, an auxiliary velocity vector is introduced to establish an error state model between the master and sub-inertial navigation systems; Step 3: Based on the attitude transformation relationship between the real navigation coordinate system and the calculated navigation coordinate system, construct the differential equation of attitude error of the sub-inertial navigation system; Step 4: Based on the difference between the sub-inertial navigation auxiliary velocity in the actual and calculated navigation coordinate systems, and considering the lever error caused by the misalignment of the main and sub-inertial navigation systems, construct the differential equation for the velocity error of the sub-inertial navigation system; the differential equation for the velocity error is: （1） in, This is the projection of the angular velocity of the real navigation coordinate system relative to the inertial coordinate system onto the real navigation coordinate system. For speed error; For the real navigation coordinate system The system is connected to the sub-inertial navigation calculation navigation coordinate system. The equivalent rotation vector of the system; This is an estimate of the auxiliary speed; This is the rotation matrix from the real navigation coordinate system to the sub-inertial navigation coordinate system; For the measurement error of the inertial gyroscope; This is due to accelerometer measurement error; This is an estimated value of the Earth's rotational angular velocity projected onto the principal inertial navigation coordinate system. This is an estimate of gravitational acceleration; This is the rotation matrix from the real navigation coordinate system to the master inertial navigation coordinate system; The projection of the position of the sub-inertial navigation coordinate system relative to the main inertial navigation coordinate system onto the main inertial navigation coordinate system; Step 5: Based on the relationship between the attitude matrices obtained from the main and sub-inertial navigation systems, construct the attitude measurement equations of the sub-inertial navigation system that take into account the installation error angle; Step 6: Based on the relationship between the velocities calculated by the main and sub-inertial navigation systems, construct the velocity measurement equation for the sub-inertial navigation system; Step 7: First, construct the system's error state vector based on attitude error, velocity error, gyroscope bias, accelerometer bias, lever arm error, and mounting angle error. Simultaneously, construct the system noise vector based on the measurement errors of the sub-inertial navigation gyroscope and accelerometer. Next, based on the differential equations of each error state, and combining the system state vector and noise vector, construct the state differential equations of the sub-inertial navigation system. Further, based on the attitude and velocity measurement equations of the sub-inertial navigation system, construct the error state space equations of the sub-inertial navigation system. Finally, organize and discretize the system to obtain a complete discrete-time error state space model. Step 8: For the discrete-time error state-space model, the unscented Kalman filter algorithm is used to accurately estimate the error state, thereby completing the master and sub-inertial navigation transfer alignment.

2. The transfer alignment method based on the right-invariant error model of a Lie group according to claim 1, characterized in that, In step 1, the coordinate system is defined as follows: System: Geocentric inertial coordinate system, or simply inertial coordinate system; System: Earth-centered, Earth-fixed coordinate system, or simply Earth coordinate 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 for navigation parameters. System: The main inertial navigation coordinate system, with the geometric center of the main inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively; System: Sub-inertial navigation coordinate system, with the geometric center of the sub-inertial navigation as the origin, and the three axes x, y, and z pointing to the right, front, and top directions of the carrier, respectively.

3. The transfer alignment method based on the right-invariant error model of a Lie group according to claim 2, characterized in that, In step 2, establishing the right-invariant error state model between the master and sub-inertial navigation systems specifically involves: Step 2.1: Build auxiliary speed : （2） Among them, symbols for , This is the projection of the Earth's rotational angular velocity onto the actual navigation coordinate system. The projection of the velocity of the inertial navigation system relative to the Earth coordinate system onto the actual navigation coordinate system; The projection of the sub-inertial navigation system's position relative to the Earth coordinate system onto the actual navigation coordinate system; Step 2.2: State in Lie group space for: （3） in, This is the rotation matrix from the real navigation coordinate system to the sub-inertial navigation coordinate system, i.e., the sub-inertial navigation attitude; Step 2.3: Based on the calculation status Compared to the actual state The deviation between them is defined by the right-invariant error as: （4） in, This is the rotation matrix from the sub-inertial navigation coordinate system to the computational navigation coordinate system; This is an estimate of the auxiliary speed; The attitude error is the right-invariant error. The velocity error is a right-invariant error.

4. The transfer alignment method based on the right-invariant error model of a Lie group according to claim 3, characterized in that, In step 3, the attitude error differential equation of the sub-inertial navigation system is specifically as follows: Step 3.1: Based on the relationship between the equivalent rotation vector and the rotation matrix, the attitude error is defined as: （5） in, For the real navigation coordinate system The system is connected to the sub-inertial navigation calculation navigation coordinate system. The equivalent rotation vector of the system, i.e., the misalignment angle error; It is a 3x3 identity matrix; Step 3.2: Attitude differential equations of the sub-inertial navigation system: （6） in, The projection of the sub-inertial navigation system's angular velocity relative to the inertial coordinate system onto the sub-inertial navigation system's coordinate system; This is the projection of the angular velocity of the real navigation coordinate system relative to the inertial coordinate system onto the real navigation coordinate system. Step 3.3: The differential equation for the attitude error of the sub-inertial navigation system is: （7） in, For the measurement error of the inertial gyroscope; Since it is not considered in system state modeling, it can be regarded as state noise.

5. The transfer alignment method based on the right-invariant error model of a Lie group according to claim 4, characterized in that, In step 4, the velocity error differential equation of the sub-inertial navigation system is specifically as follows: Step 4.1: Calculate the speed error: （8） in, The projection of the auxiliary velocity of the sub-inertial navigation system relative to the Earth coordinate system onto the navigation coordinate system; Step 4.2: Considering the influence of the lever arms between the master and sub-inertial navigation systems, the positions of the master and sub-inertial navigation systems can be modeled as follows: （9） in, The projection of the position of the sub-inertial navigation coordinate system relative to the Earth coordinate system onto the Earth coordinate system; The projection of the position of the main inertial navigation coordinate system relative to the Earth coordinate system onto the Earth coordinate system; The projection of the position of the sub-inertial navigation coordinate system relative to the main inertial navigation coordinate system onto the Earth coordinate system; This is the rotation matrix from the Earth coordinate system to the principal inertial navigation coordinate system; The position of the sub-inertial navigation coordinate system relative to the main inertial navigation coordinate system is projected onto the main inertial navigation coordinate system, i.e., the lever arm; Step 4.3: Based on the auxiliary velocity vector and the main and sub-inertial navigation positions of the linkage arm, the relationship between the auxiliary velocities of the main and sub-inertial navigation systems and the linkage arm is obtained as follows: （10） in, The projection of the angular velocity of the main inertial navigation coordinate system relative to the inertial coordinate system onto the main inertial navigation coordinate system; This is the rotation matrix from the real navigation coordinate system to the master inertial navigation coordinate system; Step 4.4: The differential equation for the auxiliary velocity is: （11） （12） in, This is the projection of gravitational acceleration onto the real navigation frame; This is the projection of gravitational acceleration onto the actual navigation frame. Centripetal force; The projection of the position of the master inertial navigation coordinate system relative to the Earth coordinate system onto the actual navigation coordinate system.

6. The transitive alignment method based on the right-invariant error model of a Lie group according to claim 5, characterized in that, In step 5, the attitude measurement equation of the sub-inertial navigation system is: （13） （14） in, The attitude matrix of the main inertial navigation system; The attitude matrix calculated for the sub-inertial navigation system; This is the attitude measurement matrix; It concerns the misalignment angle error. With installation error angle A nonlinear function; The specific input-output relationship for the transformation from rotation matrix to Euler angles is as follows: （15）。 7. The transitive alignment method based on the right-invariant error model of a Lie group according to claim 6, characterized in that, In step 6, the velocity measurement equation of the sub-inertial navigation system is: （16） （17）。 8. The transfer alignment method based on the right-invariant error model of a Lie group according to claim 7, characterized in that, Step 7 specifically involves: Step 7.1: Construct the state vector of the sub-inertial navigation system; Step 7.1.1: Construct the lever arm vector and installation error angle using constant value modeling; （18） Step 7.1.2: Transfer the state variables , , and Extending to the right-invariant error state, we construct the 18-dimensional state variables of the filtering model. for: （19） in, For the misalignment angle of the sub-inertial navigation system, For speed error, To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, The lever arm error between the master and slave inertial navigation systems The installation error angle between the main and sub-inertial navigation systems; Step 7.2: Construct the noise vector of the sub-inertial navigation system; Step 7.2.1: Construct the measurement error of the sub-inertial gyroscope in equation (7) using constant zero bias and white noise modeling. Measurement error of neutron inertial accelerometer in equation (1) ; （20） in, and Gaussian white noise with zero mean; for gyroscope zero bias and accelerometer zero bias Modeled as constant with zero bias, , ; Step 7.2.2: 6-dimensional state noise of the filtered model for: （21） Step 7.3: Construct the state-space equations of the sub-inertial navigation system; Step 7.3.1: Determine the transfer matrix based on the attitude error differential equation (7) and the velocity error differential equation (1). and noise matrix for: （22） （23） Step 7.3.2: Determine the measurement generation function based on the attitude measurement equation (13) and the velocity measurement equation (17). for: （24） Step 7.3.3: Generating Functions Based on the noise level of the actual system, the state-space equation of the sub-inertial navigation system is obtained: （25） in, For measuring noise; Step 7.4: Discretize the state-space equations to obtain: （26） （27） in, It is discrete time; It is a discrete transition matrix; It is a discrete noise matrix; This is discrete measurement noise.

9. The transfer alignment method based on the right-invariant error model of a Lie group according to claim 8, characterized in that, In step 8, the state estimation is achieved using the nonlinear state estimation method—unscented Kalman filtering—specifically as follows: Step 8.1: Construction Sigma points are used for time updates: （28） Step 8.2: The weights of each Sigma point are calculated as follows: （29） in, The weights used when calculating the mean. Weights used to calculate covariance; parameters Used to introduce prior information about non-Gaussian distributions, parameters As a proportionality constant, the distribution of Sigma points near the mean can be determined by the parameter. Decide, ; Step 8.3: Incorporate the Sigma point into state propagation: （30） in, For the predicted Sigma point; Step 8.4: Calculate the predicted mean based on steps 8.1 to 8.

3. and prediction : （31） Step 8.5: Reconstruct Sigma points are used for measurement updates: （32） Step 8.6: Substitute the newly constructed Sigma point into the system measurement equation: （33） in, For the predicted measurement Sigma point; Step 8.7: Calculate the predicted mean by measuring the quantity. and predicted covariance and the cross-covariance between state and measurement : （34） Step 8.8: Calculate the filter gain Combined measurement Obtain the filtered state estimate and variance : （35） Step 8.9: Obtain the error state vector using an unscented Kalman filter. After estimation, feedback correction is performed on the errors of the lever arm and installation angle to improve the alignment accuracy of the main and sub-inertial navigation transmission.

10. A computer device, 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