All-attitude calculation method for inertial navigation unit rotation axis and rotation angle

Abstract

The invention discloses an all-attitude calculation method for an inertial navigation unit rotation axis and a rotation angle. The all-attitude calculation method comprises the following steps: calculating to obtain theta0, xix0, xiy0 and xiz0; determining the angular velocity # imgabs0 # of the body of the strapdown inertial system relative to the navigation coordinate system at the tk moment, and resolving to obtain thetak, xi xk, xi yk and xi zk based on a differential equation; according to the theta k, the Xixk, the Xiyk, the Xizk and the # imgabs1 #, carrying out updating calculation to obtain theta k + 1, Xixk + 1, Xiyk + 1 and Xizk + 1; and according to theta < k + 1 >, Xi < x > < k + 1 >, Xi < y > < k + 1 > and Xi < z > < k + 1 >, carrying out coordinate transformation matrix updating to obtain # imgabs < 2 >, and carrying out speed updating and position updating based on the updated coordinate transformation matrix # imgabs < 3 >. According to the method, output information of the accelerometer and the gyroscope serves as input information of a unit rotation axis and a rotation angle kinematics equation, real-time updating of a coordinate transformation matrix of a carrier relative to a navigation coordinate system is achieved through integral resolving, no singular value occurs in the resolving process, the resolving result is unique, and the method is suitable for large-scale popularization and application. And the requirements of all postures and high precision of the body coordinate system relative to the navigation coordinate system are ensured.

Description

Technical Field

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

[0002] Inertial navigation is widely used in missiles, aircraft, ships, weapons and other fields. Its main function is to determine the position, velocity and attitude information of the carrier relative to the navigation system in real time. The strapdown inertial system is directly connected to the carrier, and the angular velocity is measured by the gyroscope and the coordinate transformation matrix of the carrier coordinate system relative to the navigation system is obtained after mathematical solution.

[0003] Currently, the methods for determining attitude information include direction cosine kinematic equations, Euler-Krylov angle kinematic equations, and quaternion kinematic equations.

[0004] The quaternion solution method is widely used in engineering. The unit quaternion is:

[0005]

[0006] Where λ, ρ1, ρ2 and ρ3 are the four elements of the quaternion; and is the imaginary unit vector, which is the three bases of the quaternion.

[0007] Assume that the carrier angular velocity ω′ is expressed in the moving system as:

[0008]

[0009] Then the quaternion kinematic equation is:

[0010]

[0011] Its vector form is:

[0012]

[0013] From the above formula, we can see that the advantage of quaternion solution is that there are only four parameters and their differential equations. However, since the four parameters are only intermediate variables describing the motion of the carrier, it is necessary to solve them to obtain the unit axis of rotation of the carrier relative to the navigation coordinate system. Rotation around unit axis When further describing the motion of the carrier intuitively, for example, describing exist Axis component ξ x , y and z When , it is necessary to compare and solve according to the following formula:

[0014]

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

[0016] In order to ensure the uniqueness of the solution process, it is necessary to study a new solution method for the carrier coordinate system relative to the navigation system to improve the performance of inertial navigation and flight control. Summary of the invention

[0017] The invention solves the problem by technology: overcomes the shortcomings of the prior art, provides a method for solving the full attitude of an inertial navigation unit rotation axis and rotation angle, uses information output by an accelerometer and a gyroscope orthogonally installed on a strapdown inertial system body as input information of the kinematic equation of the unit rotation axis and rotation angle, realizes real-time updating of the coordinate transformation matrix of the carrier relative to the navigation coordinate system through integral solution, does not appear singular values ​​in the solution process, and the solution result is unique, thereby ensuring the full attitude and high precision requirements of the body coordinate system relative to the navigation coordinate system.

[0018] In order to solve the above technical problems, the present invention discloses a method for calculating the full attitude of the rotation axis and rotation angle of an inertial navigation unit, comprising:

[0019] According to the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, θ and ξ are calculated. x , y and z The initial values ​​of the four parameters at time t0: θ0, ξ x0 , y0 and z0 ; where θ represents the rotation axis of the body coordinate system around the unit axis The rotation angle, ξ x , y and z Represents the unit axis Components of the X-axis, Y-axis, and Z-axis in the navigation coordinate system; unit axis and and are the three basis vectors of the quaternion;

[0020] According to the gyroscope output t k The angular velocity at time t k The angular velocity of the strapdown inertial system relative to the navigation coordinate system at the moment

[0021] Based on the differential equation, we can solve θ and ξ x , y and z The four parameters are kThe value at the moment: θ k , xk , yk and zk ;

[0022] According to θ k , xk , yk , zk and Perform update calculation to obtain θ, ξ 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 , update the coordinate transformation matrix and obtain the updated coordinate transformation matrix

[0024] Based on the updated coordinate transformation matrix Perform velocity updates and position updates.

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

[0026] In the above-mentioned full attitude solution method of the rotation axis and rotation angle of the inertial navigation unit, the coordinate system corresponding to the strapdown inertial system is the body coordinate system O-X'Y'Z', denoted as 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 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 solution method of the inertial navigation unit rotation axis and rotation angle, θ0, ξ are obtained by the following formula: x0 , y0 and z0 :

[0028]

[0029] Among them, a x0 、a y0 and a z0 They represent the apparent accelerations in the X', Y' and Z' directions output by the three accelerometers at time t0, ω x0 ,ωy0 and ω z0 Respectively represent the angular velocities in the X', Y' and Z' directions output by the three gyroscopes at time t0, ω ie represents the rotation speed of the earth, g represents the acceleration due to gravity, and φ represents the local latitude.

[0030] In the above-mentioned full attitude solution method of the inertial navigation unit rotation axis and rotation angle, Among them, ω xk ,ω yk and ω zk Respectively represent t k The angular velocities in the X', Y' and Z' directions output by the three gyroscopes at the moment.

[0031] In the above-mentioned full attitude solution method of the inertial navigation unit rotation axis and rotation angle, the strapdown navigation system is k The quaternion q described by four parameters at a moment is expressed as follows:

[0032]

[0033] in, Yes xk , yk and zk In vector form,

[0034] In the above-mentioned full attitude solution method of the inertial navigation unit rotation axis and rotation angle, θ is updated and solved by the following formula: k+1 , xk+1 , yk+1 and zk+1 :

[0035]

[0036] Among them, ΔT represents the update period, Δθ′ represents the angle increment per unit time, express The antisymmetric matrix of .

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

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

[0039]

[0040] In the above-mentioned full attitude solution method of the inertial navigation unit rotation axis and rotation angle, based on the updated coordinate transformation matrix Perform velocity updates and position updates, including:

[0041] The updated coordinate transformation matrix and the apparent acceleration vector and the gravitational acceleration vector As the velocity differential equation After the input is integrated and solved, the updated speed V is obtained;

[0042] The updated velocity V is used as the position differential equation After integral solution, the updated position r is obtained.

[0043] The present invention has the following advantages:

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

[0045] (2) The present invention discloses a method for solving the full attitude of the rotation axis and rotation angle of an inertial navigation unit. The solution process has no singular points and can achieve high-precision solution of the full attitude.

[0046] (3) The present invention discloses a method for solving the full attitude of the rotation axis and rotation angle of an inertial navigation unit. The structure of the update equation is simple, which overcomes the deficiency that nonlinear differential equations are difficult to solve analytically and is easy to implement in engineering. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a flowchart of the steps of a method for calculating the full attitude of the rotation axis and rotation angle of an inertial navigation unit in an embodiment of the present invention;

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

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

[0050] Figure 4 It is a schematic diagram of a three-dimensional trajectory of an aircraft motion calculated based on the method of the present invention in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0052] The invention discloses a method for solving the full attitude of the rotation axis and rotation angle of an inertial navigation unit, and the method is implemented based on a strapdown inertial system fixed to a carrier. Among them, three accelerometers are orthogonally mounted on the strapdown inertial system body, 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 b system, which is a moving system; the coordinate system describing the rotational motion of the carrier is the navigation coordinate system O-XYZ, denoted as p system, which is a fixed system; the origins of the body coordinate system and the navigation coordinate system coincide, such as Figure 2 During the rotation process, 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 solution method of the inertial navigation unit rotation axis and rotation angle includes:

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

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

[0056]

[0057] Among them, θ represents the rotation axis of the body coordinate system around the unit axis The rotation angle, ξ x , y and z Represents the unit axis The components of the X-axis, Y-axis and Z-axis in the navigation coordinate system; a x0 、a y0 and a z0 They represent the apparent accelerations in the X', Y' and Z' directions output by the three accelerometers at time t0, ω x0 ,ω y0 and ω z0 Respectively represent the angular velocities in the X', Y' and Z' directions output by the three gyroscopes at time t0, ω ie represents the earth's rotation speed, g represents the acceleration due to gravity, and φ represents the local latitude. and and are the three basis vectors of the quaternion.

[0058] Step 2: According to the t output by the gyroscope k The angular velocity at time t k The angular velocity of the strapdown inertial system relative to the navigation coordinate system at the moment

[0059] In this embodiment, Among them, ω xk ,ω yk and ω zk Respectively represent t k The angular velocities in the X', Y' and Z' directions output by the three gyroscopes at the moment.

[0060] Step 3: Based on the differential equation, solve θ and ξ x , y and z The four parameters are k The value at the moment: θ k , xk , yk and zk .

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

[0062]

[0063] Among them, ω x ,ω y and ω z They represent the angular velocities in the X', Y' and Z' directions output by the three gyroscopes.

[0064] Preferably, the strapdown navigation system is k The quaternion q described by four parameters at a moment is expressed as follows:

[0065]

[0066] in, Yes xk , yk and zk In vector form,

[0067] Step 4: According to θ k , xk , yk , zk and Perform update calculation to obtain θ, ξ 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 The four parameters are k+1 =t k The preferred update equation at +ΔT is as follows:

[0069]

[0070] Among them, ΔT represents the update period; Δθ′ represents the angle increment per unit time, express The antisymmetric matrix of .

[0071] Step 5, according to θ k+1 , xk+1 , yk+1 and zk+1 , update the coordinate transformation matrix and 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 velocity updates and position updates.

[0075] In this embodiment, the updated coordinate transformation matrix and the apparent acceleration vector and the gravitational acceleration vector As the velocity differential equation After the integral solution, the updated speed V is obtained; further, the updated speed V is used as the position differential equation After integral solution, the updated position r is obtained.

[0076] The full attitude solution method of the inertial navigation unit axis and rotation angle described in the present invention is used to perform navigation solution on the data of a certain full attitude movement of the aircraft ("Full attitude navigation solution method based on extended Krylov angle", Journal of Chinese Inertial Technology, Vol.29No.5, 2021). The solved rotation angle and unit axis are as follows Figure 3 As shown in the figure, the three-dimensional trajectory of the aircraft motion solved by navigation is as follows Figure 4 As shown, it can be seen that the large-attitude and high-maneuverability motion process of the aircraft is well reproduced, indicating that the full-attitude solution method of the inertial navigation unit rotation axis and rotation angle described in the present invention can realize full-attitude solution, which is beneficial to flight control.

[0077] Although the present invention has been disclosed as above in the form of a preferred embodiment, it is not intended to limit the present invention. Any person skilled in the art may make possible changes and modifications to the technical solution of the present invention by using the methods and technical contents disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall fall within the protection scope of the technical solution of the present invention.

[0078] The contents not described in detail in the specification of the present invention belong to the common knowledge of the professionals in this field.

Claims

1. A method for calculating the full attitude of the rotation axis and rotation angle of an inertial navigation unit, characterized in that: include: According to the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, θ and ξ are calculated. x , y and z The initial values ​​of the four parameters at time t0: θ0, ξ x0 , y0 and z0 ; where θ represents the rotation axis of the body coordinate system around the unit axis The rotation angle, ξ x , y and z Represents the unit axis Components of the X-axis, Y-axis, and Z-axis in the navigation coordinate system; unit axis and and are the three basis vectors of the quaternion; According to the gyroscope output t k The angular velocity at time t k The angular velocity of the strapdown inertial system relative to the navigation coordinate system at the moment Based on the differential equation, we can solve θ and ξ x , y and z The four parameters are k The value at the moment: θ k , xk , yk and zk ; According to θ k , xk , yk , zk and Perform update calculation to obtain θ, ξ 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 , update the coordinate transformation matrix and obtain the updated coordinate transformation matrix Based on the updated coordinate transformation matrix Perform velocity updates and position updates.

2. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 1, characterized in that: There are three accelerometers, which are orthogonally mounted on the strapdown inertial system body; there are three gyroscopes, which are orthogonally mounted on the strapdown inertial system body.

3. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 2, characterized in that: The coordinate system corresponding to the strapdown inertial system is the body coordinate system O-X'Y'Z', denoted as 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 p system, which is the fixed system; the origins of the body coordinate system and the navigation coordinate system coincide.

4. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 3, characterized in that: θ0 and ξ are obtained by solving the following formula x0 , y0 and z0 : Among them, a x0 、a y0 and a z0 They represent the apparent accelerations in the X', Y' and Z' directions output by the three accelerometers at time t0, ω x0 ,ω y0 and ω z0 Respectively represent the angular velocities in the X', Y' and Z' directions output by the three gyroscopes at time t0, ω ie represents the rotation speed of the earth, g represents the acceleration due to gravity, and φ represents the local latitude.

5. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 4, characterized in that: Among them, ω xk ,ω yk and ω zk Respectively represent t k The angular velocities in the X', Y' and Z' directions output by the three gyroscopes at the moment.

6. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 5, characterized in that: Strapdown navigation system k The quaternion q described by four parameters at a moment is expressed as follows: in, Yes xk , yk and zk In vector form, 7. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 6, characterized in that: The solution is updated by the following formula to obtain θ k+1 , xk+1 , yk+1 and zk+1 : Among them, ΔT represents the update period, Δθ′ represents the angle increment per unit time, express The antisymmetric matrix of .

8. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 7, characterized in that:

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

10. The method for calculating the full attitude of the rotation axis and rotation angle of the inertial navigation unit according to claim 9, characterized in that: Based on the updated coordinate transformation matrix Perform velocity updates and position updates, including: The updated coordinate transformation matrix and the apparent acceleration vector and the gravitational acceleration vector As the velocity differential equation After the input is integrated and solved, the updated speed V is obtained; The updated velocity V is used as the position differential equation After the integral solution, the updated position r is obtained.

Images