CN116380078A - Gesture resolving method of strapdown inertial navigation system in high dynamic environment - Google Patents

Abstract

The invention discloses a gesture resolving method of a strapdown inertial navigation system in a high dynamic environment, and belongs to the technical field of inertial navigation. The invention comprises the following steps: collecting original data; calculating an initial attitude angle; calculating an initial quaternion; constructing an equivalent rotation vector; constructing a posture change quaternion; performing quaternion updating; and carrying out attitude angle updating. Unlike the conventional equivalent rotation vector algorithm, which assumes that the angular velocity of the carrier motion is several polynomials, the present invention assumes that the angular velocity of the carrier motion can be represented by the sum of trigonometric functions of different frequencies, on the basis of which the equivalent rotation vector represented by the angular increment and the angular velocity correlation coefficient is derived, and then designs a posture resolving method based on the equivalent rotation vector. The method provided by the invention improves the gesture resolving precision in the high dynamic environment, and provides a new thought for solving the high-precision gesture resolving in the high dynamic environment.

Description

Gesture resolving method of strapdown inertial navigation system in high dynamic environment

Technical Field

The invention belongs to the technical field of inertial navigation, and particularly relates to a gesture resolving method of a strapdown inertial navigation system in a high dynamic environment.

Background

The strapdown inertial navigation system (Strapdown Inertial Navigation System, SINS) has no physical platform of the platform type inertial navigation system, but instead is a mathematical platform built with an inertial measurement unit (Inertial Measurement Unit, IMU) as a core. The IMU integrates inertial sensors and a micro-control unit, typically attached directly to the carrier, to sense acceleration information and angular velocity information of the carrier. The SINS utilizes carrier motion information perceived by the IMU to realize navigation and positioning through three groups of resolving algorithms, namely gesture resolving, speed resolving and position resolving. The gesture resolving is used as the core of the whole navigation algorithm, and the resolving precision directly influences the precision of the strapdown inertial navigation system.

The traditional attitude calculation algorithm comprises an Euler angle method, a direction cosine method, a quaternion method and an equivalent rotation vector method. In a low dynamic motion environment, a generally common method is a quaternion method, and in a high dynamic environment, severe linear motion and angular motion can cause the non-interchangeability error generated by vector integration to further become larger, so that the quaternion method is not applicable any more. Common high dynamic environments include, but are not limited to, high angular rate maneuver environments, angular vibration environments, and the like. The equivalent rotation vector method can realize the compensation of the non-exchangeable error by virtue of the principle characteristics, and the compensation degree is related to the construction of the equivalent rotation vector.

Since Bortz proposed to use the equivalent rotation vector method to compensate for the non-interchangeability errors in high dynamic environments, scholars at home and abroad have designed many optimization algorithms around this method. The method has the starting point that the data volume of the gyroscope in one gesture resolving period is improved so as to construct a rotation vector which is closer to the actual situation, thereby achieving the purpose of improving the accuracy of the algorithm. The invention develops a new way, and different from the condition that the traditional equivalent rotation vector algorithm assumes that the carrier movement angular velocity is a polynomial of several times, the invention assumes that the carrier movement angular velocity can be represented by the sum of trigonometric functions of different frequencies, and provides an improved equivalent rotation vector algorithm suitable for a high dynamic environment based on the sum.

Through retrieval, patent CN110879066A, a gesture resolving algorithm, a gesture resolving device and a vehicle-mounted inertial navigation system. The algorithm structure of the patent is similar to that of the patent, but the following differences are deeply studied: 1. the main acting object of the CN110879066A algorithm is a vehicle-mounted inertial navigation system, and the method aims at the strapdown inertial navigation system in a high dynamic environment; 2. the patent CN110879066a is essentially a conventional algorithm derived on the premise of the angular velocity of the carrier motion as a polynomial, and the algorithm is derived herein on the premise of the angular velocity of the carrier motion as a trigonometric function, which is the biggest difference between the two. The algorithm derived on the assumption here has the advantage of higher accuracy under the same subsampled conditions, which depends on the unique assumption here of the angular velocity of the carrier motion.

Disclosure of Invention

The present invention is directed to solving the above problems of the prior art. A gesture resolving method of a strapdown inertial navigation system in a high dynamic environment is provided. The technical scheme of the invention is as follows:

a strapdown inertial navigation system gesture resolving method in a high dynamic environment comprises the following steps:

step 1: collecting original data of a strapdown inertial navigation system;

step 2: calculating an initial attitude angle;

step 3: calculating an initial quaternion;

step 4: constructing an equivalent rotation vector;

step 5: constructing a posture change quaternion;

step 6: performing quaternion updating;

step 7: and carrying out attitude angle updating.

Further, in the step 1, the method for collecting the original gesture data of the strapdown inertial navigation system specifically includes: the angular speed and acceleration of the foot change violently when the pedestrian moves, which is a typical high dynamic environment, so the IMU is arranged at the heel of the pedestrian, and the gesture calculation adopts a coordinate system as follows: the IMU installation coordinate system is a right front upper coordinate system; the inertial system navigation coordinate system is the northeast day coordinate system.

Further, the calculating the initial attitude angle in the step 2 specifically includes:

pitch angle θ in initial attitude angle ₀ And roll angle gamma ₀ The accelerometer data in the first set of attitude updating periods are calculated, and the calculation formula is as follows:

wherein atan is an arctangent function, and the calculated result range is [ -pi/2, pi/2]Atan2 is a four-quadrant arctangent function, and the calculated result range is [ -pi, pi]The unit of the initial attitude angle is radian, a _x 、a _y 、a _z And respectively representing the triaxial acceleration information under the carrier coordinate system.

The gesture update period refers to a time period consisting of two sampling points, namely gesture calculation is performed twice per sampling.

Further, the step 3 initial quaternion Q ₀ Calculated by the following way:

the transformation matrix derived from the coordinate system rotation mode from the carrier coordinate system (b system) to the navigation coordinate system (n system) is recorded as follows:

wherein, gamma, theta and phi respectively represent roll angle, pitch angle and course angle,

representing a coordinate transformation matrix from b-system to n-system, T ₁₁ 、T ₁₂ 、T ₁₃ 、T ₂₁ 、T ₂₂ 、T ₂₃ 、T ₃₁ 、T ₃₂ 、T ₃₃ Respectively is a matrix->

A shorthand for the formula of the corresponding element in (a);

meanwhile, the transformation matrix from the carrier coordinate system to the navigation coordinate system represented by the quaternion is recorded as follows:

wherein q is ₀ 、q ₁ 、q ₂ 、q ₃ For quaternion q=q ₀ +q ₁ i ₀ +q ₂ j ₀ +q ₃ k ₀ Is a real number, satisfying the relation:

i ₀ 、j ₀ 、k ₀ respectively representing unit vectors along a three-dimensional coordinate system;

the transformation matrixes represented by the two modes are equal, the two matrixes are combined, and four elements of the initial quaternion can be obtained as follows:

where sign is a sign function, and +1 is output when the input is positive and-1 is output when the input is negative.

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

step 4.1: assuming the angular velocity of motion as the sum of trigonometric functions;

step 4.2: calculating an angle increment by integrating the inner angle speed in the updating period;

step 4.3: solving each derivative of the angular velocity and the zero moment of the angular increment;

step 4.4: expressing the equivalent rotation vector in terms of angular velocity and angular increment;

step 4.5: solving each derivative of the equivalent rotation vector at zero time;

step 4.6: solving the taylor expansion of the equivalent rotation vector at the zero moment.

Further, in the step 5, the attitude change quaternion is constructed by the constructed equivalent rotation vector, and the relationship between the attitude change quaternion and the constructed equivalent rotation vector is:

wherein, phi is the equivalent rotation vector constructed by the above, phi is the equivalent rotation vector modulus value, and the value is: Φ= |Φ|.

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

in one attitude update period

The simplification is as follows:

wherein Q (t) _k+1 )、Q(t _k ) Respectively referred to as t _k+1 Time sum t _k Attitude quaternion of moment, p ^* (h) Representing the navigation coordinate system from t _k From time to t _k+1 Conjugation of the coordinate transformation matrix at the moment.

The quaternion Q (t) of the next attitude update period _k+1 ) Quaternion Q (t) by the last attitude update period _k ) And the quaternion product update of the attitude change quaternion q (h) in the two updating period time periods.

Further, in the 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 represented by a quaternion method or a coordinate system rotation method, and the matrix represented by the carrier coordinate system and the navigation coordinate system are equal, so that the corresponding relation between the attitude angle and the quaternion is as follows:

θ＝asin(2(q ₂ q ₃ +q ₀ q ₁ ))

q ₀ 、q ₁ 、q ₂ 、q ₃ for quaternion q=q ₀ +q ₁ i ₀ +q ₂ j ₀ +q ₃ k ₀ And gamma, theta, and psi respectively represent roll angle, pitch angle, and heading angle.

The invention has the advantages and beneficial effects as follows:

according to the invention, the equivalent rotation vectors are constructed in different modes, so that the accuracy of the gesture calculation in the high dynamic environment is improved, and a new thought is provided for solving the problem of high-accuracy gesture calculation in the angular vibration environment.

In particular, in claim 5, the invention derives the equivalent rotation vector by assuming the carrier movement angular velocity to be the sum of trigonometric functions of different frequencies, and the algorithm derived on the premise of the assumption provides a new thought for the gesture calculation of the strapdown inertial navigation system in sinusoidal vibration, which is beneficial to improving the gesture calculation precision. Meanwhile, conical motion is generally used as the most complex motion environment for gesture calculation, and the high-dynamic environment generally comprises conical motion and vibration environments, so that the method provided by the invention can be beneficial to improving the gesture calculation precision under the high-dynamic environment.

These benefits all result from the algorithmic derivation preconditions unique herein: on the premise that the carrier motion angular velocity is the sum of trigonometric functions, if the angular increment is assumed to be equal to the rotation vector modulus value, the error of the Bortz equation solution only comes from the approximation error of the angular increment equal to the rotation vector modulus value and the Taylor expansion truncation error of the rotation vector, and the error caused by the fact that the conventional Picard iteration solution ignores the high-order term does not exist. This assumption is also critical to this document, and is the biggest difference from conventional algorithms.

Drawings

FIG. 1 is a general flow chart of a method for resolving the attitude of a strapdown inertial navigation system in a high dynamic environment according to a preferred embodiment of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and specifically described below with reference to the drawings in the embodiments of the present invention. The described embodiments are only a few embodiments of the present invention.

The technical scheme for solving the technical problems is as follows:

as shown in fig. 1, the embodiment provides a method for resolving the gesture of a strapdown inertial navigation system in a high dynamic environment, which includes the following steps:

step 1: collecting original data;

step 2: calculating an initial attitude angle;

step 3: calculating an initial quaternion;

step 4: constructing an equivalent rotation vector;

step 5: constructing a posture change quaternion;

step 6: performing quaternion updating;

step 7: and carrying out attitude angle updating.

In this embodiment, the original data acquisition mode is: the IMU is mounted at the heel of a pedestrian.

In this embodiment, in step 1, the IMU is mounted on the carrier, and the acceleration information and the angular velocity information of the carrier motion are acquired through an inertial sensor inside the IMU, where the acceleration information is obtained by an accelerometer, and the angular velocity information is obtained by a gyroscope.

In this embodiment, in the step 1, the IMU installation coordinate system is the upper right coordinate system, i.e. the X of the IMU _b The axis is directed to the right side of the carrier, Y _b The axis pointing forward of the carrier, Z _b The axis points to the carrier in the vertical direction; the inertial system navigation coordinate system is northeast coordinate system, namely X of the navigation coordinate system _n The axis points to the eastern direction of geography, Y _n The axis points to the geographic north direction, Z _n The axis points to the geographic direction.

In the present embodiment, in the step 2, the initial heading angle ψ ₀ The absolute position is obtained by navigation through calculation by magnetometer data, and the invention focuses on gesture calculation, so that the algorithm effect can be analyzed by setting the initial course angle to 0; pitch angle θ in initial attitude angle ₀ And roll angle gamma ₀ The accelerometer data in the first set of attitude update periods are calculated in the following way:

wherein atan is an arctangent function, and the calculated result range is [ -pi/2, pi/2]Atan2 is a four-quadrant arctangent function, and the calculated result range is [ -pi, pi]，a _x 、a _y 、a _z And respectively representing the triaxial acceleration information under the carrier coordinate system. Initial initiationThe unit of attitude angle is radian.

In this embodiment, in the step 2, one gesture update period refers to a time period consisting of two sampling points, that is, gesture calculation is performed twice per sampling.

In this embodiment, in the step 3, the initial quaternion Q ₀ Calculated by the following way:

the transformation matrix derived from the coordinate system rotation mode from the carrier coordinate system (b system) to the navigation coordinate system (n system) is recorded as follows:

wherein, gamma, theta and phi respectively represent roll angle, pitch angle and course angle,

representing a coordinate transformation matrix from b-system to n-system, T ₁₁ 、T ₁₂ 、T ₁₃ 、T ₂₁ 、T ₂₂ 、T ₂₃ 、T ₃₁ 、T ₃₂ 、T ₃₃ Respectively is a matrix->

A shorthand for the formula of the corresponding element in (a);

meanwhile, the transformation matrix from the carrier coordinate system to the navigation coordinate system represented by the quaternion is recorded as follows:

wherein q is ₀ 、q ₁ 、q ₂ 、q ₃ For quaternion q=q ₀ +q ₁ i ₀ +q ₂ j ₀ +q ₃ k ₀ Is a real number, satisfying the relation:

i ₀ 、j ₀ 、k ₀ each representing a unit vector along a three-dimensional coordinate system.

The transformation matrixes represented by the two modes are equal, the two matrixes are combined, and four elements of the initial quaternion can be obtained as follows:

where sign is a sign function, and +1 is output when the input is positive and-1 is output when the input is negative.

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

step 4.1: assuming the angular velocity of motion as the sum of trigonometric functions;

step 4.2: calculating an angle increment by integrating the inner angle speed in the updating period;

step 4.3: solving each derivative of the angular velocity and the zero moment of the angular increment;

step 4.4: expressing the equivalent rotation vector in terms of angular velocity and angular increment;

step 4.5: solving each derivative of the equivalent rotation vector at zero time;

step 4.6: solving the taylor expansion of the equivalent rotation vector at the zero moment.

In this embodiment, in the step 4.1, it is assumed that the angular velocity of the carrier motion can be represented by a sum of trigonometric functions of different frequencies, which is in the form of: omega (t) =k ₁ sint+k ₂ sin2t+k ₃ sin3t+ …, and updating sampling points in the period according to the gesturesThe different numbers are chosen to accommodate the angular velocity function. In the invention, the carrier motion angular velocity is assumed to be: ω (t) =asin+bsn2t, a, b are vector coefficients related to angular velocity.

In this embodiment, in the step 4.2, the angular increment is an integral of the angular velocity in one attitude update period:

in this embodiment, in the step 4.3, the angular velocity and the angular increment are each derived as follows at the point t=0:

ω(0)＝(asint+bsin2t)| _t＝0 ＝0

ω ⁽⁴⁾ (0)＝(asint+2 ⁴ bsin2t)| _t＝0 ＝0

ω ⁽⁵⁾ (0)＝(acost+2 ⁵ bcos2t)| _t＝0 ＝a+2 ⁵ b

Δθ ⁽⁵⁾ (0)＝ω ⁽⁴⁾ (t)| _t＝0 ＝0

……

in this embodiment, in the step 4.4, the engineering general approximate Bortz equation is:

since the attitude update period is in the order of milliseconds, the method approximates the equation of Bortz

This term is considered as a second order small amount of the equivalent rotation vector Φ and is ignored, and the equivalent rotation vector is replaced with an angular increment, namely: Φ≡Δθ. Thus (S)>

In this embodiment, in the step 4.5, each derivative of the equivalent rotation vector at t=0 is calculated, and the corresponding values of the angular velocity and the angular increment are substituted, so that it is obtained:

……

in the embodiment, in the step 4.6, Φ (h) is set as the equivalent rotation vector in one posture updating period h, and h=t _k+1 -t _k ，t _k+1 、t _k Respectively representing the next pose update period and the current pose update period. Seven-order taylor expansion is performed on phi (h):

wherein Φ (0) is [ t ] _k ,t _k ]Equivalent rotation vector in the period, Φ (0) =0 since the time interval is 0.

Substituting the derivatives of the equivalent rotation vector at t=0 yields:

in order to represent the parameters a, b in the above formula by two angular increments in the attitude update period, note that

Then two angular increments Δθ within the attitude update period ₁ 、Δθ ₂ The method comprises the following steps:

then, the sum of the angular increments in the attitude update period is:

according to the taylor expansion of the cosine function, the above can be written as:

comparing the taylor expansion of the sum of the equivalent rotation vector and the angular increment yields:

in the formula, delta theta ₁ 、Δθ ₂ For the product of the angular velocity obtained by twice sampling in the attitude updating period and the sampling time, a and b are formed by the angular increment delta theta ₁ 、Δθ ₂ The expression simultaneous solution of (2):

in this embodiment, in the step 5, the attitude change quaternion is configured as follows:

let t be _k The carrier coordinate system of time is b (k), the navigation coordinate system is n (k), t _k+1 The carrier coordinate system at the moment is b (k+1), and the navigation coordinate system is n (k+1). Let b (k) to b (k+1) denote Q (h), and n (k) to b (k) denote Q (t) _k ) The rotation quaternion of n (k+1) to b (k+1) is Q (t) _k+1 ) The rotation quaternion of n (k) to n (k+1) is p (h), then r ^{n (k+1)} Expressed in terms of a transformation matrix as:

wherein r is ^n(k+1) 、r ^b(k+1) Respectively t _k+1 A time navigation coordinate system and a rotation vector under a carrier coordinate system.

The quaternion representation of the vector coordinate transformation is equivalent to the transformation matrix representation, i.e.

Wherein r is ¹ 、r ² Is a rotation vector in a coordinate system 1 and a coordinate system 2, Q is a rotation quaternion from a 2-system to a 1-system, and Q ^* Is the conjugation of Q.

Therefore, r ^n(k+1) Equivalent to the quaternion multiplication expression:

wherein p is ^* (h) Representing the navigation coordinate system from t _k From time to t _k+1 Conjugation of the coordinate transformation matrix at the moment.

According to the quaternion multiplication combination law, r ^n(k+1) The quaternion multiplication expression can be written as:

comparing the two formulas, the following can be obtained:

in the formula, q (h) is referred to as [ t ] for convenience of distinction _k ,t _k+1 ]The pose changes by a quaternion over a period of time,

Φ is the equivalent rotation vector of b (k) to b (k+1), Φ= |Φ|.

In this embodiment, in the step 6, the quaternion is updated according to the following steps:

since the change of the navigation coordinate system is very slow in one posture updating period, p (h) ≡1+0, the formula

The simplification is as follows:

wherein Q (t) _k+1 )、Q(t _k ) Respectively referred to as t _k+1 Time sum t _k Attitude quaternion of time.

The quaternion Q (t) of the next attitude update period _k+1 ) Quaternion Q (t) by the last attitude update period _k ) And the quaternion product update of the attitude change quaternion q (h) in the two updating period time periods.

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

the coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system can be represented by a quaternion method or a coordinate system rotation method, and the matrix represented by the carrier coordinate system and the navigation coordinate system are equal, so that the corresponding relation between the attitude angle and the quaternion is as follows:

θ＝asin(2(q ₂ q ₃ +q ₀ q ₁ ))

the system, apparatus, module or unit set forth in the above embodiments may be implemented in particular by a computer chip or entity, or by a product having a certain function.

It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article or apparatus that comprises the element.

The above examples should be understood as illustrative only and not limiting the scope of the invention. Various changes and modifications to the present invention may be made by one skilled in the art after reading the teachings herein, and such equivalent changes and modifications are intended to fall within the scope of the invention as defined in the appended claims.

Claims (8)

1. The attitude resolving method of the strapdown inertial navigation system in the high dynamic environment is characterized by comprising the following steps of:

step 1: collecting original data of a strapdown inertial navigation system;

step 2: calculating an initial attitude angle;

step 3: calculating an initial quaternion;

step 4: constructing an equivalent rotation vector;

step 5: constructing a posture change quaternion;

step 6: performing quaternion updating;

step 7: and carrying out attitude angle updating.

2. The method for resolving the gesture of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein in the step 1, the method for collecting the original gesture data of the strapdown inertial navigation system is specifically as follows: the angular speed and acceleration of the foot change violently when the pedestrian moves, which is a typical high dynamic environment, so the IMU is arranged at the heel of the pedestrian, and the gesture calculation adopts a coordinate system as follows: the IMU installation coordinate system is a right front upper coordinate system; the inertial system navigation coordinate system is the northeast day coordinate system.

3. The method for resolving the attitude of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein the calculating the initial attitude angle in the step 2 specifically comprises:

pitch angle θ in initial attitude angle ₀ And roll angle gamma ₀ The accelerometer data in the first set of attitude updating periods are calculated, and the calculation formula is as follows:

wherein atan is an arctangent function, and the calculated result range is [ -pi 2, pi 2]Atan2 is a four-quadrant arctangent function, and the calculated result range is [ -pi, pi]The unit of the initial attitude angle is radian, a _x 、a _y 、a _z And respectively representing the triaxial acceleration information under the carrier coordinate system.

The gesture update period refers to a time period consisting of two sampling points, namely gesture calculation is performed twice per sampling.

4. The method for resolving the attitude of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein the initial quaternion Q in the step 3 ₀ Calculated by the following way:

the transformation matrix derived from the coordinate system rotation mode from the carrier coordinate system (b system) to the navigation coordinate system (n system) is recorded as follows:

wherein, gamma, theta and phi respectively represent roll angle, pitch angle and course angle, C _b ⁿ Representing a coordinate transformation matrix from b-system to n-system, T ₁₁ 、T ₁₂ 、T ₁₃ 、T ₂₁ 、T ₂₂ 、T ₂₃ 、T ₃₁ 、T ₃₂ 、T ₃₃ Respectively matrix C _b ⁿ A shorthand for the formula of the corresponding element in (a);

meanwhile, the transformation matrix from the carrier coordinate system to the navigation coordinate system represented by the quaternion is recorded as follows:

wherein q is ₀ 、q ₁ 、q ₂ 、q ₃ For quaternion q=q ₀ +q ₁ i ₀ +q ₂ j ₀ +q ₃ k ₀ Is a real number, satisfying the relation:

i ₀ 、j ₀ 、k ₀ respectively representing unit vectors along a three-dimensional coordinate system;

the transformation matrixes represented by the two modes are equal, the two matrixes are combined, and four elements of the initial quaternion can be obtained as follows:

where sign is a sign function, and +1 is output when the input is positive and-1 is output when the input is negative.

5. The method for resolving the attitude of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein in the step 4, the equivalent rotation vector is constructed by the steps of:

step 4.1: assuming the angular velocity of motion as the sum of trigonometric functions;

step 4.2: calculating an angle increment by integrating the inner angle speed in the updating period;

step 4.3: solving each derivative of the angular velocity and the zero moment of the angular increment;

step 4.4: expressing the equivalent rotation vector in terms of angular velocity and angular increment;

step 4.5: solving each derivative of the equivalent rotation vector at zero time;

step 4.6: solving the taylor expansion of the equivalent rotation vector at the zero moment.

6. The method for resolving the gesture of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein in the step 5, the gesture change quaternion is constructed by the constructed equivalent rotation vector, and the relationship between the gesture change quaternion and the constructed equivalent rotation vector is:

wherein, phi is the equivalent rotation vector constructed by the above, phi is the equivalent rotation vector modulus value, and the value is: Φ=Φ.

7. The method for resolving the gesture of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein in the step 6, the quaternion is updated according to the following steps:

in one attitude update period

The simplification is as follows:

wherein Q (t) _k+1 )、Q(t _k ) Respectively referred to as t _k+1 Time sum t _k Attitude quaternion of moment, p ^* (h) Representing the navigation coordinate system from t _k From time to t _k+1 Conjugation of the coordinate transformation matrix at the moment;

the quaternion Q (t) of the next attitude update period _k+1 ) Quaternion Q (t) by the last attitude update period _k ) And the quaternion product update of the attitude change quaternion q (h) in the two updating period time periods.

8. The method for resolving the attitude of the strapdown inertial navigation system in the high dynamic environment according to claim 1, wherein in the step 7, the attitude angle is updated as follows:

the coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system can be represented by a quaternion method or a coordinate system rotation method, and the matrix represented by the carrier coordinate system and the navigation coordinate system are equal, so that the corresponding relation between the attitude angle and the quaternion is as follows:

θ＝asin(2(q ₂ q ₃ +q ₀ q ₁ ))

q ₀ 、q ₁ 、q ₂ 、q ₃ for quaternion q=q ₀ +q ₁ i ₀ +q ₂ j ₀ +q ₃ k ₀ And gamma, theta, and psi respectively represent roll angle, pitch angle, and heading angle.

Images