Background
Since the advent of the strapdown inertial navigation system, scholars at home and abroad have conducted a great deal of research on high-performance strapdown inertial navigation algorithms. The navigation algorithm of the strapdown inertial navigation system consists of a posture updating algorithm, a speed updating algorithm and a position updating algorithm, wherein the posture updating algorithm is the core of the whole strapdown algorithm. This is because the attitude update algorithm not only directly determines the accuracy of the navigation attitude angle, but also has a crucial influence on the output accuracy of speed and position. For a fiber optic gyroscope strapdown inertial navigation system, the output of the gyroscope contains not only the angular velocity of the carrier, but also some high frequency noise. Averaging filtering is usually required to demodulate the angular velocity information of the carrier. Amplitude-frequency and phase-frequency characteristics of the average filter influence gyro signals, so that the filtered signals are distorted. Therefore, it is necessary to develop an error-free attitude updating method suitable for the fiber-optic gyroscope filtering signal.
However, in published articles, such as yellow Lei, liu Jian and ever-celebrated New Cone Algorithm based on high-order compensation model in the book 17 of the book of Chinese inertia technology, no. 6, higher-order analysis of rotational vector differential equations is performed to better compensate for cone errors. In addition, ignagi M B takes the limited bandwidth of the gyro into consideration in the article of "Optimal stripe integration algorithms" published in Journal of guide Control and Dynamics, and designs the Optimal cone compensation algorithm. In addition, wangman and Wu Wen started from the article "High-order approximation in connection and rotation relating" published in IEEE Transactions on Aerospace and Electronic Systems "take the consideration more thoroughly, the six-order approximation of the differential equation of the rotation vector is retained, the attitude updating precision is greatly improved, but the derivation process of the compensation algorithm is very complicated.
The published articles describe and explore the attitude updating algorithm of strapdown inertial navigation, but all the attitude updating algorithms are obtained on the basis of approximation of a rotary vector differential equation, and a principle error exists essentially. In addition, the above algorithms are based on ideal sensor output signals, and in practical situations, the sensor signals are distorted, resulting in reduced algorithm performance. Therefore, the method for researching the error-free attitude updating algorithm based on the output signals of the sensors under the actual conditions has innovativeness and actual engineering value.
Disclosure of Invention
The invention provides an error-free attitude updating method suitable for a filter signal of a fiber-optic gyroscope, and aims to realize error-free attitude updating under the condition of distortion of an output signal of a sensor.
The purpose of the invention is realized by the following steps:
step 1. Given initial navigation parameters (time t = 0): obtaining an initial pitch attitude angle theta by initial alignment of a fiber optic gyroscope strapdown inertial navigation system 0 Initial roll attitude angle gamma 0 Initial azimuth psi 0 。
Step 2, the system sets a sampling period h, an attitude calculation period T, T = h, and an attitude calculation period cycle mark k, namely T k To t k+1 The time period represents a speed resolving period T, and the output signals omega of the fiber-optic gyroscope on three axes after average filtering processing are collected in real time k ,k=0,1,2…;
Step 3, phase compensation digital filter H
F (Z) carrying out phase compensation on the output signal of the fiber-optic gyroscope after average filtering processing to obtain the gyroscope output signal without phase delay
Step 4. Recursion measuring t k+1 Angular velocity alpha of rotation of the movement of the time rotation coordinate system s relative to the carrier coordinate system b k+1 。
Step 5. Recursion measuring t
k+1 Transformation matrix from time carrier coordinate system b to rotating coordinate system s
Step 6. Recursion measuring t
k+1 Time of day rotating coordinate system s to t
k Transformation matrix of time carrier coordinate system b
Step 7. Recursion measuring t
k+1 Time carrier coordinate system b to t
k Transformation matrix of time carrier coordinate system b
Using t
k+1 Time of dayCarrier coordinate system b to t
k Transformation matrix of time carrier coordinate system b
And the attitude updating without error suitable for the filtering signal of the fiber-optic gyroscope is completed by combining the result of the last resolving period.
The invention also includes:
the specific expression of the phase compensation digital filter in the step 3 is as follows:
where M is the order of the phase compensated digital filter, λ
1 ,λ
2 ,...,λ
M Are weight coefficients, i.e. the undetermined coefficients of the digital filter. Weight coefficient lambda
1 ,λ
2 ,...,λ
M The expression is as follows:
wherein Q 2 Is λ 2 ,λ 3 ,λ 4 ,…,λ M A column vector of M-1 dimension, Z is a square matrix of (M-1) × (M-1), E is a column vector of M-1 dimension:
Z(r,s)={S(Ω)[1-cos(rΩT)]+C(Ω)sin(rΩT)}·{S(Ω)[1-cos(sΩT)]+C(Ω)sin(sΩT)}
E(r)=S(Ω){S(Ω)[1-cos(rΩT)]+C(Ω)sin(rΩT)}
wherein r is the row number r =0,1,2, \8230;, M-1 of the square matrix Z and the column vector E; s is the number of columns of the square matrix s =0,1,2, \ 8230;, M-1; omega is a vibration main frequency point set by a user according to the working environment of the fiber-optic gyroscope, and S (omega) = sin [ gamma ] F (Ω)],C(Ω)=cos[γ F (Ω)],γ F (Ω) is the phase delay of the phase compensated digital filter at the main frequency point Ω.
The process in the step 4 specifically comprises the following steps: using t
k Gyro output signal without phase delay at any moment
And t
k+1 Gyro output signal without phase delay at any moment
Obtaining the rotation angular velocity alpha of the movement of the rotating coordinate system s relative to the carrier coordinate system b
k+1 Is composed of
Wherein omega is a vibration dominant frequency point, | H, set by a user according to the working environment of the fiber-optic gyroscope F (omega) | is the amplitude-frequency gain of the phase compensation digital filter at the main frequency point omega; sin for medical use -1 (. -) represents the arcsine value of.i | represents the module value of.x represents the vector cross product.
Step 5 said transformation matrix
The solving process of (2) is specifically as follows: using t obtained in step 4
k+1 Angular velocity alpha of rotation of the movement of the time rotation coordinate system s relative to the carrier coordinate system b
k+1 Calculating to obtain t
k+1 Transformation matrix from time carrier coordinate system b to rotating coordinate system s
Wherein
Each represents alpha
k+1 The x, y, z axis components of (a).
Step 6 said transformation matrix
The solving process is as follows: using t
k Gyro output signal without phase delay at moment
And t obtained in step 4
k+1 Angular velocity alpha of rotation of the movement of the time-of-day rotating coordinate system s relative to the carrier coordinate system b
k+1 Calculating to obtain t
k+1 Time of day rotating coordinate system s to t
k Transformation matrix of time carrier coordinate system b
Step 7 the transformation matrix
The solving process is as follows: using t obtained in step 5
k+1 Transformation matrix from time carrier coordinate system b to rotating coordinate system s
And t obtained in step 6
k+1 Time of day rotating coordinate system s to t
k Transformation matrix of time carrier coordinate system b
Calculating to obtain t
k+1 Time carrier coordinate system b to t
k Transformation matrix of time carrier coordinate system b
The invention has the beneficial effects that:
aiming at the problem of attitude updating of a strapdown inertial navigation system, the invention provides an error-free attitude updating method suitable for a filter signal of a fiber-optic gyroscope, so that error-free attitude updating under the condition of signal distortion of a sensor is realized, and a good foundation is laid for the next speed and position updating.
Detailed Description
The invention is further described below with reference to the accompanying drawings:
the invention provides an error-free attitude updating method suitable for a fiber optic gyroscope filtering signal. The method comprises the following steps: obtaining an initial pitch attitude angle theta by initial alignment of the fiber-optic gyroscope strapdown inertial navigation system 0 Initial roll attitude angle gamma 0 Initial azimuth psi 0 ;
The system sets a sampling period h and an attitude resolving period T, and acquires output signals omega of the fiber optic gyroscope subjected to average filtering processing on three axes in real time k ,k=0,1,2…;
Phase compensated digital filter H
F (Z) carrying out phase compensation on the collected fiber-optic gyroscope filtering signal to obtain a gyroscope output signal without phase delay
Using t
k Gyro output signal without phase delay at any moment
And t
k+1 Gyro output signal without phase delay at any moment
Calculating the rotation angular velocity alpha of the motion of the rotating coordinate system s relative to the carrier coordinate system b by using vector cross multiplication
k+1 ;
Using alpha obtained as above
k+1 Calculating to obtain t
k+1 Conversion matrix from time carrier coordinate system to rotating coordinate system
Using t
k Gyro output signal without phase delay at moment
And t
k+1 Angular velocity alpha of rotation of the movement of the time-of-day rotating coordinate system relative to the carrier coordinate system
k+1 Calculating to obtain t
k+1 Rotating the coordinate system by time t
k Transformation matrix of time carrier coordinate system
Using the transformation matrix obtained above
And
calculating to obtain t
k+1 Time carrier coordinate system to t
k Transformation matrix of time carrier coordinate system
And the attitude updating without error suitable for the filtering signal of the fiber-optic gyroscope is completed by combining the result of the previous resolving period. The invention solves the problem of error-free attitude updating of the fiber-optic gyroscope filtering signal.
The method for updating the attitude of the filtering signal of the fiber-optic gyroscope without error comprises the following steps:
step 1. Given initial navigation parameters (time t = 0): obtaining an initial pitch attitude angle theta by initial alignment of the fiber-optic gyroscope strapdown inertial navigation system 0 Initial roll attitude angle gamma 0 Initial azimuth psi 0 。
Step 2, the system sets a sampling period h, an attitude calculation period T, T = h, and an attitude calculation period cycle mark k, namely T k To t k+1 The time period represents one speed resolving period T. Acquiring output signals omega of the fiber optic gyroscope subjected to average filtering processing on three axes in real time k ,k=0,1,2…;
Step 3. PhaseCompensating digital filter H
F (Z) carrying out phase compensation on the output signal of the fiber-optic gyroscope after average filtering processing to obtain the gyroscope output signal without phase delay
The specific expression of the phase compensation digital filter is as follows:
where M is the order of the phase compensated digital filter, λ
1 ,λ
2 ,...,λ
M Are weight coefficients, i.e. the undetermined coefficients of the digital filter. Weight coefficient lambda
1 ,λ
2 ,...,λ
M The expression is as follows:
wherein Q is 2 Is λ 2 ,λ 3 ,λ 4 ,…,λ M A column vector of M-1 dimension, Z is a square matrix of (M-1) × (M-1), E is a column vector of M-1 dimension:
Z(r,s)={S(Ω)[1-cos(rΩT)]+C(Ω)sin(rΩT)}·{S(Ω)[1-cos(sΩT)]+C(Ω)sin(sΩT)}
E(r)=S(Ω){S(Ω)[1-cos(rΩT)]+C(Ω)sin(rΩT)}
wherein r is the row number r =0,1,2, \8230;, M-1 of the square matrix Z and the column vector E; s is the number of columns of the square matrix s =0,1,2, \ 8230;, M-1; omega is a vibration main frequency point set by a user according to the working environment of the fiber-optic gyroscope, and S (omega) = sin [ gamma ], (gamma) F (Ω)],C(Ω)=cos[γ F (Ω)],γ F (Ω) is the phase delay of the phase compensated digital filter at the main frequency point Ω.
Step 4. Recursion measuring t
k+1 Angular velocity alpha of rotation of the movement of the time rotation coordinate system s relative to the carrier coordinate system b
k+1 . I.e. using t
k Gyro output signal without phase delay at any moment
And t
k+1 Time of day without phase delayOutput signal of gyro
Obtaining the rotation angular velocity alpha of the movement of the rotating coordinate system s relative to the carrier coordinate system b
k+1 Is composed of
Wherein omega is a vibration dominant frequency point, | H, set by a user according to the working environment of the fiber-optic gyroscope F (omega) is the amplitude-frequency gain of the phase compensation digital filter at the main frequency point omega; sin for medical use -1 (. -) represents the arcsine value of.i | represents the module value of.x represents the vector cross product.
Step 5. Recursion measuring t
k+1 Transformation matrix from time carrier coordinate system b to rotating coordinate system s
Using t obtained in step 4
k+1 Angular velocity alpha of rotation of the movement of the time-of-day rotating coordinate system s relative to the carrier coordinate system b
k+1 Calculating to obtain t
k+1 Transformation matrix from time carrier coordinate system b to rotating coordinate system s
Wherein
Each represents alpha
k+1 The x, y, z axis components of (a).
Step 6. Recursion measuring t
k+1 Time of day rotating coordinate system s to t
k Transformation matrix of time carrier coordinate system b
Using t
k Time of day without phaseDelayed gyro output signal
And t obtained in step 4
k+1 Angular velocity alpha of rotation of the movement of the time-of-day rotating coordinate system s relative to the carrier coordinate system b
k+1 Calculating to obtain t
k+1 Time of day rotating coordinate system s to t
k Transformation matrix of time carrier coordinate system b
Step 7. Recursion measuring t
k+1 Time carrier coordinate system b to t
k Transformation matrix of time carrier coordinate system b
Using t obtained in step 5
k+1 Transformation matrix from time carrier coordinate system b to rotating coordinate system s
And t obtained in step 6
k+1 Time of day rotating coordinate system s to t
k Transformation matrix of time carrier coordinate system b
Calculating to obtain t
k+1 Time carrier coordinate system b to t
k Transformation matrix of time carrier coordinate system b
Wherein
Wherein
Wherein
Representing a transformation matrix
The first row and column elements, and so on.
Using t
k+1 Time carrier coordinate system to t
k Transformation matrix of time carrier coordinate system
And the attitude updating without error suitable for the filtering signal of the fiber-optic gyroscope is completed by combining the result of the last resolving period.
The invention realizes the attitude update without error under the condition of sensor signal distortion. In order to verify the beneficial effect of the method, the method is used for simulating in a typical conical environment, so that the algorithm error is mainly reflected on an x axis and the error is dispersed along with time. Taking the half cone angle a of the conical motion to be =5 degrees, the angular frequency omega =20 pi rad/s, the sampling period of the gyroscope is 0.005s, and the simulation time is 60s. Compared with the traditional quaternion four-order Runge Kutta method and the equivalent rotation vector method of angular rate fitting, the result is shown in FIG. 2: group A is the simulation result of the traditional quaternion four-order Runge Kutta method, group B is the simulation result of the equivalent rotation vector method of angular rate fitting, and group C is the simulation result of the invention.