A method for full attitude calculation of variable length axis and rotation angle in inertial navigation

Abstract

This invention discloses a full attitude calculation method for inertial navigation systems with variable-length pivots and rotation angles, including: calculating θ0 and ρ x0 ρ y0 and ρ z0 Determine 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 ρ xk ρ yk and ρ zk According to θ k ρ xk ρ yk ρ zk And perform an update calculation to obtain θ k+1 ρ xk+1 ρ yk+1 and ρ zk+1 According to θ k+1 ρ xk+1 ρ yk+1 and ρ zk+1 The method involves updating the coordinate transformation matrix to obtain the updated velocity and position. The method described in this invention uses the output information from the accelerometer and gyroscope as input information for the kinematic equations of the variable-length axis of rotation and rotation angle. 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 body 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 a full attitude calculation method for inertial navigation variable length rotating 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 variable-length rotation axis of the carrier relative to the navigation coordinate system. Rotation angle θ. When further describing the motion of the carrier visually, 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 variable length axis and rotation angle of an inertial navigation system. The method uses the information output by the accelerometer and gyroscope orthogonally mounted on the body of the strapdown inertial system as the input information for the kinematic equations of the variable length 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 during the calculation process, and the solution result is unique, thus ensuring the full attitude and high precision requirements of the carrier 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 systems with variable-length rotation axes and rotation angles, 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 ​​at time t0: θ0, ρ x0 ρ y0 and ρ z0 Where θ represents the body coordinate system about the variable length axis of rotation. The rotation angle, ρ x ρ y and ρ z These represent variable length shafts. Components of the X, Y, and Z axes in the navigation coordinate system; variable length pivot. 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 k The 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 variable length 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 variable length 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 variable-length rotation axis 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 full attitude calculation method with variable length axis of rotation and rotation angle, 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 variable-length rotation axis and rotation angle in 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 full attitude calculation method with variable length axis of rotation and rotation angle, θ 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 full attitude calculation method with variable length axis of rotation and rotation angle,

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

[0039]

[0040] In the above-mentioned inertial navigation full attitude calculation method with variable length axis of rotation 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 variable length rotating 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 inertial navigation variable length axis and rotation angle. The calculation process has no singular points and can achieve high-precision full attitude calculation.

[0046] (3) This invention discloses a full attitude calculation method for variable length rotating axis and rotation angle of inertial navigation. The updated equation has a simple structure, 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 a variable-length rotating axis and rotation angle in inertial navigation according to 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 a variable-length shaft calculated based on the method described in this invention, according to 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 inertial navigation with variable-length rotation axis and rotation angle, 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 variable-length axis of rotation and rotation angle of the inertial navigation system 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 ​​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]

[0058] Where θ represents the body coordinate system about the variable length axis of rotation. The rotation angle, ρ x ρ y and ρ z These represent variable length shafts. Components of the X, Y, and Z axes in the navigation coordinate system; variable length pivot. and and Let a be the three basis vectors of the quaternion; 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.

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

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

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

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

[0063]

[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 in terms of four parameters, is represented as follows:

[0067]

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

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

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

[0071]

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

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

[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 speed and location updates.

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

[0078] The inertial navigation method for calculating the full attitude of an aircraft during a specific full attitude motion ("Full Attitude Navigation Calculation Method Based on Extended Krylov Angles," Chinese Journal of Inertial Technology, Vol. 29 No. 5, 2021) described in this invention is used to perform navigation calculations on the data. The calculated rotation angles and rotation vectors 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 motion process of the aircraft with large attitude and high maneuverability is reproduced very well, which shows that the full attitude calculation method of the inertial navigation variable length axis and rotation angle described in this invention can realize full attitude calculation, which is beneficial to flight control.

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

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

Claims

1. A method for solving the full attitude of an inertial navigation variable-length rotation axis and rotation angle, characterized in that, Comprise: 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 ​​at time t0: θ0, ρ x0 ρ y0 and ρ z0 : wherein θ represents the rotation angle of the body coordinate system around the variable-length rotation shaft ρ x , ρ y and ρ z respectively represent the components of the variable-length rotation shaft in the navigation coordinate system X axis, Y axis and Z axis; the variable-length rotation shaft and and are three base vectors of the quaternion, a x0 , a y0 and a z0 respectively represent the apparent accelerations of X', Y' and Z' three directions output by the three accelerometers at t0, ω x0 , ω y0 and ω z0 respectively represent the angular velocities of X', Y' and Z' three 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 time t k , the angular velocity of the body of the strapdown inertial system relative to the navigation coordinate system at the time t k is determined ω xk , ω yk and ω zk represent the angular velocities of the three gyros in the X', Y' and Z' directions at the time t k , respectively 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 , the update calculation is performed to obtain the updated values of θ, ρ x , ρ y and ρ z : θ k+1 , ρ xk+1 , ρ yk+1 and ρ zk+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 , ρ xk+1 , ρ yk+1 and ρ zk+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 performed.

2. The method of claim 1, wherein, The three accelerometers are orthogonally installed on the body of the strapdown inertial system; the three gyroscopes are orthogonally installed on the body of the strapdown inertial system.

3. The method of claim 2, wherein the method further comprises: determining the rotation angle of the rotation axis based on the first and second rotation angles. 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.

4. The method of claim 1, wherein, The strapdown navigation system describes the four parameters at time t in a quaternion q as follows: k q = cos(φ / 2) + (sin(φ / 2) * e1) + (sin(φ / 2) * e2) + (sin( wherein is the vector form of p xk , p yk , and p zk , 5. The method of claim 1, wherein, 6. The method of claim 1, wherein, Updated coordinate transformation matrix is represented as follows:

7. The method of claim 6, wherein, 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 inputs of the velocity differential equation and the updated velocity V is obtained after integration. The updated velocity V is input to the position differential equation and the updated position r is obtained by integration.

Images