Normality-based inertial navigation five-parameter coordinate transformation matrix resolving method
Through the 5-parameter coordinate transformation matrix solution method based on normalization, the integral solution and parameter selection are used to solve the problem of inequality and large calculations of posture information solution in the existing technology, and the inertial navigation is realized. Fast and high-precision updates.
Patent Information
- Application Number
- CN202410983863.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-22
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2044-07-22
AI Technical Summary
In the existing inertial navigation technology, the attitude information solution method has problems such as inunique parameters, large calculation amount, and singular points, which leads to large solution errors and is difficult to achieve fast and accurate coordinate transformation matrix updates.
Using the 5-parameter coordinate transformation matrix solution method based on normalization, the angular rate of the strap navigation system is calculated by integrating the solution, and 5 parameters are selected to update to ensure that the real-time update and calculation amount of the coordinate transformation matrix is small, and avoiding Singular value appears.
The judgment conditions are simplified, the calculation amount is reduced, the speed and accuracy of inertial navigation and flight control are improved, and the full-pose high-precision solution is achieved.
Smart Images

Figure CN118999541A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a normalization-based inertial navigation 5-parameter coordinate transformation matrix solution method, belonging to the technical field of inertial navigation. Background Art
[0002] Inertial navigation is widely used in spacecraft, aircraft, ships 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] There are only four quaternion parameters and their differential equations that are widely used in engineering, but the disadvantage is that these four parameters are only intermediate variables, and nine equations are required to solve the coordinate transformation matrix of the carrier coordinate system relative to the navigation system. In addition, the coordinate transformation matrix described by the quaternion is not the only corresponding relationship with the four parameters, because one coordinate transformation matrix can correspond to two different sets of quaternions.
[0005] Relatively speaking, there are only three Euler-Krylov angle kinematic equations, but in the technical field of "Inertial Devices (Part 1)" (China Aerospace Press) on page 46, it is believed that the kinematic equations described by Euler-Krylov angles have singularities and the equations will degenerate. Chinese patent CN202010333184.9 proposes a posture solution method based on Krylov angles, which can realize the full posture motion description of the carrier. But the problem is that when the pitch angle = 90°, the error of the attitude solution is large during discretization, which leads to large errors in the solved speed and position.
[0006] The method of using the direction cosine matrix to represent the rotational motion relationship and attitude matrix between two coordinate systems is convenient, intuitive and easy to understand, but the disadvantage is that the parameters of the transformation matrix and its differential equation are 9, and the integral calculation is large. In this technical field, Chapter 18 of "Inertial Devices (Part 2)" (China Aerospace Press) gives a method for solving the coordinate transformation matrix based on the direction cosine kinematic equation and the quaternion kinematic equation.
[0007] Under the premise of full posture, in order to achieve a one-to-one correspondence between the coordinate transformation matrix and the parameters, and at the same time solve the problem of large calculation amount caused by too many 9 parameters of the direction cosine array, a method for solving the coordinate transformation matrix of 5 parameters of inertial navigation based on orthogonality is proposed in Chinese patent CN202210617894.3, but it is necessary to select one of the 6 conditions as the update equation. Summary of the invention
[0008] The technical problem to be solved by the present invention is: to overcome the shortcomings of the prior art, to provide a normalized inertial navigation 5-parameter coordinate transformation matrix solution method, to simplify the judgment conditions and reduce the amount of calculation, and to improve the rapidity of inertial navigation and flight control.
[0009] The purpose of the present invention is achieved through the following technical solutions:
[0010] A method for solving the five-parameter coordinate transformation matrix of inertial navigation based on normalization takes the angular rate output by the gyroscope orthogonally installed on the strapdown inertial system body as the input information of the five-parameter kinematic equation of the coordinate transformation matrix. Through integral solution, the real-time update of the inertial navigation coordinate transformation matrix is realized. No singular values appear in the solution process, which ensures the full attitude of the body coordinate system relative to the navigation coordinate system and the requirement of small calculation amount.
[0011] Specifically include:
[0012] The calculated strapdown navigation system body coordinate system relative to the navigation coordinate system is t k The coordinate transformation matrix at the moment;
[0013] Calculated at t k The angular velocity of the body coordinate system relative to the navigation coordinate system at this moment;
[0014] Select 5 parameters from the coordinate transformation matrix;
[0015] According to the coordinate transformation matrix and the angular velocity, the five parameters are updated and solved to obtain an updated coordinate transformation matrix;
[0016] Speed update and position update are performed according to the updated coordinate transformation matrix.
[0017] Furthermore, in the above solution method, the coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is:
[0018]
[0019] In the formula, a 11 、a 12 、a 13 、a 21 、a 22 、a 23 、a 31 、a 32 and a 33 These are the 9 parameters of the coordinate transformation matrix.
[0020] In t k The coordinate transformation matrix of the strapdown navigation system's main coordinate system relative to the navigation coordinate system is:
[0021]
[0022] In the formula, a 11,k 、a 12,k 、a 13,k 、a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k For k 9 parameters of the moment coordinate transformation matrix.
[0023] Furthermore, in the above solution method, the calculation at t k The angular velocity of the body coordinate system relative to the navigation coordinate system at this moment is:
[0024]
[0025] in, are the angular velocities of the body on the x, y, and z axes respectively.
[0026] Furthermore, in the above solution method, 5 parameters out of 9 parameters are selected from the coordinate transformation matrix, specifically:
[0027] Select a in the coordinate transformation matrix 11,k 、a 21,k and a 31,k , and in the 3 elements a 13,k 、a 23,k and a 33,k The remaining two elements after excluding the maximum absolute value are used as 5 parameters.
[0028] Furthermore, in the above-mentioned solution method, the five parameters are updated and solved according to the coordinate transformation matrix and the angular velocity, and the updated coordinate transformation matrix is solved, specifically:
[0029] when|a 13,k |Maximum, select 5 parameters a 11,k 、a 21,k 、a 23,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 ;
[0030] when|a23,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 31,k+1 and a 33,k+1 ;
[0031] when|a 33,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 23,k and a 31,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 23,k+1 and a 31,k+1 ;
[0032] Furthermore, in the above solution method, when |a 13,k |Maximum, select 5 parameters a 11,k 、a 21,k 、a 23,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 , the specific calculation formula is:
[0033]
[0034] Put t k+1 Moment a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:
[0035]
[0036] Furthermore, in the above-mentioned solution method, when |a23,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 31,k+1 and a 33,k+1 , the calculation formula is:
[0037]
[0038] Put t k+1 Moment a 11,k+1 、a 12,k+1 、a 13,k+1 、a 31,k+1 and a 32,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:
[0039]
[0040] Furthermore, in the above-mentioned solution method, when |a 33,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 23,k and a 31,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 23,k+1 and a 31,k+1 , the calculation formula is:
[0041]
[0042] Put t k+1 Moment a 11,k+1 、a 12,k+1 、a 21,k+1 、a 22,k+1 and a 23,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:
[0043]
[0044] Furthermore, in the above-mentioned solution method, the speed update and position update are performed according to the updated coordinate transformation matrix, specifically:
[0045] When p is an inertial system, the navigation equation is:
[0046]
[0047] in, is the updated coordinate transformation matrix, is the apparent acceleration, is the gravitational acceleration, V is the updated velocity, T is the transpose of the matrix, and r is the updated position.
[0048] Compared with the prior art, the present invention has the following beneficial effects:
[0049] (1) The present invention discloses a method for solving the coordinate transformation matrix of an inertial navigation 5 parameters based on normalization. Compared with the method for solving the coordinate transformation matrix of an inertial navigation 5 parameters based on orthogonality, the number of judgment conditions is reduced from 6 to 3, thereby reducing the amount of calculation;
[0050] (2) The present invention discloses a method for solving the 5-parameter coordinate transformation matrix of inertial navigation, which has 4 fewer differential equations than the 9-parameter direction cosine method, and has a unique relationship between the 5 parameters and the coordinate transformation matrix compared to the coordinate transformation matrix represented by the quaternion;
[0051] (3) The present invention discloses a method for solving the five-parameter coordinate transformation matrix of inertial navigation, which can achieve high-precision solution of the whole attitude without singularity relative to the coordinate transformation matrix characterized by three Krylov angles;
[0052] (4) The present invention discloses a method for solving the inertial navigation 5-parameter coordinate transformation matrix, which has a simple structure and is easy to implement in engineering. BRIEF DESCRIPTION OF THE DRAWINGS
[0053] Figure 1 It is a flow chart of the steps of the inertial navigation 5 parameter coordinate transformation matrix solution method of the present invention;
[0054] Figure 2 It is a schematic diagram of the relationship between the strapdown inertial system body and the navigation coordinate system of the present invention;
[0055] Figure 3 The change process of 9 parameters of the coordinate transformation matrix calculated by the inertial navigation 5-parameter coordinate transformation matrix calculation method of the present invention during the aircraft rolling three times;
[0056] Figure 4 It is the three-dimensional trajectory of the aircraft motion calculated based on the inertial navigation 5-parameter coordinate transformation matrix calculation method of the present invention. DETAILED DESCRIPTION
[0057] To make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.
[0058] A method for solving a 5-parameter coordinate transformation matrix of an inertial navigation system based on normalization includes: determining 5 parameters in two rows of the coordinate transformation matrix as variables for real-time update; using an integral formula to perform integral update calculation on the 5-parameter differential equation to obtain the 5 parameters at t k The present invention uses the angular rate output by the gyroscope orthogonally mounted on the strapdown inertial system body as input information, realizes the real-time update of the inertial navigation coordinate transformation matrix, simplifies the calculation amount while ensuring the solution accuracy.
[0059] like Figure 1 As shown, specifically including:
[0060] like Figure 2 As shown, it is realized based on a strapdown inertial system fixed to the carrier. The body coordinate system (b system, the moving system) corresponding to the strapdown inertial system is Ox′y′z′; the navigation coordinate system (p system, the fixed system) describing the rotational motion of the carrier is Oxyz; the origins of the two coordinate systems coincide. During the rotation process, the angular velocity of the body of the strapdown inertial system relative to the navigation coordinate system is The steps for solving the normalized inertial navigation 5-parameter coordinate transformation matrix are as follows:
[0061] (1) The coordinate transformation matrix of the strapdown navigation system’s main coordinate system relative to the navigation coordinate system is:
[0062]
[0063] In the formula, a 11 、a 12 、a 13 、a 21 、a 22 、a 23 、a 31 、a 32 and a 33 These are the 9 parameters of the coordinate transformation matrix.
[0064] Calculate t k The coordinate transformation matrix of the strapdown navigation system's main coordinate system relative to the navigation coordinate system is:
[0065]
[0066] In the formula, a11,k 、a 12,k 、a 13,k 、a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k For k 9 parameters of the moment coordinate transformation matrix.
[0067] (2) According to the angular velocity output by the gyroscope installed on the strapdown inertial system body, we can obtain k The angular velocity of the body relative to the navigation system at this moment
[0068] (3) Let the sampling time be ΔT, for a 13,k 、a 23,k and a 33,k Sort the absolute values of from small to large, and select a in the coordinate transformation matrix 11,k 、a 21,k and a 31,k , and in the 3 elements a 13,k 、a 23,k and a 33,k The remaining two elements after the maximum absolute value is excluded are used as five parameters, and the five parameters are updated and solved in the following three cases, and the coordinate transformation matrix is solved;
[0069] (3.1), when |a 13,k |Maximum, select 5 parameters a 11,k 、a 21,k 、a 23,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 , the specific calculation formula is:
[0070]
[0071] Put t k+1 Moment a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:
[0072]
[0073] (3.2), when |a 23,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 31,k+1 and a 33,k+1 , the specific calculation formula is as follows:
[0074]
[0075] Put t k+1 Moment a 11,k+1 、a 12,k+1 、a 13,k+1 、a 31,k+1 and a 32,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:
[0076]
[0077] (3.3), when |a 33,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 23,k and a 31,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 23,k+1 and a 31,k+1 , the specific calculation formula is as follows:
[0078]
[0079] Put t k+1 Moment a 11,k+1 、a 12,k+1 、a 21,k+1 、a 22,k+1 and a 23,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system:
[0080]
[0081] (4) Update the calculated coordinate transformation matrix based on the integral, and support velocity update and position update to improve the accuracy of inertial navigation.
[0082] The gyroscopes installed on the strapdown inertial system body are three single-degree-of-freedom gyroscopes or two double-degree-of-freedom gyroscopes.
[0083] In step (4), the updated coordinate transformation matrix and apparent acceleration and the acceleration due to gravity As the velocity differential equation After the integral solution, the updated speed V is obtained. The updated speed V is used as the position differential equation After integral solution, the updated position r is obtained.
[0084] Example:
[0085] First, the 5-parameter coordinate transformation matrix solution method of the present invention has only 5 differential equations that need to be solved by integration, while the 9-parameter direction cosine matrix needs to solve the following 9 differential equations.
[0086]
[0087] Therefore, the 5-parameter coordinate transformation matrix solution method of the present invention reduces the amount of calculation compared to the 9-parameter direction cosine method.
[0088] In addition, compared with the 5-parameter coordinate transformation matrix solution method based on orthogonality, the judgment condition is ①|a 13,k |Maximum,|a 23,k |Minimum, ②|a 13,k |Maximum,|a 33,k |Minimum, ③|a 23,k |Maximum,|a 13,k |Minimum, ④|a 23,k |Maximum,|a 33,k |Minimum, ⑤|a 33,k |Maximum,|a 13,k |Minimum, ⑥|a 33,k |Maximum,|a 23,k |The minimum number of 6 is reduced to 3: ①|a 13,k |Maximum,②|a 23,k |Maximum,③|a 33,k |Maximum, reducing the amount of calculation.
[0089] Secondly, although the five-parameter coordinate transformation matrix solution method of the present invention has one more differential equation than the quaternion method, the coordinate transformation matrix represented by the quaternion
[0090]
[0091] Where λ is the element of the scalar part of the quaternion, and ρ1, ρ2, and ρ3 are the three elements of the vector part of the quaternion.
[0092] The coordinate transformation matrix must correspond to no less than two sets of parameters: λ, ρ1, ρ2, ρ3; -λ, -ρ1, -ρ2, -ρ3, which causes the coordinate transformation matrix and the carrier's relative navigation system rotation relationship to be non-unique. The five-parameter coordinate transformation matrix of the present invention ensures that the relationship between the five parameters and the coordinate transformation matrix is unique.
[0093] Finally, the 5-parameter coordinate transformation matrix solution method of the present invention 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 angle", Journal of Chinese Inertial Technology, Vol. 29 No. 5, 2021). The 9 parameters solved are as follows Figure 3 As shown, the three-dimensional trajectory of the aircraft motion solved by navigation is shown in Figure 4 It can be seen that the motion process of the aircraft with large attitude and high maneuverability is well reproduced, which shows that the 5-parameter coordinate transformation matrix solution method of the present invention can realize full attitude solution, which is beneficial to flight control.
[0094] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.
[0095] 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.
Claims
1. A method for solving the coordinate transformation matrix of inertial navigation 5 parameters based on normalization, characterized in that: include: The calculated strapdown navigation system body coordinate system relative to the navigation coordinate system is t k The coordinate transformation matrix at the moment; Calculated at t k The angular velocity of the body coordinate system relative to the navigation coordinate system at this moment; Select 5 parameters from the coordinate transformation matrix; According to the coordinate transformation matrix and the angular velocity, the five parameters are updated and solved to obtain an updated coordinate transformation matrix; Speed update and position update are performed according to the updated coordinate transformation matrix.
2. The 4-parameter coordinate transformation matrix solving method according to claim 1, characterized in that: The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is: In the formula, a 11 、a 12 、a 13 、a 21 、a 22 、a 23 、a 31 、a 32 and a 33 9 parameters of the coordinate transformation matrix; In t k The coordinate transformation matrix of the strapdown navigation system's main coordinate system relative to the navigation coordinate system is: In the formula, a 11,k 、a 12,k 、a 13,k 、a 21,k 、a 22,k 、a 23,k 、a 31,k 、a 32,k and a 33,k For k 9 parameters of the moment coordinate transformation matrix.
3. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 2, characterized in that: t k The angular velocity of the body coordinate system relative to the navigation coordinate system at the moment is in, are the angular velocities of the body on the x, y, and z axes respectively.
4. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 3, characterized in that: The way to select 5 of the 9 parameters from the coordinate transformation matrix is: Select a in the coordinate transformation matrix 11,k 、a 21,k and a 31,k , and in the 3 elements a 13,k 、a 23,k and a 33,k The remaining two elements after excluding the maximum absolute value are used as 5 parameters.
5. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 4, characterized in that: when|a 13,k |Maximum, select 5 parameters a 11,k 、a 21,k 、a 23,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 The calculation formula is: Put t k+1 Moment a 11,k+1 、a 21,k+1 、a 23,k+1 、a 31,k+1 and a 33,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system: ΔT is the sampling time, and sign() is the sign function.
6. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 4, characterized in that: when|a 23,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 31,k and a 33,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 31,k+1 and a 33,k+1 The calculation formula is: Put t k+1 Moment a 11,k+1 、a 12,k+1 、a 13,k+1 、a 31,k+1 and a 32,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system: ΔT is the sampling time, and sign() is the sign function.
7. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 4, characterized in that: when|a 33,k |Maximum, select 5 parameters a 11,k 、a 13,k 、a 21,k 、a 23,k and a 31,k ; At the next moment t k+1 =t k +ΔT integrated value a 11,k+1 、a 13,k+1 、a 21,k+1 、a 23,k+1 and a 31,k+1 The calculation formula is: Put t k+1 Moment a 11,k+1 、a 12,k+1 、a 21,k+1 、a 22,k+1 and a 23,k+1 Substitute the values into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system: ΔT is the sampling time, and sign() is the sign function.
8. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 1, characterized in that: According to the updated coordinate transformation matrix, updating the speed and the position includes: When p is an inertial system, the navigation equation is: in, is the updated coordinate transformation matrix, is the apparent acceleration, is the gravitational acceleration, V is the updated velocity, T is the transpose of the matrix, and r is the updated position.
9. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 1, characterized in that: The gyroscopes installed on the strapdown inertial system body are three single-degree-of-freedom gyroscopes.
10. The method for solving the inertial navigation 5 parameter coordinate transformation matrix according to claim 1, characterized in that: The gyroscopes installed on the strapdown inertial system body are two dual-degree-of-freedom gyroscopes.
Citation Information
Patent Citations
Initial alignment method for MEMS-IMU
CN105180937A
Error-free fiber gyroscope strapdown inertial navigation system speed update method
CN110319833A
Inertial navigation high-precision attitude angle calculation method and system based on Krylov angle
CN113447025A
Inertial navigation six-parameter coordinate transformation matrix resolving method
CN116642485A
Inertial navigation five-parameter coordinate transformation matrix resolving method
CN116642486A
