Inertial navigation solution method and system based on matrix form function iteration

Abstract

The invention provides an inertial navigation solution method and system based on matrix form function iteration, comprising: constructing an inertial navigation equation in a navigation reference coordinate system, obtaining an inertial navigation attitude integral equation, an inertial navigation velocity integral equation and an inertial navigation position integral equation based on the constructed inertial navigation equation; interpolating or fitting the observed values ​​of a gyroscope and an accelerometer to obtain a Chebyshev polynomial approximate expression of an angular velocity and a specific force within an interval; solving the inertial navigation attitude integral equation by using a matrix form function iteration method based on the Chebyshev polynomial approximate expression of the angular velocity and the specific force within the interval, thereby accurately solving the attitude; solving the inertial navigation velocity integral equation by using a matrix form function iteration method according to the calculated attitude value, thereby accurately solving the speed; solving the inertial navigation position integral equation by using a matrix form function iteration method based on the calculated speed value, thereby accurately solving the position.

Description

Technical Field

[0001] The present invention relates to the field of sensor technology, and in particular, to an inertial navigation solution method and system based on matrix form function iteration, and more particularly, to a high-precision and high-efficiency inertial navigation solution method based on matrix form function iteration. Background Art

[0002] Inertial navigation is to integrate and solve the attitude, velocity and position of the carrier in the form of dead reckoning based on the output of gyroscopes and accelerometers (such as angular velocity / acceleration, angular increment / velocity increment, etc.). The error sources of inertial navigation mainly include sensor error, earth gravity error and solution error. It is generally believed that the accuracy of multi-sample inertial navigation algorithms based on traditional conic compensation is high enough. However, with the emergence of ultra-high precision cold atom inertial navigation devices and high-dynamic large-maneuver applications, the accuracy of inertial navigation solutions needs to be further improved to give full play to the potential of inertial devices.

[0003] The recently proposed high-precision inertial navigation solution method based on Chebyshev polynomial function iteration can reduce the error in inertial navigation solution to the level of computer truncation error, completely eliminating the non-commutativity error in traditional inertial navigation algorithms. However, the function iteration method has a larger amount of calculation than the traditional inertial navigation solution method, which limits its application and promotion on low-cost processors. Therefore, how to improve the execution efficiency of the function iteration algorithm without losing the calculation accuracy is a problem that needs to be solved.

[0004] Patent document CN106643726A (application number: 201611051763.4) discloses a unified inertial navigation solution method, the steps of which include: 1. Update the conversion matrix from the ground-fixed coordinate system to the navigation coordinate system; 2. Update the conversion matrix from the fixed coordinate system to the navigation coordinate system; 3. Update the velocity vector of the carrier in the ground-fixed coordinate system; 4. Update the position vector of the carrier in the ground-fixed coordinate system according to the velocity vector of the carrier in the ground-fixed coordinate system; and update the position coordinates of the carrier in the geographic coordinate system according to the velocity vector; 5. Repeat steps 1 to 4, and iteratively calculate the velocity vector of the carrier in the ground-fixed coordinate system, the position vector of the carrier in the ground-fixed coordinate system, and the position coordinates of the carrier in the geographic coordinate system at each moment within the set inertial measurement period. Summary of the invention

[0005] In view of the defects in the prior art, the purpose of the present invention is to provide an inertial navigation solution method and system based on matrix form function iteration.

[0006] According to the present invention, an inertial navigation solution method based on matrix form function iteration is provided, comprising:

[0007] Step S1: constructing an inertial navigation equation in a navigation reference coordinate system, and obtaining an inertial navigation attitude integral equation, an inertial navigation velocity integral equation, and an inertial navigation position integral equation based on the constructed inertial navigation equation;

[0008] Step S2: interpolating or fitting the observed values ​​of the gyroscope and the accelerometer to obtain an approximate Chebyshev polynomial expression of the angular velocity and specific force within the interval;

[0009] Step S3: Based on the Chebyshev polynomial approximate expression of the angular velocity and the specific force in the interval, the inertial navigation attitude integral equation is solved by using a function iteration method in matrix form, so as to accurately calculate the attitude;

[0010] Step S4: solving the inertial navigation velocity integral equation using a function iteration method in matrix form according to the calculated attitude value, thereby accurately calculating the velocity;

[0011] Step S5: Solving the inertial navigation position integral equation using a function iteration method in matrix form based on the calculated velocity value, thereby accurately calculating the position;

[0012] The navigation reference coordinate system includes an earth coordinate system;

[0013] Preferably, the step S1 adopts:

[0014] The expression of the inertial navigation equation in the earth coordinate system is:

[0015]

[0016] in, represents the inertial navigation attitude differential equation; the superscript / subscript b represents the carrier coordinate system; e represents the earth coordinate system; i represents the inertial coordinate system; The quaternion representing the attitude of the e system relative to the b system; Represents the attitude rotation matrix of system b relative to system e; represents the angular velocity measured by the gyroscope; represents the angular velocity of the Earth's rotation; represents the inertial velocity differential equation; v e represents the velocity of the carrier relative to the earth in the earth coordinate system; f b Indicates the specific force measured by the accelerometer; g e represents the earth's gravitational acceleration vector in the earth's coordinate system; × represents vector cross product; Represents the multiplication of quaternions; represents the inertial position differential equation;

[0017] Consider integrating both sides of the inertial navigation equation within the current sampling time window [0, t] to obtain the inertial navigation equation in integral form:

[0018]

[0019] Among them, q(0), v e (0), p e (0) represents the initial attitude quaternion, initial velocity and initial position respectively; q represents the attitude quaternion; p e Indicates the position of the carrier in the earth coordinate system;

[0020] Preferably, the step S2 adopts:

[0021] Assume that in the current sampling time window t∈[0,t N ], there is the angular velocity measured by the gyroscope Or angular increment Specific force measured by accelerometer Or speed increment where t k Where k = 1, 2, ..., N, N is the number of samples; the time window is mapped to τ∈[-1, 1] using affine transformation;

[0022]

[0023] Interpolate or fit the gyro and accelerometer observations to obtain the angular velocity within the interval Comparison b The Chebyshev polynomial approximation of is:

[0024]

[0025] Among them, n w , n f Represents the maximum order of polynomial fitting for angular velocity and specific force respectively; c i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the angular velocity polynomial, d i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the Bili polynomial; F i represents the first kind of Chebyshev polynomials, whose cyclic definition is as follows

[0026] F0(τ)=1,F1(τ=τ,

[0027] F i+1 (τ)=2τF i (τ)-F i-1 (τ),i≥1(10);

[0028] Preferably, the step S3 adopts:

[0029] The function iteration form is constructed from the integral form of the inertial navigation attitude equation:

[0030]

[0031] Among them, the subscript l represents the result of the lth iteration; It is approximated using Chebyshev polynomials as Among them, m q Indicates the maximum order of Chebyshev polynomial fitting; α l,i Represents the Chebyshev coefficients; subscript l represents the lth iteration, and subscript i corresponds to the coefficients of the i-th order Chebyshev polynomial; the Chebyshev coefficients are written in matrix form:

[0032]

[0033] The quaternion q of the lth iteration attitude is known l And the angular velocity obtained by polynomial interpolation but:

[0034]

[0035] Z=diag([1 / 2,1,…,1]) (14)

[0036]

[0037] Where, subscript M = m q , σ k is the Chebyshev zero, defined as

[0038]

[0039] For a matrix of dimension n×m, A=[a1,a2,…,a n ] T ,diag(A) is defined as:

[0040]

[0041] Represent the attitude quaternion of the l+1th iteration as a Chebyshev polynomial

[0042]

[0043] The value of the attitude quaternion of the l+1th iteration at the Chebyshev zero point is expressed in matrix form,

[0044]

[0045] Among them, b l+1,i represents the Chebyshev coefficient of the l+1th iteration; the subscript i corresponds to the i-th order Chebyshev polynomial; in the above α l After that, the Chebyshev coefficient matrix b of the attitude quaternion of the l+1th iteration isl+1 Calculated by the following formula:

[0046]

[0047] in,

[0048] χ0=[q(0),0,…,0] T (twenty two)

[0049] C s =UDZF T

[0050]

[0051] The element in the 1st row and jth column of matrix D is Therefore, based on the posture matrix Q obtained in the previous iteration l , the Chebyshev polynomial coefficient matrix b of the next iteration process is obtained through matrix calculation l+1 , and then get the new posture matrix Q for the next iteration process l+1 ;

[0052] According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient of two adjacent iterations is less than the given error or the maximum number of iterations is reached. The error of two adjacent iterations is calculated as follows

[0053]

[0054] Preferably, the step S4 adopts:

[0055] The function iteration form is constructed from the integral form of the inertial velocity equation:

[0056]

[0057] Among them, q * represents the conjugate quaternion of the quaternion q; The Chebyshev polynomial approximation is Among them, m v Indicates the maximum order of Chebyshev polynomial fitting; β l,i Represents the Chebyshev coefficient; write the Chebyshev coefficient in the form of a matrix

[0058]

[0059] Given the quaternion iteration result q of the current interval, the specific force f obtained by polynomial interpolation b And the speed of the first time Location Iteration result, β l Calculated by the following formula

[0060]

[0061] The l+1th velocity iteration result is expressed using Chebyshev polynomials

[0062]

[0063] Write the value of the l+1th velocity at the Chebyshev zero point in matrix form

[0064]

[0065] Among them, s l+1,i represents the velocity Chebyshev coefficient of the l+1th iteration, and the subscript i corresponds to the i-th order Chebyshev polynomial; in the calculation of β l After that, the l+1th iteration velocity Chebyshev coefficient is calculated by the following formula:

[0066]

[0067] Where M = m v ,η0=[v e (0),0,…,0] T Therefore, based on the previously completed attitude solution results, the velocity matrix obtained in the previous iteration is used The velocity Chebyshev polynomial coefficient matrix s of the next iteration process is obtained through matrix calculation l+1 , and then get the new velocity matrix for the next iteration process

[0068] According to the above iterative process, the iteration is terminated until the error of the velocity Chebyshev coefficient of two consecutive iterations is less than the given error or the maximum number of iterations is reached. The error of two velocity iterations is calculated as follows

[0069]

[0070] Preferably, the step S5 adopts:

[0071] The function iteration form is constructed by the integral form of the inertial navigation position equation and the velocity Chebyshev coefficient calculated by the l+1th iteration:

[0072]

[0073] On the other hand, the result of the l+1th position iteration is expressed using the Chebyshev polynomial

[0074]

[0075] Write the value of the l+1th position at the Chebyshev zero in matrix form

[0076]

[0077] Among them, m p Indicates the maximum order of Chebyshev polynomial fitting. The definition of F is the same as that in attitude solution, where M = m p , ρ l+1,i Represents the Chebyshev coefficient, and writes the Chebyshev coefficient in the form of a matrix

[0078]

[0079] ρ l+1 The velocity Chebyshev coefficient s obtained directly from the current iteration l+1 calculate:

[0080]

[0081] in, C d =UD;

[0082] Using the velocity Chebyshev polynomial coefficients s obtained in the current iteration process l+1 , based on the function iteration method in matrix form, the coefficient matrix ρ of the position Chebyshev polynomial is solved l+1 , and then solve the position matrix in the next iteration process, so as to complete the position solution;

[0083] According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient between two adjacent iterative positions is less than the given error or the maximum number of iterations is reached. The error of two position iterations is calculated as follows

[0084]

[0085] According to the present invention, an inertial navigation solution system based on matrix form function iteration is provided, comprising:

[0086] Module M1: Construct the inertial navigation equation in the navigation reference coordinate system, and obtain the inertial navigation attitude integral equation, inertial navigation velocity integral equation and inertial navigation position integral equation based on the constructed inertial navigation equation;

[0087] Module M2: interpolate or fit the observed values ​​of the gyroscope and accelerometer to obtain the Chebyshev polynomial approximate expression of the angular velocity and specific force in the interval;

[0088] Module M3: Based on the Chebyshev polynomial approximate expression of angular velocity and specific force in the interval, the inertial navigation attitude integral equation is solved using the function iteration method in matrix form, so as to accurately calculate the attitude;

[0089] Module M4: Solve the inertial navigation velocity integral equation using a matrix function iteration method based on the calculated attitude value, so as to accurately calculate the velocity;

[0090] Module M5: Based on the calculated velocity value, the inertial navigation position integral equation is solved using a function iteration method in matrix form to accurately calculate the position;

[0091] The navigation reference coordinate system includes an earth coordinate system;

[0092] Preferably, the module M1 adopts:

[0093] The expression of the inertial navigation equation in the earth coordinate system is:

[0094]

[0095]

[0096] in, represents the inertial navigation attitude differential equation; the superscript / subscript b represents the carrier coordinate system; e represents the earth coordinate system; i represents the inertial coordinate system; The quaternion representing the attitude of the e system relative to the b system; Represents the attitude rotation matrix of system b relative to system e; represents the angular velocity measured by the gyroscope; represents the angular velocity of the Earth's rotation; represents the inertial velocity differential equation; v e represents the velocity of the carrier relative to the earth in the earth coordinate system; f b Indicates the specific force measured by the accelerometer; g e represents the earth's gravitational acceleration vector in the earth's coordinate system; × represents vector cross product; Represents the multiplication of quaternions; represents the inertial position differential equation;

[0097] Consider integrating both sides of the inertial navigation equation within the current sampling time window [0, t] to obtain the inertial navigation equation in integral form:

[0098]

[0099] Among them, q(0), v e (0), p e (0) represents the initial attitude quaternion, initial velocity and initial position respectively; q represents the attitude quaternion; p e Indicates the position of the carrier in the earth coordinate system;

[0100] Preferably, the module M2 adopts:

[0101] Assume that in the current sampling time window t∈[0,t N ], there is the angular velocity measured by the gyroscope Or angular increment Specific force measured by accelerometer Or speed increment where t k Where k = 1, 2, ..., N, N is the number of samples; use affine transformation to map the time window to τ∈[-1, 1]

[0102]

[0103] Interpolate or fit the gyro and accelerometer observations to obtain the angular velocity within the interval Comparison b The Chebyshev polynomial approximation of is:

[0104]

[0105] Among them, n w , n f Represents the maximum order of polynomial fitting for angular velocity and specific force respectively; c i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the angular velocity polynomial, d i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the Bili polynomial; F i represents the first kind of Chebyshev polynomials, whose cyclic definition is as follows

[0106] F0(τ)=1,F1(τ=τ,

[0107] F i+1 (τ)=2τF i (τ)-F i-1 (τ),i≥1(10);

[0108] Preferably, the module M3 adopts:

[0109] The function iteration form is constructed from the integral form of the inertial navigation attitude equation:

[0110]

[0111] Among them, the subscript l represents the result of the lth iteration; It is approximated using Chebyshev polynomials as Among them, m q Indicates the maximum order of Chebyshev polynomial fitting; α l,i Represents the Chebyshev coefficients; subscript l represents the lth iteration, and subscript i corresponds to the coefficients of the i-th order Chebyshev polynomial; the Chebyshev coefficients are written in matrix form:

[0112]

[0113] The quaternion q of the first iteration is known l And the angular velocity obtained by polynomial interpolation but:

[0114]

[0115] Z=diag([1 / 2,1,…,1]) (14)

[0116]

[0117] Where, subscript M = m q , σ k is the Chebyshev zero, defined as

[0118]

[0119] For a matrix of dimension n×m, A=[a1,a2,…,a n ] T ,diag(A) is defined as:

[0120]

[0121] Represent the attitude quaternion of the l+1th iteration as a Chebyshev polynomial

[0122]

[0123] The value of the attitude quaternion of the l+1th iteration at the Chebyshev zero point is expressed in matrix form,

[0124]

[0125] Among them, b l+1,i represents the Chebyshev coefficient of the l+1th iteration; the subscript i corresponds to the i-th order Chebyshev polynomial; in the above α l After that, the Chebyshev coefficient matrix b of the attitude quaternion of the l+1th iteration is l+1 Calculated by the following formula:

[0126]

[0127] in,

[0128] χ0=[q(0),0,…,0] T (twenty two)

[0129] C s =UDZF T

[0130]

[0131]

[0132] The element in the 1st row and jth column of matrix D is Therefore, based on the posture matrix Q obtained in the previous iteration l , the Chebyshev polynomial coefficient matrix b of the next iteration process is obtained through matrix calculation l+1 , and then get the new posture matrix Q for the next iteration process l+1 ;

[0133] According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient of two adjacent iterations is less than the given error or the maximum number of iterations is reached. The error of two adjacent iterations is calculated as follows

[0134]

[0135] Preferably, the module M4 adopts:

[0136] The function iteration form is constructed from the integral form of the inertial velocity equation:

[0137]

[0138] Among them, q * represents the conjugate quaternion of the quaternion q; The Chebyshev polynomial approximation is Among them, m v Indicates the maximum order of Chebyshev polynomial fitting; β l,i Represents the Chebyshev coefficient; write the Chebyshev coefficient in the form of a matrix

[0139]

[0140] Given the quaternion iteration result q of the current interval, the specific force f obtained by polynomial interpolation b And the speed of the first time Location Iteration result, β l Calculated by the following formula

[0141]

[0142]

[0143] The l+1th velocity iteration result is expressed using Chebyshev polynomials

[0144]

[0145] Write the value of the l+1th velocity at the Chebyshev zero point in matrix form

[0146]

[0147] Among them, s l+1,i represents the velocity Chebyshev coefficient of the l+1th iteration, and the subscript i corresponds to the i-th order Chebyshev polynomial; in the calculation of β l After that, the l+1th iteration velocity Chebyshev coefficient is calculated by the following formula:

[0148]

[0149] Where M = m v ,η0=[v e (0),0,…,0] T Therefore, based on the previously completed attitude solution results, the velocity matrix obtained in the previous iteration is used The velocity Chebyshev polynomial coefficient matrix s of the next iteration process is obtained through matrix calculation l+1 , and then get the new velocity matrix for the next iteration process

[0150] According to the above iterative process, the iteration is terminated until the error of the velocity Chebyshev coefficient of two consecutive iterations is less than the given error or the maximum number of iterations is reached. The error of two velocity iterations is calculated as follows

[0151]

[0152] Preferably, the module M5 adopts:

[0153] The function iteration form is constructed by the integral form of the inertial navigation position equation and the velocity Chebyshev coefficient calculated by the l+1th iteration:

[0154]

[0155] On the other hand, the l+1th position iteration result is expressed using Chebyshev polynomials

[0156]

[0157] Write the value of the l+1th position at the Chebyshev zero in matrix form

[0158]

[0159] Among them, m p Indicates the maximum order of Chebyshev polynomial fitting. The definition of F is the same as that in attitude solution, where M = m p , ρ l+1,iRepresents the Chebyshev coefficient, and writes the Chebyshev coefficient in the form of a matrix

[0160]

[0161] ρ l+1 The velocity Chebyshev coefficient s obtained directly from the current iteration l+1 calculate:

[0162]

[0163] in, C d =UD;

[0164] Using the velocity Chebyshev polynomial coefficients s obtained in the current iteration process l+1 , based on the function iteration method in matrix form, the position Chebyshev polynomial coefficient matrix ρl is solved +1 , and then solve the position matrix in the next iteration process, so as to complete the position solution;

[0165] According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient between two adjacent iterative positions is less than the given error or the maximum number of iterations is reached. The error of two position iterations is calculated as follows

[0166]

[0167] Compared with the prior art, the present invention has the following beneficial effects: the present invention can reduce the inertial navigation solution error to the level of computer truncation error by the function iteration approximation method, that is, the cone, paddle and scroll errors in the attitude, speed and position solution can be almost completely eliminated. Compared with the most accurate algorithm currently, the present invention can achieve the same high-precision solution, and the operating efficiency has been greatly improved, reaching a high efficiency comparable to the traditional two-sample algorithm. Therefore, the present invention enables the inertial navigation solution to achieve both high precision and high efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0168] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0169] Figure 1 This is the flow chart of inertial navigation solution based on matrix form function iteration method. DETAILED DESCRIPTION

[0170] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0171] Example 1

[0172] In view of the defects in the prior art, the purpose of the present invention is to provide a high-precision and high-efficiency inertial navigation solution method based on matrix form function iteration, including: constructing an inertial navigation equation in a navigation reference coordinate system, and obtaining an inertial navigation attitude integral equation, an inertial navigation velocity integral equation and an inertial navigation position integral equation based on the constructed inertial navigation equation; the navigation reference coordinate system includes an earth coordinate system, but is not limited to the earth coordinate system.

[0173] 1) Based on the measurement data of the gyroscope, the inertial navigation attitude differential equation is solved using the function iteration method in matrix form to accurately calculate the attitude.

[0174] 2) Based on the attitude value calculated in the first step and the measurement data added to the table, the inertial navigation velocity differential equation is solved using a function iteration method in matrix form to accurately calculate the velocity.

[0175] 3) Based on the speed value calculated above, the inertial navigation position differential equation is solved by using the function iteration method in matrix form, so as to accurately calculate the position.

[0176] Among them, the inertial navigation equation is expressed in the earth coordinate system as follows:

[0177]

[0178] Wherein the superscript / subscript b represents the carrier coordinate system (b system), e represents the earth coordinate system (e system), and i represents the inertial coordinate system (i system). Without loss of generality, the present invention defines the navigation coordinate system as north-sky-east. Represents the attitude quaternion of the e system relative to the b system, represents the angular velocity measured by the gyroscope, represents the angular velocity of the Earth's rotation, v e : represents the velocity of the carrier relative to the earth in the e system (also called earth velocity), f b Indicates the specific force measured by the accelerometer, g e represents the earth's gravitational acceleration vector in the e system. × represents vector cross product, Represents the multiplication of quaternions. For two quaternions and Where s and η represent the scalar and vector parts of the quaternion respectively, and the multiplication of quaternions is defined as follows:

[0179]

[0180] and The definition is as follows

[0181]

[0182] Where (□)× represents the antisymmetric matrix corresponding to the three-dimensional vector, I n Represents the n-dimensional identity matrix.

[0183] Consider integrating both sides of the above inertial navigation equation within the current sampling time window [0, t], and the integral form of the inertial navigation equation is as follows:

[0184]

[0185] where q(0), v e (0), p e (0) represent the initial attitude quaternion, initial velocity and initial position respectively; for the sake of simplicity, the - and subscripts of the attitude in the inertial navigation equation are ignored when there is no ambiguity; p e Indicates the position of the carrier in the earth coordinate system;

[0186] Assume that in the current sampling time window t∈[0,t N ], there is the angular velocity measured by the gyroscope Or angular increment Specific force measured by accelerometer Or speed increment where t k Where k = 1, 2, ..., N, N is the number of samples. Use affine transformation to map the time window to τ∈[-1, 1];

[0187]

[0188] For the needs of subsequent calculations, the present invention first interpolates or fits the observed values ​​of the gyroscope and the accelerometer to obtain the angular velocity in the interval Comparison b The Chebyshev polynomial approximation of

[0189]

[0190] where n w , n f Represents the maximum order of polynomial fitting for angular velocity and specific force, respectively. irepresents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the angular velocity polynomial, d i Represents the coefficients of the i-th order Chebyshev polynomial in the β-polynomial approximation.

[0191] F i is the first kind of Chebyshev polynomial (abbreviated as Chebyshev polynomial), and its cyclic definition is as follows

[0192] F0(τ)=1,F1(τ=τ,

[0193] F i+1 (τ)=2τF i (τ)-F i-1 (τ),i≥1

[0194] 1) Inertial navigation attitude solution algorithm based on matrix form function iteration

[0195] From the integral form of the inertial navigation attitude equation, the following function iteration form can be constructed:

[0196]

[0197] The subscript l represents the result of the lth iteration. It is approximated using Chebyshev polynomials as

[0198]

[0199] Where m q represents the maximum order of Chebyshev polynomial fitting, α l,i represents the Chebyshev coefficients, the subscript l represents the lth iteration, and the subscript i corresponds to the coefficient of the i-th order Chebyshev polynomial. The Chebyshev coefficients are written in the form of a matrix

[0200]

[0201] The quaternion q of the first iteration is known l And the angular velocity obtained by polynomial interpolation α l It can be calculated by the following formula

[0202]

[0203] in

[0204] Z = diag([1 / 2,1,…,1])

[0205]

[0206] Where subscript M = m q , σ k is the Chebyshev zero, defined as

[0207]

[0208] For a matrix of dimension n×m, A=[a1,a2,…,a n ] T ,diag(A) is defined as

[0209]

[0210] Represent the attitude quaternion of the l+1th iteration as a Chebyshev polynomial

[0211]

[0212] The value of the attitude quaternion of the l+1th iteration at the Chebyshev zero point is expressed in matrix form,

[0213]

[0214] b l+1,i represents the Chebyshev coefficient of the l+1th iteration, and the subscript i corresponds to the i-th order Chebyshev polynomial. l After that, the Chebyshev coefficient matrix b of the attitude quaternion of the l+1th iteration is l+1 It can be calculated by the following formula

[0215]

[0216] in

[0217] χ0=[q(0),0,…,0] T

[0218] C s =UDZF T

[0219]

[0220] The element in the 1st row and jth column of matrix D is Therefore, based on the posture matrix Q obtained in the previous iteration l , the Chebyshev polynomial coefficient matrix b of the next iteration process can be obtained by matrix calculation l+1 , and then the new posture matrix Q for the next iteration process can be obtained l+1 , thus we completed the posture solution through a simple matrix form function iteration method.

[0221] According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient of two adjacent iterations is less than the given error or the maximum number of iterations is reached. The error of two adjacent iterations is calculated as follows

[0222]

[0223] 2) Inertial navigation velocity solution algorithm based on matrix form function iteration;

[0224] Similar to the attitude solution, the following function iteration form can be constructed from the integral form of the inertial velocity equation:

[0225]

[0226] in

[0227]

[0228] q * Represents the conjugate quaternion of the quaternion q. The Chebyshev polynomial approximation is

[0229]

[0230] Where m v represents the maximum order of Chebyshev polynomial fitting, β l,i Represents the Chebyshev coefficient. The meaning of the subscript is consistent with the above attitude solution. The Chebyshev coefficient is written in the form of a matrix

[0231]

[0232] Given the quaternion iteration result q of the current interval, the specific force f obtained by polynomial interpolation b And the speed of the first time Location Iteration result, β l It can be calculated by the following formula

[0233]

[0234] in

[0235]

[0236] in

[0237]

[0238] The l+1th velocity iteration result is expressed using Chebyshev polynomials

[0239]

[0240] Write the value of the l+1th velocity at the Chebyshev zero point in matrix form

[0241]

[0242] where s l+1,i represents the velocity Chebyshev coefficient of the l+1th iteration, and the subscript i corresponds to the i-th order Chebyshev polynomial. l After that, the l+1th iteration speed Chebyshev coefficient can be calculated by the following formula:

[0243]

[0244] Where M = m v ,η0=[v e (0),0,…,0] T Therefore, based on the previously completed attitude solution results, the velocity matrix obtained in the previous iteration is used The velocity Chebyshev polynomial coefficient matrix s of the next iteration process can be obtained by matrix calculation. l+1 , and then the new velocity matrix of the next iteration process can be obtained Thus, we have completed the speed calculation through a simple matrix iteration form.

[0245] According to the above iterative process, the iteration is terminated until the error of the velocity Chebyshev coefficient of two consecutive iterations is less than the given error or the maximum number of iterations is reached. The error of two velocity iterations is calculated as follows

[0246]

[0247] 3) Position solving algorithm based on matrix form function iteration

[0248] Based on the integral form of the inertial navigation position equation and the velocity Chebyshev coefficient calculated in the l+1th iteration, the following function iteration form can be constructed:

[0249]

[0250] On the other hand, the result of the l+1th position iteration is expressed using the Chebyshev polynomial

[0251]

[0252] Write the value of the l+1th position at the Chebyshev zero in matrix form

[0253]

[0254] Where m p Indicates the maximum order of Chebyshev polynomial fitting. The definition of F is the same as that in attitude solution, where M = m p , ρ l+1,i Represents the Chebyshev coefficient. Its subscript has the same meaning as in the above attitude and velocity calculation. The Chebyshev coefficient is written in the form of a matrix.

[0255]

[0256] ρ l+1 The velocity Chebyshev coefficient s can be directly obtained from the current iteration l+1 calculate

[0257]

[0258] in

[0259]

[0260] C d =UD

[0261] Using the velocity Chebyshev polynomial coefficients s obtained in the current iteration process l+1 Based on the function iteration method in matrix form, we can also easily solve the position Chebyshev polynomial coefficient matrix ρ l+1 , and then solve the position matrix in the next iteration process, and so on to complete the position solution.

[0262] According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient between two adjacent iterative positions is less than the given error or the maximum number of iterations is reached. The error of two position iterations is calculated as follows

[0263]

[0264] The flowchart of the above inertial navigation attitude, velocity and position solution algorithm is as follows: Figure 1 In the above solution process, C s With C d is a constant matrix that can be calculated in advance and stored in the computer. In addition, the calculation of variables at the Chebyshev zero point, such as in the attitude calculation And the speed calculation It can be accelerated by parallel computing. Compared with traditional function iteration, the present invention can significantly improve the efficiency of inertial navigation solution.

[0265] The inertial navigation solution system based on matrix form function iteration provided by the present invention can be implemented by the step flow in the inertial navigation solution method based on matrix form function iteration provided by the present invention. Those skilled in the art can understand the inertial navigation solution method based on matrix form function iteration as a preferred example of the inertial navigation solution system based on matrix form function iteration.

[0266] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0267] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. An inertial navigation solution method based on matrix form function iteration, characterized in that: include: Step S1: constructing an inertial navigation equation in a navigation reference coordinate system, and obtaining an inertial navigation attitude integral equation, an inertial navigation velocity integral equation, and an inertial navigation position integral equation based on the constructed inertial navigation equation; Step S2: interpolating or fitting the observed values ​​of the gyroscope and the accelerometer to obtain an approximate Chebyshev polynomial expression of the angular velocity and specific force within the interval; Step S3: Based on the Chebyshev polynomial approximate expression of the angular velocity and the specific force in the interval, the inertial navigation attitude integral equation is solved by using a function iteration method in matrix form, so as to accurately calculate the attitude; Step S4: solving the inertial navigation velocity integral equation using a function iteration method in matrix form according to the calculated attitude value, thereby accurately calculating the velocity; Step S5: Solving the inertial navigation position integral equation using a function iteration method in matrix form based on the calculated velocity value, thereby accurately calculating the position; The navigation reference coordinate system includes an earth coordinate system; The step S1 adopts: The expression of the inertial navigation equation in the earth coordinate system is: in, represents the inertial navigation attitude differential equation; the superscript / subscript b represents the carrier coordinate system; e represents the earth coordinate system; i represents the inertial coordinate system; The quaternion representing the attitude of the e system relative to the b system; Represents the attitude rotation matrix of system b relative to system e; represents the angular velocity measured by the gyroscope; represents the angular velocity of the Earth's rotation; represents the inertial velocity differential equation; v e represents the velocity of the carrier relative to the earth in the earth coordinate system; f b Indicates the specific force measured by the accelerometer; g e represents the earth's gravitational acceleration vector in the earth's coordinate system; × represents vector cross product; Represents the multiplication of quaternions; represents the inertial position differential equation; Consider integrating both sides of the inertial navigation equation within the current sampling time window [0, t] to obtain the inertial navigation equation in integral form: Among them, q(0), v e (0), p e (0) represents the initial attitude quaternion, initial velocity and initial position respectively; q represents the attitude quaternion; p e Indicates the position of the carrier in the earth coordinate system; The step S2 adopts: Assume that in the current sampling time window t∈[0,t N ], there is the angular velocity measured by the gyroscope Or angular increment Specific force measured by accelerometer Or speed increment where t k Where k = 1, 2, ..., N, N is the number of samples; the time window is mapped to τ∈[-1, 1] using affine transformation; Interpolate or fit the gyro and accelerometer observations to obtain the angular velocity within the interval Comparison b The Chebyshev polynomial approximation of is: Among them, n w , n f Represents the maximum order of polynomial fitting for angular velocity and specific force respectively; c i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the angular velocity polynomial, d i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the Bili polynomial; F i represents the first kind of Chebyshev polynomials, whose cyclic definition is as follows F0(τ)=1,F1(τ=τ, F i+1 (τ)=2τF i (t)-F i-1 (τ),i≥1 (10); The step S3 adopts: The function iteration form is constructed from the integral form of the inertial navigation attitude equation: Among them, the subscript l represents the result of the lth iteration; It is approximated using Chebyshev polynomials as Among them, m q Indicates the maximum order of Chebyshev polynomial fitting; α l,i Represents the Chebyshev coefficients; the subscript l represents the lth iteration, and the subscript i corresponds to the coefficients of the i-th order Chebyshev polynomial; the Chebyshev coefficients are written in matrix form: The quaternion q of the lth iteration attitude is known l And the angular velocity obtained by polynomial interpolation but: Z=diag([1 / 2,1,…,1]) (14) Where, subscript M = m q , σ k is the Chebyshev zero, defined as For a matrix of dimension n×m, A=[a1,a2,…,a n ] T ,diag(A) is defined as: Represent the attitude quaternion of the l+1th iteration as a Chebyshev polynomial The value of the attitude quaternion of the l+1th iteration at the Chebyshev zero point is expressed in matrix form, Among them, b l+1,i represents the Chebyshev coefficient of the l+1th iteration; the subscript i corresponds to the i-th order Chebyshev polynomial; in the above α l After that, the Chebyshev coefficient matrix b of the attitude quaternion of the l+1th iteration is l+1 Calculated by the following formula: in, x0=[q(0),0,…,0] T (22) C s =UDZF T The element in the 1st row and jth column of matrix D is Therefore, based on the posture matrix Q obtained in the previous iteration l , the Chebyshev polynomial coefficient matrix b of the next iteration process is obtained through matrix calculation l+1 , and then get the new posture matrix Q for the next iteration process l+1 ; According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient of two adjacent iterations is less than the given error or the maximum number of iterations is reached. The error of two adjacent iterations is calculated as follows The step S4 adopts: The function iteration form is constructed from the integral form of the inertial velocity equation: Among them, q * represents the conjugate quaternion of the quaternion q; The Chebyshev polynomial approximation is Among them, m v Indicates the maximum order of Chebyshev polynomial fitting; β l,i Represents the Chebyshev coefficient; write the Chebyshev coefficient in the form of a matrix Given the quaternion iteration result q of the current interval, the specific force f obtained by polynomial interpolation b And the speed of the first time Location Iteration result, β l Calculated by the following formula The l+1th velocity iteration result is expressed using Chebyshev polynomials Write the value of the l+1th velocity at the Chebyshev zero point in matrix form Among them, s l+1,i represents the velocity Chebyshev coefficient of the l+1th iteration, and the subscript i corresponds to the i-th order Chebyshev polynomial; in the calculation of β l After that, the l+1th iteration velocity Chebyshev coefficient is calculated by the following formula: Where M = m v ,η0=[v e (0),0,…,0] T Therefore, based on the previously completed attitude solution results, the velocity matrix obtained in the previous iteration is used The velocity Chebyshev polynomial coefficient matrix s of the next iteration process is obtained through matrix calculation l+1 , and then get the new velocity matrix for the next iteration process According to the above iterative process, the iteration is terminated until the error of the velocity Chebyshev coefficient of two consecutive iterations is less than the given error or the maximum number of iterations is reached. The error of two velocity iterations is calculated as follows The step S5 adopts: The function iteration form is constructed by the integral form of the inertial navigation position equation and the velocity Chebyshev coefficient calculated by the l+1th iteration: On the other hand, the result of the l+1th position iteration is expressed using the Chebyshev polynomial Write the value of the l+1th position at the Chebyshev zero in matrix form Among them, m p Indicates the maximum order of Chebyshev polynomial fitting. The definition of F is the same as that in attitude solution, where M = m p , ρ l+1,i Represents the Chebyshev coefficient, and writes the Chebyshev coefficient in the form of a matrix ρ l+1 The velocity Chebyshev coefficient s obtained directly from the current iteration l+1 calculate: in, C d =UD; Using the velocity Chebyshev polynomial coefficients s obtained in the current iteration process l+1 , based on the function iteration method in matrix form, the coefficient matrix ρ of the position Chebyshev polynomial is solved l+1 , and then solve the position matrix in the next iteration process, so as to complete the position solution; According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient between two adjacent iterative positions is less than the given error or the maximum number of iterations is reached. The error of two position iterations is calculated as follows 2. An inertial navigation solution system based on matrix form function iteration, characterized in that: include: Module M1: Construct the inertial navigation equation in the navigation reference coordinate system, and obtain the inertial navigation attitude integral equation, inertial navigation velocity integral equation and inertial navigation position integral equation based on the constructed inertial navigation equation; Module M2: interpolate or fit the observed values ​​of the gyroscope and accelerometer to obtain the Chebyshev polynomial approximate expression of the angular velocity and specific force in the interval; Module M3: Based on the Chebyshev polynomial approximate expression of angular velocity and specific force in the interval, the inertial navigation attitude integral equation is solved using the function iteration method in matrix form, so as to accurately calculate the attitude; Module M4: Solve the inertial navigation velocity integral equation using a matrix function iteration method based on the calculated attitude value, so as to accurately calculate the velocity; Module M5: Based on the calculated velocity value, the inertial navigation position integral equation is solved using a function iteration method in matrix form to accurately calculate the position; The navigation reference coordinate system includes an earth coordinate system; The module M1 adopts: The expression of the inertial navigation equation in the earth coordinate system is: in, represents the inertial navigation attitude differential equation; the superscript / subscript b represents the carrier coordinate system; e represents the earth coordinate system; i represents the inertial coordinate system; The quaternion representing the attitude of the e system relative to the b system; Represents the attitude rotation matrix of system b relative to system e; represents the angular velocity measured by the gyroscope; represents the angular velocity of the Earth's rotation; represents the inertial velocity differential equation; v e represents the velocity of the carrier relative to the earth in the earth coordinate system; f b Indicates the specific force measured by the accelerometer; g e represents the earth's gravitational acceleration vector in the earth's coordinate system; × represents vector cross product; Represents the multiplication of quaternions; represents the inertial position differential equation; Consider integrating both sides of the inertial navigation equation within the current sampling time window [0, t] to obtain the inertial navigation equation in integral form: Among them, q(0), v e (0), p e (0) represents the initial attitude quaternion, initial velocity and initial position respectively; q represents the attitude quaternion; p e Indicates the position of the carrier in the earth coordinate system; The module M2 adopts: Assume that in the current sampling time window t∈[0,t N ], there is the angular velocity measured by the gyroscope Or angular increment Specific force measured by accelerometer Or speed increment where t k Where k = 1, 2, ..., N, N is the number of samples; use affine transformation to map the time window to τ∈[-1, 1] Interpolate or fit the gyro and accelerometer observations to obtain the angular velocity within the interval Comparison b The Chebyshev polynomial approximation of is: Among them, n w , n f Represents the maximum order of polynomial fitting for angular velocity and specific force respectively; c i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the angular velocity polynomial, d i represents the coefficient of the i-th order Chebyshev polynomial in the approximate expression of the Bili polynomial; F i represents the first kind of Chebyshev polynomials, whose cyclic definition is as follows F0(τ)=1,F1(τ=τ, F i+1 (τ)=2τF i (t)-F i-1 (τ),i≥1 (10); The module M3 adopts: The function iteration form is constructed from the integral form of the inertial navigation attitude equation: Among them, the subscript l represents the result of the lth iteration; It is approximated using Chebyshev polynomials as Among them, m q Indicates the maximum order of Chebyshev polynomial fitting; α l,i Represents the Chebyshev coefficients; subscript l represents the lth iteration, and subscript i corresponds to the coefficients of the i-th order Chebyshev polynomial; the Chebyshev coefficients are written in matrix form: The quaternion q of the lth iteration attitude is known l And the angular velocity obtained by polynomial interpolation but: Z=diag([1 / 2,1,…,1]) (14) Where, subscript M = m q , σ k is the Chebyshev zero, defined as For a matrix of dimension n×m, A=[a1,a2,…,a n ] T ,diag(A) is defined as: Represent the attitude quaternion of the l+1th iteration as a Chebyshev polynomial The value of the attitude quaternion of the l+1th iteration at the Chebyshev zero point is expressed in matrix form, Among them, b l+1,i represents the Chebyshev coefficient of the l+1th iteration; the subscript i corresponds to the i-th order Chebyshev polynomial; in the above α l After that, the Chebyshev coefficient matrix b of the attitude quaternion of the l+1th iteration is l+1 Calculated by the following formula: in, x0=[q(0),0,…,0] T (22) C s =UDZF T The element in the 1st row and jth column of matrix D is Therefore, based on the posture matrix Q obtained in the previous iteration l , the Chebyshev polynomial coefficient matrix b of the next iteration process is obtained through matrix calculation l+1 , and then get the new posture matrix Q for the next iteration process l+1 ; According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient of two adjacent iterations is less than the given error or the maximum number of iterations is reached. The error of two adjacent iterations is calculated as follows The module M4 adopts: The function iteration form is constructed from the integral form of the inertial velocity equation: Among them, q * represents the conjugate quaternion of the quaternion q; The Chebyshev polynomial approximation is Among them, m v Indicates the maximum order of Chebyshev polynomial fitting; β l,i Represents the Chebyshev coefficient; write the Chebyshev coefficient in the form of a matrix Given the quaternion iteration result q of the current interval, the specific force f obtained by polynomial interpolation b And the speed of the first time Location Iteration result, β l Calculated by the following formula The l+1th velocity iteration result is expressed using Chebyshev polynomials Write the value of the l+1th velocity at the Chebyshev zero point in matrix form Among them, s l+1,i represents the velocity Chebyshev coefficient of the l+1th iteration, and the subscript i corresponds to the i-th order Chebyshev polynomial; in the calculation of β l After that, the l+1th iteration velocity Chebyshev coefficient is calculated by the following formula: Where M = m v ,η0=[v e (0),0,…,0] T Therefore, based on the previously completed attitude solution results, the velocity matrix obtained in the previous iteration is used The velocity Chebyshev polynomial coefficient matrix s of the next iteration process is obtained through matrix calculation l+1 , and then get the new velocity matrix for the next iteration process According to the above iterative process, the iteration is terminated until the error of the velocity Chebyshev coefficient of two consecutive iterations is less than the given error or the maximum number of iterations is reached. The error of two velocity iterations is calculated as follows The module M5 adopts: The function iteration form is constructed by the integral form of the inertial navigation position equation and the velocity Chebyshev coefficient calculated by the l+1th iteration: On the other hand, the result of the l+1th position iteration is expressed using the Chebyshev polynomial Write the value of the l+1th position at the Chebyshev zero in matrix form Among them, m p Indicates the maximum order of Chebyshev polynomial fitting. The definition of F is the same as that in attitude solution, where M = m p , ρ l+1,i Represents the Chebyshev coefficient, and writes the Chebyshev coefficient in the form of a matrix ρ l+1 The velocity Chebyshev coefficient s obtained directly from the current iteration l+1 calculate: in, C d =UD; Using the velocity Chebyshev polynomial coefficients s obtained in the current iteration process l+1 , based on the function iteration method in matrix form, the coefficient matrix ρ of the position Chebyshev polynomial is solved l+1 , and then solve the position matrix in the next iteration process, so as to complete the position solution; According to the above iterative process, the iteration is terminated until the error of the Chebyshev coefficient between two adjacent iterative positions is less than the given error or the maximum number of iterations is reached. The error of two position iterations is calculated as follows

Images