A method for solving 4-parameter coordinate transformation matrix of inertial navigation

Abstract

The application discloses a 4-parameter coordinate transformation matrix solution method of inertial navigation combining normalization and orthogonality, which comprises the following steps: determining 4 parameters in two columns of a coordinate transformation matrix as real-time updated variables; using an integral formula to perform integral updating calculation on 4-parameter differential equations to obtain values of the 4 parameters at t k time; calculating the remaining 5 parameters of the coordinate transformation matrix to finally obtain coordinate transformation matrix updating and 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 of inertial navigation, and simplifies the calculation amount while ensuring the solution precision.

Description

TECHNICAL FIELD

[0001] The application relates to a method for solving a four-parameter coordinate transformation matrix of inertial navigation, in particular to a method for solving a four-parameter coordinate transformation matrix of inertial navigation combining normalization and orthogonality, and belongs to the technical field of inertial navigation. BACKGROUND

[0002] Inertial navigation is widely used in spacecraft, aircraft, ships and other fields, and mainly serves to determine the position, velocity and attitude information of a carrier relative to a navigation system. A strapdown inertial system is directly connected to the carrier, and a coordinate transformation matrix of a carrier coordinate system relative to the navigation system is obtained by measuring the angular velocity through a gyroscope and performing mathematical calculation.

[0003] At present, methods for determining attitude information include a direction cosine kinematic equation, an Euler-Krylov angle kinematic equation and a quaternion kinematic equation.

[0004] The quaternion parameter and its differential equation, which are most commonly used in engineering, have only four parameters, but the 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 uniquely related to the four parameters, because one coordinate transformation matrix can correspond to two different quaternions.

[0005] Relatively speaking, the Euler-Krylov angle kinematic equation has only three parameters, but in the technical field of Inertial Devices (Part 1) (China Astronautics Publishing House) on page 46, it is considered that the kinematic equation described by the Euler-Krylov angle has a singularity, and the equation will degenerate. A method for solving the attitude based on the Krylov angle is proposed in Chinese patent CN202010333184.9, which can realize the full attitude motion description of the carrier, but the problem is that when the pitch angle is 90 degrees, the error of the attitude solution is large when discretization processing is performed, thereby causing large errors in the speed and position solution.

[0006] The method for representing the rotational motion relationship between two coordinate systems and the attitude matrix by using the direction cosine array is convenient, intuitive and easy to understand, but the parameters of the transformation matrix and its differential equation have nine parameters, and the integral calculation amount is large. In the technical field of Inertial Devices (Part 2) (China Astronautics Publishing House) in Chapter 18, a method for solving the coordinate transformation matrix based on the direction cosine kinematic equation and the quaternion kinematic equation is given.

[0007] Under the premise of full attitude, in order to realize one-to-one correspondence between the coordinate transformation matrix and the parameters, and solve the problem of large amount of calculation caused by too many parameters of the direction cosine matrix 9, a 5-parameter coordinate transformation matrix solving method based on orthogonality of inertial navigation is proposed in Chinese patent CN202210617894.3, but there is still room for improvement in further reducing the number of differential equations and improving the rapidity and accuracy of inertial navigation and flight control. SUMMARY

[0008] The technical problem to be solved by the present application is to overcome the shortcomings of the prior art and provide a 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality.

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

[0010] A 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality, which takes the angular rate output by the gyroscope orthogonally installed on the body of the strapdown inertial system as the input information of the kinematic equation of the 4 parameters of the coordinate transformation matrix, realizes real-time updating of the inertial navigation coordinate transformation matrix through integral solving, and does not appear singular value in the solving process, thereby ensuring the full attitude of the body coordinate system relative to the navigation coordinate system and the requirement of small amount of calculation.

[0011] Specifically, it includes:

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

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

[0014] Four parameters are selected from the coordinate transformation matrix;

[0015] The four parameters are updated and solved according to the coordinate transformation matrix and the angular velocity, and an updated coordinate transformation matrix is obtained;

[0016] According to the updated coordinate transformation matrix, the velocity is updated and the position is updated.

[0017] Preferably, the coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system is:

[0018]

[0019] In the formula, a 11 , a 12 , a 13 , a 21 , a 22 , a23 a 31 a 32 and a 33 These are the nine parameters of the coordinate transformation matrix;

[0020] In t k The coordinate transformation matrix of the body coordinate system relative to the navigation coordinate system in a time-strap navigation 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 in t k Nine parameters of the time coordinate transformation matrix.

[0023] Preferred, t k The angular velocity of the body coordinate system relative to the navigation coordinate system at any given time is as follows:

[0024]

[0025] Where, ω xb ω yb ω zb These are the angular velocities on the x, y, and z axes of the body coordinate system, respectively.

[0026] Preferably, the step of selecting 4 parameters from the 9 parameters in the coordinate transformation matrix is ​​as follows:

[0027] The absolute value of the three elements in the third column |a 13,k |、|a 23,k |and|a 33,k Sort the elements, select the two smaller elements as parameters, and then select the two elements in the corresponding row positions in the first column as the other two parameters.

[0028] Preferred:

[0029] when|a 13,k When the value in the third column is the largest, select four parameters 'a'. 21,k a 23,k a 31,k and a 33,k ; Calculate at the next time t k+1 =t k +ΔT after integration a 21,k+1 a 23,k+1 a 31,k+1and a 33,k+1 The value;

[0030] when|a 23,k When the value in the third column is the largest, select four parameters 'a'. 11,k a 13,k a 31,k and a 33,k ; Calculate at the next time t k+1 =t k +ΔT after integration a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 The value;

[0031] when|a 33,k When the value in the third column is the largest, select four parameters 'a'. 11,k a 13,k a 21,k and a 23,k ; Calculate at the next time t k+1 =t k +ΔT after integration a 11,k+1 a 13,k+1 a 21,k+1 and a 23,k+1 The value of .

[0032] Preferably, when |a 13,k When the value in the third column is the largest, select four parameters 'a'. 21,k a 23,k a 31,k and a 33,k ; at the next moment t k+1 =t k The value of +ΔT after integration is a 21,k+1 a 23,k+1 a 31,k+1 and a 33,k+1 The specific calculation formula is as follows:

[0033]

[0034] sign() is the sign function;

[0035] Integral calculations are performed using trigonometric functions, when At time t k+1 =t k The value of +ΔT after integration is a 21,k+1 a 23,k+1 a 31,k+1 and a 33,k+1 The calculation formula is:

[0036]

[0037] In the formula, angular velocity Calculate the angular increment within the sampling time ΔT;

[0038] Put a 21,k+1 a 23,k+1 a 31,k+1 and a 33,k+1 Substitute into the following formula and solve for t. k+1 a of time 11,k+1 a 13,k+1 for

[0039]

[0040] Put a 21,k+1 a 23,k+1 a 31,k+1 and a 33,k+1 Substitute into the following formula and solve for t. k+1 The coordinate transformation matrix from the navigation coordinate system to the body coordinate system at time t is:

[0041]

[0042] Preferably, when |a 23,k When the value in the third column is the largest, select four parameters 'a'. 11,k a 13,k a 31,k and a 33,k ; at the next moment t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 The calculation formula is:

[0043]

[0044] sign() is the sign function;

[0045] Integral calculations are performed using trigonometric functions, when At time t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 The calculation formula is:

[0046]

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

[0048] Substitute a 11,k+1 , a 13,k+1 , a 31,k+1 and a 33,k+1 into the following equation to solve a k+1 , a 21,k+1 , a 23,k+1 and a

[0049]

[0050] Substitute a 11,k+1 , a 13,k+1 , a 31,k+1 and a 33,k+1 into the following equation to solve a k+1 , a

[0051]

[0052] Preferably, when |a 33,k | is the largest in the third column, select four parameters a 11,k , a 13,k , a 21,k and a 23,k ; and the values of a k+1 , a k , a 11,k+1 and a 13,k+1 after integration at the next time t 21,k+1 = t 23,k+1 + ΔT are calculated by the following equations:

[0053]

[0054] sign() is a sign function;

[0055] The integral calculation uses trigonometric functions. When , the values of a k+1 , a k , a 11,k+1 and a 13,k+1 after integration at the next time t 21,k+1 = t 23,k+1 + ΔT are calculated by the following equations:

[0056]

[0057]

[0058] In the equation, ω is the angular velocity to calculate the angular increment within the sampling time ΔT;

[0059] Substitute a11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 Substitute the following formula, solve t k+1 moment a 31,k+1 , a 33,k+1 ,

[0060]

[0061] Put a 13,k+1 , a 21,k+1 , a 23,k+1 and a 31,k+1 into the following formula, solve t k+1 The coordinate transformation matrix from the navigation coordinate system to the body coordinate system at the moment is:

[0062]

[0063] Preferably, according to the updated coordinate transformation matrix, the speed update and position update are carried out, specifically:

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

[0065]

[0066] wherein, is the updated coordinate transformation matrix, is the apparent acceleration, is the gravitational acceleration, V is the updated speed, T is the transpose of the matrix, and r is the updated position.

[0067] A computer program product stored on a non-transitory computer readable medium, the computer program product comprising program code for implementing the above-mentioned 4-parameter coordinate transformation matrix solving method.

[0068] Compared with the prior art, the present application has the following beneficial effects:

[0069] (1) The present application discloses an inertial navigation based on normalized 4-parameter coordinate transformation matrix solving method, which has 5 fewer differential equations than the 9-parameter direction cosine method, reducing the amount of calculation.

[0070] (2) The present application discloses an inertial navigation based on normalized 4-parameter coordinate transformation matrix solving method, which has a unique relationship between the 4 parameters and the coordinate transformation matrix compared with the quaternion representation of the coordinate transformation matrix.

[0071] (3) The present application discloses an inertial navigation based on normalized 4-parameter coordinate transformation matrix solving method, which can realize full-attitude singular-free high-precision solution compared with the 3-Krein angle representation of the coordinate transformation matrix.

[0072] (4) The application discloses a normalized 4-parameter coordinate transformation matrix solving method for inertial navigation, which is simple in structure and easy to implement in engineering. BRIEF DESCRIPTION OF DRAWINGS

[0073] Figure 1 is a step flow chart of the 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality;

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

[0075] Figure 3 is a change process of 9 parameters of a coordinate transformation matrix solved by the 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality during three turns of an airplane;

[0076] Figure 4 is a three-dimensional trajectory of airplane movement solved by the 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality. DETAILED DESCRIPTION

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

[0078] A 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality, comprising: determining 4 parameters in two columns of a coordinate transformation matrix as real-time updated variables; using an integral formula to perform integral updating calculation on 4-parameter differential equations to obtain the values of the 4 parameters at t k time; calculating the remaining 5 parameters of the coordinate transformation matrix, and finally obtaining coordinate transformation matrix updating and supporting velocity updating and position updating to improve the precision of inertial navigation. The application takes the angular rate output by a gyroscope orthogonally installed on a body of a strapdown inertial system as input information, realizes real-time updating of the coordinate transformation matrix of inertial navigation, and simplifies the calculation amount while ensuring the solving precision.

[0079] A 4-parameter coordinate transformation matrix solving method for inertial navigation combining normalization and orthogonality, a step flow chart is shown as Figure 1 , comprising:

[0080] (1) the coordinate transformation matrix of a strapdown navigation system body coordinate system relative to a navigation coordinate system is:

[0081]

[0082] In the formula, a 11 , a 12, a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are 9 parameters of the coordinate transformation matrix;

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

[0084]

[0085] wherein 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 are 9 parameters of the coordinate transformation matrix at time t k .

[0086] (2) As shown in FIG. 2, a corresponding body coordinate system (b system, as a moving system) of a strapdown inertial system fixed to a carrier is Ox'y'z'; a navigation coordinate system (p system, as a fixed system) describing the rotating motion of the carrier is Oxyz; the origins of the two coordinate systems coincide. During the rotation, the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k is calculated according to the angular velocity output by the gyroscope installed on the body of the strapdown inertial system, and is specifically:

[0087]

[0088] wherein ω , ω and ω

[0089] are the angular velocities on the x, y and z axes of the body coordinate system, respectively.

[0090] (3) Assuming that the sampling time is ΔT, the absolute values |a 13,k |, |a 23,k | and |a 33,k | of the 3 elements in the third column are sorted, and the 2 elements with relatively small absolute values are selected as parameters, and then the 2 elements in the first column corresponding to the row positions are selected as the other 2 parameters.

[0091] The 4 parameters are updated and calculated in the following 3 cases, and the coordinate transformation matrix is calculated. (3.1) When |a 13,k| In the third column maximum, select four parameters a 21,k , a 23,k , a 31,k and a 33,k ; in the next moment t k+1 = t k + ΔT after the value of the integral a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 , the specific formula is:

[0092]

[0093] sign() is the sign function;

[0094] Integral calculation using trigonometric solution, when , in the next moment t k+1 = t k + ΔT after the value of the integral a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 , the formula is:

[0095]

[0096] In the formula, is the angular velocity The calculation of the angular increment in the sampling time ΔT;

[0097] Put a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 into the following formula, solve a k+1 , a 11,k+1 , a 13,k+1 at t 21,k+1 moment

[0098]

[0099] Put a 23,k+1 , a 31,k+1 , a 33,k+1 into the following formula, solve a k+1 moment coordinate transformation matrix from navigation coordinate system to body coordinate system is

[0100]

[0101] (3.2), when |a 23,k | In the third column maximum, select four parameters a 11,k , a 13,k , a 31,k and a33,k ; at the next moment t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 The calculation formula is:

[0102]

[0103] Integral calculations are performed using trigonometric functions, when At time t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 The calculation formula is:

[0104]

[0105] In the formula, angular velocity Calculate the angular increment within the sampling time ΔT;

[0106] Put a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 Substitute into the following formula and solve for t. k+1 a of time 21,k+1 a 23,k+1 for

[0107]

[0108] Put a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 Substitute into the following formula and solve for t. k+1 The coordinate transformation matrix from the navigation coordinate system to the body coordinate system at time t is:

[0109]

[0110] (3.3), when |a 33,k When the value in the third column is the largest, select four parameters 'a'. 11,k a 13,k a 21,k and a 23,k ; at the next moment t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 13,k+1 a 21,k+1 and a23,k+1 , the calculation formula is:

[0111]

[0112] The integral calculation uses trigonometric function solution, when , at the next time t k+1 = t k + ΔT, the integral value a 11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 , the calculation formula is:

[0113]

[0114] In the formula, is the angular velocity The angular increment in the sampling time ΔT is calculated;

[0115] Substitute a 11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 into the following formula to solve a k+1 , a 31,k+1 at the time t 33,k+1 , which are

[0116]

[0117] Substitute a 13,k+1 , a 21,k+1 , a 23,k+1 and a 31,k+1 into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system at the time t k+1

[0118]

[0119] (4) Update the calculated coordinate transformation matrix according to the integral, and support velocity update and position update to improve the accuracy of inertial navigation.

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

[0121] In step (4), the updated coordinate transformation matrix and the apparent acceleration and the gravitational acceleration are taken as the input of the velocity differential equation After integral calculation, the updated velocity V is obtained. The updated velocity V is taken as the input of the position differential equation ​The input is integrated to obtain the updated position r.

[0122] Based on the above embodiment, the following is described through a set of example comparisons.

[0123] Embodiment:

[0124] Firstly, the 4-parameter coordinate transformation matrix solving method of the inertial navigation combining normalization and orthogonality has only 4 differential equations to be integrated, while the 9-parameter direction cosine matrix needs to solve the following 9 differential equations.

[0125]

[0126] Therefore, the 4-parameter coordinate transformation matrix solving method of the inertial navigation combining normalization and orthogonality reduces the calculation amount compared with the 9-parameter direction cosine method.

[0127] Secondly, although the 4-parameter coordinate transformation matrix solving method based on normalization and orthogonality of the present application has two more differential equations than the quaternion method, the coordinate transformation matrix represented by the quaternion

[0128]

[0129] Corresponds to no less than two groups of parameters: λ, ρ1, ρ2, ρ3; -λ, -ρ1, -ρ2, -ρ3, which causes the non-uniqueness of the relationship between the coordinate transformation matrix and the relative navigation system rotation of the carrier. The 4-parameter coordinate transformation matrix based on normalization and orthogonality of the present application ensures the uniqueness of the relationship between the 4 parameters and the coordinate transformation matrix.

[0130] Finally, the data of a certain full attitude motion of an airplane (Full Attitude Navigation Solving Method Based on Extended Krawczyk Angle, Chinese Journal of Inertial Technology, Vol. 29 No. 5, 2021) is solved by using the 4-parameter coordinate transformation matrix solving method based on normalization and orthogonality of the present application. The solved 9 parameters are as shown in Figure 3 The three-dimensional trajectory of the airplane motion solved by navigation is shown in Figure 4 It can be seen that the large attitude high maneuvering motion process of the airplane is well reproduced, which shows that the 4-parameter coordinate transformation matrix solving method of the present application can realize full attitude solving and is beneficial to flight control.

[0131] The contents not described in detail in the specification of the present application are known to those skilled in the art.

[0132] Although the present application has been disclosed with reference to the preferred embodiments, it is not intended to limit the present application, and any person skilled in the art can make possible changes and modifications to the technical solutions of the present application using the disclosed methods and technical contents without departing from the spirit and scope of the present application. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application without departing from the technical solutions of the present application shall fall within the protection scope of the technical solutions of the present application.

Claims

1. A 4-parameter coordinate transformation matrix solution method of inertial navigation combining normalization and orthogonality, characterized in that, Comprise: The coordinate transformation matrix of the body coordinate system of the strapdown navigation system relative to the navigation coordinate system at time t is calculated. k t. Computing the angular velocity of the body coordinate system relative to the navigation coordinate system at time t k t; selecting 4 parameters from the coordinate transformation matrix; updating the 4 parameters according to the coordinate transformation matrix and the angular velocity, to obtain an updated coordinate transformation matrix; performing velocity updating and position updating according to the updated coordinate transformation matrix; The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is: where a 11 , a 12 , a 13 , a 21 , a 22 , a 23 , a 31 , a 32 and a 33 are nine parameters of the coordinate transformation matrix; At t k The coordinate transformation matrix of the strapdown navigation system body coordinate system relative to the navigation coordinate system is: where 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 are nine parameters of the coordinate transformation matrix at time t k . selecting 4 parameters from the 9 parameters in the coordinate transformation matrix, specifically: The absolute values of 3 elements of the third column |a 13,k |a 23,k |a 33,k |a are sorted, and the relatively smaller 2 elements are selected as parameters, and then the 2 elements of the corresponding row position in the first column are selected as the other 2 parameters.

2. The 4-parameter coordinate transformation matrix solution method of claim 1, wherein, t k the angular velocity of the body coordinate system relative to the navigation coordinate system at the moment, specifically: wherein, are the angular velocities on the body coordinate system x, y, z axes, respectively.

3. The 4-parameter coordinate transformation matrix solving method according to claim 2, characterized in that: When |a 13,k | is maximum in the third column, select four parameters a 21,k , a 23,k , a 31,k and a 33,k ; calculate the values of a k+1 , a k , a 21,k+1 and a 23,k+1 after integration at the next time t 31,k+1 = t 33,k+1 + ΔT. When |a 23,k | is maximum in the third column, select four parameters a 11,k , a 13,k , a 31,k and a 33,k ; calculate the values of a 11,k+1 , a 13,k+1 , a 31,k+1 and a 33,k+1 after integration at the next time t k+1 = t k + ΔT. When |a 33,k is maximum in the third column, four parameters a 11,k , a 13,k , a 21,k and a 23,k are selected; the values of a 11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 after integration at the next instant t k+1 = t k + ΔT are calculated.

4. The 4-parameter coordinate transformation matrix solving method according to claim 3, characterized in that, when|a 13,k When the value in the third column is the largest, select four parameters 'a'. 21,k a 23,k a 31,k and a 33,k ; at the next moment t k+1 =t k The value of +ΔT after integration is a 21,k+1 a 23,k+1 a 31,k+1 and a 33,k+1 The specific calculation formula is as follows: sign() is a sign function; The integral calculation uses trigonometric function solution, when the next time t k+1 = t k + ΔT, the value a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 after integration, the calculation formula is: wherein is the angular velocity calculating the angular increment over the sampling time ΔT; Substitute a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 into the following equation to solve for a k+1 at time t 11,k+1 , a 13,k+1 is Substitute a 21,k+1 , a 23,k+1 , a 31,k+1 and a 33,k+1 into the following equation to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system at time t k+1 ​ 5. The 4-parameter coordinate transformation matrix solving method according to claim 3, characterized in that, When |a 23,k | is maximum in the third column, four parameters a 11,k , a 13,k , a 31,k and a 33,k are selected; the values a 11,k+1 , a 13,k+1 , a 31,k+1 and a 33,k+1 after integration at the next time t k+1 = t k + ΔT are calculated according to the following formula: sign() is a sign function; Integral calculations are performed using trigonometric functions, when At time t k+1 =t k The value of +ΔT after integration is a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 The calculation formula is: wherein is the angular velocity calculating the angular increment over the sampling time ΔT; Put a 11,k+1 a 13,k+1 a 31,k+1 and a 33,k+1 Substitute into the following formula and solve for t. k+1 a of time 21,k+1 a 23,k+1 for Put a 11,k+1 , a 13,k+1 , a 31,k+1 and a 33,k+1 into the following formula, solve t k+1 The coordinate transformation matrix from the navigation coordinate system to the body coordinate system at time is 6. The 4-parameter coordinate transformation matrix solving method according to claim 3, characterized in that, When |a 33,k | is maximum in the third column, four parameters a 11,k , a 13,k , a 21,k and a 23,k are selected; the values a 11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 after integration at the next moment t k+1 = t k + ΔT are calculated according to the following formula: sign() is a sign function; The integral calculation uses trigonometric function solution, when the next time t k+1 = t k + ΔT after the integral value a 11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 , the calculation formula is: wherein is the angular velocity calculating the angular increment over the sampling time ΔT; Substitute a 11,k+1 , a 13,k+1 , a 21,k+1 and a 23,k+1 into the following equation to solve for a k+1 at time t 31,k+1 , a 33,k+1 is Substitute a 13,k+1 , a 21,k+1 , a 23,k+1 and a 31,k+1 into the following formula to solve the coordinate transformation matrix from the navigation coordinate system to the body coordinate system at time t k+1 :

7. The 4-parameter coordinate transformation matrix solution method of claim 1, wherein, performing velocity updating and position updating according to the updated coordinate transformation matrix, specifically: When p is the inertial system, the navigation equation is: wherein, is the updated coordinate transformation matrix, is the visual acceleration, is the gravitational acceleration, V is the updated velocity, T is the transpose of a matrix, and r is the updated position.

8. A computer program product stored on a non-transitory computer readable medium, the computer program product comprising program code for carrying out the method of any one of claims 1 to 7.

Images