Inertial navigation four-angle all-attitude navigation resolving method

Abstract

The invention discloses an inertial navigation four-angle all-attitude navigation resolving method. The method comprises the following steps: resolving to obtain theta 0, alpha 0, beta 0 and gamma 0; 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, alpha k, beta k and gamma k based on a differential equation; performing updating calculation according to theta k, alpha k, beta k, gamma k and # imgabs1 # to obtain theta k + 1, alpha k + 1, beta k + 1 and gamma k + 1; and according to theta (k + 1), alpha (k + 1), beta (k + 1) and gamma (k + 1), updating the coordinate transformation matrix to obtain an updated coordinate transformation matrix # imgabs2 #, and performing speed updating and position updating based on the updated coordinate transformation matrix # imgabs3 #. According to the method, output information of an accelerometer and a gyroscope is used as input information of a four-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 an inertial navigation four-angle full-attitude navigation solution method. 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 rotation axis of the carrier relative to the navigation coordinate system. Rotation angle θ. When further describing the motion of the carrier intuitively, for example,

[0014] describe and When the angles of the axes are α, β, and γ, they need to be compared and solved according to the following formula:

[0015]

[0016] The problem is that the four angles θ = ±2arccosλ, When distributed in different intervals, there are 2×2×2×2=16 combinations.

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

[0018] The invention solves the problem by overcoming the shortcomings of the prior art and providing an inertial navigation four-angle full-attitude navigation solution method. The output information of an accelerometer and a gyroscope orthogonally installed on a strapdown inertial system body is used as input information of a four-angle kinematic equation. The coordinate transformation matrix of a carrier relative to a navigation coordinate system is updated in real time through integral solution. No singular values ​​appear 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.

[0019] In order to solve the above technical problems, the present invention discloses an inertial navigation four-angle full-attitude navigation solution method, comprising:

[0020] According to the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, the initial values ​​of the four angles θ, α, β and γ at time t0 are calculated: θ0, α0, β0 and γ0; where θ represents the rotation axis of the body coordinate system. The rotation angle of the axis is The angle with the X axis of the navigation coordinate system, β represents the rotation axis The angle with the Y axis of the navigation coordinate system, γ represents the rotation axis The angle with the Z axis of the navigation coordinate system;

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

[0022] Based on the differential equation, the four angles θ, α, β and γ are solved at t k The value at the moment: θ k , α k , β k and γ k ;

[0023] According to θ k , α k , β k , γ k and Perform update calculations to obtain the updated values ​​of the four angles θ, α, β, and γ: θk+1 , α k+1 , β k+1 and γ k+1 ;

[0024] According to θ k+1 , α k+1 , β k+1 and γ k+1 , update the coordinate transformation matrix and obtain the updated coordinate transformation matrix

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

[0026] In the above-mentioned inertial navigation four-angle full-attitude navigation solution method, 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; 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.

[0027] In the above-mentioned inertial navigation four-angle full-attitude navigation solution method, θ0, α0, β0 and γ0 are obtained by solving the following formula:

[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 inertial navigation four-angle full-attitude navigation solution method, 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 inertial navigation four-angle full-attitude navigation solution method, the strapdown navigation system is kThe quaternion q describing the moment in four angles is expressed as follows:

[0032]

[0033] in, and are the three basis vectors of the quaternion.

[0034] In the above inertial navigation four-angle full-attitude navigation solution method, the rotation axis It is expressed as follows:

[0035]

[0036] In the above inertial navigation four-angle full-attitude navigation solution method, θ is updated and solved by the following formula: k+1 , α k+1 , β k+1 and γ k+1 :

[0037]

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

[0039] In the above-mentioned inertial navigation four-angle full-attitude navigation solution method,

[0040] In the above inertial navigation four-angle full-attitude navigation solution method, the updated coordinate transformation matrix It is expressed as follows:

[0041]

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

[0043] 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;

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

[0045] The present invention has the following advantages:

[0046] (1) The present invention discloses an inertial navigation four-angle full-attitude navigation solution method. Compared with the method of solving four angles by quaternions, the present invention has the advantage of a unique solution result and overcomes the problem of multiple solutions.

[0047] (2) The present invention discloses an inertial navigation four-angle full-attitude navigation solution method, which has no singular points in the solution process and can achieve high-precision solution of the full attitude.

[0048] (3) The present invention discloses an inertial navigation four-angle full-attitude navigation solution method, which has a simple update equation 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

[0049] Figure 1 It is a flowchart of the steps of an inertial navigation four-angle full-attitude navigation solution method in an embodiment of the present invention;

[0050] 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;

[0051] Figure 3 is a schematic diagram of four angles calculated based on the method of the present invention in an embodiment of the present invention;

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

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

[0054] The invention discloses an inertial navigation four-angle full-attitude navigation solution method, which is implemented based on a strapdown inertial system fixed to a carrier. Among them, three accelerometers are orthogonally installed on the strapdown inertial system body, and three gyroscopes are orthogonally installed 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

[0055] like Figure 1 As shown, in this embodiment, the inertial navigation four-angle full-attitude navigation solution method includes:

[0056] Step 1, according to the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, the initial values ​​of the four angles θ, α, β and γ at time t0 are calculated: θ0, α0, β0 and γ0.

[0057] In this embodiment, θ0, α0, β0 and γ0 can be obtained under static base conditions, and the specific solution formula is as follows:

[0058]

[0059] Where θ represents the rotation axis of the body coordinate system The rotation angle of the axis is The angle with the X axis of the navigation coordinate system, β represents the rotation axis The angle with the Y axis of the navigation coordinate system, γ represents the rotation axis The angle with the Z axis of 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 rotation speed of the earth, g represents the acceleration due to gravity, and φ represents the local latitude.

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

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

[0062] Step 3: Based on the differential equation, solve the four angles θ, α, β and γ at t k The value at the moment: θ k , α k , β k and γ k .

[0063] In this embodiment, the differential equations based on which the four angles are updated are as follows:

[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 describing the moment in four angles is expressed as follows:

[0067]

[0068] in, and are the three basis vectors of the quaternion.

[0069] Then, the shaft It is expressed as follows:

[0070]

[0071] Step 4: According to θ k , α k , β k , γ k and Perform update calculations to obtain the updated values ​​of the four angles θ, α, β, and γ: θ k+1 , α k+1 , β k+1 and γ k+1 .

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

[0073]

[0074]

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

[0076] Step 5, according to θ k+1 , α k+1 , β k+1 and γ k+1 , update the coordinate transformation matrix and obtain the updated coordinate transformation matrix

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

[0078]

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

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

[0081] The four-angle full-attitude navigation solution method of inertial navigation 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 four angles solved 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 inertial navigation four-angle full-attitude navigation solution method described in the present invention can realize full-attitude solution, which is beneficial to flight control.

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

[0083] 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 four-angle full-attitude navigation solution method for inertial navigation, characterized in that: include: According to the apparent acceleration and angular velocity at time t0 output by the accelerometer and gyroscope, the initial values ​​of the four angles θ, α, β and γ at time t0 are calculated: θ0, α0, β0 and γ0; where θ represents the rotation axis of the body coordinate system. The rotation angle of the axis is The angle with the X axis of the navigation coordinate system, β represents the rotation axis The angle with the Y axis of the navigation coordinate system, γ represents the rotation axis The angle with the Z axis of the navigation coordinate system; 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, the four angles θ, α, β and γ are solved at t k The value at the moment: θ k , α k , β k and γ k ; According to θ k , α k , β k , γ k and Perform update calculations to obtain the updated values ​​of the four angles θ, α, β, and γ: θ k+1 , α k+1 , β k+1 and γ k+1 ; According to θ k+1 , α k+1 , β k+1 and γ k+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 inertial navigation four-angle full-attitude navigation solution method according to claim 1 is characterized in that: 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; 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.

3. The inertial navigation four-angle full-attitude navigation solution method according to claim 2 is characterized in that: θ0, α0, β0 and γ0 are obtained by solving the following formula: 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.

4. The inertial navigation four-angle full-attitude navigation solution method according to claim 3 is 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.

5. The inertial navigation four-angle full-attitude navigation solution method according to claim 4 is characterized in that: Strapdown navigation system k The quaternion q describing the moment in four angles is expressed as follows: in, and are the three basis vectors of the quaternion.

6. The inertial navigation four-angle full-attitude navigation solution method according to claim 5 is characterized in that: Shaft It is expressed as follows:

7. The inertial navigation four-angle full-attitude navigation solution method according to claim 5 is characterized in that: The solution is updated by the following formula to obtain θ k+1 , α k+1 , β k+1 and γ k+1 : Among them, ΔT represents the update period, Δθ′ represents the angle increment per unit time, express The antisymmetric matrix of .

8. The inertial navigation four-angle full-attitude navigation solution method according to claim 7 is characterized in that:

9. The inertial navigation four-angle full-attitude navigation solution method according to claim 1, characterized in that: Updated coordinate transformation matrix It is expressed as follows:

10. The inertial navigation four-angle full-attitude navigation solution method 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