Inertial navigation unit rotation axis and rotation angle full attitude solution method

Abstract

The application discloses a kind of inertial navigation unit rotation axis and full attitude calculation method of rotation angle, comprising:θ x0 ,ξ y0 Andξ z0 Are solved;Determine the angular velocity of body relative navigation coordinate system of strapdown inertial system at t k Based on differential equation,θ k ,ξ xk ,ξ yk Andξ zk Are solved;According to θ k ,ξ xk ,ξ yk ,ξ zk Update calculation,θ k+1 ,ξ xk+1 ,ξ yk+1 Andξ zk+1 Are obtained;According to θ k+1 ,ξ xk+1 ,ξ yk+1 Andξ zk+1 , coordinate transformation matrix update is carried out, and velocity update and position update based on updated coordinate transformation matrix are carried out.The method disclosed in the application uses accelerometer and gyroscope output information as the input information of unit rotation axis and rotation angle kinematics equation, realizes the real-time update of coordinate transformation matrix of carrier relative navigation coordinate system by integral calculation, singular value does not appear in the solving process, solution is unique, ensure that the full attitude of body coordinate system relative navigation coordinate system, high-precision requirement.

Description

Technical Field

[0001] This invention belongs to the field of aviation and aerospace technology, and in particular relates to a method for calculating the full attitude of an inertial navigation unit axis and rotation angle. Background Technology

[0002] Inertial navigation is widely used in missiles, aircraft, ships, and weapons, primarily to determine the position, velocity, and attitude of a vehicle relative to the navigation system in real time. Strapdown inertial systems are directly fixed to the vehicle, using gyroscopes to measure angular velocity and mathematically calculating the coordinate transformation matrix of the vehicle's coordinate system relative to the navigation system.

[0003] Currently, methods for determining attitude information include direction cosine kinematics, Euler-Krylov angular kinematics, and quaternion kinematics.

[0004] Quaternion solutions are frequently used in engineering. The unit quaternion is:

[0005]

[0006] In the formula, λ, ρ1, ρ2 and ρ3 are the four elements of the quaternion; and It is the imaginary unit vector and one of the three bases of quaternions.

[0007] Let the angular velocity ω′ of the carrier be expressed in the moving frame as:

[0008]

[0009] The quaternion kinematic equations are:

[0010]

[0011] Its vector form is:

[0012]

[0013] As can be seen from the above formula, the advantage of quaternion solving is that there are only four parameters and their differential equations. However, since these four parameters are only intermediate variables describing the motion of the carrier, they need to be solved to obtain the unit rotation axis of the carrier relative to the navigation coordinate system. Rotation around the unit axis The rotation angle θ. When further visually describing the motion of the carrier, for example, describing... exist The component ξ of the axis x ξ y and ξ z When this is done, the following formula needs to be used for comparison and solution:

[0014]

[0015] The problem is that when the rotation angle θ=±2arccosλ that satisfies this expression is distributed in different intervals, there are two possible combinations.

[0016] To ensure the uniqueness of the solution process, a new method for calculating the carrier coordinate system relative to the navigation system needs to be studied to improve the performance of inertial navigation and flight control. Summary of the Invention

[0017] The technical problem solved by this invention is to overcome the shortcomings of the prior art and provide a full attitude calculation method for the unit rotation axis and rotation angle of an inertial navigation system. The method uses the information output by the accelerometer and gyroscope orthogonally mounted on the strapdown inertial system body as the input information for the kinematic equations of the unit rotation axis and rotation angle. Through integral calculation, the coordinate transformation matrix of the carrier relative to the navigation coordinate system is updated in real time. No singular values ​​appear in the calculation process, and the solution result is unique, thus ensuring the full attitude and high precision requirements of the body coordinate system relative to the navigation coordinate system.

[0018] To address the aforementioned technical problems, this invention discloses a full attitude calculation method for inertial navigation unit axis of rotation and rotation angle, comprising:

[0019] Based on the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, θ and ξ are calculated. x ξ y and ξ z Initial values ​​of the four parameters at time t0: θ0, ξ x0 ξ y0 and ξ z0 Where θ represents the rotation of the body coordinate system about the unit axis. The rotation angle, ξ x ξ y and ξ z They represent the unit axis of rotation. Components of the X, Y, and Z axes in the navigation coordinate system; unit axis of rotation and and These are the three basis vectors of the quaternion;

[0020] According to the t output by the gyroscope k The angular velocity at time t is used to determine t. k Angular velocity of the strapdown inertial system relative to the navigation coordinate system at any time

[0021] Based on the differential equation, θ and ξ are obtained by solving it. x ξ y and ξ z Four parameters in t kThe value at time: θ k ξ xk ξ yk and ξ zk ;

[0022] According to θ k ξ xk ξ yk ξ zk and Perform an update calculation to obtain θ and ξ. x ξ y and ξ z The updated values ​​of the four parameters: θ k+1 ξ xk+1 ξ yk+1 and ξ zk+1 ;

[0023] According to θ k+1 ξ xk+1 ξ yk+1 and ξ zk+1 Perform coordinate transformation matrix update to obtain the updated coordinate transformation matrix.

[0024] Based on the updated coordinate transformation matrix Perform speed and location updates.

[0025] In the above-mentioned full attitude calculation method for the unit rotation axis and rotation angle of inertial navigation, there are three accelerometers, which are orthogonally mounted on the strapdown inertial system body; there are also three gyroscopes, which are orthogonally mounted on the strapdown inertial system body.

[0026] In the above-mentioned full attitude calculation method for unit rotation axis and rotation angle of inertial navigation, the coordinate system corresponding to the strapdown inertial system is the body coordinate system O-X'Y'Z', denoted as the b system, which is the moving system; the coordinate system describing the rotational motion of the carrier is the navigation coordinate system O-XYZ, denoted as the p system, which is the fixed system; the origins of the body coordinate system and the navigation coordinate system coincide.

[0027] In the above-mentioned full attitude calculation method for the unit axis of rotation and rotation angle of inertial navigation, θ0 and ξ are obtained by solving using the following formulas. x0 ξ y0 and ξ z0 :

[0028]

[0029] Among them, a x0 a y0 and a z0 ω represents the apparent acceleration in the X', Y', and Z' directions output by the three accelerometers at time t0. x0 ωy0 and ω z0 ω represents the angular velocities in the X, Y, and Z directions output by the three gyroscopes at time t0. ie φ represents the Earth's rotational speed, g represents gravitational acceleration, and φ represents the local latitude.

[0030] In the above-mentioned inertial navigation unit axis and rotation angle full attitude calculation method, Where, ω xk ω yk and ω zk They represent t respectively k The angular velocities in the X', Y' and Z' directions output by the three gyroscopes at any given time.

[0031] In the above-mentioned full attitude calculation method for the unit axis of rotation and rotation angle of inertial navigation, the strapdown navigation system at t k The quaternion q, which describes time in terms of four parameters, is represented as follows:

[0032]

[0033] in, It is ξ xk ξ yk and ξ zk vector form,

[0034] In the above-mentioned inertial navigation unit axis and rotation angle full attitude calculation method, θ is obtained by updating the calculation using the following formula. k+1 ξ xk+1 ξ yk+1 and ξ zk+1 :

[0035]

[0036] Where ΔT represents the update period, and Δθ′ represents the angle increment per unit time. express An antisymmetric matrix.

[0037] In the above-mentioned inertial navigation unit axis and rotation angle full attitude calculation method,

[0038] In the above-mentioned inertial navigation unit axis and rotation angle full attitude calculation method, the updated coordinate transformation matrix It is expressed as follows:

[0039]

[0040] In the above-mentioned full attitude calculation method for inertial navigation unit axis and rotation angle, based on the updated coordinate transformation matrix... Perform speed and location updates, including:

[0041] Updated coordinate transformation matrix and the apparent acceleration vector and gravitational acceleration vector As the velocity differential equation The input is integrated and the updated velocity V is obtained.

[0042] The updated velocity V is used as the position differential equation. The input is integrated to obtain the updated position r.

[0043] The present invention has the following advantages:

[0044] (1) This invention discloses a full attitude calculation method for the unit rotation axis and rotation angle of inertial navigation. Compared with the traditional method of solving four angles by quaternions, it has the advantage of uniqueness and overcomes the problem of multiple solutions.

[0045] (2) This invention discloses a full attitude calculation method for the unit rotation axis and rotation angle of inertial navigation. The calculation process has no singularities and can achieve high-precision full attitude calculation.

[0046] (3) This invention discloses a full attitude calculation method for unit rotation axis and rotation angle of inertial navigation. The updated equation structure is simple, which overcomes the shortcomings of nonlinear differential equations that are not easy to solve analytically and is easy to implement in engineering. Attached Figure Description

[0047] Figure 1 This is a flowchart illustrating the steps of a full attitude calculation method for an inertial navigation unit axis and rotation angle in an embodiment of the present invention.

[0048] Figure 2 This is a schematic diagram illustrating the relationship between the body coordinate system and the navigation coordinate system in an embodiment of the present invention;

[0049] Figure 3 This is a schematic diagram of the rotation angle and three components of the unit axis of rotation calculated based on the method described in this invention in an embodiment of the present invention;

[0050] Figure 4 This is a schematic diagram of a three-dimensional trajectory of an aircraft obtained by the method described in this invention, as shown in an embodiment of the invention. Detailed Implementation

[0051] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments disclosed in the present invention will be described in further detail below with reference to the accompanying drawings.

[0052] The invention discloses a full attitude calculation method for a unit rotation axis and rotation angle in inertial navigation, based on a strapdown inertial system fixed to the carrier. Three accelerometers and three gyroscopes are orthogonally mounted on the strapdown inertial system body. The coordinate system corresponding to the strapdown inertial system is the body coordinate system O-X'Y'Z', denoted as the b system, which is the moving frame; the coordinate system describing the carrier's rotational motion is the navigation coordinate system O-XYZ, denoted as the p system, which is the fixed frame; the origins of the body coordinate system and the navigation coordinate system coincide, as shown below. Figure 2 As shown. During the rotation, the angular velocity of the strapdown inertial system relative to the navigation coordinate system is...

[0053] like Figure 1 As shown, in this embodiment, the full attitude calculation method for the inertial navigation unit axis and rotation angle includes:

[0054] Step 1: Based on the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, calculate θ and ξ. x ξ y and ξ z Initial values ​​of the four parameters at time t0: θ0, ξ x0 ξ y0 and ξ z0 .

[0055] In this embodiment, θ0, ξ x0 ξ y0 and ξ z0 This can be obtained under static base conditions, and the specific calculation formula is as follows:

[0056]

[0057] Where θ represents the rotation of the body coordinate system about the unit axis. The rotation angle, ξ x ξ y and ξ z They represent the unit axis of rotation. Components of the X, Y, and Z axes in the navigation coordinate system; a x0 a y0 and a z0 ω represents the apparent acceleration in the X', Y', and Z' directions output by the three accelerometers at time t0. x0 ω y0 and ω z0 ω represents the angular velocities in the X, Y, and Z directions output by the three gyroscopes at time t0. ie The value represents the Earth's rotational speed, g represents the acceleration due to gravity, and φ represents the local latitude. (Unit axis of rotation) and and These are the three basis vectors of a quaternion.

[0058] Step 2, based on the t output by the gyroscope k The angular velocity at time t is used to determine t. k Angular velocity of the strapdown inertial system relative to the navigation coordinate system at any time

[0059] In this embodiment, Where, ω xk ω yk and ω zk They represent t respectively k The angular velocities in the X', Y' and Z' directions output by the three gyroscopes at any given time.

[0060] Step 3: Based on the differential equation, solve for θ and ξ. x ξ y and ξ z Four parameters in t k The value at time: θ k ξ xk ξ yk and ξ zk .

[0061] In this embodiment, θ, ξ x ξ y and ξ z The differential equations underlying the updates of the four parameters are as follows:

[0062]

[0063] Where, ω x ω y and ω z These represent the angular velocities in the X, Y, and Z directions output by the three gyroscopes, respectively.

[0064] Preferably, the strapdown navigation system in t k The quaternion q, which describes time in terms of four parameters, is represented as follows:

[0065]

[0066] in, It is ξ xk ξ yk and ξ zk vector form,

[0067] Step 4, based on θ k ξ xk ξ yk ξ zk and Perform an update calculation to obtain θ and ξ. x ξ y and ξ z The updated values ​​of the four parameters: θ k+1 ξ xk+1 ξ yk+1 and ξ zk+1 .

[0068] In this embodiment, θ, ξ x ξ y and ξ z Four parameters in t k+1 =t k The optimal update equation at time +ΔT is as follows:

[0069]

[0070] Where ΔT represents the update period; Δθ′ represents the angle increment per unit time. express An antisymmetric matrix.

[0071] Step 5, based on θ k+1 ξ xk+1 ξ yk+1 and ξ zk+1 Perform coordinate transformation matrix update to obtain the updated coordinate transformation matrix.

[0072] In this embodiment, the updated coordinate transformation matrix It is expressed as follows:

[0073]

[0074] Step 6, based on the updated coordinate transformation matrix Perform speed and location updates.

[0075] In this embodiment, the updated coordinate transformation matrix is... and the apparent acceleration vector and gravitational acceleration vector As the velocity differential equation The input is integrated to obtain the updated velocity V; further, the updated velocity V is used as the position differential equation. The input is integrated to obtain the updated position r.

[0076] The inertial navigation unit axis and rotation angle full attitude calculation method described in this invention is used to perform navigation calculation on the data of an aircraft during a certain full attitude motion ("Full Attitude Navigation Calculation Method Based on Extended Krylov Angles", Chinese Journal of Inertial Technology, Vol. 29 No. 5, 2021). The calculated rotation angle and unit axis are as follows: Figure 3 As shown, the three-dimensional trajectory of the aircraft calculated by navigation is as follows: Figure 4 As shown, the large-attitude, high-maneuverability motion process of the aircraft is reproduced very well, indicating that the full attitude calculation method of the inertial navigation unit axis and rotation angle described in this invention can achieve full attitude calculation, which is beneficial to flight control.

[0077] Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make possible changes and modifications to the technical solutions of the present invention by utilizing the methods and techniques disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the protection scope of the technical solutions of the present invention.

[0078] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A method for calculating the full attitude of an inertial navigation unit axis and rotation angle, characterized in that, include: Based on the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, θ and ξ are calculated. x ξ y and ξ z Initial values ​​of the four parameters at time t0: θ0, ξ x0 ξ y0 and ξ z0 Where θ represents the rotation of the body coordinate system about the unit axis. The rotation angle, ξ x ξ y and ξ z They represent the unit axis of rotation. Components of the X, Y, and Z axes in the navigation coordinate system; unit axis of rotation and and The three basis vectors are quaternions; there are three accelerometers, orthogonally mounted on the strapdown inertial system body; there are three gyroscopes, orthogonally mounted on the strapdown inertial system body; the coordinate system corresponding to the strapdown inertial system is the body coordinate system O-X'Y'Z', denoted as the b system, which is the moving frame; the coordinate system describing the rotational motion of the carrier is the navigation coordinate system O-XYZ, denoted as the p system, which is the fixed frame; the origins of the body coordinate system and the navigation coordinate system coincide. According to the t output by the gyroscope k The angular velocity at time t determines t. k Angular velocity of the strapdown inertial system relative to the navigation coordinate system at any time Based on the differential equation, θ and ξ are obtained by solving it. x ξ y and ξ z Four parameters in t k The value at time: θ k ξ xk ξ yk and ξ zk ; According to θ k ξ xk ξ yk ξ zk and Perform an update calculation to obtain θ and ξ. x ξ y and ξ z The updated values ​​of the four parameters: θ k+1 ξ xk+1 ξ yk+1 and ξ zk+1 ; According to θ k+1 ξ xk+1 ξ yk+1 and ξ zk+1 Perform coordinate transformation matrix update to obtain the updated coordinate transformation matrix. Based on the updated coordinate transformation matrix Perform speed and location updates; θ0 and ξ are obtained by solving the following formula. x0 ξ y0 and ξ z0 : Among them, a x0 a y0 and a z0 ω represents the apparent acceleration in the X', Y', and Z' directions output by the three accelerometers at time t0. x0 ω y0 and ω z0 ω represents the angular velocities in the X, Y, and Z directions output by the three gyroscopes at time t0. ie φ represents the Earth's rotational speed, g represents gravitational acceleration, and φ represents the local latitude.

2. The method for calculating the full attitude of an inertial navigation unit axis and rotation angle according to claim 1, characterized in that, Where, ω xk ω yk and ω zk They represent t respectively k The angular velocities in the X', Y' and Z' directions output by the three gyroscopes at any given time.

3. The method for calculating the full attitude of an inertial navigation unit axis and rotation angle according to claim 2, characterized in that, Strapdown navigation system in t k The quaternion q, which describes time in terms of four parameters, is represented as follows: in, It is ξ xk ξ yk and ξ zk The vector form, 4. The method for calculating the full attitude of an inertial navigation unit axis and rotation angle according to claim 3, characterized in that, θ is obtained by updating the solution using the following formula. k+1 ξ xk+1 ξ yk+1 and ξ zk+1 : Where ΔT represents the update period, and Δθ′ represents the angle increment per unit time. express An antisymmetric matrix.

5. The method for calculating the full attitude of an inertial navigation unit axis and rotation angle according to claim 4, characterized in that, 6. The method for calculating the full attitude of an inertial navigation unit axis and rotation angle according to claim 1, characterized in that, Updated coordinate transformation matrix It is expressed as follows:

7. The method for calculating the full attitude of an inertial navigation unit axis and rotation angle according to claim 6, characterized in that, Based on the updated coordinate transformation matrix Perform speed and location updates, including: Updated coordinate transformation matrix and the apparent acceleration vector and gravitational acceleration vector As the velocity differential equation The input is integrated and the updated velocity V is obtained. The updated velocity V is used as the position differential equation. The input is integrated to obtain the updated position r.

Images