Inertial Navigation 6-Parameter Coordinate Transformation Matrix Calculation Method

Abstract

This invention discloses a method for solving the 6-parameter coordinate transformation matrix of inertial navigation. The method includes: determining six parameters in two rows of the coordinate transformation matrix as variables for real-time updates; and using an integral formula to perform integral update calculations on the 6-parameter differential equation to obtain the six parameters at time t. k The time value is calculated; six parameters are orthogonalized and normalized; the remaining three parameters of the coordinate transformation matrix are calculated, and the updated coordinate transformation matrix is ​​obtained, supporting velocity and position updates to improve the accuracy of inertial navigation. This invention uses the angular rate output by the gyroscope orthogonally mounted on the strapdown inertial system as input information to achieve real-time updating of the inertial navigation coordinate transformation matrix, simplifying the computation while ensuring solution accuracy.

Description

Technical Field

[0001] This invention belongs to the field of aviation and aerospace technology, and in particular relates to a method for solving the six-parameter coordinate transformation matrix of inertial navigation. 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] In engineering, only four quaternion parameters and their differential equations are commonly used. However, the drawback is that these four parameters are only intermediate variables, and nine more equations are needed to solve for the coordinate transformation matrix of the carrier coordinate system relative to the navigation system. Furthermore, the coordinate transformation matrix described by quaternions does not have a unique correspondence with the four parameters, because one coordinate transformation matrix can correspond to two different sets of quaternions.

[0005] In comparison, the Euler-Krylov angle kinematic equations only have three. However, in the book "Inertial Devices (Volume 1)" (China Aerospace Publishing House), page 46, it is argued that the kinematic equations described by Euler-Krylov angles contain singularities and degenerate. Nevertheless, a Krylov angle-based attitude calculation method is proposed in patent (202010333184.9), which can achieve full attitude motion description of the carrier. However, the problem is that when the pitch angle is 90°, the attitude calculation error is large during discretization, resulting in significant errors in the calculated velocity and position.

[0006] The method of using direction cosine matrices to represent the rotational motion relationship and attitude matrix between two coordinate systems is convenient, intuitive, and easy to understand. However, its drawback is that the transformation matrix has nine parameters and its differential equations, resulting in a large amount of integral calculation. In Chapter 18 of the book "Inertial Devices (Part 2)" (China Aerospace Publishing House), a method for solving coordinate transformation matrices based on direction cosine kinematic equations and quaternion kinematic equations is presented.

[0007] To achieve a one-to-one correspondence between the coordinate transformation matrix and parameters under all attitude conditions, and to address the problem of excessive computation caused by too many parameters in the direction cosine matrix (9 parameters), it is necessary to study a new method for solving the relative coordinate system of the carrier coordinate system to the navigation system, so as to improve the speed and accuracy of inertial navigation and flight control. Summary of the Invention

[0008] The technical problem solved by this invention is to overcome the shortcomings of the prior art and provide a method for solving the inertial navigation coordinate transformation matrix based on 6 parameters. The angular rate output by the gyroscope orthogonally mounted on the body of the strapdown inertial system is used as the input information for the kinematic equations of the 6 parameters of the coordinate transformation matrix. Through integral calculation, normalization and orthogonalization processing, the inertial navigation coordinate transformation matrix is ​​updated in real time. No singular values ​​appear during the calculation process, ensuring the requirements of full attitude of the body coordinate system relative to the navigation coordinate system and low computational load.

[0009] The technical solution of this invention is as follows:

[0010] This invention discloses a method for solving the 6-parameter coordinate transformation matrix of inertial navigation, including:

[0011] The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system at time tk is calculated;

[0012] Based on the angular velocity output by the gyroscope mounted on the strapdown inertial system, the value at t is obtained. k The angular velocity of the body relative to the navigation frame at any given moment;

[0013] Six parameters are selected from the coordinate transformation matrix;

[0014] Based on the coordinate transformation matrix and angular velocity, the six parameters are updated and solved to obtain the updated coordinate transformation matrix;

[0015] Based on the updated coordinate transformation matrix, velocity and position are updated.

[0016] Furthermore, in the above solution method, the calculated body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t k The coordinate transformation matrix at time t is as follows:

[0017]

[0018] In the formula, a 21,k a 22,k a 23,k a 31,k a 32,k and a 33,k For in t k The six parameters of the time coordinate transformation matrix.

[0019] Furthermore, in the above solution method, the angular velocity at t is obtained based on the angular velocity output by the gyroscope installed on the strapdown inertial system. k The angular velocity of the body relative to the navigation frame at any given moment is as follows:

[0020]

[0021] Where, ω xb ω yb ω zb These are the angular velocities of the body along the x, y, and z axes, respectively.

[0022] Furthermore, in the above solution method, the selection of 6 parameters from the coordinate transformation matrix specifically includes:

[0023] Select a in the coordinate transformation matrix 21,k a 22,k a 23,k a 31,k a 32,k and a 33,k For in t k The six parameters of the time coordinate transformation matrix.

[0024] Furthermore, in the above solution method, the six parameters are updated and solved based on the coordinate transformation matrix and angular velocity, and the updated coordinate transformation matrix is ​​calculated, specifically as follows:

[0025]

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] Among them, a 21,k+1 a 22,k+1 a 23,k+1 a 31,k+1 a 32,k+1 and a 33,k+1 For the updated 6 parameters; b 21,k+1 b 22,k+1 b 23,k+1 b 31,k+1 b 32,k+1 and b 33,k+1 These are the 6 intermediate parameters during the update process.

[0033] Furthermore, in the above solution method, the six intermediate parameters in the update process are specifically as follows:

[0034]

[0035] Among them, b 21,k+1 b 22,k+1 b 23,k+1 b 31,k+1 b 32,k+1 and b 33,k+1 These are the 6 intermediate parameters during the update process; a 21,k a 22,k a 23,k a 31,k a 32,k and a 33,k For in t k The six parameters of the time coordinate transformation matrix.

[0036] Furthermore, in the above solution method,

[0037]

[0038] In the formula, K1, K4, K5, and K6 are six parameters a. 21,k a 22,k a 23,k a 31,k a 32,k and a 33,k Use incremental values ​​with different time intervals.

[0039] Furthermore, in the above solution method, the incremental value specifically refers to:

[0040]

[0041]

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050]

[0051] Among them, K1, K2, K3, K4, K5, and K6 are six parameters a.21,k a 22,k a 23,k a 31,k a 32,k and a 33,k Using incremental values ​​with different time intervals; J1, J2, J3, J4, and J5 are six parameters a. 21,k a 22,k a 23,k a 31,k a 32,k and a 33,k Different correction values ​​are used for different time periods.

[0052] Furthermore, in the above solution method, the step of updating the velocity and position based on the updated coordinate transformation matrix specifically involves:

[0053] When frame p is an inertial frame, the navigation equations are:

[0054]

[0055]

[0056]

[0057] in, This is the updated coordinate transformation matrix. For apparent acceleration, Let be the gravitational acceleration, T be the transpose of the matrix, and r be the updated position.

[0058] The present invention has the following advantages:

[0059] (1) This invention discloses a method for solving the coordinate transformation matrix of inertial navigation with 6 parameters, which has 3 fewer differential equations than the 9-parameter direction cosine method, thus reducing the amount of calculation.

[0060] (2) This invention discloses a method for solving the 6-parameter coordinate transformation matrix of inertial navigation. Compared with the coordinate transformation matrix represented by quaternions, the relationship between the 6 parameters and the coordinate transformation matrix is ​​unique.

[0061] (3) This invention discloses a method for solving the 6-parameter coordinate transformation matrix of inertial navigation, which can achieve high-precision solution without singularities in all attitudes compared to the coordinate transformation matrix represented by 3 Krylov angles.

[0062] (4) This invention discloses a method for solving the 6-parameter coordinate transformation matrix of inertial navigation, which has a simple structure and is easy to implement in engineering. Attached Figure Description

[0063] Figure 1This is a flowchart illustrating the steps of the inertial navigation 6-parameter coordinate transformation matrix calculation method in this embodiment of the invention.

[0064] Figure 2 This is a schematic diagram showing the relationship between the strapdown inertial system body and the navigation coordinate system.

[0065] Figure 3 This describes the changes of the nine parameters of the coordinate transformation matrix calculated by the inertial navigation six-parameter coordinate transformation matrix calculation method of this invention during the three-roll process of the aircraft.

[0066] Figure 4 The three-dimensional trajectory of the aircraft is calculated based on the inertial navigation six-parameter coordinate transformation matrix calculation method of the present invention. Detailed Implementation

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

[0068] The flowchart of the inertial navigation 6-parameter coordinate transformation matrix calculation method in this embodiment of the invention is as follows: Figure 1 As shown. In this embodiment, the inertial navigation 6-parameter coordinate transformation matrix calculation method includes:

[0069] like Figure 2 As shown, this is achieved using a strapdown inertial system fixed to the carrier. The body coordinate system (b-frame, the moving frame) corresponding to the strapdown inertial system is Ox′y′z′; the navigation coordinate system (p-frame, the fixed frame) describing the carrier's rotational motion is Oxyz; the origins of the two coordinate systems coincide. During rotation, the angular velocity of the body of the strapdown inertial system relative to the navigation coordinate system is... The implementation steps of the inertial navigation 6-parameter coordinate transformation matrix calculation method are as follows:

[0070] (1) The relative coordinate system of the strapdown navigation system to the navigation coordinate system at t is calculated. k The coordinate transformation matrix at time t is

[0071]

[0072] In the formula, a 21,k a 22,k a 23,k a 31,k a 32,k and a 33,k For in t k The six parameters of the time coordinate transformation matrix.

[0073] (2) Based on the angular velocity output by the gyroscope installed on the strapdown inertial system, the value at t is obtained. kangular velocity of the body relative to the navigation frame at any moment

[0074] (3) Let the sampling time be ΔT, then the 6 parameters a 21 a 22 a 23 a 31 a 32 and a 33 At the next moment t k+1 =t k The value of +ΔT after integration is b 21,k+1 b 22,k+1 b 23,k+1 b 31,k+1 b 32,k+1 and b 33,k+1 The specific calculation formula is as follows:

[0075]

[0076] (4) Put b 21,k+1 b 22,k+1 b 23,k+1 b 31,k+1 b 32,k+1 and b 33,k+1 Substitute into the following formula and solve for t. k+1 a of time 21,k+1 a 22,k+1 a 23,k+1 a 31,k+1 a 32,k+1 and a 33,k+1 The values, and the coordinate transformation matrix from the navigation coordinate system to the body coordinate system are:

[0077]

[0078] In the formula,

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085] (5) Update the coordinate transformation matrix calculated by the integral and support velocity and position updates to improve the accuracy of inertial navigation.

[0086] The gyroscopes installed on the strapdown inertial system body are either three single-degree-of-freedom gyroscopes or two two-degree-of-freedom gyroscopes.

[0087] In step (5), the updated coordinate transformation matrix will be... And apparent acceleration and gravitational acceleration As the velocity differential equation The input is integrated to obtain the updated velocity V. The updated velocity V is then used as the position differential equation. The input is integrated to obtain the updated position r.

[0088] Based on the above embodiments, the following is an explanation through a set of comparative examples.

[0089] Example 1:

[0090] First, the 6-parameter coordinate transformation matrix solution method in this embodiment only requires integration to solve 6 differential equations, while the 9-parameter direction cosine matrix requires solving the following 9 differential equations.

[0091]

[0092] Therefore, the 6-parameter coordinate transformation matrix solution method in this embodiment reduces the computational load compared to the 9-parameter direction cosine method.

[0093] Secondly, although the 6-parameter coordinate transformation matrix solution method in this embodiment has two more differential equations than the quaternion method, the coordinate transformation matrix represented by quaternions...

[0094]

[0095] There must be at least two sets of parameters: λ, ρ1, ρ2, ρ3; -λ, -ρ1, -ρ2, -ρ3. This causes the relationship between the coordinate transformation matrix and the rotation of the carrier relative to the navigation system to be non-unique. However, the 6-parameter coordinate transformation matrix in this embodiment ensures that the relationship between the 6 parameters and the coordinate transformation matrix is ​​unique.

[0096] Finally, the 6-parameter coordinate transformation matrix solution method of this embodiment is used to perform navigation solution on the data of a certain full-attitude motion of the aircraft ("Full-Attitude Navigation Solution Method Based on Extended Krylov Angles", Chinese Journal of Inertial Technology, Vol.29 No.5, 2021). The 9 parameters to be solved are as follows: Figure 3 As shown, the three-dimensional trajectory of the aircraft calculated by navigation is shown below. Figure 4 As can be seen, the motion process of the aircraft with large attitude and high maneuverability is reproduced very well, which shows that the 6-parameter coordinate transformation matrix solution method of this embodiment can realize full attitude solution, which is beneficial to flight control.

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

[0098] 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 6-parameter coordinate transformation matrix of inertial navigation, characterized in that, include: The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system at time tk is calculated; Based on the angular velocity output by the gyroscope mounted on the strapdown inertial system, the following is obtained: t k The angular velocity of the body relative to the navigation frame at any given moment; Six parameters are selected from the coordinate transformation matrix; Based on the coordinate transformation matrix and angular velocity, the six parameters are updated and solved to obtain the updated coordinate transformation matrix; Based on the updated coordinate transformation matrix, the velocity and position are updated. The calculations yielded the body coordinate system of the strapdown navigation system relative to the navigation coordinate system. t k The coordinate transformation matrix at time t is as follows: In the formula, a 21,k , a 22,k , a 23,k , a 31,k , a 32,k and a 33,k In order to be in t k The six parameters of the time coordinate transformation matrix; Six parameters are selected from the coordinate transformation matrix, specifically: Select coordinate transformation matrix a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k In order to be in t k The six parameters of the time coordinate transformation matrix.

2. The inertial navigation 6-parameter coordinate transformation matrix calculation method according to claim 1, characterized in that: The angular velocity is obtained based on the output of the gyroscope mounted on the strapdown inertial system. t k The angular velocity of the body relative to the navigation frame at any given moment is as follows: in, , , These are the angular velocities of the body along the x, y, and z axes, respectively.

3. The inertial navigation 6-parameter coordinate transformation matrix calculation method according to claim 1, characterized in that: Based on the coordinate transformation matrix and angular velocity, the six parameters are updated and solved, and the updated coordinate transformation matrix is ​​calculated, specifically as follows: in, a 21,k+1 , a 22,k+1 , a 23,k+1 , a 31,k+1 , a 32,k+1 and a 33,k+1 These are the updated 6 parameters; b 21,k+1 , b 22,k+1 , b 23,k+1 , b 31,k+1 , b 32,k+1 and b 33,k+1 These are the 6 intermediate parameters used in the update process.

4. The inertial navigation 6-parameter coordinate transformation matrix calculation method according to claim 3, characterized in that: The six intermediate parameters in the update process are as follows: in, b 21,k+1 , b 22,k+1 , b 23,k+1 , b 31,k+1 , b 32,k+1 and b 33,k+1 These are the 6 intermediate parameters used in the update process; a 21,k 、 a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k In order to be in t k The six parameters of the time coordinate transformation matrix; , , These are the angular velocities of the body along the x, y, and z axes, respectively. t k+1 For the time series k +1 corresponds to the time.

5. The inertial navigation 6-parameter coordinate transformation matrix calculation method according to claim 4, characterized in that: In the formula, K 1. K 4. K 5. K 6 represents 6 parameters a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k Use incremental values ​​with different time intervals.

6. The inertial navigation 6-parameter coordinate transformation matrix calculation method according to claim 5, characterized in that: The incremental value is specifically: ； ； ； ； ； ； ； ； ； ； in, K 1. K 2. K 3. K 4. K 5. K 6 represents 6 parameters a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k Use asynchronous, long-term incremental values; J 1. J 2. J 3. J 4. J 5 represents 6 parameters a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k Different correction values ​​are used for different time periods.

7. The inertial navigation 6-parameter coordinate transformation matrix calculation method according to claim 1, characterized in that: The step of updating the velocity and position based on the updated coordinate transformation matrix specifically involves: when p When the frame of reference is an inertial frame, the navigation equations are: in, This is the updated coordinate transformation matrix. For apparent acceleration, It is the acceleration due to gravity. T This is the transpose of the matrix. This is the updated position.

Images