A calculation method of Lie group strapdown inertial navigation error model based on Rodriguez parameters

Abstract

The invention relates to a unified formula of a Lie group strapdown inertial navigation error model based on Rodriguez parameters, comprising: firstly, an attitude update equation based on Rodriguez parameters and modified Rodriguez parameters is derived, and a unified formula of a Lie group strapdown inertial navigation error model based on Rodriguez parameters is proposed in combination with a Lie group inertial navigation error model. The model can express the Lie group strapdown inertial navigation error model based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters in a unified form. The Lie group strapdown inertial navigation error model based on Rodriguez parameters and modified Rodriguez parameters is compared with the Lie group strapdown inertial navigation error model based on Euler angles and quaternions, and the advantages and disadvantages of the four models are compared through experiments. The invention improves the deficiency of existing literature in studying the combined navigation model under large misalignment conditions, and the model can complete high-precision nonlinear combined navigation in a short time under large misalignment conditions, thereby meeting the requirements of high-precision combined navigation.

Description

Technical Field

[0001] The invention relates to the field of navigation technology, and in particular to a calculation method of a Lie group strapdown inertial navigation error model based on Rodriguez parameters. Background Art

[0002] The strapdown inertial navigation system is mainly composed of two types of sensors: gyroscopes and accelerometers. It achieves high-precision navigation and positioning through dead reckoning based on Newton's laws of motion. The gyroscopes and accelerometers of the strapdown inertial navigation system are fixed to the moving carrier, and are sensitive to the linear velocity and angular velocity information of the carrier. Under the given initial navigation information conditions, the linear acceleration and angular acceleration output by the gyroscope and accelerometer can realize autonomous closed-loop solution of navigation parameters such as attitude, velocity and position. Compared with other navigation methods, the strapdown inertial navigation system has the advantages of strong autonomy, high reliability, good concealment, comprehensive output of navigation information and all-weather operation, and is being widely used in aviation, aerospace, navigation and other fields.

[0003] The working principle of strapdown inertial navigation is integral extrapolation. Affected by many error sources such as installation error, device error, initial error, etc., its positioning error will continue to accumulate over time, reducing the accuracy of navigation parameter estimation. This process is relatively slow, but the navigation accuracy will deteriorate during long-distance flights, and it is impossible to complete long-distance missions independently. Therefore, in order to overcome this shortcoming of strapdown inertial navigation, information fusion and modern filtering technology are often used to fuse the data of two or more navigation systems, fully utilize the advantages of multiple sensors, and achieve long-term high-precision navigation positioning. This is combined navigation. Among many combined navigation systems, combined navigation with strapdown inertial navigation as the core and satellite navigation, Doppler log navigation and other navigation methods as auxiliary navigation systems is the most common and effective. This combined navigation has become the most widely used dominant combination method in aerospace, navigation and land use.

[0004] As a dead reckoning system, the strapdown inertial navigation system requires initial attitude, velocity, position and other information before entering the inertial navigation solution and integrated navigation, otherwise it will greatly affect the subsequent performance, and the acquisition of these initial information is completed by the initial alignment. It is relatively easy to obtain information such as initial velocity and position, which can be bound by external auxiliary equipment such as the global satellite navigation system, Doppler log, odometer, etc. Therefore, how to obtain accurate initial attitude in the initial alignment stage is the key and difficulty of alignment technology research.

[0005] The traditional initial alignment process is generally to perform coarse alignment first and then fine alignment. In the process of coarse alignment, the carrier attitude is generally regarded as a constant, and the attitude relationship between the gyroscope and accelerometer output and the earth's rotation angular velocity and gravity information is directly obtained by analytical methods. The purpose of coarse alignment is to initialize the attitude of the inertial navigation system and make the attitude error reach a small angle error state, so as to ensure that the classic inertial line linear error equation is established. The fine alignment criterion is to use the inertial navigation error equation as the state model with the assistance of external observations and adopt the state estimation method. However, since the strapdown inertial navigation system is fixed on the moving carrier, its output must be coupled with the carrier's own angular motion and linear motion information and related interference information, which leads to the inability of the traditional coarse alignment method to effectively obtain the carrier's rough attitude, and the fine alignment with coarse alignment as a necessary condition cannot be completed normally. In addition, for ships, in some emergency scenarios and complex sea conditions, the strapdown inertial navigation does not have the objective conditions for static alignment, and the coarse alignment cannot meet the small misalignment angle requirements required for fine alignment, and fine alignment cannot be performed normally. Therefore, it is very necessary to study the nonlinear initial alignment of the dynamic base under large misalignment angle conditions, which will help improve the emergency start-up capability of ships, the emergency response of guided weapons, and the survivable strike capability, and has important theoretical research and application value.

[0006] Combined with group theory, the attitude and speed of strapdown inertial navigation are simultaneously included in the group, a new error amount can be constructed, and a strapdown inertial navigation error model based on Lie group is established. Currently, there are more Lie group strapdown inertial navigation error models based on Euler angles and Lie group strapdown inertial navigation error models based on quaternions. Compared with the attitude representation of Euler angles and quaternions, the attitude representation of Rodrigues parameters has the advantages of small amount of calculation, good filtering consistency, and global non-singularity. Therefore, the present invention mainly carries out research around the Lie group strapdown inertial navigation error model based on Rodrigues parameters (Rodrigues Paramater, RP) and modified Rodrigues parameters (ModifiedRodrigues Paramater, RP), which has advantages such as good estimation accuracy and smaller amount of calculation compared to the Lie group strapdown inertial navigation error model based on Euler angles and quaternions. In addition, a unified Lie group strapdown inertial navigation error model is proposed, which can incorporate the Lie group strapdown inertial navigation error models based on Euler angles, quaternions, Rodrigues parameters and modified Rodrigues parameters, and has the consistency of theoretical research. Summary of the invention

[0007] In view of the above problems, the present invention aims to provide a calculation method of the Lie group strapdown inertial navigation error model of Rodriguez parameters, which aims to enable the ship to perform fast and high-precision dynamic start under large misalignment conditions, so as to improve the deficiencies of existing literature on the research of combined navigation models under large misalignment conditions. The model can quickly complete high-precision nonlinear combined navigation in a short time under large misalignment conditions, meeting the requirements of high-precision combined navigation.

[0008] A method for calculating a Lie group strapdown inertial navigation error model based on Rodriguez parameters includes: proposing a Lie group strapdown inertial navigation error model based on Rodriguez parameters and modified Rodriguez parameters, establishing a unified formula of the Lie group strapdown inertial navigation error model including Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters, and comparing the advantages and disadvantages of the four models through experiments. The specific steps include:

[0009] 1) Analyze the Rodriguez parameter pose representation, which is mainly divided into Rodriguez parameter pose estimation and modified Rodriguez parameter pose estimation;

[0010] 2) Analyze the Lie group strapdown inertial navigation error model, incorporate the attitude error and velocity error into one group, and construct a new Lie group strapdown inertial navigation error model;

[0011] 3) Based on the definition of Rodriguez parameter and modified Rodriguez parameter attitude error, the Lie group strapdown inertial navigation error model based on Rodriguez parameter and the Lie group strapdown inertial navigation error model based on modified Rodriguez parameter are derived; wherein:

[0012] definition The attitude error is defined in RP form, The attitude error is defined in MRP form. Then the attitude error of Rodriguez parameter and modified Rodriguez parameter can be expressed by the attitude error in quaternion form.

[0013]

[0014] The quaternion attitude error can also be expressed by RP and MRP attitude errors, as follows:

[0015]

[0016] The attitude error matrix in quaternion form is known The following conversion relationship exists

[0017]

[0018] definition and are the attitude error matrices in Rodriguez parameter form and modified Rodriguez parameter form, respectively. First, convert RP and MRP into quaternions, and then substitute the quaternions into the above formula to obtain and

[0019] According to the conversion relationship between RP, MRP and quaternion, the attitude error models based on RP and MRP are obtained as follows:

[0020]

[0021] 5) According to the conversion relationship between Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters, a unified Lie group strapdown inertial navigation error model is proposed, and these four Lie group strapdown inertial navigation error models are expressed at the same time; among them:

[0022] The unified Lie Group strapdown inertial navigation attitude error model uses the attitude error matrix Represents the attitude error, then the attitude error matrix differential equation is obtained

[0023]

[0024] This is the unified Lie Group strapdown inertial navigation attitude error model. and Substituting in the above, we can get the Lie group strapdown inertial navigation attitude error model based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters;

[0025] In addition, the unified velocity error is defined as τ v , then the unified velocity error differential equation is

[0026]

[0027] The unified position and velocity differential equation is:

[0028]

[0029] 5) Through two sets of vehicle-mounted experiments, the performance of Lie group strapdown inertial navigation error models based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters are compared.

[0030] The present invention is mainly aimed at the high-precision emergency start-up scenario of ships under large misalignment angle conditions. Starting from the strapdown inertial navigation error model based on Lie group, combined with the attitude representation method of Rodriguez parameters and modified Rodriguez parameters, a Lie group strapdown inertial navigation error model based on Rodriguez parameters and modified Rodriguez parameters is proposed, and a unified formula of the Lie group strapdown inertial navigation error model including Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters is established. Two groups of vehicle-mounted experiments prove that the performance of the Lie group strapdown inertial navigation error model based on Rodriguez parameters and modified Rodriguez parameters is better than that of the Lie group strapdown inertial navigation error model based on Euler angles and quaternions, especially when the initial heading angle is large, the performance advantage of the Lie group strapdown inertial navigation error model based on Rodriguez parameters and modified Rodriguez parameters is more obvious. The present invention is of great significance for the emergency start-up of ships under large misalignment conditions, and has strong theoretical research and application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The specific implementation modes of the present invention are further described in detail below in conjunction with the accompanying drawings.

[0032] Figure 1 The present invention relates to a principle block diagram of a nonlinear error model based on the carrier system quaternion attitude error and a nonlinear error model based on the navigation system quaternion attitude error;

[0033] Figure 2 This is the trajectory diagram of the vehicle-mounted experiment of the present invention;

[0034] Figure 3 The heading angle estimation error curve diagram obtained by the experiment of the present invention when the initial misalignment angle is [10°; 10°; 30°];

[0035] Figure 4 The heading angle estimation error curve diagram obtained by the experiment of the present invention when the initial misalignment angle is [20°; 20°; 60°];

[0036] Figure 5 This is a heading angle estimation error curve diagram obtained from the experiment of the present invention when the initial misalignment angle is [30°; 30°; 90°]. DETAILED DESCRIPTION

[0037] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0038] The preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principles of the present invention and are not intended to limit the protection scope of the present invention.

[0039] refer to Figures 1 to 5 As shown, the present invention relates to a Lie group strapdown inertial navigation error model calculation method based on Rodriguez parameters. In order to explain in detail the Lie group strapdown inertial navigation error model calculation method based on Rodriguez parameters proposed by the present invention, a Lie group strapdown inertial navigation error model based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters is first given as follows:

[0040] 1. Lie Group Strapdown Inertial Navigation Attitude Error Model Based on Euler Angles

[0041] The traditional strapdown inertial navigation attitude error model is:

[0042]

[0043] If the posture and speed are included in one group at the same time, a new state quantity can be constructed

[0044]

[0045] Then the new error state quantity can be obtained

[0046]

[0047] in

[0048] When the misalignment angle error Taking the minimum value, the following relationship exists

[0049]

[0050] So the Lie group strapdown inertial navigation error model based on Euler angle can be obtained:

[0051]

[0052] In the above formula

[0053]

[0054] The superscript n in the formula represents the navigation system, b represents the carrier system, and the physical meanings of the relevant quantities are as follows:

[0055] represents the direction cosine matrix from b system to n system, is the carrier angular velocity of the b system output by the gyroscope relative to the inertial coordinate system (i system), is the Earth's rotation angular velocity, It is the sum of the acceleration caused by the rotation of the earth and the angular velocity caused by the motion of the carrier on the earth's surface, g n is the acceleration due to gravity;

[0056] is the actual output carrier angular velocity of the gyroscope, is the inertial angular velocity with error, is the actual output gravity acceleration;

[0057] is the gyroscope measurement error, is the accelerometer measurement error, is the calculation error;

[0058] I 3 is a 3×3 identity matrix, R M is the meridian radius of the earth, R N is the radius of the Earth's circumference;

[0059] 2. Lie Group Strapdown Inertial Navigation Attitude Error Model Based on Quaternion

[0060] If the attitude is expressed by quaternion, the Lie group strapdown inertial navigation error model based on quaternion can be constructed

[0061] Attitude error equation:

[0062]

[0063] Velocity error equation:

[0064]

[0065] Position error equation:

[0066]

[0067] 3. Lie Group Strapdown Inertial Navigation Attitude Error Model Based on Rodriguez Parameters

[0068] definition The attitude error is defined in RP form, The attitude error is defined in MRP form. Then the attitude error of Rodriguez parameter and modified Rodriguez parameter can be expressed by the attitude error in quaternion form.

[0069]

[0070] The quaternion attitude error can also be expressed by RP and MRP attitude errors as follows:

[0071]

[0072] The attitude error matrix in quaternion form is known The following conversion relationship exists

[0073]

[0074] definition and are the attitude error matrices in Rodriguez parameter form and modified Rodriguez parameter form, respectively. First, convert RP and MRP into quaternions, and then substitute the quaternions into the above formula to obtain and

[0075] According to the above conversion relationship between RP, MRP and quaternion, the attitude error models based on RP and MRP can be obtained as follows:

[0076]

[0077] 4. Unified Lie Group Strapdown Inertial Navigation Attitude Error Model Based on Rodriguez Parameters

[0078] The unified Lie Group strapdown inertial navigation attitude error model uses the attitude error matrix Represents the attitude error, then the attitude error matrix differential equation can be obtained

[0079]

[0080] This is the unified Lie Group strapdown inertial navigation attitude error model. and Substituting these into the above, we can obtain the Lie group strapdown inertial navigation attitude error model based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters.

[0081] In addition, the unified velocity error is defined as τ v , then the unified velocity error differential equation is

[0082]

[0083] The unified position and velocity differential equation is:

[0084]

[0085] The experimental verification method of the present invention is specifically implemented by comprising the following steps:

[0086] 1) In order to more conveniently represent these four models, they are respectively denoted as SE(3)-Euler, SE(3)-Quat, SE(3)-RP and SE(3)-MRP. For these four models, the selection of state quantities is specifically expressed as:

[0087] SE(3)-Euler:

[0088] SE(3)-Quat:

[0089] SE(3)-RP:

[0090] SE(3)-MRP:

[0091] In the formula, ε b is the gyro constant drift, is the accelerometer constant drift, SE(3)-Euler represents the Lie group strapdown inertial navigation error model based on Euler angles, SE(3)-Quat represents the Lie group strapdown inertial navigation error model based on quaternions, SE(3)-RP represents the Lie group strapdown inertial navigation error model based on Rodriguez parameters, and SE(3)-MRP represents the Lie group strapdown inertial navigation error model based on modified Rodriguez parameters.

[0092] 2) For the selection of observation quantities, velocity and position are used as observation quantities for integrated navigation:

[0093]

[0094] In the formula, is the speed and position information obtained by inertial navigation, z GPS It is the speed and position reference information obtained by satellite navigation.

[0095] 3) Carry out vehicle-mounted combined navigation experiments to analyze and compare the performance of these four Lie group strapdown inertial navigation error models. There is a consumer-grade inertial navigation system inside the vehicle to measure the sensor output information of the gyroscope and acceleration. The constant drift of the gyroscope is 0.3° / h, and the zero bias of the accelerometer is 20μg. There is a GPS antenna on the roof to receive satellite signals. The speed error of GPS is 0.1m / s and the position error is 10m. The sampling frequencies of inertial navigation and GPS are 200Hz and 1Hz respectively. In addition, a positioning and attitude system (POS system) is placed on the vehicle to provide an attitude reference benchmark. The constant drift of the gyroscope in the POS system is 0.01° / h. The experiment was conducted on an open road for about 800s. The trajectory is as follows Figure 2 As shown in the figure, the blue mark is the starting point and the green mark is the end point. During the movement, the GPS signal is well received, and the speed and position measured by GPS are used as observations for combined navigation. The system noise and measurement noise methods are set according to sensor indicators and experience values.

[0096] Initial error of the inertial navigation system: The initial error of the strapdown inertial navigation system mainly includes the initial attitude error, initial velocity error and initial position error. The main application scenario of the present invention is when the initial attitude error is a large misalignment angle. Therefore, the initial attitude error is set to a large misalignment angle, and the initial velocity error and initial position error are set according to experience. The parameter settings are as follows:

[0097] Initial error:

[0098] Attitude error: pitch, roll, and heading angles are [10°; 10°; 30°], [20°; 20°; 60°], and [30°; 30°; 90°], respectively.

[0099] Speed ​​error: The speed error in the northeast sky is 1m / s

[0100] Position error: The position error of latitude, longitude and altitude is 10m

[0101] According to the above parameters, the combined navigation experiment is carried out through the vehicle experimental data to obtain the attitude estimation error. Since the attitude estimation of the pitch angle and the roll angle is not much different, the estimation error of the heading angle is mainly concerned in the experiment. The heading angle estimation errors of the three groups of experiments are as follows: Figure 3-5 As shown in the figure, under the conditions of three sets of large misalignment angles, SE(3)-Euler, SE(3)-Quat, SE(3)-RP and SE(3)-MRP can all converge the heading angle estimation error, but in terms of overall performance, SE(3)-Quat, SE(3)-RP and SE(3)-MRP are better than SE(3)-Euler. In addition, as the initial misalignment angle gradually increases, the convergence time is also longer, and the convergence speed of SE(3)-RP and SE(3)-MRP is faster than that of SE(3)-Euler. The final heading angle estimation error SE(3)-RP and SE(3)-MRP is also significantly smaller than SE(3)-Euler. Therefore, the Lie group strapdown inertial navigation error model based on Rodriguez parameters and the Lie group strapdown inertial navigation error model based on modified Rodriguez parameters proposed in the present invention are superior to the traditional Lie group strapdown inertial navigation error model based on Euler angles, reflecting the advantages of the Lie group strapdown inertial navigation error model based on Rodriguez parameters and the Lie group strapdown inertial navigation error model based on modified Rodriguez parameters proposed in the present invention in fast startup application under large misalignment angle conditions.

[0102] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims

1. A Lie group strapdown inertial navigation error model calculation method based on Rodriguez parameters, characterized in that: include: The Lie group strapdown inertial navigation error model based on Rodriguez parameters and modified Rodriguez parameters is proposed, and the Lie group strapdown inertial navigation error models based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters are incorporated into a unified Lie group strapdown inertial navigation error model. Then, the advantages and disadvantages of these four models are compared through experiments. The specific steps include: 1) Analyze the Rodriguez parameter pose representation, which is mainly divided into Rodriguez parameter pose estimation and modified Rodriguez parameter pose estimation; 2) Analyze the Lie group strapdown inertial navigation error model, incorporate the attitude error and velocity error into one group, and construct a new Lie group strapdown inertial navigation error model; 3) Based on the definition of Rodriguez parameter and modified Rodriguez parameter attitude error, the Lie group strapdown inertial navigation error model based on Rodriguez parameter and the Lie group strapdown inertial navigation error model based on modified Rodriguez parameter are derived; wherein: definition The attitude error is defined in RP form, The attitude error is defined in MRP form, then the Rodriguez parameter and modified Rodriguez parameter attitude error are represented by the quaternion form attitude error The quaternion form attitude error is also represented by the RP and MRP attitude errors, as follows: The attitude error matrix in quaternion form is known The following conversion relationship exists definition and are the attitude error matrices in the Rodriguez parameter form and the modified Rodriguez parameter form, respectively. First, convert RP and MRP into quaternions, and then substitute the quaternions into the above formula to obtain and According to the above conversion relationship between RP, MRP and quaternion, the attitude error models based on RP and MRP are obtained as follows: 4) According to the conversion relationship between Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters, a unified Lie group strapdown inertial navigation error model is proposed, and these four Lie group strapdown inertial navigation error models are expressed at the same time; among them: The unified Lie Group strapdown inertial navigation attitude error model uses the attitude error matrix Represents the attitude error, then the attitude error matrix differential equation is obtained This is the unified Lie Group strapdown inertial navigation attitude error model. and Substituting in the above, we can get the Lie group strapdown inertial navigation attitude error model based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters; In addition, the unified velocity error is defined as τ v , then the unified velocity error differential equation is The unified position and velocity differential equation is: 5) Through two sets of vehicle-mounted experiments, the performance of Lie group strapdown inertial navigation error models based on Euler angles, quaternions, Rodriguez parameters and modified Rodriguez parameters are compared.

Images