A strapdown inertial navigation attitude algorithm precision evaluation method

Abstract

A kind of strapdown inertial navigation attitude algorithm precision evaluation method, according to the required carrier dynamic environment setting polynomial coefficient;According to polynomial offline calculation all the attitude true value of required reference time;According to the attitude true value, deduce angular velocity analytical expression, generate gyro observation;Gyro observation is regarded as the input of the evaluated strapdown inertial navigation attitude algorithm, and the carrier attitude parameter is updated, to obtain the attitude solution result;After each attitude algorithm solution, the algorithm performance is quantitatively evaluated;After each algorithm performance evaluation, the parameters of attitude trajectory model, coefficient matrix are adjusted or the current evaluation data is saved;The adjusted model parameters or evaluation data are saved, when new high-order attitude algorithm needs to be verified or further extended maneuvering scene, the initial step is entered and recursive iteration is sequentially carried out until the evaluation task is completed.The present application can consider large-angle maneuvering scene simulation and attitude / angular velocity analytical true value acquisition, and more accurately and efficiently verify high-order attitude algorithm performance.

Description

Technical Field

[0001] This invention relates to a method for evaluating the accuracy of strapdown inertial navigation attitude algorithms, belonging to the field of autonomous navigation technology for aircraft. Background Technology

[0002] Attitude algorithms, as a core component of strapdown inertial navigation systems, directly determine the overall performance of the navigation system. With the continuous improvement of the precision of inertial components and the increasing complexity of dynamic scenarios involving vehicles, such as large-angle maneuvers for aircraft and high-speed turns for ships, the limitations of traditional second-order attitude algorithms are becoming increasingly apparent. This is mainly manifested in the fact that, in complex dynamic environments, the algorithm error easily exceeds 5% of the component measurement error, failing to meet the requirements of high-precision navigation. To address this, existing technologies have proposed higher-order attitude algorithms such as Picard iteration and Taylor series expansion, which can improve accuracy by 1-3 orders of magnitude compared to traditional algorithms, becoming the mainstream direction of current attitude algorithm research. However, the performance verification of these higher-order algorithms urgently requires a reliable simulation evaluation environment.

[0003] In the simulation evaluation of high-order attitude algorithms, complex dynamic environment models are crucial. Currently, commonly used models are mainly divided into two types: one is the conical motion model, which is a fully analytical model with explicit analytical expressions for both attitude and angular velocity ground truth values. It offers high evaluation accuracy and is easy to operate, making it widely used for basic performance verification. However, since it can only simulate specific periodic maneuvers and cannot cover non-periodic large-angle maneuver scenarios in actual vehicle operation, its evaluation scenarios are significantly limited. The other type is the polynomial angular rate motion model, which describes angular velocity changes through polynomials, can simulate large-angle maneuver scenarios, and has analytical expressions for angular velocity ground truth values, making it convenient to simulate gyroscope output. However, its core drawback is that due to the lack of analytical expressions for attitude, it is necessary to use numerical algorithms with ultra-short step sizes, such as the 4th-order Runge-Kutta algorithm, to solve the ordinary differential equations of attitude parameters to obtain the attitude reference ground truth values. This not only increases the complexity of the evaluation process but also introduces numerical approximation errors, causing the accuracy of the attitude reference ground truth values ​​to be affected by the step size of the numerical algorithm, making it difficult to accurately measure the true performance of high-order attitude algorithms.

[0004] In summary, existing large-angle maneuver evaluation models suffer from insufficient scene coverage and cumbersome ground truth acquisition with limited accuracy, failing to meet the high-precision and convenient evaluation requirements of high-order attitude algorithms. Summary of the Invention

[0005] The purpose of this invention is to provide a method for evaluating the accuracy of strapdown inertial navigation attitude algorithms. This method can take into account both large-angle maneuver scenario simulation and attitude / angular velocity analysis for true value acquisition, and can more accurately and efficiently verify the performance of high-order attitude algorithms.

[0006] To achieve the above objectives, this invention provides a method for evaluating the accuracy of strapdown inertial navigation attitude algorithms, comprising the following steps: S1. Construct the Gibbs vector polynomial and set the polynomial coefficients according to the required dynamic environment of the carrier. S2. Calculate the true pose values ​​at all reference times offline using the Gibbs vector polynomial constructed based on S1. S3. Derive the analytical expression for angular velocity based on the true attitude value obtained in S2, and generate gyroscope observations; S4. Use the gyroscope observations obtained in S3 as input to the attitude algorithm of the strapdown inertial navigation system to be evaluated, update the attitude parameters of the carrier, and obtain the attitude calculation result. S5. After each attitude algorithm calculation, the algorithm performance is quantitatively evaluated by comparing the attitude calculation result with the true attitude value of the attitude trajectory model. S6. After each algorithm performance evaluation, adjust the parameters and coefficient matrix of the attitude trajectory model or save the current evaluation data. S7. Save the adjusted model parameters or evaluation data. When it is necessary to verify new high-order attitude algorithms or further expand the maneuvering scenarios, switch to S1 and perform recursive iteration operations in sequence until the evaluation tasks of all target scenarios and algorithms are completed.

[0007] Furthermore, the Gibbs vector polynomial in S1 is: (1); in, A, B, and C represent the Gibbs vector, the three-axis components of the Gibbs vector, and the coefficient matrix of the polynomial model, respectively. i =1, 2, 3, subscript t Indicates time; m represents the highest order of t in the time coefficient vector, and the lowest order is 0; These are the last elements of each row of the coefficient matrix A, and are real numbers.

[0008] Furthermore, the process of offline calculation of the true pose values ​​at all reference times in S2 is as follows: The transformation relationship between Gibbs vectors and attitude quaternions is shown below: (2); in, Represents attitude quaternions, For the scalar part, For the vector part. The Gibbs vector. Substitute into equation (2) to calculate the attitude quaternion at the corresponding time, so that the attitude can be accurately calculated by analytical method in the proposed Gibbs vector polynomial model, without the need for approximate calculation by numerical method.

[0009] Furthermore, the process of generating the gyroscope observations in S3 is as follows: S3.1 According to the gyroscope measurement principle, without considering measurement errors, the sensitive quantity / output of the gyroscope is the angular velocity / angular increment at the current measurement moment; the derivation process of the analytical expression of angular velocity is as follows: Taking the derivative of equation (1), we obtain the time derivative of the Gibbs vector at any given time as follows: (3); The derivative of the Gibbs vector, i.e., the differential equation of the Gibbs vector, is: (4); in, It is the angular velocity vector. For cross product operation; according to equation (4): (5); Substituting the Gibbs vector calculated according to equation (1) and the derivative of the Gibbs vector calculated according to equation (3) into equation (5), the angular velocity vector at that moment is calculated, which is the measurement output of the gyroscope: (6); The angular velocity sample of the gyroscope; The derivation of the analytical expression for the angular increment is as follows: Assuming angular velocity vector It follows a fourth-order polynomial model as shown in the following equation: (7); in, For gyroscope three-axis output, For polynomial coefficients, , It is a fourth-order polynomial coefficient matrix. This is a column vector of time parameters at time t; S3.2. By combining the four sets of angular velocity observations, the following observation equations are obtained: (8); in, Considering that equation (8) is a well-posed system, the least squares estimate of the parameters is: (9); Under uniform sampling, for all solution cycles, Therefore, its inverse matrix is ​​calculated offline; S3.3. Based on equation (7), establish the following cumulative angle increment polynomial function model: (10); in, ; Represents angular velocity. , indicating that the integral is performed with respect to the angular velocity. ; According to equation (10), the four corner increment samples in the reconstructed update cycle are: (11); in, All are angular increment samples. A column vector of time parameters at different times The difference column vector matrix, ; Under uniform sampling, for all solution cycles, Therefore, its inverse matrix is ​​calculated offline.

[0010] Furthermore, in step S4, the attitude parameter update uses gyroscope observation data as input to perform a complete attitude algorithm update calculation. The specific process is as follows: Introduce auxiliary variables: (12); The differential equation for quaternions is as follows: (13); in, Represent quaternion multiplication; given the first... Obtained by iterative function The polynomial expression of the first, second, After the second iteration The polynomial expression is calculated as follows: (14); in, Representative quaternion, ; Substituting t=T into equation (14), we obtain the attitude increment solution value within the solution cycle: (15); in, ; Substituting the quaternion increment in the above formula into equation (16), we obtain the attitude solution value at the end of the current solution epoch: (16); in, This is the quaternion solution value at the end of the previous solution cycle. The incremental quaternion for solving equation (15), This is the quaternion at the end of the current solution cycle.

[0011] Furthermore, the performance evaluation process of the attitude algorithm in S5 is as follows: Let the truth value and solution value of the attitude quaternion be expressed as follows: Then the error quaternion is ,in, The conjugate quaternion is represented by the formula where the scalar part of the quaternion remains unchanged, while the vector part is negative. The Euler rotation angle corresponding to the error quaternion is extracted using the following formula: (17); The angle in equation (17) is taken as the attitude calculation error. , This is for absolute value operations; The Euler rotation angle is the rotation angle corresponding to the equivalent rotation vector of the error quaternion. To further analyze the triaxial error, the error quaternion is converted into triaxial error Euler angles, as expressed below: (18); in, These represent the roll angle, pitch angle, and yaw angle, respectively.

[0012] This invention constructs a Gibbs vector polynomial and sets the polynomial coefficients according to the required dynamic environment of the carrier. It then calculates the true attitude values ​​for all reference moments offline using the constructed Gibbs vector polynomial. Based on the obtained true attitude values, it derives the analytical expression for angular velocity and generates gyroscope observations. These gyroscope observations are used as input to the strapdown inertial navigation attitude algorithm to be evaluated, updating the carrier's attitude parameters and obtaining the attitude calculation result. After each attitude algorithm calculation, the algorithm performance is quantitatively evaluated by comparing the attitude calculation result with the true attitude values ​​of the attitude trajectory model. After each algorithm performance evaluation, the parameters and coefficient matrix of the attitude trajectory model are adjusted or the current evaluation data is saved. The adjusted model parameters or evaluation data are saved. When it is necessary to verify new higher-order attitude algorithms or further expand maneuver scenarios, the initial steps are returned and recursive iterations are performed sequentially until the evaluation tasks for all target scenarios and algorithms are completed. Compared to traditional evaluation methods that rely on conical motion models or models with numerical errors, this invention provides an analytical truth benchmark for high-order attitude algorithms without numerical approximation errors, completely eliminating errors introduced by numerical integration and other operations, and accurately measuring the true performance of the algorithm. Its core model parameters can be pre-configured offline; during real-time evaluation, only analytical formulas need to be substituted for calculation, eliminating the need for complex numerical preprocessing, and it can directly generate angular increment samples and truth references. It provides comprehensive performance information for high-order attitude algorithms under different complexity scenarios, including accuracy, convergence characteristics, and stability trends, thereby enabling quality control such as algorithm optimization and selection verification. This invention is not only applicable to verifying Picard iteration and Taylor series expansion algorithms, but can also be extended to other types of attitude algorithms, possessing broad algorithm adaptability. Furthermore, this invention has strong scenario coverage capabilities; by adjusting the PAM model parameters, it can simulate large-angle non-periodic maneuvers with varying angular velocity and angular acceleration ranges, covering the dynamic characteristics of real-world carriers in multiple fields. Attached Figure Description

[0013] Figure 1 This is a flowchart of the process of the method of the present invention. Detailed Implementation

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

[0015] like Figure 1 As shown, a method for evaluating the accuracy of a strapdown inertial navigation attitude algorithm includes the following steps: S1. Construct the Gibbs vector polynomial and set the polynomial coefficients according to the required dynamic environment of the carrier. S2. Calculate the true pose values ​​at all reference times offline using the Gibbs vector polynomial constructed based on S1. S3. Derive the analytical expression for angular velocity based on the true attitude value obtained in S2, and generate gyroscope observations; S4. Use the gyroscope observations obtained in S3 as input to the attitude algorithm of the strapdown inertial navigation system to be evaluated, update the attitude parameters of the carrier, and obtain the attitude calculation result. S5. After each attitude algorithm calculation, the algorithm performance is quantitatively evaluated by comparing the attitude calculation result with the true attitude value of the attitude trajectory model. S6. After each algorithm performance evaluation, adjust the parameters and coefficient matrix of the attitude trajectory model or save the current evaluation data. S7. Save the adjusted model parameters or evaluation data. When it is necessary to verify new high-order attitude algorithms or further expand the maneuvering scenarios, switch to S1 and perform recursive iteration operations in sequence until the evaluation tasks of all target scenarios and algorithms are completed.

[0016] In a preferred embodiment, the Gibbs vector polynomial in S1 is: (1); in, A, B, and C represent the Gibbs vector, the three-axis components of the Gibbs vector, and the coefficient matrix of the polynomial model, respectively. i =1, 2, 3, subscript t Indicates time; m represents the highest order of t in the time coefficient vector, and the lowest order is 0; These are the last elements of each row of the coefficient matrix A, and are real numbers.

[0017] As a preferred implementation, the process of offline calculation of the true attitude values ​​at all reference times in S2 is as follows: The transformation relationship between Gibbs vectors and attitude quaternions is shown below: (2); in, Represents attitude quaternions, For the scalar part, This is the vector part.

[0018] Substituting the Gibbs vector into equation (2) calculates the attitude quaternion at the corresponding moment, thus realizing that the attitude is accurately calculated analytically in the proposed Gibbs vector polynomial model, without the need for approximate calculation by numerical methods.

[0019] In a preferred embodiment, the process of generating the gyroscope observations in S3 is as follows: S3.1 According to the gyroscope measurement principle, without considering measurement errors, the sensitive quantity / output of the gyroscope is the angular velocity / angular increment at the current measurement moment; the derivation process of the analytical expression of angular velocity is as follows: Taking the derivative of equation (1), we obtain the time derivative of the Gibbs vector at any given time as follows: (3); The derivative of the Gibbs vector, i.e., the differential equation of the Gibbs vector, is: (4); in, It is the angular velocity vector. For cross product operation; according to equation (4): (5); Substituting the Gibbs vector calculated according to equation (1) and the derivative of the Gibbs vector calculated according to equation (3) into equation (5), the angular velocity vector at that moment is calculated, which is the measurement output of the gyroscope: (6); The angular velocity sample of the gyroscope; The derivation of the analytical expression for the angular increment is as follows: Assuming angular velocity vector It follows a fourth-order polynomial model as shown in the following equation: (7); in, For gyroscope three-axis output, For polynomial coefficients, , It is a fourth-order polynomial coefficient matrix. This is a column vector of time parameters at time t; S3.2. By combining the four sets of angular velocity observations, the following observation equations are obtained: (8); in, Considering that equation (8) is a well-posed system, the least squares estimate of the parameters is: (9); Under uniform sampling, for all solution cycles, Therefore, its inverse matrix is ​​calculated offline; S3.3. Based on equation (7), establish the following cumulative angle increment polynomial function model: (10); in, ; Represents angular velocity. , indicating that the integral is performed with respect to the angular velocity. ; According to equation (10), the four corner increment samples in the reconstructed update cycle are: (11); in, All are angular increment samples. A column vector of time parameters at different times The difference column vector matrix, ; Under uniform sampling, for all solution cycles, Therefore, its inverse matrix is ​​calculated offline.

[0020] In a preferred embodiment, the attitude parameter update in step S4 uses gyroscope observation data as input to perform a complete attitude algorithm update calculation. The specific process is as follows: Introduce auxiliary variables: (12); The differential equation for quaternions is as follows: (13); in, Represent quaternion multiplication; given the first... Obtained by iterative function The polynomial expression of the first, second, After the second iteration The polynomial expression is calculated as follows: (14); in, Representative quaternion, ; Substituting t=T into equation (14), we obtain the attitude increment solution value within the solution cycle: (15); in, ; Substituting the quaternion increment in the above formula into equation (16), we obtain the attitude solution value at the end of the current solution epoch: (16); in, This is the quaternion solution value at the end of the previous solution cycle. The incremental quaternion for solving equation (15), This is the quaternion at the end of the current solution cycle.

[0021] As a preferred implementation, the performance evaluation process of the attitude algorithm in S5 is as follows: Let the truth value and solution value of the attitude quaternion be expressed as follows: Then the error quaternion is ,in, The conjugate quaternion is represented by the formula where the scalar part of the quaternion remains unchanged, while the vector part is negative. The Euler rotation angle corresponding to the error quaternion is extracted using the following formula: (17); The angle in equation (17) is taken as the attitude calculation error. , This is for absolute value operations; The Euler rotation angle is the rotation angle corresponding to the equivalent rotation vector of the error quaternion. To further analyze the triaxial error, the error quaternion is converted into triaxial error Euler angles, as expressed below: (18); in, These represent the roll angle, pitch angle, and yaw angle, respectively.

[0022] In a preferred embodiment, the model in S6 is adjusted according to requirements. The purpose of modifying the coefficient matrix is ​​to select a suitable motion environment. The method for adjusting the parameters and coefficient matrix of the attitude trajectory model is as follows: the parameters of the attitude trajectory model are generally fixed and identical, and the coefficient matrix is ​​as follows: ; in, .

[0023] Through experimental comparison, the Gibbs vector polynomial large-angle maneuver model of this invention has advantages over existing polynomial large-angle maneuver models in terms of reference true value, angular velocity, compensation measures, and operational difficulty indicators, as detailed in Table 1: .

Claims

1. A method for evaluating the accuracy of a strapdown inertial navigation attitude algorithm, characterized in that, Includes the following steps: S1. Construct the Gibbs vector polynomial and set the polynomial coefficients according to the required dynamic environment of the carrier. S2. Calculate the true pose values ​​at all reference times offline using the Gibbs vector polynomial constructed based on S1. S3. Derive the analytical expression for angular velocity based on the true attitude value obtained in S2, and generate gyroscope observations; S4. Use the gyroscope observations obtained in S3 as input to the attitude algorithm of the strapdown inertial navigation system to be evaluated, update the attitude parameters of the carrier, and obtain the attitude solution result. S5. After each attitude algorithm calculation, the algorithm performance is quantitatively evaluated by comparing the attitude calculation result with the true attitude value of the attitude trajectory model. S6. After each algorithm performance evaluation, adjust the parameters and coefficient matrix of the attitude trajectory model or save the current evaluation data. S7. Save the adjusted model parameters or evaluation data. When it is necessary to verify new high-order attitude algorithms or further expand the maneuvering scenarios, switch to S1 and perform recursive iteration operations in sequence until the evaluation tasks of all target scenarios and algorithms are completed.

2. The method for evaluating the accuracy of strapdown inertial navigation attitude algorithm according to claim 1, characterized in that, The Gibbs vector polynomial in S1 is: （1）； in, A, B, and C represent the Gibbs vector, the three-axis components of the Gibbs vector, and the coefficient matrix of the polynomial model, respectively. i =1, 2, 3, subscript t Indicates time; m represents the highest order of t in the time coefficient vector, and the lowest order is 0; These are the last elements of each row of the coefficient matrix A, and are real numbers.

3. The method for evaluating the accuracy of strapdown inertial navigation attitude algorithm according to claim 2, characterized in that, The process of offline calculation of the true pose values ​​at all reference times in S2 is as follows: The transformation relationship between Gibbs vectors and attitude quaternions is shown below: （2）； in, Represents attitude quaternions, For the scalar part, This is the vector part.

4. The method for evaluating the accuracy of strapdown inertial navigation attitude algorithm according to claim 3, characterized in that, The process of generating gyroscope observations in S3 is as follows: S3.1 According to the gyroscope measurement principle, without considering measurement errors, the sensitive quantity / output of the gyroscope is the angular velocity / angular increment at the current measurement moment; the derivation process of the analytical expression of angular velocity is as follows: Taking the derivative of equation (1), we obtain the time derivative of the Gibbs vector at any given time as follows: (3)； The derivative of the Gibbs vector, i.e., the differential equation of the Gibbs vector, is: (4)； in, It is the angular velocity vector. This is a cross product operation; according to equation (4), we get: (5)； Substituting the Gibbs vector calculated according to equation (1) and the derivative of the Gibbs vector calculated according to equation (3) into equation (5), the angular velocity vector at that moment is calculated, which is the measurement output of the gyroscope: (6)； The angular velocity sample of the gyroscope; The derivation of the analytical expression for the angular increment is as follows: Assuming angular velocity vector It follows a fourth-order polynomial model as shown in the following equation: （7）； in, For gyroscope three-axis output, For polynomial coefficients, , It is a fourth-order polynomial coefficient matrix. This is a column vector of time parameters at time t; S3.

2. By combining the four sets of angular velocity observations, the following observation equations are obtained: (8)； in, Considering that equation (8) is a well-posed system, the least squares estimate of the parameters is: （9）； Under uniform sampling, for all solution cycles, Therefore, its inverse matrix is ​​calculated offline; S3.

3. Based on equation (7), establish the following cumulative angle increment polynomial function model: （10）； in, ; Represents angular velocity. , indicating that the integral is performed with respect to the angular velocity. ; According to equation (10), the four corner increment samples in the reconstructed update cycle are: （11）； in, All are angular increment samples. A column vector of time parameters at different times The difference column vector matrix, ; Under uniform sampling, for all solution cycles, Therefore, its inverse matrix is ​​calculated offline.

5. The method for evaluating the accuracy of strapdown inertial navigation attitude algorithm according to claim 4, characterized in that, In step S4, the attitude parameter update uses gyroscope observation data as input to perform a complete attitude algorithm update calculation. The specific process is as follows: Introduce auxiliary variables: ； The differential equation for quaternions is as follows: ； in, Represent quaternion multiplication; given the first... Obtained by iterative function The polynomial expression of the first, second, After the second iteration The polynomial expression is calculated as follows: ； in, Representative quaternion, ; Substituting t=T into equation (14), we obtain the attitude increment solution value within the solution cycle: ； in, ; Substituting the quaternion increment in the above formula into equation (16), we obtain the attitude solution value at the end of the current solution epoch: ； in, This is the quaternion solution value at the end of the previous solution cycle. The incremental quaternion for solving equation (15), This is the quaternion at the end of the current solution cycle.

6. The method for evaluating the accuracy of strapdown inertial navigation attitude algorithm according to claim 5, characterized in that, The performance evaluation process of the attitude algorithm in S5 is as follows: Let the truth value and solution value of the attitude quaternion be expressed as follows: Then the error quaternion is ,in, The conjugate quaternion is represented by the formula where the scalar part of the quaternion remains unchanged, while the vector part is negative. The Euler rotation angle corresponding to the error quaternion is extracted using the following formula: ； The angle in (17) will be used as the attitude calculation error. , This is for absolute value operations; The Euler rotation angle is the rotation angle corresponding to the equivalent rotation vector of the error quaternion. To further analyze the triaxial error, the error quaternion is converted into triaxial error Euler angles, as expressed below: ； in, These represent the roll angle, pitch angle, and yaw angle, respectively.

Images