Inertial navigation 3-parameter variable structure quaternion solution method

Abstract

The application discloses a 3-parameter variable structure quaternion solution method for inertial navigation, which comprises the following steps: determining 3 parameters in a quaternion as real-time updated variables; adopting an integral formula to perform integral updating calculation on a 3-parameter differential equation to obtain values of the 3 parameters at t k ; calculating the remaining 1 parameter in the quaternion to finally obtain an updated value of the quaternion, and obtaining a coordinate transformation matrix to support velocity updating and position updating, so as to improve the precision of inertial navigation. The application takes angular rates output by gyroscopes orthogonally installed on a body of a strapdown inertial system as input information, realizes real-time updating of the coordinate transformation matrix for inertial navigation, and simplifies the calculation amount while ensuring the solution precision.

Description

TECHNICAL FIELD

[0001] The present application relates to a kind of inertial navigation 3 parameter variable structure quaternion solution method, belong to the technical field of inertial navigation. BACKGROUND

[0002] Inertial navigation is widely used in spacecraft, aircraft, ship and other fields, and 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 calculation. At present, the methods for determining the attitude information include direction cosine kinematic equation, Euler-Krylov angle kinematic equation and quaternion kinematic equation.

[0003] The quaternion parameter and its differential equation commonly used in engineering have only 4 parameters. To further reduce the number of integral equations and improve the calculation efficiency, a new method for solving the carrier coordinate system relative to the navigation system is needed to improve the rapidity and accuracy of inertial navigation and flight control. SUMMARY

[0004] The technical problem to be solved by the present application is to overcome the shortcomings of the prior art, while ensuring the accuracy of the solution, the calculation efficiency is improved.

[0005] The object of the present application is achieved by the following technical solutions:

[0006] A kind of inertial navigation 3 parameter variable structure quaternion solution method, the angular rate output by the gyroscope orthogonally installed on the body of strapdown inertial system is taken as the input information of the kinematic equation of three parameters of quaternion, and the real-time updating of quaternion is realized by integral solution. No singular value appears 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.

[0007] A kind of inertial navigation 3 parameter variable structure quaternion solution method, comprising:

[0008] The coordinate transformation matrix of the body coordinate system of strapdown navigation system relative to the navigation coordinate system at time t k is calculated;

[0009] The angular velocity of the body coordinate system relative to the navigation coordinate system at time t k is calculated;

[0010] Three parameters are selected from the quaternion of strapdown navigation system at time t k ;

[0011] According to the three parameters and the angular velocity, the three parameters are updated and solved to obtain the updated quaternion;

[0012] According to the updated quaternion, a coordinate transformation matrix update, a velocity update and a position update are performed.

[0013] Further, in the above solving method, the quaternion of the strapdown navigation system at time t k is calculated, specifically:

[0014]

[0015] In the formula, λ k is an element of the scalar part of the quaternion at time t k , ρ 1,k , ρ 2,k and ρ 3,k are three elements of the vector part of the quaternion at time t k .

[0016] Further, in the above solving method, the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k is calculated, specifically:

[0017]

[0018] wherein ω , ω and ω

[0019] are the angular velocities on the body x, y and z axes respectively.

[0020] Further, in the above solving method, three elements of the four elements of the quaternion are selected, specifically: the absolute values of the four elements λ k , ρ 1,k , ρ 2,k and ρ 3,k of the quaternion are sorted in size, and the three relatively smaller elements are selected as the three parameters.

[0021] When |λ k | is the largest, the three parameters ρ 1,k , ρ 2,k and ρ 3,k are selected; at the next time t k+1 = t k + ΔT, the updated values ρ 1,k+1 , ρ 2,k+1 and ρ 3,k+1 are obtained by integration;

[0022] When |ρ 1,k | is the largest, the three parameters λ k , ρ 2,k and ρ3,k ; at next time t k+1 = t k + ΔT the updated values λ k , ρ 2,k+1 and ρ 3,k+1 are obtained by integration;

[0023] When |ρ 2,k | is maximum, the three parameters λ k , ρ 1,k and ρ 3,k are selected; at next time t k+1 = t k + ΔT the updated values λ k , ρ 1,k+1 and ρ 3,k+1 are obtained by integration;

[0024] When |ρ 3,k | is maximum, the three parameters λ k , ρ 1,k and ρ 2,k are selected; at next time t k+1 = t k + ΔT the updated values λ k , ρ 1,k+1 and ρ 2,k+1 are obtained by integration.

[0025] Further, in the above solving method, when |λ k | is maximum, the three parameters ρ 1,k , ρ 2,k and ρ 3,k are selected; at next time t k+1 = t k + ΔT the values ρ 1,k+1 , ρ 2,k+1 and ρ 3,k+1 after integration, the specific calculation formula is:

[0026]

[0027] Substitute ρ 1,k+1 , ρ 2,k+1 and ρ 3,k+1 into the following formula to solve the fourth element of the quaternion at time t k+1

[0028]

[0029] sign() is a sign function;

[0030] Further, in the above solving method, when |ρ 1,k | is maximum, the three parameters λ k , ρ​2,k and p 3,k ; at the next time t k+1 = t k + ΔT, the values λ k+1 , p 2,k+1 and p 3,k+1 after integration are calculated by the following formulae:

[0031]

[0032] Substitute λ k+1 , p 2,k+1 and p 3,k+1 into the following formula to solve the fourth element of the quaternion at time t k+1

[0033]

[0034] Further, in the above solving method, when |p 2,k | is maximum, three parameters λ k , p 1,k and p 3,k are selected; at the next time t k+1 = t k + ΔT, the values λ k+1 , p 1,k+1 and p 3,k+1 after integration are calculated by the following formulae:

[0035]

[0036] Substitute λ k+1 , p 1,k+1 and p 3,k+1 into the following formula to solve the fourth element of the quaternion at time t k+1

[0037]

[0038] Further, in the above solving method, when |p 3,k | is maximum, three parameters λ k , p 1,k and p 2,k are selected; at the next time t k+1 = t k + ΔT, the values λ k+1 , p 1,k+1 and p 2,k+1 after integration are calculated by the following formulae:

[0039]

[0040] Substitute λ k+1 , p 1,k+1 and p​​2,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is

[0041]

[0042] Furthermore, in the above solution method, trigonometric function solution is preferred for integral calculation, at the next time t k+1 =t k The value of +ΔT after integration is ρ 1,k+1 ρ 2,k+1 and ρ 3,k+1 The calculation formula is:

[0043]

[0044] In the formula, angular velocity Calculate the angular increment within the sampling time ΔT.

[0045] Furthermore, in the above solution method, trigonometric function solution is preferred for integral calculation, at the next time t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 2,k+1 and ρ 3,k+1 The calculation formula is:

[0046]

[0047] In the formula, angular velocity Calculate the angular increment within the sampling time ΔT.

[0048] Furthermore, in the above solution method, trigonometric function solution is preferred for integral calculation, at the next time t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 1,k+1 and ρ 3,k+1 The calculation formula is:

[0049]

[0050] Furthermore, in the above solution method, trigonometric function solution is preferred for integral calculation, at the next time t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 1,k+1 and ρ 2,k+1 The calculation formula is:

[0051]

[0052] Further, in the above solving method, according to the updated quaternion, the coordinate transformation matrix updating, velocity updating and position updating are carried out, specifically:

[0053] When p is an inertial system, the navigation equation is:

[0054]

[0055] wherein, is the updated coordinate transformation matrix, is the visual acceleration, is the gravity 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 application has the following beneficial effects:

[0057] (1) The inertial navigation 3-parameter variable structure quaternion solving method disclosed by the present application has one less differential equation than the 4-parameter quaternion solving method, and the calculation amount is reduced.

[0058] (2) The inertial navigation 3-parameter variable structure quaternion solving method disclosed by the present application has no singular point in the solving process, and can realize high-precision solving of all attitudes.

[0059] (3) The inertial navigation 3-parameter variable structure quaternion solving method disclosed by the present application has a simple structure and is easy to implement in engineering. BRIEF DESCRIPTION OF DRAWINGS

[0060] Figure 1 is a step flow chart of the inertial navigation 3-parameter variable structure quaternion solving method of the present application;

[0061] Figure 2 is a schematic diagram of the relationship between the body of the strapdown inertial system and the navigation coordinate system of the present application;

[0062] Figure 3 is the four parameters of the quaternion solved based on the inertial navigation 3-parameter variable structure quaternion solving method of the present application;

[0063] Figure 4 is the working period of the four structures solved based on the inertial navigation 3-parameter variable structure quaternion solving method of the present application;

[0064] Figure 5 is the three-dimensional trajectory of the aircraft motion solved based on the inertial navigation 3-parameter variable structure quaternion solving method of the present application. DETAILED DESCRIPTION

[0065] To make the purpose, technical scheme and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0066] A 3-parameter variable structure quaternion solution method for inertial navigation, comprising: determining 3 parameters in a quaternion as real-time updated variables; using an integral formula to perform integral update calculation on a 3-parameter differential equation to obtain values of the 3 parameters at t k ; calculating the remaining 1 parameter in the quaternion, and finally obtaining the updated value of the quaternion, on the basis of which a coordinate transformation matrix is obtained to support velocity update and position update, so as to improve the accuracy of inertial navigation. The 3-parameter variable structure quaternion solution method for inertial navigation takes the angular rate output by a gyroscope orthogonally mounted on a body of a strapdown inertial system as input information, realizes real-time update of the coordinate transformation matrix for inertial navigation, and simplifies the calculation amount while ensuring the solution accuracy.

[0067] The step flow chart of the 3-parameter variable structure quaternion solution method for inertial navigation in the present application is shown in Figure 1 . The 3-parameter variable structure quaternion solution method for inertial navigation comprises:

[0068] As shown in Figure 2 , a strapdown inertial system fixed to a carrier is used for implementation, the body coordinate system (b system, as a moving system) corresponding to the strapdown inertial system is Ox′y′z′, a navigation coordinate system (p system, as a fixed system) describing the rotational motion of the carrier is Oxyz, and the origins of the two coordinate systems coincide.

[0069] (1), a coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at t k is calculated;

[0070]

[0071] (2), the angular velocity of the body relative to the navigation system at t k is obtained according to the angular velocity output by the gyroscope mounted on the body of the strapdown inertial system

[0072]

[0073] wherein ω x, ω y and ω

[0074] z are the angular velocities on the x, y and z axes of the body respectively. (3), the quaternion of the strapdown navigation system at t k , specifically:

[0075]

[0076] In the formula, λ k is an element of the scalar part of the quaternion at t k , ρ 1,k , ρ 2,k and ρ 3,k are the elements of the vector part of the quaternion at tk The 3 elements of the quaternion vector part at time t.

[0077] Let the sampling time be ΔT, and let the absolute values of the 4 elements λ k , ρ 1,k , ρ 2,k , and ρ 3,k of the quaternion be sorted in descending order, and let the 3 smaller elements be selected as the 3 parameters.

[0078] The 3 parameters are updated and the quaternion is calculated in the following 4 cases:

[0079] (3.1) When |λ k | is the largest, the 3 parameters ρ 1,k , ρ 2,k , and ρ 3,k are selected; the values ρ k+1 , ρ k , and ρ 1,k+1 after integration at the next time t 2,k+1 = t 3,k+1 + ΔT are calculated as follows:

[0080]

[0081] Substitute ρ 1,k+1 , ρ 2,k+1 , and ρ 3,k+1 into the following equation to calculate the 4th element of the quaternion at time t k+1

[0082]

[0083] (3.2) When |ρ 1,k | is the largest, the 3 parameters λ k , ρ 2,k , and ρ 3,k are selected; the values λ k+1 , ρ k , and ρ k+1 after integration at the next time t 2,k+1 = t 3,k+1 + ΔT are calculated as follows:

[0084]

[0085] Substitute λ k+1 , ρ 2,k+1 , and ρ 3,k+1 into the following equation to calculate the 4th element of the quaternion at time t k+1

[0086]

[0087] ​​(3.3) When |ρ 2,k | is maximum, three parameters λ k , ρ 1,k and ρ 3,k are selected; the values of λ k+1 , ρ k and ρ k+1 after integration at the next time t 1,k+1 = t 3,k+1 + ΔT are calculated according to the following formula:

[0088]

[0089] Substitute λ k+1 , ρ 1,k+1 and ρ 3,k+1 into the following formula to solve the fourth element of the quaternion at time t k+1

[0090]

[0091] (3.4) When |ρ 3,k | is maximum, three parameters λ k , ρ 1,k and ρ 2,k are selected; the values of λ k+1 , ρ k and ρ k+1 after integration at the next time t 1,k+1 = t 2,k+1 + ΔT are calculated according to the following formula:

[0092]

[0093] Substitute λ k+1 , ρ 1,k+1 and ρ 2,k+1 into the following formula to solve the fourth element of the quaternion at time t k+1

[0094]

[0095] (4) Update the calculated quaternion according to the integration, and support the update of the coordinate transformation matrix, the update of the velocity and the update of the position to improve the precision of the inertial navigation.

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

[0097] In step (4), the updated coordinate transformation matrix , the visual acceleration and the gravitational acceleration are used as the velocity differential equation ​​The input of the position differential equation is the updated velocity V, and the updated position r is obtained after integral calculation. The input of the position differential equation is the updated velocity V, and the updated position r is obtained after integral calculation.

[0098] The following is illustrated by a set of examples.

[0099] Embodiment:

[0100] The 3-parameter variable structure quaternion solution method of the application only needs to integrate 3 differential equations, while the 4-parameter quaternion needs to solve the following 4 differential equations.

[0101]

[0102] Therefore, the 3-parameter variable structure quaternion solution method of the application reduces the calculation amount caused by integration compared with the 4-parameter quaternion solution method.

[0103] The data of a certain full attitude motion of an airplane ( "Full attitude navigation solution method based on extended Krylov angle", Journal of Chinese Inertial Technology, Vol. 29 No. 5, 2021) is navigated by using the 3-parameter variable structure quaternion solution method of the application. The solved 4 parameters are as shown in Figure 3 The working time periods of the four structures of m=1 (|λ|max), m=2 (|ρ1|max), m=3 (|ρ2|max), and m=4 (|ρ3|max) are as shown in Figure 4 The three-dimensional trajectory of the airplane motion in navigation solution is shown in Figure 5 It can be seen that the large attitude high maneuvering motion process of the airplane is well reproduced, which shows that the 5-parameter coordinate transformation matrix solution method of the application can realize full attitude solution and is beneficial to flight control.

[0104] The contents not described in detail in the specification of the application are the known technology of those skilled in the art.

[0105] Although the application has been disclosed as above with the preferred embodiments, it is not intended to limit the application, and any person skilled in the art can make possible changes and modifications to the technical solutions of the application by using the disclosed methods and technical contents without departing from the spirit and scope of the application. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the application, which does not depart from the technical solutions of the application, belongs to the protection scope of the technical solutions of the application.

Claims

1. A method for solving quaternions of three parameters in inertial navigation, characterized in that, include: The calculation yields the relative coordinate system of the strapdown navigation system to the navigation coordinate system at time t. k The coordinate transformation matrix at time step; Calculate at t k The angular velocity of the body coordinate system relative to the navigation coordinate system at any given moment; From strapdown navigation system in t k Three parameters are selected from the quaternions at each time step; Based on the selected three parameters and angular velocity, the three parameters are updated and solved to obtain the updated quaternion; Based on the updated quaternion, the coordinate transformation matrix is ​​updated, the velocity is updated, and the position is updated. t k The angular velocity of the body coordinate system relative to the navigation coordinate system at time t is: in, These are the angular velocities of the body along the x, y, and z axes, respectively. Strapdown navigation system in t k The quaternion at time t is: In the formula, λ k For in t k The elements of the scalar part of the quaternion at time ρ 1,k ρ 2,k and ρ 3,k For in t k The three elements of the time-time quaternion vector part; From strapdown navigation system in t k Three parameters are selected from the quaternion at time point, specifically: For the 4 elements λ of the quaternion k ρ 1,k ρ 2,k and ρ 3,k Sort by absolute value and select the three smaller elements as the three parameters.

2. The inertial navigation 3-parameter variable structure quaternion solution method according to claim 1, characterized in that, The update calculation of the three parameters based on the selected three parameters and angular velocity includes: When |λ k At its maximum, select 3 parameters ρ. 1,k ρ 2,k and ρ 3,k ; at the next moment t k+1 =t k The updated value ρ is obtained by integrating +ΔT. 1,k+1 ρ 2,k+1 and ρ 3,k+1 ; When |ρ 1,k At its maximum, select 3 parameters λ. k ρ 2,k and ρ 3,k ; at the next moment t k+1 =t k The updated value λ is obtained by integrating +ΔT. k ρ 2,k+1 and ρ 3,k+1 ; When |ρ 2,k At its maximum, select 3 parameters λ. k ρ 1,k and ρ 3,k ; at the next moment t k+1 =t k The updated value λ is obtained by integrating +ΔT. k ρ 1,k+1 and ρ 3,k+1 ; When |ρ 3,k At its maximum, select 3 parameters λ. k ρ 1,k and ρ 2,k ; at the next moment t k+1 =t k The updated value λ is obtained by integrating +ΔT. k ρ 1,k+1 and ρ 2,k+1 ; ΔT is the sampling time.

3. The inertial navigation 3-parameter variable structure quaternion solution method according to claim 2, characterized in that: When |λ k At its maximum, select 3 parameters ρ. 1,k ρ 2,k and ρ 3,k ; at the next moment t k+1 =t k The value of +ΔT after integration is ρ 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 formula and solve for t. k+1 The fourth element of the quaternion at time is sign() is the sign function; When |ρ 1,k At its maximum, select 3 parameters λ. k ρ 2,k and ρ 3,k ; at the next moment t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 2,k+1 and ρ 3,k+1 for: Put λ k+1 ρ 2,k+1 and ρ 3,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is When |ρ 2,k At its maximum, select 3 parameters λ. k ρ 1,k and ρ 3,k ; at the next moment t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 1,k+1 and ρ 3,k+1 for: Put λ k+1 ρ 1,k+1 and ρ 3,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is When |ρ 3,k At its maximum, select 3 parameters λ. k ρ 1,k and ρ 2,k ; at the next moment t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 1,k+1 and ρ 2,k+1 for: Put λ k+1 ρ 1,k+1 and ρ 2,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is 4. The inertial navigation 3-parameter variable structure quaternion solution method according to claim 3, characterized in that, At the next moment t k+1 =t k All integral calculations of +ΔT are solved using trigonometric functions.

5. The inertial navigation 3-parameter variable structure quaternion solution method according to claim 2, characterized in that, When |λ k At its maximum, select 3 parameters ρ. 1,k ρ 2,k and ρ 3,k ; at the next moment t k+1 =t k The value of +ΔT after integration is ρ 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 formula and solve for t. k+1 The fourth element of the quaternion at time is sign() is the sign function; in the formula, angular velocity Calculate the angular increment within the sampling time ΔT; When |ρ 1,k At its maximum, select 3 parameters λ. k ρ 2,k and ρ 3,k ; at the next moment t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 2,k+1 and ρ 3,k+1 for: Put λ k+1 ρ 2,k+1 and ρ 3,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is When |ρ 2,k At its maximum, select 3 parameters λ. k ρ 1,k and ρ 3,k ; at the next moment t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 1,k+1 and ρ 3,k+1 for: Put λ k+1 ρ 1,k+1 and ρ 3,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is When |ρ 3,k At its maximum, select 3 parameters λ. k ρ 1,k and ρ 2,k ; at the next moment t k+1 =t k The value of +ΔT after integration is λ k+1 ρ 1,k+1 and ρ 2,k+1 for: Put λ k+1 ρ 1,k+1 and ρ 2,k+1 Substitute into the following formula and solve for t. k+1 The fourth element of the quaternion at time is 6. The inertial navigation 3-parameter variable structure quaternion solution method according to claim 1, characterized in that, Based on the updated quaternion, the coordinate transformation matrix update, velocity update, and position update include: When p is an inertial frame of reference, the navigation equations are: in, This is the updated coordinate transformation matrix. For apparent acceleration, Let V be the acceleration due to gravity, V be the updated velocity, T be the transpose of the matrix, and r be the updated position.

7. The inertial navigation 3-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.

8. The inertial navigation 3-parameter variable structure quaternion solution method according to claim 1, characterized in that, The gyroscopes installed on the strapdown inertial system body are two two-degree-of-freedom gyroscopes.

Images