A non-commutative error suppression method based on large dynamic environment

Abstract

The present invention claims a method for suppressing non-commutative errors in a large dynamic environment, mainly for the large errors of the inertial sensor IMU in a large dynamic environment, and the traditional equivalent rotation vector algorithm cannot improve the algorithm accuracy by simply increasing the number of sub-samples, ignoring the problem that high-order terms will produce large errors in a large dynamic environment. On this basis, an improved high-order iterative attitude optimization algorithm is studied. Using the gyroscope and accelerometer as data sources, three sub-samples as a sampling cycle of the gyroscope, the equivalent rotation vector algorithm is used to calculate and solve in the first cycle, and the next cycle also uses three sub-samples as a sampling cycle, and a high-order curve is used to fit it, and then it is iterated with the results of the previous cycle to solve the high-order iterative solution. The present invention can better suppress non-commutative errors and improve the attitude solution accuracy of IMU in a large dynamic environment. Compared with the traditional algorithm, the present invention achieves an improvement of two orders of magnitude in attitude solution accuracy.

Description

Technical Field

[0001] The present invention belongs to the application field of inertial sensor technology, and in particular to a non-commutative error suppression algorithm in a large dynamic environment. Background Art

[0002] In the attitude update algorithm of the inertial navigation system, the carrier is constantly rotating at high speed in a large dynamic environment, which places high demands on the real-time performance of the attitude solution algorithm. Because the rotation of a rigid body is non-commutative, it can be seen that the rotation is not a vector, and it is meaningless to integrate and solve the angular velocity vector over a period of time. At the same time, in a large dynamic environment, the rotation of a rigid body will introduce a large non-commutative error.

[0003] Bortz J and Jordon proposed to use the equivalent rotation vector method to compensate for the non-commutative error in a large dynamic environment, so as to improve the accuracy of attitude solution. Zhou Zhaofa et al. proposed the three-sample quaternion method, setting the sampling number to three, first solving the rotation loss, and then solving the quaternion to obtain the carrier attitude angle. Based on the equivalent rotation vector method, many scholars have proposed various different extended and improved algorithms, most of which increase the number of samples to solve the non-commutative error in a large dynamic environment.

[0004] However, nowadays, more and more scholars have proved through experiments and simulations that simply increasing the number of samples cannot fundamentally improve the accuracy of attitude solution in a large dynamic environment, and as the number of samples increases, the amount of algorithm calculation will increase significantly. This type of algorithm ignores the high-order terms of the multi-sample algorithm, but the errors stimulated by the high-order terms in a large dynamic environment cannot be ignored. In response to the above problems, considering the accuracy and amount of calculation of the attitude solution algorithm, this paper proposes a high-order iterative attitude optimization algorithm based on the traditional multi-sample equivalent rotation vector algorithm.

[0005] After searching, the application publication number CN112033437A, a strapdown inertial navigation system optimization method based on an equivalent rotating vector algorithm, the method uses the cone motion trajectory as a test condition, optimizes the coefficients in the rotating vector algorithm, and can obtain a theoretical quaternion according to the cone motion. The calculated quaternion is obtained by using the rotating vector, and the theoretical quaternion is subtracted from the calculated quaternion to obtain an error quaternion. The error quaternion is subjected to Taylor series expansion, and the error term is made zero by selecting the optimal coefficient. According to the characteristic that the output of the fiber optic gyroscope is an angular velocity, the angular increment and angular velocity are used, with the angular velocity as the main factor. Its accuracy is higher than that of the two-sample algorithm, and the amount of calculation is equivalent. This method can also effectively compensate for the cone error of the fiber optic inertial navigation. When only the angular velocity is input, the cone compensation accuracy of the improved algorithm is 2 orders of magnitude higher than that of the traditional optimization algorithm, and the amount of calculation is smaller than that of the traditional optimization algorithm. The algorithm is suitable for a strapdown inertial navigation system composed of a fiber optic gyroscope.

[0006] After searching, the application publication number CN112033437A, a strapdown inertial navigation system optimization method based on equivalent rotating vector algorithm, is applied to fiber optic gyroscopes with high device precision. Currently, miniaturized MEMS inertial gyroscopes have become the mainstream of the industry, but their device precision is lower than that of fiber optic gyroscopes, so higher requirements are placed on the algorithm. CN112033437A does not perform algorithm analysis on large dynamic environments, ignores high-order terms when solving equivalent rotating vectors, and only fits the first two terms of the rotating vector differential equation using quadratic curves. Moreover, the present invention uses a dual loop to update the rotating vector, which can better balance the algorithm efficiency and performance. Summary of the invention

[0007] The present invention aims to solve the above problems of the prior art. A method for suppressing non-commutative errors in a large dynamic environment is proposed. The technical solution of the present invention is as follows:

[0008] A non-commutative error suppression method based on a large dynamic environment comprises the following steps: using a gyroscope and an accelerometer as data sources, taking three sub-samples as a sampling period of the gyroscope, and using an equivalent rotation vector algorithm to calculate and solve in the first period; in the next period, also taking three sub-samples as a sampling period, using a high-order curve to fit it, and then iterating with the result of the previous period to solve a high-order iterative solution.

[0009] Furthermore, the three sub-samples are used as a sampling period of the gyroscope, and an equivalent rotation vector algorithm is used to calculate and solve in the first period, specifically including:

[0010] Using the gyroscope and accelerometer as data sources, the equivalent rotation vector is obtained, and the correction term of the non-commutative error is obtained based on the equivalent rotation vector. The equivalent rotation vector is expanded by Taylor series, and then three sub-samples are used as the gyroscope sampling period. The angular rate update period is divided into three time intervals of equal length to obtain three sampling values, and the rotation vector solution of the previous period is calculated based on this.

[0011] Furthermore, the next cycle also uses three sub-samples as one sampling cycle, and a high-order curve is used to fit it, specifically including:

[0012] The fitting curve of the next period is obtained by fitting the fourth-order curve, and the solution of the undetermined coefficients of multiple samples is obtained.

[0013] Furthermore, the iterative solution with the result of the previous cycle to obtain a high-order iterative solution specifically includes:

[0014] Using the rotating vector of the previous cycle as the observation value, the corresponding derivatives of each order of the fourth-order equivalent rotating vector at the initial moment of this cycle are solved, and the undetermined solution of the rotating vector is obtained by combining the Taylor series expansion of the rotating vector differential equation, and then the optimal solution of this cycle is obtained through the iterative solution of two cycles;

[0015] Based on the obtained high-order iterative equivalent rotation vector solution, the rotation quaternion of the carrier coordinate system within a posture update cycle is obtained, and the posture information is updated after repeating until the posture information update cycle is reached.

[0016] Furthermore, the equivalent rotation vector algorithm is optimized by increasing the number of subsamples within an angular rate update cycle. The equivalent rotation vector differential equation can be expressed as follows:

[0017]

[0018] in represents the equivalent rotation vector differential, ω represents the angular rate sampling value, and Φ represents the rotation vector.

[0019] In the angular rate update period T = t k+1 -t k In the time period, the rotation vector formula is obtained by fitting the carrier angular velocity with a straight line, a quadratic parabola, and a cubic curve. The order of the fitting is related to the number of sub-samples. Three sub-samples need to be fitted with a quadratic parabola:

[0020] ω b (t k +τ)=a+2bτ+3cτ 2 (2)

[0021] where t k Indicates the initial time of the current cycle update, ω b (t k +τ) represents t k +τ time period, a, b, c represent the curve fitting coefficients, which are constants to be determined. Divide the angular rate update period T into three time intervals of equal length The general formula of the angular increment is Δθ i It can be expressed as:

[0022]

[0023] i represents the number of corner increment samplings in the corresponding time interval.

[0024] Substituting formula (2) into formula (3), the angular increment of each time interval can be obtained as follows:

[0025]

[0026] Derivative formula (2) is obtained at the initial time t of the cycle k The derivatives of various orders are as follows:

[0027]

[0028] Let T = t k+1 -t k The solution of the differential equation for the rotating vector in equation (1) is Φ(t k +T), then its Taylor series expansion can be expressed as follows:

[0029]

[0030] By taking the derivative of the first two terms of formula (1), we can obtain the equivalent rotation vector at the initial time t of the cycle: k The corresponding derivatives are as follows:

[0031]

[0032] Substitute the derivatives of the rotating vector of each order solved by formula (7) into the Taylor series expansion of the rotating vector differential equation, that is, formula (6), and rearrange it to obtain:

[0033]

[0034] From formula (4), we can get the equations of gyro output samples Δθ1, Δθ2, Δθ3 and assumed parameters a, b, c about the update cycle T. Substituting formula (4) into formula (8), we can get:

[0035]

[0036] Substituting formula (9) into formula (8), and optimizing the coefficients according to the optimal solution of the three-sample basic algorithm obtained by Miller, the solution of the three-sample equivalent rotation vector algorithm can be obtained:

[0037]

[0038] Therefore, the rotation vector can be updated in each rotation vector update cycle to obtain the rotation vector solution of the previous cycle.

[0039] Furthermore, for the slow loop calculation period [t k +h,t k +2h] is fitted using a high-order curve:

[0040] ω b (t k +τ)=a'+2b'τ+3c'τ 2 +4dτ 3 (11)

[0041] h represents the rotation vector update period. The same method as in step (1) can be used to calculate the slow loop period [t k +h,t k+2h] The angular increments Δθ4, Δθ5, Δθ6 are expressed as follows:

[0042]

[0043] Among them, a', b', c' represent the coefficients of the high-order fitting curve, which are constants to be determined. Thus, the solution of the coefficients to be determined for multiple samples is obtained.

[0044] Furthermore, the solution of the multi-sample undetermined coefficients of the previous cycle is obtained as follows:

[0045]

[0046] Combining formula (12) and formula (13), we can get the following relationship:

[0047]

[0048] The relationship between the fitting parameters and the angular rate cross product results is as follows:

[0049]

[0050] Where i and j represent the parameters of the i-th and j-th terms of the fitting curve, respectively, and i≠j must be satisfied. From the definition of the angular velocity vector, we can get:

[0051]

[0052] From this, we can further obtain the relationship between the coefficients:

[0053]

[0054] Combined with formula (11), formula (1) is derived to obtain the equivalent rotation vector at the initial time t of the cycle under the fourth-order curve fitting: k The corresponding derivatives are as follows:

[0055]

[0056] Substituting the derivatives of the rotating vector in formula (18) into the Taylor series expansion of the rotating vector differential equation, and combining the parameter relationship expressions in formula (12) and formula (17), the optimal solution of the improved algorithm can be obtained as shown in formula (19):

[0057]

[0058] From the comparative analysis of the equivalent rotation vector solutions of the two cycles before and after, it can be seen that the complexity of the traditional multi-sample equivalent rotation vector algorithm is lower than that of the improved algorithm, but the introduction of high-order terms can better solve the impact of non-commutative errors. In terms of comprehensive consideration of algorithm accuracy and algorithm efficiency, the algorithm of the present invention plays a better balancing role between the two.

[0059] The advantages and beneficial effects of the present invention are as follows:

[0060] The present invention aims at the problem that the traditional equivalent rotation vector algorithm cannot improve the solution accuracy well by only increasing the number of sub-samples in a large dynamic environment, and ignores the problem that high-order terms will produce large errors in a large dynamic environment. On this basis, an algorithm for suppressing non-commutative errors in a large dynamic environment is proposed. The method uses a gyroscope and an accelerometer as data sources, and three sub-samples as a sampling cycle of the gyroscope. In the first cycle, the equivalent rotation vector algorithm is used to calculate and solve. In the next cycle, three sub-samples are also used as a sampling cycle, and a high-order curve is used to fit it, and then the high-order iterative solution is iterated with the result of the previous cycle. The present invention can better suppress non-commutative errors and improve the attitude solution accuracy of IMU in a large dynamic environment. The main innovation of the present invention is that the update of the equivalent rotation vector is divided into two loops, fast and slow. The fast loop uses low-order terms for solution, and the slow loop uses high-order terms for solution, which not only ensures the algorithm efficiency, but also improves the algorithm accuracy. At the same time, in the traditional algorithm, the high-order terms of the equivalent rotation vector are often ignored. The present invention introduces high-order terms to solve the equivalent rotation vector, which can solve the non-commutative error problem in a large dynamic environment. Compared with traditional algorithms, the present invention achieves an improvement of two orders of magnitude in attitude solution accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 The present invention provides a preferred embodiment Figure 1 This is the overall algorithm block diagram.

[0062] Figure 2 This is a cone environment simulation diagram of the traditional three-sample algorithm.

[0063] Figure 3 This is a cone environment simulation diagram of the non-commutative error suppression algorithm in a large dynamic environment.

[0064] Figure 4 This is the solution diagram of the three-axis turntable swing posture using the traditional three-sample algorithm.

[0065] Figure 5 This is a diagram of the three-axis turntable sway posture solution using the non-commutative error suppression algorithm in a large dynamic environment. DETAILED DESCRIPTION

[0066] The following will describe the technical solutions in the embodiments of the present invention in detail in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention.

[0067] The technical solution of the present invention to solve the above technical problems is:

[0068] The non-commutative error suppression algorithm in a large dynamic environment is characterized by comprising the following steps:

[0069] Step (1), taking the last two terms of the equivalent rotation vector as the correction terms of the non-commutative error, and performing Taylor series expansion on the equivalent rotation vector. Then, taking three sub-samples as the gyroscope sampling period, the angular rate update period is divided into three time intervals of equal length, and three sampling values ​​are obtained, so as to calculate the rotation vector solution of the previous period;

[0070] Step (2), the same as step (1) to obtain three sampling values, adopt a fourth-order curve to fit to obtain a fitting curve of the next period, and obtain a multi-sample undetermined coefficient solution;

[0071] Step (3), using the rotation vector of the previous cycle as the observation value, solving the corresponding derivatives of each order of the fourth-order equivalent rotation vector at the initial time of the current cycle, combining the Taylor series expansion of the rotation vector differential equation to obtain the undetermined solution of the rotation vector, and then obtaining the optimal solution of the current cycle through two cycles of iterative solutions;

[0072] Step (4), based on the obtained high-order iterative equivalent rotation vector solution, obtain the rotation quaternion of the carrier coordinate system within a posture update cycle, repeat steps (1) to (3) to reach the posture information update cycle, and then update the posture information.

[0073] In the step (1), the equivalent rotation vector algorithm is optimized by increasing the number of subsamples within an angular rate update cycle. The equivalent rotation vector differential equation can be expressed as follows:

[0074]

[0075] In the angular rate update period T = t k+1 -t k In the time period, the rotation vector formula is obtained by fitting the carrier angular velocity with a straight line, a quadratic parabola, and a cubic curve. The order of fitting is related to the number of sub-samples. Three sub-samples need to be fitted with a quadratic parabola:

[0076] ω b (t k +τ)=a+2bτ+3cτ 2 (40)

[0077] where t k Indicates the initial time of the current cycle update, ω b Represents the angular rate. Divide the angular rate update period into three equal time intervals From this we can know the general formula of the angular increment:

[0078]

[0079] Substituting formula (40) into formula (41), the angular increments of each time interval can be obtained as follows:

[0080]

[0081] Derivative formula (40) is obtained at the initial time t of the cycle k The derivatives of various orders are as follows:

[0082]

[0083] Let T = t k+1 -t k The solution of the differential equation for the rotating vector in equation (39) is Φ(t k +T), then its Taylor series expansion can be expressed as follows:

[0084]

[0085] By taking the derivative of the first two terms of formula (39), we can obtain the equivalent rotation vector at the initial time t of the cycle: k The corresponding derivatives are as follows:

[0086]

[0087] Substituting the derivatives of the rotating vector solved by formula (45) into the Taylor series expansion of the rotating vector differential equation, that is, formula (44), we can get:

[0088]

[0089] From formula (42), we can get the equations of the gyro output samples Δθ1, Δθ2, Δθ3 and the assumed parameters a, b, c with respect to the update cycle T, that is, put formula (42) into formula (46) to get:

[0090]

[0091] Substituting formula (47) into formula (46), and optimizing the coefficients according to the optimal solution of the three-sample basic algorithm obtained by Miller, the solution of the three-sample equivalent rotation vector algorithm can be obtained:

[0092]

[0093] Therefore, the rotation vector can be updated in each rotation vector update cycle to obtain the rotation vector solution of the previous cycle.

[0094] In step (2), based on step (1), the slow loop calculation period [t k +h,t k+2h] is fitted using a high-order curve:

[0095] ω b (t k +τ)=a'+2b'τ+3c'τ 2 +4dτ 3 (49)

[0096] Using the same method as in step (1), the slow loop calculation period [t k +h,t k +2h] The angular increments Δθ4, Δθ5, Δθ6 are expressed as follows:

[0097]

[0098] Thus, we can obtain multiple samples of undetermined coefficient solutions.

[0099] In step (3), the solution of the multi-sample undetermined coefficients of the previous period can be obtained by the method of step (1):

[0100]

[0101] Combining formula (50) and formula (51), we can get the following relationship:

[0102]

[0103] The relationship between the fitting parameters and the angular rate cross product results is as follows:

[0104]

[0105] Where i and j represent the parameters of the i-th and j-th terms of the fitting curve, respectively, and i≠j must be satisfied. From the definition of the angular velocity vector, we can get:

[0106]

[0107] From this, we can further obtain the relationship between the coefficients:

[0108]

[0109] Combined with formula (50), formula (39) is derived to obtain the equivalent rotation vector at the initial time t of the cycle under the fourth-order curve fitting: k The corresponding derivatives are as follows:

[0110]

[0111] Substituting the derivatives of the rotating vector of formula (56) into the Taylor series expansion of the rotating vector differential equation, and combining the parameter relationship expressions of formula (50) and formula (55), the optimal solution of the improved algorithm can be obtained as shown in formula (57):

[0112]

[0113] From the comparative analysis of the equivalent rotation vector solutions of the two cycles before and after, it can be seen that the complexity of the traditional multi-sample equivalent rotation vector algorithm is lower than that of the improved algorithm, but the introduction of high-order terms can better solve the impact of non-commutative errors. In terms of comprehensive consideration of algorithm accuracy and algorithm efficiency, the algorithm of the present invention plays a better balancing role between the two.

[0114] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0115] The above embodiments should be understood to be only used to illustrate the present invention and not to limit the protection scope of the present invention. After reading the contents of the present invention, technicians can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A non-commutative error suppression method based on a large dynamic environment, characterized in that: The following steps are involved: Step (1), taking the last two terms of the equivalent rotation vector as the correction terms of the non-commutative error, performing Taylor series expansion on the equivalent rotation vector, and then taking three sub-samples as the gyroscope sampling period, the angular rate update period is divided into three time intervals of equal length, and three sampling values ​​are obtained, and the rotation vector solution of the previous period is calculated by this solution; Step (2), the same as step (1) to obtain three sampling values, adopt a fourth-order curve to fit to obtain the fitting curve of the next period, and obtain the multi-sample undetermined coefficient solution; Step (3), using the rotation vector of the previous cycle as the observation value, solving the corresponding derivatives of each order of the fourth-order equivalent rotation vector at the initial time of the current cycle, combining the Taylor series expansion of the differential equation of the rotation vector to obtain the undetermined solution of the rotation vector, and then obtaining the optimal solution of the current cycle through two cycles of iterative solutions; Step (4), obtaining the rotation quaternion of the carrier coordinate system within a posture update period according to the obtained high-order iterative equivalent rotation vector solution, repeating steps (1) to (3) to reach the posture information update period, and then updating the posture information; In the step (1), the equivalent rotation vector algorithm is optimized by increasing the number of subsamples within an angular rate update cycle; the equivalent rotation vector differential equation can be expressed as follows: represents the equivalent rotation vector differential, ω represents the angular rate sampling value, and Φ represents the rotation vector; in the angular rate update period T = t k+1 -t k In the time period, the rotation vector formula is obtained by fitting the carrier angular velocity with a straight line, a quadratic parabola, and a cubic curve; the order of the fitting is related to the number of sub-samples, and three sub-samples need to be fitted with a quadratic parabola: oh b (t k +τ)=a+2bτ+3cτ 2 (40) where t k Indicates the initial time of the current cycle update, ω b (t k +τ) represents t k +τ angular rate in the time period, a, b, c represent the curve fitting coefficients, which are constants to be determined. The angular rate update period is divided into three time intervals of equal length. From this we can know the general formula of the angular increment: i represents the number of corner increment samplings in the corresponding time interval; Substituting formula (40) into formula (41), the angular increments of each time interval can be obtained as follows: Derivative formula (40) is obtained at the initial time t of the cycle k The derivatives of various orders are as follows: Let T = t k+1 -t k The solution of the differential equation for the rotating vector in equation (39) is Φ(t k +T), then its Taylor series expansion can be expressed as follows: By taking the derivative of the first two terms of formula (39), we can obtain the equivalent rotation vector at the initial time t of the cycle: k The corresponding derivatives are as follows: Substitute the derivatives of the rotating vector solved by formula (45) into the Taylor series expansion of the rotating vector differential equation, that is, formula (44), and rearrange it to obtain: From formula (42), we can get the equations of the gyro output samples Δθ1, Δθ2, Δθ3 and the assumed parameters a, b, c about the update cycle T, that is, put formula (42) into formula (46) to get: Substituting formula (47) into formula (46), and optimizing the coefficients according to the optimal solution of the three-sample basic algorithm obtained by Miller, the solution of the three-sample equivalent rotation vector algorithm can be obtained: Therefore, the rotation vector can be updated in each rotation vector update cycle to obtain the rotation vector solution of the previous cycle; In step (2), based on step (1), the slow loop calculation period [t k +h,t k +2h] is fitted using a high-order curve: oh b (t k +τ)=a'+2b'τ+3c'τ 2 +4dτ 3 (49) Using the same method as in step (1), the slow loop calculation period [t k +h,t k +2h] angular increments Δθ4, Δθ5, Δθ6 are expressed as follows: h represents the rotation vector update period, a', b', c', d represent the high-order fitting curve coefficients, which are the constants to be determined, and thus the multi-sample undetermined coefficient solutions are obtained; In step (3), the solution of the multi-sample undetermined coefficients of the previous cycle can be obtained by the method of step (1): Combining formula (50) and formula (51), we can get the following relationship: The relationship between the fitting parameters and the angular rate cross product results is as follows: Where i and j represent the parameters of the i-th and j-th terms of the fitting curve, respectively, and i≠j must be satisfied. From the definition of the angular velocity vector, we can get: From this, we can further obtain the relationship between the coefficients: Combined with formula (50), formula (39) is derived to obtain the equivalent rotation vector at the initial time t of the cycle under the fourth-order curve fitting: k The corresponding derivatives are as follows: Substituting the derivatives of the rotating vector of formula (56) into the Taylor series expansion of the rotating vector differential equation, and combining the parameter relationship expressions of formula (50) and formula (55), the optimal solution of the improved algorithm can be obtained as shown in formula (57):

Images