All-attitude calculation method for inertial navigation variable-length rotating shaft and rotating angle

Abstract

The invention discloses an all-attitude calculation method for an inertial navigation variable-length rotating shaft and a rotating angle. The all-attitude calculation method comprises the following steps: calculating to obtain theta0, rhox0, rhoy0 and rhoz0; 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 theta k, rho xk, rho yk and rho zk based on a differential equation; according to the theta < k >, the rho < x > < k >, the rho < y > < k >, the rho < z > < k > and the # imgabs < 1 > < 1 >, updating and resolving to obtain theta < k + 1 >, rho < x > < k + 1 >, rho < y > < k + 1 > and rho And according to theta < k + 1 >, rho < x > < k + 1 >, rho < y > < k + 1 > and rho < z > < k + 1 >, updating the coordinate transformation matrix to obtain # imgabs < 2 >, and performing 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 kinematics equation of the variable-length rotating shaft and the rotating angle, 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 an inertial navigation variable-length rotating shaft and a rotating angle. 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] It can be seen from the above formula 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 variable length rotation axis of the carrier relative to the navigation coordinate system. Rotation angle θ. 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 overcoming the shortcomings of the prior art and providing a method for solving the full attitude of an inertial navigation variable-length rotating shaft and a rotating angle. The method uses information output by an accelerometer and a gyroscope orthogonally mounted on a strapdown inertial system body as input information of a kinematic equation of the variable-length rotating shaft and the rotating angle. The method realizes real-time updating of a coordinate transformation matrix of a carrier relative to a navigation coordinate system through integral solving. No singular values ​​appear in the solving process, and the solving 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 full attitude solution method of an inertial navigation variable-length rotation axis and rotation angle, 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 Initial values ​​at time t0: θ0, ρ x0 , y0 and ρ z0 ; where θ represents the rotation axis of the body coordinate system around the variable length axis The rotation angle, ρ x , y and ρ z Respectively represent variable length axes Components of the X, Y, and Z axes in the navigation coordinate system; variable length rotation 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 k The 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 inertial navigation variable-length rotation axis and rotation angle, 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 for inertial navigation variable-length rotation axis and rotation angle, 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 inertial navigation variable length axis and rotation angle, θ0 and ρ 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 inertial navigation variable length 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 inertial navigation variable length 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, is xk , yk and ρ zk In vector form,

[0034] In the above-mentioned full attitude solution method of inertial navigation variable length 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 inertial navigation variable length axis and rotation angle,

[0038] In the above-mentioned full attitude solution method of inertial navigation variable length 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 inertial navigation variable length 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 an inertial navigation variable-length rotation axis and rotation angle. 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 inertial navigation with a variable length axis and rotation angle. 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 an inertial navigation variable-length rotating axis and a rotating angle. The updating equation has a simple structure, 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 solving the full attitude of an inertial navigation variable-length rotating axis and a rotating angle 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 three components of a rotation angle and a variable-length 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 an inertial navigation variable-length axis and rotation angle, 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 variable length 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 Initial values ​​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]

[0058] Among them, θ represents the rotation axis of the body coordinate system around the variable length axis The rotation angle, ρ x , y and ρ z Respectively represent variable length axes Components of the X, Y, and Z axes in the navigation coordinate system; variable length rotation axis and and are the three basis vectors of the quaternion; 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 ω z0Respectively 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.

[0059] 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

[0060] 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.

[0061] 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 .

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

[0063]

[0064]

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

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

[0067]

[0068] in, is xk , yk and ρ zk In vector form,

[0069] 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 .

[0070] In this embodiment, the four parameters are k+1 =t k The preferred update equation at +ΔT is as follows:

[0071]

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

[0073] 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

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

[0075]

[0076] Step 6: Based on the updated coordinate transformation matrix Perform velocity updates and position updates.

[0077] 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.

[0078] The full attitude solution method of inertial navigation variable length 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 rotation vector 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 motion process of the aircraft with large attitude and high maneuverability is well reproduced, which shows that the full attitude solution method of the inertial navigation variable length shaft and rotation angle described in the present invention can realize full attitude solution, which is beneficial to flight control.

[0079] 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.

[0080] 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 an inertial navigation variable-length rotation axis and rotation angle, 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 Initial values ​​at time t0: θ0, ρ x0 , y0 and ρ z0 ; where θ represents the rotation axis of the body coordinate system around the variable length axis The rotation angle, ρ x , y and ρ z Respectively represent variable length axes Components of the X, Y, and Z axes in the navigation coordinate system; variable length rotation 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 inertial navigation with variable length axis and rotation angle according to claim 1 is 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 inertial navigation with variable length axis and rotation angle according to claim 2 is 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 inertial navigation with variable length axis and rotation angle according to claim 3 is 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 inertial navigation with variable length axis and rotation angle 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 inertial navigation variable-length rotation axis and rotation angle 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, is xk , yk and ρ zk In vector form, 7. The method for calculating the full attitude of inertial navigation variable-length rotation axis and rotation angle 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 inertial navigation with variable length axis and rotation angle according to claim 7, characterized in that:

9. The method for calculating the full attitude of inertial navigation with variable length axis and rotation angle 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 inertial navigation variable-length rotation axis and rotation angle 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