A method for calculating the four-angle full attitude of inertial navigation

Abstract

This invention discloses an inertial navigation four-angle full attitude navigation solution method, including: calculating θ0, α0, β0, and γ0; determining t k The angular velocity of the strapdown inertial system relative to the navigation coordinate system at any given time is calculated using a differential equation, θ. k α k β k and γ k According to θ k α k β k γ k And perform an update calculation to obtain θ k+1 α k+1 β k+1 and γ k+1 According to θ k+1 α k+1 β k+1 and γ k+1 The method involves updating the coordinate transformation matrix to obtain the updated matrix, and then updating the velocity and position based on this updated matrix. The method described in this invention uses the output information from the accelerometer and gyroscope as input information for the four-angle kinematic equations. Through integral calculation, it achieves real-time updating of the coordinate transformation matrix of the carrier relative to the navigation coordinate system. No singular values ​​occur during the calculation process, and the solution result is unique, ensuring the requirements of full attitude and high precision of the carrier coordinate system relative to the navigation coordinate system.

Description

Technical Field

[0001] This 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 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 rotation axis of the carrier relative to the navigation coordinate system. Rotation angle θ. When further visually describing the motion of the carrier, for example,

[0014] describe and When the included angles α, β, and γ of the axes are being calculated, the following formula needs to be used for comparison and solution:

[0015]

[0016] The problem is that the four angles satisfying this expression are θ = ±2arccosλ. There are a total of 2×2×2×2=16 combinations when distributed in different intervals.

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

[0018] The technical problem solved by this invention is to overcome the shortcomings of the prior art and provide an inertial navigation four-angle full attitude navigation solution method. The output information of accelerometers and gyroscopes orthogonally mounted on the body of the strapdown inertial system is used as the input information of the four-angle kinematic equations. 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 solution process, the solution result is unique, and the requirements of full attitude and high precision of the body coordinate system relative to the navigation coordinate system are guaranteed.

[0019] To address the aforementioned technical problems, this invention discloses an inertial navigation four-angle full-attitude navigation calculation method, comprising:

[0020] Based on 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 body coordinate system about the rotation axis. The rotation angle, α represents the axis of rotation. The angle between the axis of rotation and the X-axis of the navigation coordinate system, β represents the axis of rotation. The angle between the axis of rotation and the Y-axis of the navigation coordinate system, γ, represents the axis of rotation. The angle between the coordinate system and the Z-axis of the navigation coordinate system;

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

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

[0023] According to θ k α k β k γ k and The updated solutions are performed 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 Perform coordinate transformation matrix update to obtain the updated coordinate transformation matrix.

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

[0026] In the above-mentioned inertial navigation four-angle full attitude navigation solution method, 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; 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 inertial navigation four-angle full attitude navigation solution method, θ0, α0, β0 and γ0 are obtained by solving using the following formulas:

[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 four-angle full attitude navigation solution 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 inertial navigation four-angle full-attitude navigation calculation method, the strapdown navigation system at t kThe quaternion q, which describes time from four perspectives, is represented as follows:

[0032]

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

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

[0035]

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

[0037]

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

[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 speed and location updates, including:

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

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

[0045] The present invention has the following advantages:

[0046] (1) This invention discloses an inertial navigation four-angle full attitude navigation solution method, which has the advantage of unique solution result compared with the method of solving four angles by quaternion, and overcomes the problem of multiple solutions.

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

[0048] (3) This invention discloses an inertial navigation four-angle full attitude navigation solution method. The update 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

[0049] Figure 1 This is a flowchart illustrating the steps of an inertial navigation four-angle full attitude navigation calculation method according to an embodiment of the present invention;

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

[0051] Figure 3 This is a schematic diagram of four angles calculated based on the method described in this invention, according to an embodiment of the invention.

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

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

[0054] This invention discloses a four-angle, full-attitude navigation calculation method for 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...

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

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

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

[0058]

[0059] Where θ represents the body coordinate system about the rotation axis The rotation angle, α represents the axis of rotation. The angle between the axis of rotation and the X-axis of the navigation coordinate system, β represents the axis of rotation. The angle between the axis of rotation and the Y-axis of the navigation coordinate system, γ, represents the axis of rotation. The angle between the coordinate system and the Z-axis of 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 φ represents the Earth's rotational speed, g represents gravitational acceleration, and φ represents the local latitude.

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

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

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

[0063] In this embodiment, the differential equations underlying the four angle updates are as follows:

[0064]

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

[0066] Preferably, the strapdown navigation system in t k The quaternion q, which describes time from four perspectives, is represented as follows:

[0067]

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

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

[0070]

[0071] Step 4, based on θ k α k β k γ k and The updated solutions are performed 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 at t k+1 =t k The optimal update equation at time +ΔT is as follows:

[0073]

[0074]

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

[0076] Step 5, based on θ k+1 α k+1 β k+1 and γ k+1 Perform coordinate transformation matrix update to 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 speed and location updates.

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

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

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

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

Claims

1. A four-angle all-attitude inertial navigation solution method, characterized in that, Comprising: According to the visual acceleration and angular velocity at t0 moment output by the accelerometer and the gyroscope, the initial values of the four angles θ, α, β and γ at t0 moment are calculated: θ0, α0, β0 and γ0: Wherein, θ represents the rotation angle of the body coordinate system around the rotation axis , α represents the included angle between the rotation axis and the X axis of the navigation coordinate system, β represents the included angle between the rotation axis and the Y axis of the navigation coordinate system, and γ represents the included angle between the rotation axis and the Z axis of the navigation coordinate system; a x0 , a y0 and a z0 respectively represent the apparent accelerations in the X', Y' and Z' directions output by the three accelerometers at t0, ω x0 , ω y0 and ω z0 respectively represent the angular velocities in the X', Y' and Z' directions output by the three gyroscopes at t0, ω ie represents the earth rotation speed, g represents the gravity acceleration, and φ represents the local latitude; According to the angular velocity of the gyro output at time t k , the angular velocity of the body of the strapdown inertial system relative to the navigation coordinate system at time t k is determined ω xk , ω yk , and ω zk represent the angular velocities of the X', Y', and Z' directions of the three gyro outputs at time t k , respectively Based on the differential equation, the values of the four angles θ, α, β and γ at time t are calculated as follows: θ k , α k , β k , and γ k . k ; According to θ k , α k , β k , γ k and , the updated solution is obtained, and the updated values of θ, α, β and γ are θ k+1 , α k+1 , β k+1 and γ k+1 : where ΔT represents an update period, and Δθ' represents an angle increment per unit time, represents an anti-symmetric matrix of According to θ k+1 , α k+1 , β k+1 and γ k+1 , the coordinate transformation matrix is updated to obtain an updated coordinate transformation matrix based on the updated coordinate transformation matrix Velocity and position updates are made.

2. The inertial navigation four-angle full-attitude navigation solution method according to claim 1, characterized in that, The accelerometer is three, three accelerometers are orthogonally installed on the body of the strapdown inertial system; the gyroscope is three, three gyroscopes are orthogonally installed on the body of the strapdown inertial system; the corresponding coordinate system of the strapdown inertial system is the body coordinate system O-X'Y'Z', denoted as b system, as a moving system; the coordinate system describing the rotating motion of the carrier is the navigation coordinate system O-XYZ, denoted as p system, as a 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 1, characterized in that, The strapdown navigation system describes the four quaternion q at time t in four angles as follows: k q = cos(ψ / 2) + i sin(ψ / 2) cos(θ / 2) + j sin(ψ / 2) sin(θ / 2) + k wherein and are three basis vectors of the quaternion.

4. The inertial navigation four-angle full-attitude navigation solution method according to claim 3, characterized in that, rotating shaft is represented as follows:

5. The inertial navigation four-angle full-attitude navigation solution method of claim 1, wherein, 6. The inertial navigation four-angle all-attitude navigation solution method of claim 1, wherein, Updated coordinate transformation matrix is represented as follows:

7. The inertial navigation four-angle full-attitude navigation solution method according to claim 6, characterized in that, based on the updated coordinate transformation matrix performing velocity updates and position updates, including: The updated coordinate transformation matrix and the visual acceleration vector and the gravity acceleration vector as the input of the velocity differential equation After integration, the updated velocity V is obtained. The updated velocity V is input to the position differential equation and the updated position r is obtained by integration.

Images