Inertial navigation three-parameter variable structure quaternion resolving method
Through the 3-parameter variable structure quaternion solution method of inertial navigation, the quaternion is updated in real time using the angular rate information of the strap-inerature inertial system, the problems of low calculation efficiency and singular values in the existing technology are solved, and high-precision Full-pose navigation and flight control.
Patent Information
- Application Number
- CN202410983873.2
- 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
The existing inertial navigation technology has low calculation efficiency when determining attitude information, and it is difficult to avoid singular values, which affects the solution accuracy.
Using the quaternion solution method of 3-parameter variable structure inertial navigation, the quaternion is updated in real time through integrated solution, reducing the calculation amount and avoiding singular points, realizing high-precision solution for the full posture .
It improves computing efficiency and solution accuracy, simplifies engineering implementation, and realizes high-precision navigation and flight control in full posture.
Smart Images

Figure CN118999543A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to an inertial navigation three-parameter variable structure quaternion 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. After mathematical solution, the coordinate transformation matrix of the carrier coordinate system relative to the navigation system is given. At present, the methods for determining attitude information include direction cosine kinematic equations, Euler-Krylov angular kinematic equations and quaternion kinematic equations.
[0003] There are only four quaternion parameters and their differential equations that are widely used in engineering. In order to further reduce the number of integral equations and improve computational efficiency, it is necessary to study a new method for solving the carrier coordinate system relative to the navigation system to improve the speed and accuracy of inertial navigation and flight control. Summary of the invention
[0004] The technical problem to be solved by the present invention is to overcome the deficiencies of the prior art and improve the calculation efficiency while ensuring the calculation accuracy.
[0005] The purpose of the present invention is achieved through the following technical solutions:
[0006] An inertial navigation three-parameter variable structure quaternion solution method uses the angular rate output by a gyroscope orthogonally installed on a strapdown inertial system body as the input information of the quaternion three-parameter kinematic equation. The real-time update of the quaternion is achieved through integral solution. No singular values appear in the solution process, ensuring the full attitude of the body coordinate system relative to the navigation coordinate system and the requirement of small calculation amount.
[0007] A method for solving inertial navigation three-parameter variable structure quaternion, comprising:
[0008] The calculated strapdown navigation system body coordinate system relative to the navigation coordinate system is t k The coordinate transformation matrix at the moment;
[0009] Calculated at t k The angular velocity of the body coordinate system relative to the navigation coordinate system at this moment;
[0010] From the strapdown navigation system in t k Select 3 parameters from the quaternion at the moment;
[0011] According to the three parameters and the angular velocity, the three parameters are updated and solved to obtain an updated quaternion;
[0012] According to the updated quaternion, coordinate transformation matrix update, velocity update and position update are performed.
[0013] Furthermore, in the above-mentioned solution method, the strapdown navigation system is calculated at t k The quaternion at the moment is:
[0014]
[0015] In the formula, λ k For k The element of the scalar part of the moment quaternion, ρ 1,k , 2,k and ρ 3,k For k The 3 elements of the vector portion of the moment quaternion.
[0016] 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:
[0017]
[0018] in, are the angular velocities of the body on the x, y, and z axes respectively.
[0019] Furthermore, in the above-mentioned solution method, the step of selecting three elements from the four elements of the quaternion is as follows: for the four elements λ of the quaternion k , 1,k , 2,k and ρ 3,k The absolute values are sorted and three relatively small elements are selected as three parameters.
[0020] Furthermore, in the above-mentioned solution method, the three parameters are updated and solved according to the quaternion and the angular velocity, and the updated quaternion is solved, specifically:
[0021] When |λ k |Maximum, select 3 parameters ρ 1,k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value ρ 1,k+1 , 2,k+1 and ρ 3,k+1 ;
[0022] When |ρ 1,k |Maximum, select 3 parameters λ k , 2,k and ρ3,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value λ k , 2,k+1 and ρ 3,k+1 ;
[0023] When |ρ 2,k |Maximum, select 3 parameters λ k , 1,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value λ k , 1,k+1 and ρ 3,k+1 ;
[0024] When |ρ 3,k |Maximum, select 3 parameters λ k , 1,k and ρ 2,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value λ k , 1,k+1 and ρ 2,k+1 .
[0025] Furthermore, in the above solution method, when |λ k |Maximum, select 3 parameters ρ 1,k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration ρ 1,k+1 , 2,k+1 and ρ 3,k+1 , the specific calculation formula is:
[0026]
[0027] Put ρ 1,k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0028]
[0029] sign() is the sign function;
[0030] Furthermore, in the above solution method, when |ρ 1,k |Maximum, select 3 parameters λ k ,2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 2,k+1 and ρ 3,k+1 , the calculation formula is:
[0031]
[0032] Put λ k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0033]
[0034] Furthermore, in the above solution method, when |ρ 2,k |Maximum, select 3 parameters λ k , 1,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 3,k+1 , the calculation formula is:
[0035]
[0036] Put λ k+1 , 1,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0037]
[0038] Furthermore, in the above solution method, when |ρ 3,k |Maximum, select 3 parameters λ k , 1,k and ρ 2,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 2,k+1 , the calculation formula is:
[0039]
[0040] Put λ k+1 , 1,k+1 and ρ2,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0041]
[0042] Furthermore, in the above-mentioned solution method, the integral calculation is preferably solved by trigonometric function. At the next moment t k+1 =t k +ΔT after integration ρ 1,k+1 , 2,k+1 and ρ 3,k+1 , the calculation formula is:
[0043]
[0044] In the formula, is the angular velocity Calculate the angular increment within the sampling time ΔT.
[0045] Furthermore, in the above-mentioned solution method, the integral calculation is preferably solved by trigonometric function. At the next moment t k+1 =t k +ΔT after integration λ k+1 , 2,k+1 and ρ 3,k+1 , the calculation formula is:
[0046]
[0047] In the formula, is the angular velocity Calculate the angular increment within the sampling time ΔT.
[0048] Furthermore, in the above-mentioned solution method, the integral calculation is preferably solved by trigonometric function. At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 3,k+1 , the calculation formula is:
[0049]
[0050] Furthermore, in the above-mentioned solution method, the integral calculation is preferably solved by trigonometric function. At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 2,k+1 , the calculation formula is:
[0051]
[0052] Furthermore, in the above-mentioned solution method, coordinate transformation matrix update, velocity update and position update are performed according to the updated quaternion, specifically:
[0053] When p is an inertial system, the navigation equation is:
[0054]
[0055] 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.
[0056] Compared with the prior art, the present invention has the following beneficial effects:
[0057] (1) The present invention discloses a 3-parameter variable structure quaternion solution method for inertial navigation, which has one less differential equation than a 4-parameter quaternion solution method, thus reducing the amount of calculation;
[0058] (2) The present invention discloses a method for solving the three-parameter variable structure quaternion of inertial navigation, which has no singular points in the solving process and can realize high-precision solution of the whole attitude;
[0059] (3) The present invention discloses a method for solving three-parameter variable structure quaternion in inertial navigation, which has a simple structure and is easy to implement in engineering. BRIEF DESCRIPTION OF THE DRAWINGS
[0060] Figure 1 It is a flow chart of the steps of the inertial navigation three-parameter variable structure quaternion solution method of the present invention;
[0061] 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;
[0062] Figure 3 The four parameters of the quaternion are solved based on the inertial navigation three-parameter variable structure quaternion solution method of the present invention;
[0063] Figure 4 The working periods of the four structures solved by the inertial navigation three-parameter variable structure quaternion solution method of the present invention;
[0064] Figure 5 The three-dimensional trajectory of the aircraft motion is solved based on the inertial navigation three-parameter variable structure quaternion solution method of the present invention. DETAILED DESCRIPTION
[0065] 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.
[0066] A method for solving an inertial navigation three-parameter variable structure quaternion comprises: determining three parameters in the quaternion as variables to be updated in real time; using an integral formula to perform integral update calculation on the three-parameter differential equation to obtain the three 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.
[0067] The flow chart of the steps of the inertial navigation three-parameter variable structure quaternion solution method in the present invention is as follows: Figure 1 The inertial navigation three-parameter variable structure quaternion solution method includes:
[0068] like Figure 2 As shown, it is implemented based on a strapdown inertial system fixed to the carrier, and the body coordinate system (b system, a moving system) corresponding to the strapdown inertial system is Ox′y′z′; the navigation coordinate system (p system, a fixed system) describing the rotational motion of the carrier is Oxyz; the origins of the two coordinate systems coincide.
[0069] (1) Calculate the relative position of the strapdown navigation system's main coordinate system to the navigation coordinate system at t k The coordinate transformation matrix at the moment;
[0070]
[0071] (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
[0072]
[0073] in, are the angular velocities of the body on the x, y, and z axes respectively.
[0074] (3) Strapdown navigation system k The quaternion at the moment is:
[0075]
[0076] In the formula, λ k For k The element of the scalar part of the moment quaternion, ρ 1,k , 2,k and ρ 3,k Fork The 3 elements of the vector portion of the moment quaternion.
[0077] Assume the sampling time is ΔT, for the four elements λ of the quaternion k , 1,k , 2,k and ρ 3,k The absolute values are sorted and three relatively small elements are selected as three parameters.
[0078] In the following four cases, the three parameters are updated and solved, and the quaternion is solved;
[0079] (3.1), when |λ k |Maximum, select 3 parameters ρ 1,k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration ρ 1,k+1 , 2,k+1 and ρ 3,k+1 , the specific calculation formula is:
[0080]
[0081] Put ρ 1,k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0082]
[0083] (3.2), when |ρ 1,k |Maximum, select 3 parameters λ k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 2,k+1 and ρ 3,k+1 , the calculation formula is:
[0084]
[0085] Put λ k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0086]
[0087] (3.3), when |ρ 2,k |Maximum, select 3 parameters λ k , 1,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 3,k+1 , the calculation formula is:
[0088]
[0089] Put λ k+1 , 1,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0090]
[0091] (3.4), when |ρ 3,k |Maximum, select 3 parameters λ k , 1,k and ρ 2,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 2,k+1 , the calculation formula is:
[0092]
[0093] Put λ k+1 , 1,k+1 and ρ 2,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is
[0094]
[0095] (4) The solved quaternion is updated based on the integral, and the coordinate transformation matrix update, velocity update and position update are supported to improve the accuracy of inertial navigation.
[0096] The gyroscopes installed on the strapdown inertial system body are three single-degree-of-freedom gyroscopes or two double-degree-of-freedom gyroscopes.
[0097] 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.
[0098] The following is a comparison of a set of examples.
[0099] Example:
[0100] The three-parameter variable structure quaternion solution method of the present invention has only three differential equations that need to be solved by integration, while the four-parameter based quaternion needs to solve the following four differential equations.
[0101]
[0102] Therefore, the three-parameter variable structure quaternion solution method of the present invention reduces the amount of calculation caused by integration compared with the four-parameter quaternion solution method.
[0103] The three-parameter variable structure quaternion 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.29No.5, 2021). The four parameters of the solution are as follows Figure 3 As shown, the working time periods of the four structures, namely, m=1 (|λ| is maximum), m=2 (|ρ1| is maximum), m=3 (|ρ2| is maximum), and m=4 (|ρ3| is maximum), are shown in FIG. Figure 4 As shown, the three-dimensional trajectory of the aircraft motion solved by navigation is shown in Figure 5 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.
[0104] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.
[0105] 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 three-parameter variable structure quaternion in inertial navigation, 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; From the strapdown navigation system in t k Select 3 parameters from the quaternion at the moment; According to the selected three parameters and angular velocity, the three parameters are updated and solved to obtain an updated quaternion; According to the updated quaternion, coordinate transformation matrix update, velocity update and position update are performed.
2. The inertial navigation three-parameter variable structure quaternion solution method according to claim 1, characterized in that: t k The angular velocity of the body coordinate system relative to the navigation coordinate system at this moment is: in, are the angular velocities of the body on the x, y, and z axes respectively.
3. The inertial navigation three-parameter variable structure quaternion solution method according to claim 2, characterized in that: Strapdown navigation system k The quaternion at this moment is: In the formula, λ k For k The element of the scalar part of the moment quaternion, ρ 1,k , 2,k and ρ 3,k For k The 3 elements of the vector part of the moment quaternion; From the strapdown navigation system in t k Select 3 parameters from the quaternion at the moment, specifically: For the 4 elements of the quaternion λ k , 1,k , 2,k and ρ 3,k The absolute values are sorted and three relatively small elements are selected as three parameters.
4. The inertial navigation three-parameter variable structure quaternion solution method according to claim 3, characterized in that: According to the selected three parameters and angular velocity, updating and solving the three parameters includes: When |λ k |Maximum, select 3 parameters ρ 1,k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value ρ 1,k+1 , 2,k+1 and ρ 3,k+1 ; When |ρ 1,k |Maximum, select 3 parameters λ k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value λ k , 2,k+1 and ρ 3,k+1 ; When |ρ 2,k |Maximum, select 3 parameters λ k , 1,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value λ k , 1,k+1 and ρ 3,k+1 ; When |ρ 3,k |Maximum, select 3 parameters λ k , 1,k and ρ 2,k ; At the next moment t k+1 =t k +ΔT is integrated to obtain the updated value λ k , 1,k+1 and ρ 2,k+1 ; ΔT is the sampling time.
5. The inertial navigation three-parameter variable structure quaternion solution method according to claim 4, characterized in that: When |λ k |Maximum, select 3 parameters ρ 1,k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration ρ 1,k+1 , 2,k+1 and ρ 3,k+1 for: Put ρ 1,k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is sign() is the sign function; When |ρ 1,k |Maximum, select 3 parameters λ k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 2,k+1 and ρ 3,k+1 for: Put λ k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is When |ρ 2,k |Maximum, select 3 parameters λ k , 1,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 3,k+1 for: Put λ k+1 , 1,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is When |ρ 3,k |Maximum, select 3 parameters λ k , 1,k and ρ 2,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 2,k+1 for: Put λ k+1 , 1,k+1 and ρ 2,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is 6. The inertial navigation three-parameter variable structure quaternion solution method according to claim 5, characterized in that: At the next moment t k+1 =t k +ΔT All integral calculations are solved using trigonometric functions.
7. The inertial navigation three-parameter variable structure quaternion solution method according to claim 4, characterized in that: When |λ k |Maximum, select 3 parameters ρ 1,k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration ρ 1,k+1 , 2,k+1 and ρ 3,k+1 for: Put ρ 1,k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is sign() is the sign function; where, is the angular velocity Calculate the angular increment within the sampling time ΔT; When |ρ 1,k |Maximum, select 3 parameters λ k , 2,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 2,k+1 and ρ 3,k+1 for: Put λ k+1 , 2,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is When |ρ 2,k |Maximum, select 3 parameters λ k , 1,k and ρ 3,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 3,k+1 for: Put λ k+1 , 1,k+1 and ρ 3,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is When |ρ 3,k |Maximum, select 3 parameters λ k , 1,k and ρ 2,k ; At the next moment t k+1 =t k +ΔT after integration λ k+1 , 1,k+1 and ρ 2,k+1 for: Put λ k+1 , 1,k+1 and ρ 2,k+1 Substitute into the following equation and solve for t k+1 The fourth element of the quaternion at time is 8. The inertial navigation three-parameter variable structure quaternion solution method according to claim 1, characterized in that: According to the updated quaternion, updating the coordinate transformation matrix, speed and 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 inertial navigation three-parameter variable structure quaternion solution method 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 inertial navigation three-parameter variable structure quaternion solution method 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
Attitude angle resolving method based on Krelov angle singular condition
CN111623768A
Unscented quaternion attitude estimation method based on weight and reference quaternion correction
CN115655285A
Helicopter landing relative navigation method based on variational Bayesian adaptive filtering
CN115790580A
Modeling method of dyskinesia symptom quantitative evaluation model
CN116597940A
