A Method for Attitude Resolution of Strapdown Inertial Navigation System in High Dynamic Environment

Abstract

The present invention claims protection for a method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment, belonging to the technical field of inertial navigation. The present invention includes the following steps: raw data acquisition; calculation of the initial attitude angle; calculation of the initial quaternion; construction of an equivalent rotation vector; construction of an attitude change quaternion; quaternion update; attitude angle update. Different from the traditional equivalent rotation vector algorithm that assumes the carrier motion angular velocity is a polynomial of several degrees, the present invention assumes that the carrier motion angular velocity can be expressed as the sum of trigonometric functions with different frequencies. Based on this, an equivalent rotation vector expressed by the angular increment and the angular velocity correlation coefficient is derived, and then an attitude calculation method based on this equivalent rotation vector is designed. The method proposed by the present invention improves the accuracy of attitude calculation in a high-dynamic environment and provides a new idea for solving high-precision attitude calculation in a high-dynamic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of inertial navigation, and particularly relates to an attitude algorithm for a strapdown inertial navigation system in a high-dynamic environment. Background Art

[0002] A strapdown inertial navigation system (SINS) does not have a physical platform like a platform inertial navigation system. Instead, it constructs a mathematical platform with an inertial measurement unit (IMU) as the core. The IMU integrates inertial sensors and a micro-control unit and is usually directly fixed to a vehicle to sense the acceleration information and angular velocity information of the vehicle. The SINS uses the motion information of the vehicle sensed by the IMU and realizes navigation and positioning through three groups of algorithms, namely attitude algorithm, velocity algorithm, and position algorithm. As the core of the entire navigation algorithm, the accuracy of the attitude algorithm directly affects the accuracy of the strapdown inertial navigation system.

[0003] Traditional attitude algorithms include Euler angle method, direction cosine method, quaternion method, and equivalent rotation vector method. In a low-dynamic motion environment, the quaternion method is generally commonly used. However, in a high-dynamic environment, severe linear motion and angular motion will cause the non-commutative error generated by vector integration to become larger, and the quaternion method is no longer applicable. Common high-dynamic environments include, but are not limited to, large angular rate maneuvering environments, angular vibration environments, etc. The equivalent rotation vector method can compensate for the non-commutative error due to its principle characteristics, and the degree of compensation is related to the construction of the equivalent rotation vector.

[0004] Since Bortz proposed using the equivalent rotation vector method to compensate for the non-commutative error in a high-dynamic environment, scholars at home and abroad have designed many optimization algorithms around this method. The starting point of these methods is more to increase the amount of gyroscope data within an attitude algorithm cycle to construct a rotation vector closer to the actual situation, so as to improve the accuracy of the algorithm. Different from the traditional equivalent rotation vector algorithm that assumes the angular velocity of the vehicle motion is a polynomial of several degrees, the present invention assumes that the angular velocity of the vehicle motion can be represented by the sum of trigonometric functions with different frequencies, and based on this, an improved equivalent rotation vector algorithm applicable to a high-dynamic environment is proposed.

[0005] After retrieval, patent CN110879066A, an attitude calculation algorithm, device, and vehicle-mounted inertial navigation system. The algorithm structure described in this patent is similar to that of this patent, but upon further investigation, there are the following differences: 1. The main object of the algorithm in patent CN110879066A is the vehicle-mounted inertial navigation system, while this patent targets the strapdown inertial navigation system in a high-dynamic environment; 2. The essence of patent CN110879066A is an algorithm derived on the premise of the carrier motion angular velocity in the form of a polynomial, while in this article, the algorithm is derived on the premise of the carrier motion angular velocity in the form of a trigonometric function. This is the biggest difference between the two. The algorithm derived based on the premise of this article has the advantage of higher accuracy under the same sub-sample conditions, which depends on the unique premise of the carrier motion angular velocity assumption in this article. Summary of the Invention

[0006] The present invention aims to solve the above problems of the prior art. A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment is proposed. The technical solution of the present invention is as follows:

[0007] A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment, which includes the following steps:

[0008] Step 1: Collect the original data of the strapdown inertial navigation system;

[0009] Step 2: Calculate the initial attitude angle;

[0010] Step 3: Calculate the initial quaternion;

[0011] Step 4: Construct an equivalent rotation vector;

[0012] Step 5: Construct an attitude change quaternion;

[0013] Step 6: Perform quaternion update;

[0014] Step 7: Perform attitude angle update.

[0015] Further, in the above step 1, the method for collecting the original attitude data of the strapdown inertial navigation system is specifically as follows: When a pedestrian moves, the angular velocity and acceleration of their feet change violently, which is a typical high-dynamic environment. Therefore, the IMU is installed at the heel of the pedestrian, and the coordinate system used for attitude calculation is: The IMU installation coordinate system is the right front up coordinate system; the inertial system navigation coordinate system is the northeast sky coordinate system.

[0016] Further, in the above step 2, the calculation of the initial attitude angle specifically includes:

[0017] The pitch angle θ0 and roll angle γ0 in the initial attitude angle are calculated from the accelerometer data within the first group of attitude update periods, and the calculation formula is:

[0018]

[0019] Where atan is the inverse tangent function, and the calculation result range is [-π / 2,π / 2]; atan2 is the four-quadrant inverse tangent function, and the calculation result range is [-π,π]; the unit of the initial attitude angle is radian, and a x 、a y 、a z They respectively represent the three-axis acceleration information in the carrier coordinate system.

[0020] The attitude update cycle refers to a time period consisting of two sampling points, that is, the attitude solution is performed once every two samplings.

[0021] Furthermore, the initial quaternion Q0 in step 3 is calculated by:

[0022] The transformation matrix from the carrier coordinate system (b system) to the navigation coordinate system (n system) derived from the coordinate system rotation method is:

[0023]

[0024] Where γ, θ, and ψ represent the roll angle, pitch angle, and heading angle, respectively. represents the coordinate transformation matrix from b system to n system, T 11 、T 12 、T 13 、T 21 、T 22 、T 23 、T 31 、T 32 、T 33 The matrices are A brief note of the formula of the corresponding element in ;

[0025] At the same time, the transformation matrix from the vehicle coordinate system to the navigation coordinate system represented by the quaternion is recorded as:

[0026]

[0027] Where q0, q1, q2, q3 are the four real elements of the quaternion Q = q0 + q1i0 + q2j0 + q3k0, satisfying the relationship: i0, j0, and k0 represent unit vectors along the three-dimensional coordinate system;

[0028] The transformation matrices expressed in the two ways are equal. By combining the two matrices, the four elements of the initial quaternion can be obtained as follows:

[0029]

[0030] In the formula, sign is the sign function, which outputs +1 when the input is positive and -1 when the input is negative.

[0031] Further, in step 4, the equivalent rotation vector is constructed by the following steps:

[0032] Step 4.1: Assume that the motion angular velocity is the sum of trigonometric functions;

[0033] Step 4.2: Integrate the angular velocity over the update period to obtain the angular increment;

[0034] Step 4.3: Calculate the derivatives of all orders of the angular velocity and the angular increment at the zero moment;

[0035] Step 4.4: Express the equivalent rotation vector in terms of the angular velocity and the angular increment;

[0036] Step 4.5: Calculate the derivatives of all orders of the equivalent rotation vector at the zero moment;

[0037] Step 4.6: Perform the Taylor expansion of the equivalent rotation vector at the zero moment.

[0038] Further, in step 5, the attitude change quaternion is constructed from the equivalent rotation vector constructed above, and the relationship between the two is:

[0039]

[0040] In the formula, Φ is the equivalent rotation vector constructed above, Φ is the modulus value of the equivalent rotation vector, and its value is: Φ = |Φ|.

[0041] Further, in step 6, the quaternion is updated according to the following steps:

[0042] Within one attitude update period, the formula is simplified to:

[0043]

[0044] In the formula, Q(t k+1 ) and Q(t k ) are respectively called the attitude quaternions at time t k+1 and time t k , and p * (h) represents the conjugate of the coordinate transformation matrix of the navigation coordinate system from time t k to time t k+1 .

[0045] Then the quaternion Q(t k+1 ) of the next attitude update period is updated by the quaternion product of the quaternion Q(t k ) of the previous attitude update period and the attitude change quaternion q(h) within two update period time intervals.

[0046] Further, in step 7, the attitude angle is updated as follows:

[0047] The coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system can be represented by the quaternion method or by the coordinate system rotation method. The matrices represented by the two are equal. Therefore, the corresponding relationship between the attitude angle and the quaternion is:

[0048] θ = asin(2(q2q3 + q0q1))

[0049]

[0050] q0, q1, q2, q3 are the four real elements of the quaternion Q = q0 + q1i0 + q2j0 + q3k0, and γ, θ, ψ represent the roll angle, pitch angle, and heading angle respectively.

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

[0052] The present invention constructs an equivalent rotation vector in different ways, improves the accuracy of attitude solution in a high-dynamic environment, and provides a new idea for solving the high-precision attitude solution in an angular vibration environment.

[0053] Specifically, the present invention derives the equivalent rotation vector by assuming that the vehicle motion angular velocity is the sum of trigonometric functions with different frequencies. The algorithm derived under this assumption provides a new idea for the attitude solution of a strapdown inertial navigation system in sinusoidal vibration, which is beneficial to improving the attitude solution accuracy. At the same time, conical motion is generally regarded as the most complex motion environment for attitude solution, and the high-dynamic environment generally includes conical motion and vibration environment. Therefore, the method proposed in this paper can also be beneficial to improving the attitude solution accuracy in a high-dynamic environment.

[0054] These benefits all come from the unique algorithm derivation premise of this paper: the vehicle motion angular velocity is the sum of trigonometric functions. Under this premise, if it is assumed that the angular increment is equal to the modulus value of the rotation vector, the error of the solution of the Bortz equation only comes from the approximation error that the angular increment is equal to the modulus value of the rotation vector and the truncation error of the Taylor expansion of the rotation vector, and there is no error caused by ignoring the higher-order terms in the traditional Picard iterative solution. This assumption premise is also the key point of this paper and the biggest difference from the traditional algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 is the overall flowchart of the attitude solution method of the strapdown inertial navigation system in a high-dynamic environment provided by the preferred embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0056] The technical solutions in the embodiments of the present invention will be clearly and detailedly described below with reference to the accompanying drawings in the embodiments of the present invention. The described embodiments are only a part of the embodiments of the present invention.

[0057] The technical solution of the present invention to solve the above technical problems is as follows:

[0058] As Figure 1 shown, this embodiment provides an attitude algorithm method for a strapdown inertial navigation system in a high-dynamic environment, including the following steps:

[0059] Step 1: Raw data acquisition;

[0060] Step 2: Calculate the initial attitude angle;

[0061] Step 3: Calculate the initial quaternion;

[0062] Step 4: Construct an equivalent rotation vector;

[0063] Step 5: Construct an attitude change quaternion;

[0064] Step 6: Update the quaternion;

[0065] Step 7: Update the attitude angle.

[0066] In this embodiment, the raw data acquisition method is: installing the IMU at the heel of the pedestrian.

[0067] In this embodiment, in step 1, the IMU is installed on the vehicle, and the vehicle motion acceleration information and angular velocity information are collected through the internal inertial sensors of the IMU, where the acceleration information is obtained by the accelerometer and the angular velocity information is obtained by the gyroscope.

[0068] In this embodiment, in step 1, the installation coordinate system of the IMU is the right-front-up coordinate system, that is, the X b axis of the IMU points to the right side of the vehicle, the Y b axis points to the front of the vehicle, and the Z b axis points to the sky of the vehicle; the inertial system navigation coordinate system is the northeast-sky coordinate system, that is, the X n axis of the navigation coordinate system points to the geographic east direction, the Y n axis points to the geographic north direction, and the Z n axis points to the geographic sky direction.

[0069] In this embodiment, in step 2, the initial heading angle ψ0 should have been calculated from the magnetometer data to obtain the absolute position by navigation. However, the focus of the present invention is on attitude algorithm, so setting the initial heading angle to 0 can also analyze the algorithm effect; the pitch angle θ0 and roll angle γ0 in the initial attitude angle are calculated from the accelerometer data within the first set of attitude update periods, and the calculation method is:

[0070]

[0071] In the formula, atan is the inverse tangent function, and the calculation result range is [-π / 2,π / 2], atan2 is the four-quadrant inverse tangent function, and the calculation result range is [-π,π], a x 、a y 、a z They represent the three-axis acceleration information in the carrier coordinate system. The unit of the initial attitude angle is radian.

[0072] In this embodiment, in step 2, a posture update cycle refers to a time period consisting of two sampling points, that is, a posture solution is performed once every two samplings.

[0073] In this embodiment, in step 3, the initial quaternion Q0 is calculated by:

[0074] The transformation matrix from the carrier coordinate system (b system) to the navigation coordinate system (n system) derived from the coordinate system rotation method is:

[0075]

[0076] Where γ, θ, and ψ represent the roll angle, pitch angle, and heading angle, respectively. represents the coordinate transformation matrix from b system to n system, T 11 , T 12 , T 13 , T 21 , T 22 , T 23 , T 31 , T 32 , T 33 The matrices are A brief note of the formula of the corresponding element in ;

[0077] At the same time, the transformation matrix from the vehicle coordinate system to the navigation coordinate system represented by the quaternion is recorded as:

[0078]

[0079] Where q0, q1, q2, q3 are the four real elements of the quaternion Q = q0 + q1i0 + q2j0 + q3k0, satisfying the relationship: i0, j0, and k0 represent unit vectors along the three-dimensional coordinate system respectively.

[0080] The transformation matrices expressed in the two ways are equal. By combining the two matrices, the four elements of the initial quaternion can be obtained as follows:

[0081]

[0082] Wherein, sign is the sign function, which outputs +1 when the input is a positive number and -1 when the input is a negative number.

[0083] In this embodiment, in the said step 4, the equivalent rotation vector is constructed by the following steps:

[0084] Step 4.1: Assume that the motion angular velocity is the sum of trigonometric functions;

[0085] Step 4.2: Integrate the angular velocity within the update period to obtain the angular increment;

[0086] Step 4.3: Calculate the derivatives of all orders of the angular velocity and the angular increment at the zero moment;

[0087] Step 4.4: Represent the equivalent rotation vector with the angular velocity and the angular increment;

[0088] Step 4.5: Calculate the derivatives of all orders of the equivalent rotation vector at the zero moment;

[0089] Step 4.6: Perform the Taylor expansion of the equivalent rotation vector at the zero moment.

[0090] In this embodiment, in the said step 4.1, it is assumed that the carrier motion angular velocity can be represented by the sum of trigonometric functions with different frequencies, and its form is as follows: ω(t) = k1sin t + k2sin2t + k3sin3t +..., and the appropriate angular velocity function is selected according to the different numbers of sampling points within the attitude update period. In the present invention, two samplings are performed within one attitude update period, so it is assumed that the carrier motion angular velocity is: ω(t) = a sin t + b sin 2t, where a and b are vector coefficients related to the angular velocity.

[0091] In this embodiment, in the said step 4.2, the angular increment is the integral of the angular velocity within one attitude update period:

[0092] In this embodiment, in the said step 4.3, the derivatives of all orders of the angular velocity and the angular increment at the point t = 0 are:

[0093] ω(0) = (a sin t + b sin 2t)| t＝0 = 0

[0094]

[0095] ω (4) (0) = (a sin t + 2 4 b sin 2t)| t＝0 = 0

[0096] ω (5) (0) = (a cos t + 2 5 b cos 2t)|t＝0 = a + 2 5 b

[0097]

[0098] Δθ (5) (0) = ω (4) (t)| t＝0 = 0

[0099] ……

[0100] In this embodiment, in step 4.4, the commonly used approximate Bortz equation in engineering is:

[0101]

[0102] Since the attitude update period is in milliseconds, the term in the approximate Bortz equation is regarded as a second-order small quantity of the equivalent rotation vector Φ and neglected, and the equivalent rotation vector is replaced by the angular increment, that is: Φ≈Δθ. Then,

[0103] In this embodiment, in step 4.5, the derivatives of the equivalent rotation vector at t = 0 are obtained, and the corresponding values of the angular velocity and the angular increment are substituted, and we can get:

[0104]

[0105] In this embodiment, in step 4.6, let Φ(h) be the equivalent rotation vector within an attitude update period h, h = t k+1 - t k , t k+1 、t k represent the next attitude update period and the current attitude update period respectively. Perform a seventh-order Taylor expansion on Φ(h):

[0106]

[0107] where Φ(0) is the equivalent rotation vector within the time period [t k , t k . Since the time interval is 0, so Φ(0) = 0.

[0108] Substitute the derivatives of the equivalent rotation vector at t = 0, and we can get:

[0109]

[0110] In order to express the parameters a and b in the above formula with two angular increments within the attitude update period, denote

[0111]

[0112] Then, the two angular increments Δθ1 and Δθ2 in the attitude update cycle are:

[0113]

[0114] Therefore, the sum of the angular increments during the attitude update cycle is:

[0115]

[0116] According to the Taylor expansion of the cosine function, the above formula can be written as:

[0117]

[0118] Comparing the Taylor expansion of the sum of the equivalent rotation vector and the angular increment, we get:

[0119]

[0120] Where Δθ1 and Δθ2 are the products of the angular velocity obtained by two samplings in the attitude update period and the sampling time, and a and b are solved by the expressions of the angular increments Δθ1 and Δθ2:

[0121]

[0122] In this embodiment, in step 5, the attitude change quaternion is constructed according to the following steps:

[0123] Assume t k The carrier coordinate system at time is b(k), the navigation coordinate system is n(k), and t k+1 The carrier coordinate system at the moment is b(k+1), and the navigation coordinate system is n(k+1). The rotation quaternion from b(k) to b(k+1) is q(h), and the rotation quaternion from n(k) to b(k) is Q(t k ), the rotation quaternion from n(k+1) to b(k+1) is Q(t k+1 ), the rotation quaternion from n(k) to n(k+1) is p(h), then r n (k+1) It can be expressed as a transformation matrix:

[0124]

[0125] In the formula, r n(k+1) 、r b(k+1) t k+1 The rotation vector in the moment navigation coordinate system and the carrier coordinate system.

[0126] In addition, the quaternion multiplication representation of vector coordinate transformation is equivalent to the transformation matrix representation, that is,

[0127]

[0128] where r 1 , r 2 are the rotation vectors in coordinate system 1 and coordinate system 2, Q is the rotation quaternion from system 2 to system 1, and Q * is the conjugate of Q.

[0129] Therefore, r n(k+1) is equivalent to the quaternion multiplication expression:

[0130]

[0131] where p * (h) represents the conjugate of the coordinate transformation matrix of the navigation coordinate system from time t k to time t k+1 .

[0132] According to the associative law of quaternion multiplication, r n(k+1) the quaternion multiplication expression can be written as:

[0133]

[0134] Comparing the above two equations, we can obtain:

[0135]

[0136] where, for the sake of distinction, q(h) is called the attitude change quaternion during the time period k , t k+1 . Φ is the equivalent rotation vector from b(k) to b(k + 1), and Φ = |Φ|.

[0137] In this embodiment, in step 6, the quaternion is updated as follows:

[0138] Since the change of the navigation coordinate system is very slow within one attitude update period, p(h) ≈ 1 + 0, so the formula is simplified to:

[0139]

[0140] where Q(t k+1 ), Q(t k ) are respectively called the attitude quaternions at time t k+1 and time t k .

[0141] Then the quaternion Q(t k+1 ) of the next attitude update period is obtained from the quaternion Q(t k) and the quaternion product update of the attitude change quaternion q(h) within two update cycle time periods.

[0142] In this embodiment, in step 7, the attitude angle is updated as follows:

[0143] The coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system can be represented by the quaternion method or by the coordinate system rotation method. The matrices represented by the two are equal. Therefore, the corresponding relationship between the attitude angle and the quaternion is:

[0144] θ = asin(2(q2q3 + q0q1))

[0145]

[0146] The systems, devices, modules or units illustrated in the above embodiments can be specifically implemented by computer chips or entities, or by products with certain functions.

[0147] It should also be noted that the term "including", "comprising" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. Without further limitation, the element defined by the statement "including one..." does not exclude the existence of other identical elements in the process, method, commodity or device including the said element.

[0148] The above embodiments should be understood as being only used to illustrate the present invention and not to limit the protection scope of the present invention. After reading the content recorded in the present invention, those skilled in the art 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 method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment, characterized in that, The following steps are involved: Step 1: Collect raw data of strapdown inertial navigation system; Step 2: Calculate the initial attitude angle; Step 3: Calculate the initial quaternion; Step 4: Construct an equivalent rotation vector; Step 5: Construct attitude change quaternion; Step 6: Update quaternion; Step 7: Update the attitude angle; In step 4, the equivalent rotation vector is constructed by the following steps: Step 4.1: Assume that the angular velocity of motion is the sum of trigonometric functions; Step 4.2: Integrate the angular velocity within the update period to obtain the angular increment; Step 4.3: Calculate the derivatives of angular velocity and angular increment at zero time; Step 4.4: Express the equivalent rotation vector in terms of angular velocity and angular increment; Step 4.5: Calculate the derivatives of each order of the equivalent rotating vector at time zero; Step 4.6: Find the Taylor expansion of the equivalent rotation vector at time zero.

2. The attitude algorithm method of the strapdown inertial navigation system in a high dynamic environment according to claim 1, wherein In the step 1, the method of collecting the original attitude data of the strapdown inertial navigation system is specifically as follows: when the pedestrian moves, the angular velocity and acceleration of the pedestrian's foot change dramatically, which is a typical high-dynamic environment. Therefore, the IMU is installed at the heel of the pedestrian, and the coordinate system used for attitude solution is: the IMU installation coordinate system is the right front upper coordinate system; the inertial system navigation coordinate system is the northeast celestial coordinate system.

3. A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment according to claim 1, characterized in that, The calculation of the initial attitude angle in step 2 specifically includes: The pitch angle θ0 and roll angle γ0 in the initial attitude angle are calculated from the accelerometer data in the first attitude update cycle, and the calculation formula is: Where atan is the arctangent function, and the calculation result range is [-π / 2, π / 2]; atan2 is the four-quadrant arctangent function, and the calculation result range is [-π, π]. The unit of the initial attitude angle is radians, and a x , a y , a z respectively represent the three-axis acceleration information in the carrier coordinate system; An attitude update cycle refers to a time period consisting of two sampling points, that is, the attitude solution is performed once every two sampling points.

4. A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment according to claim 1, characterized in that, The initial quaternion Q0 in step 3 is calculated as follows: The transformation matrix from the carrier coordinate system b to the navigation coordinate system n derived from the coordinate system rotation method is: where γ, θ, and ψ represent the roll angle, pitch angle, and heading angle, respectively, represents the coordinate transformation matrix from the b-frame to the n-frame, T 11 , T 12 , T 13 , T 21 , T 22 , T 23 , T 31 , T 32 , T 33 are the abbreviations of the expressions corresponding to the elements in the matrix respectively; At the same time, the transformation matrix from the vehicle coordinate system to the navigation coordinate system represented by the quaternion is recorded as: Wherein, q0, q1, q2, and q3 are the four real number elements of the quaternion Q = q0 + q1i0 + q2j0 + q3k0, satisfying the relational expression: i0, j0, and k0 respectively represent unit vectors along the three-dimensional coordinate system; The transformation matrices expressed in the two ways are equal. By combining the two matrices, the four elements of the initial quaternion can be obtained as follows: Where sign is the sign function, which outputs +1 when the input is a positive number and -1 when the input is a negative number.

5. A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment according to claim 1, characterized in that, In step 5, the attitude change quaternion is constructed by the equivalent rotation vector constructed above, and the relationship between the two is: Wherein, Φ is the constructed equivalent rotation vector, and Φ is the modulus of the equivalent rotation vector, and its value is: Φ=|Φ|.

6. A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment according to claim 1, characterized in that, In step 6, the quaternion is updated according to the following steps: Within an attitude update period, Equation is simplified to: Wherein, Q(t k+1 ) and Q(t k ) are respectively called the attitude quaternions at time t k+1 and time t k , and p * (h) represents the conjugate of the coordinate transformation matrix of the navigation coordinate system from time t k to time t k+1 ; Then the quaternion Q(t k+1 ) of the next attitude update period is updated by the quaternion product of the quaternion Q(t k ) of the previous attitude update period and the attitude change quaternion q(h) within two update period time intervals.

7. A method for attitude calculation of a strapdown inertial navigation system in a high-dynamic environment according to claim 1, characterized in that, In step 7, the attitude angle is updated according to the following steps: The coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system can be expressed by the quaternion method or by the coordinate system rotation method. The matrices expressed by the two are equal, so the corresponding relationship between the attitude angle and the quaternion is: θ=asin(2(q2q3+q0q1)) q0, q1, q2, and q3 are four real elements of the quaternion Q=q0+q1i0+q2j0+q3k0, and γ, θ, and ψ represent the roll angle, pitch angle, and heading angle, respectively.

Images