CN109086250B - Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope - Google Patents
Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope Download PDFInfo
- Publication number
- CN109086250B CN109086250B CN201811004559.6A CN201811004559A CN109086250B CN 109086250 B CN109086250 B CN 109086250B CN 201811004559 A CN201811004559 A CN 201811004559A CN 109086250 B CN109086250 B CN 109086250B
- Authority
- CN
- China
- Prior art keywords
- mems
- filter
- fog
- sub
- gyroscope
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
- G01C19/58—Turn-sensitive devices without moving masses
- G01C19/64—Gyrometers using the Sagnac effect, i.e. rotation-induced shifts between counter-rotating electromagnetic beams
- G01C19/72—Gyrometers using the Sagnac effect, i.e. rotation-induced shifts between counter-rotating electromagnetic beams with counter-rotating light beams in a passive ring, e.g. fibre laser gyrometers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Abstract
The invention discloses a data fusion method suitable for an MEMS (micro electro mechanical system) inertial measurement unit with an inclined fiber gyroscope, wherein the algorithm of the invention is designed aiming at the redundant structures of a single-axis FOG (FOG) and the MEMS inertial measurement unit, wherein the FOG sensitive axis and three measurement axes of the MEMS inertial measurement unit are arranged at equal included angles; the MEMS inertial set comprises: integrating a three-axis MEMS gyroscope and a three-axis MEMS accelerometer which are mutually orthogonal; the method comprises the following steps: s1: carrying out error compensation on the MEMS gyroscope by utilizing a grey prediction model; s2: the FOG is respectively combined with the MEMS gyroscope and the MEMS accelerometer, a part of feedback type federal filter is adopted for filtering, meanwhile, the feedback coefficient of the part of feedback type federal filter is adjusted, and the output of the FOG and the MEMS inertial group are subjected to data fusion. The method provided by the invention can effectively improve the precision of the whole inertial measurement unit.
Description
Technical Field
The invention relates to the technical field of research on redundancy methods of low-precision inertial measurement units and high-precision inertial measurement units, in particular to a data fusion method suitable for an MEMS inertial measurement unit with an inclined fiber-optic gyroscope.
Background
With the high-speed development of micro-mechanical manufacturing technology, micro-nano technology and integrated optical technology, an inertia measurement unit (MEMS inertial unit for short) composed of micro-electromechanical (MEMS) inertial sensors is widely and rapidly developed, and the application field of the inertia technology is widened.
The MEMS inertial unit generally comprises an MEMS gyroscope and an MEMS accelerometer, has the advantages of small volume, light weight, easiness in batch production, low cost and the like, and is rapidly popularized and applied in the fields of common civil use and certain unmanned system navigation. But the disadvantages are also obvious: the accuracy is relatively low. Specifically, the processing and design technology development of the MEMS accelerometer is faster than that of the MEMS gyroscope, and the accuracy of the MEMS accelerometer can reach dozens of μ g at present, but the accuracy of the MEMS gyroscope is generally lower due to the limitation of the conditions such as the current process level and the detection circuit, and the accuracy of a single MEMS gyroscope has been improved to a stage limit, which becomes a main factor restricting the application of the MEMS accelerometer in the high-accuracy measurement field. The measurement precision of the MEMS gyroscope is improved, accurate navigation information is obtained, and the method is the most direct and effective means for expanding the application field of the MEMS inertial measurement unit. However, due to uncertainty of environmental factors such as temperature, vibration, magnetic field and the like of the MEMS carrier, the dynamic characteristics of the MEMS gyroscope cause severe nonlinearity, time-varying property and uncertainty, and the improvement effect of the MEMS gyroscope precision only by using various error compensation methods is very limited.
Therefore, how to effectively improve the precision of the low-cost MEMS inertial set is a problem that needs to be solved urgently by those skilled in the art.
Disclosure of Invention
In view of the above, the invention provides a data fusion method suitable for an MEMS inertial measurement unit with an inclined fiber optic gyroscope, which improves an angular velocity output value of the MEMS gyroscope by using a gray prediction model, combines the MEMS gyroscope with a fiber optic gyroscope FOG, performs filtering processing by using a partial feedback federal filter, and performs data fusion on inertial information output of the fiber optic gyroscope and the MEMS inertial measurement unit by adjusting a feedback coefficient of the federal filter, thereby further reducing system errors and improving system accuracy.
In order to achieve the purpose, the invention adopts the following technical scheme:
the invention relates to a data fusion method suitable for an MEMS inertial measurement unit with an inclined fiber-optic gyroscope, wherein the algorithm is designed aiming at a single-axis FOG and a redundant structure of the MEMS inertial measurement unit, wherein an FOG sensitive axis and three measuring axes of the MEMS inertial measurement unit are arranged at equal included angles; integrating a three-axis MEMS gyroscope and a three-axis MEMS accelerometer which are mutually orthogonal;
the method comprises the following steps:
s1: carrying out error compensation on the MEMS gyroscope by utilizing a grey prediction model;
s2: and respectively combining the FOG with the MEMS gyroscope and the MEMS accelerometer, filtering by adopting a partial feedback type federal filter, adjusting the feedback coefficient of the partial feedback type federal filter, and performing data fusion on the output of the FOG and the MEMS inertial group.
Preferably, step S1 specifically includes:
let the angular velocity output value of the MEMS gyroscope be x (0) (i) I 1,2, … n, incrementally added to generate a new sequence of 1-AGO:
wherein x is (1) (1)＝x (0) (1) Outputting a value of the first angular velocity of the MEMS gyroscope; x is the number of (1) (k) Expressing the k-th angular velocity predicted value, and obtaining an adaptive residual gray prediction model:
in the formula, a represents a gray prediction model development coefficient, and b represents a gray prediction model control parameter;
the values of parameters a and b are estimated by the least squares method:
[a b] T ＝(B T B) -1 B T y n
in the formula, B represents a coefficient,
(x (1) (1) … x (1) (n)) represents the predicted angular velocity value, y n Representing angular velocity values of gyro output, y n ＝[x (0) (1),x (0) (2),…,x (0) (n)] T ；
Obtaining a predicted fitting value by using a gray reduction model 1-IAGO so as to obtain an MEMS gyroscope angular velocity output new sequence after error compensation:
in the formula (I), the compound is shown in the specification,
representing the k +1 th angular velocity estimate,
represents the k +1 th angular velocity predicted value,
represents the predicted value of the k-th angular velocity,
the 1 st estimate of the angular velocity is shown,
the 1 st predicted angular velocity is indicated.
Preferably, step S2 specifically includes:
s21: combining and inputting FOG and MEMS accelerometer to a sub-filter 1;
measuring angular velocity of FOG under carrier coordinate system
Subtracting a pass transformation matrix
The converted earth angular rate under the carrier coordinate system obtains the angular rate of the carrier
Then, the FOG attitude matrix is solved by utilizing a quaternion method
Then eliminating the attitude error estimated by the main filter to obtain the output of the public system
At the same time, the output of the MEMS accelerometer is measured
Subtracting a pass transformation matrix
Converted gravitational acceleration in a carrier coordinate system, wherein G n The gravity acceleration under the navigation coordinate system is input into the sub-filter 1;
s22: combining and inputting the FOG and the MEMS gyroscope subjected to error compensation into a sub-filter 2;
MEMS gyroscope compensated by gray prediction model and outputting angular speed of MEMS gyroscope in carrier coordinate system
Subtracting a pass transformation matrix
The converted earth angular rate under the carrier coordinate system obtains the angular rate of the carrier
Then the output of the MEMS gyroscope is output
Subtracting the output of FOG
As a measure of the quantity of the sub-filter 2;
s23: the main filter of the Federal filter combines the error states of the sub-filter 1 and the sub-filter 2
And covariance P 1 、P 2 Introducing and carrying out optimal fusion to obtain global optimal estimation
And global covariance matrix P g ；
The main filter estimates the global optimum according to the information conservation principle and the information distribution principle
And global covariance matrix P g Feeding back to the sub-filter 1 and the sub-filter 2, and performing data fusion on the output of the FOG and the MEMS inertial measurement unit; wherein the covariance P g Part is introduced into one of the sub-filters and the remaining part is introduced into the other sub-filter.
Preferably, the method further comprises the following steps: after S22, the measured variances of the two sub-filters are adaptively designed:
in the formula, R 1 Represents the measured variance, R, of the filter 1 2 Represents the measured variance, C, of the filter 2 ra Denotes the adaptive coefficient, C, of the filter 1 rm Represents the adaptive coefficients of the filter 2;
in addition, consider a global covariance matrix P g Is a sub-covariance matrix P 1 、P 2 The fusion of (1) cannot accurately reflect the error state of each sub-filter; therefore, the MEMS accelerometer and the MEMS gyroscope and FOG difference are introduced into the covariance matrix, so that the covariance matrix P 1 、P 2 Also according to the error state, making self-adaptive adjustment, and making covariance matrix P 1 、P 2 Respectively satisfy:
in the formula, C pa Denotes the adaptive coefficient, C, of the filter 1 pm The adaptive coefficients of the filter 2 are represented.
Preferably, C ra ＝0.03，C rm ＝4。
Preferably, C pa ＝9，C pm ＝0.001。
Preferably, the included angle formed by mounting the single-axis FOG sensitive axis and the three measuring axes of the MEMS inertial measurement unit at equal included angles is 54.75 °.
According to the technical scheme, compared with the prior art, the invention discloses a data fusion method suitable for an MEMS (micro electro mechanical system) inertial set with an inclined fiber-optic gyroscope, which starts from a redundant structure of the MEMS inertial set and a single-axis FOG (field oriented G), a self-adaptive residual high-precision gray prediction model is constructed, the angular velocity output value of the MEMS gyroscope is improved by using the gray prediction model, the MEMS gyroscope and the FOG of the fiber-optic gyroscope are combined, a part of feedback type federal filter is adopted for filtering, meanwhile, the inertial information output of the fiber-optic gyroscope and the MEMS inertial set are subjected to data fusion by adjusting the feedback coefficient of the federal filter, the system error is further reduced, and the system precision is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
FIG. 1 is a schematic structural diagram of a Federal Kalman filtering algorithm provided by the present invention;
FIG. 2 is a schematic diagram of an arrangement of a single-axis FOG and MEMS inertial set provided by the present invention;
FIG. 3 is a schematic diagram of a gray prediction model and a partial feedback federal filter according to the present invention;
FIG. 4 is a diagram illustrating a comparison of attitude angles of an inertial set with tilted FOG and a pure MEMS inertial set according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The embodiment of the invention discloses a data fusion method suitable for an MEMS (micro-electromechanical system) inertial group with an inclined fiber-optic gyroscope, which is characterized in that a gray prediction model is utilized to improve the angular velocity output value of the MEMS gyroscope, then the MEMS gyroscope and the FOG (fiber-optic gyroscope) are combined, a part of feedback type federal filter is adopted for filtering, meanwhile, the feedback coefficient of the federal filter is adjusted, the inertial information output of the fiber-optic gyroscope and the MEMS inertial group are subjected to data fusion, the system error is further reduced, and the system precision is improved.
Before describing the specific method provided by the present invention, first, the basic principle adopted by the present invention is introduced.
The grey prediction theory is created by professor Dengdong in 1982, mainly aims at the problems of information uncertainty, data deficiency and the like, processes irregular original data by an accumulation operation method, weakens the randomness and the fluctuation of data caused by environment, devices and the like, generates data with a regular quasi-exponential law, and models the data. The method has the characteristics that the prediction formula and the modeling process are simpler, the used sample values are fewer, and the solution is easier. However, since the method is not suitable for the middle-long term prediction, in recent years, many researchers have proposed methods for improving the gray prediction model, such as a method for improving model parameter estimation, a method for improving whitening background value, and a method for changing initial conditions.
The federal kalman filtering technique differs from other dispersive filtering methods in that it includes an information distribution process in which the motion information of the system is distributed among the local filters based on an information distribution principle. The use of the information distribution principle makes the federal filter conceptually distinguishable from centralized filtering algorithms and other decentralized filtering methods. The structure of the Federal Kalman filtering algorithm is shown in FIG. 1.
In FIG. 1, X denotes a state quantity of a public system, Z 1 Representing the state quantity, Z, of the subsystem 1 2 Representing the state quantity, Z, of the subsystem 2 n Representing the state quantity of the subsystem n,
representing the global estimate, P, of the main filter g Representing the covariance matrix of the main filter,
representing the global estimate of the first sub-filter, P 1 Representing the first sub-filter covariance matrix,
expressed as the first sub-filter feedback coefficient p 1 The inverse of the matrix of (a) is,
representing the global estimate of the second sub-filter, P 2 Representing the second sub-filter covariance matrix,
expressed as the first sub-filter feedback coefficient p 2 The inverse of the matrix of (a) is,
representing the global estimate, P, of the nth sub-filter n Representing the nth sub-filter covariance matrix,
expressed as the first sub-filter feedback coefficient p n The inverse matrix of (c).
The specific method provided by the present invention is described in detail below.
The method starts from a redundant structure of an MEMS inertial measurement unit and a single-axis FOG, constructs a self-adaptive residual error type high-precision gray prediction model, improves a whitening formula of the gray prediction model by using a residual error of a whitened measured value, changes an initial condition of the measured value in the prediction model according to the theory that the cognitive proportion of new information is greater than that of old information, and improves the prediction model, so that the prediction precision of the constructed model is greatly improved. And data fusion is carried out on the MEMS inertial unit and the single-axis fiber-optic gyroscope through a partial feedback filter, so that the precision of the whole inertial unit system is improved.
1. System space configuration
According to the structural characteristics of the MEMS inertial group and the uniaxial FOG, a configuration scheme is designed such that an α ═ β ═ γ ═ 54.75 ° is installed at equal included angles between a sensitive axis of the uniaxial FOG and three measurement axes of the MEMS inertial group, as shown in fig. 2.
In the figure, α represents an angle between the fiber optic gyroscope and an x-axis coordinate system, β represents an angle between the fiber optic gyroscope and a y-axis coordinate system, and γ represents an angle between the fiber optic gyroscope and a z-axis coordinate system.
The theoretical angular velocity on an orthogonal coordinate system defining the carrier is:
ω＝[ω x ω y ω z ] T (1)
in the formula, ω x Representing theoretical angular velocity, omega, of the MEMS gyroscope on the x-axis y Representing theoretical angular velocity, omega, of the MEMS gyroscope on the y-axis z Representing the theoretical angular velocity of the MEMS gyroscope on the z-axis.
The angular velocity measurements of the triaxial MEMS gyroscope and uniaxial FOG were:
m＝[m x m y m z m f ] T (2)
in the formula, m x Represents the angular velocity measurement value m of the x-axis MEMS gyroscope y Represents the angular velocity measurement m of the y-axis MEMS gyroscope z Represents the angular velocity measurement value m of the z-axis MEMS gyroscope f Indicates angular velocity measurements of FOG. The relationship that the measured value and the theoretical output value of the orthogonal axis satisfy is as follows:
m＝Hω+ξ (3)
in the formula, H represents a measurement matrix; ξ represents the residual noise.
And according to the configuration scheme, analyzing by adopting a redundant gyro data fusion algorithm based on a least square method. The least square method is an algorithm proposed by gaussian in 1795 for measuring planets, and is a method for minimizing the sum of squares of deviations of an estimated value from each measured value. The data fusion is performed using the least squares method, based first on the following assumptions:
a. the modeling is accurate, and the measured value m and the theoretical angular velocity omega have a definite function relation of m ═ H omega + xi;
b. the measurement error of each measurement value of the quantity m only comprises a random error xi and is equal in variance;
c. the measurement error of each measurement value of the quantity m is only random error xi, is independent and unbiased and follows Gaussian distribution.
The assumption can be determined to be true from the output characteristics of each axis gyro. The least square method is used for seeking the estimation value of the theoretical angular velocity omega
The sum of the squares Q of the deviations is minimized, i.e.:
therefore, only need to make
The set of estimates can be obtained
At this time, the following conditions are satisfied:
in the formula, H T Representing the transpose of the measurement matrix H.
Calculating to obtain H for the configuration scheme T H is a non-singular matrix, so the solution of this set of estimates is also unique, resulting in the formula:
angular velocity measurement
Including the true angular velocity ω and the fusion error Δ ω, as shown in equation (6):
the measurement vector of the sensor can be obtained by the same method as follows:
where m represents the measurement accuracy and Δ m represents the measurement error.
The simultaneous formulas (6), (7) and (8) give:
ω+Δω＝(H T H) -1 H T (m+Δm) (9)
thus, there are:
Δω＝(H T H) -1 H T Δm (10)
as can be seen from equation (10), there is a linear relationship between the measurement error and the fusion error. The following is a development analysis of the error relationship of the configuration scheme in fig. 1, and first the measurement matrix of the scheme in fig. 1 is given as:
2. self-adaptive residual gray prediction model
Let the output of the MEMS gyroscope be x (0) (i) I 1,2, … n, incrementally added to generate a new sequence of 1-AGO:
wherein x is (1) (1)＝x (0) (1) Outputting a value of the first angular velocity of the MEMS gyroscope; x is the number of (1) (k) Expressing the k-th angular velocity predicted value, and obtaining an adaptive residual gray prediction model:
in the formula, a represents a gray prediction model development coefficient, and b represents a gray prediction model control parameter;
the values of parameters a and b are estimated by the least squares method:
[a b] T ＝(B T B) -1 B T y n
in the formula, B represents a coefficient,
(x (1) (1) … x (1) (n)) represents the predicted angular velocity value, y n Representing angular velocity values of gyro output, y n ＝[x (0) (1),x (0) (2),…,x (0) (n)] T ；
Obtaining a predicted fitting value by using a gray reduction model 1-IAGO so as to obtain an MEMS gyroscope angular velocity output new sequence after error compensation:
in the formula (I), the compound is shown in the specification,
representing the k +1 th angular velocity estimate,
represents the k +1 th angular velocity predicted value,
represents the predicted value of the k-th angular velocity,
the 1 st estimate of the angular velocity is shown,
the 1 st predicted angular velocity is indicated.
3. Partial feedback federal filter
The MEMS gyroscope output value improved by the grey prediction model improves the precision in a short time and reduces the error of the MEMS gyroscope. For long-term use, in order to further improve the precision, the MEMS gyroscope and the fiber-optic gyroscope are combined, and a partial feedback type federal filter is adopted for filtering. Meanwhile, the feedback coefficient of the federal filter is adjusted, and the inertial information output of the fiber-optic gyroscope and the MEMS inertial unit are subjected to data fusion, so that the system error is further reduced, and the system precision is improved. The main structure is shown in fig. 3.
As shown in FIG. 3, the FOG is little affected by the outside world, and the error is derived from the sensor drifting itself. The gyro output without the earth angular rate is used for solving the FOG attitude matrix by a quaternion method
Eliminating the attitude error estimated by the main filter to obtain an output attitude matrix
And obtaining the attitude angle output of the system through the attitude matrix, and taking the attitude angle as the output of the Federal filter public system. The FOG/MEMS accelerometer sub-filter (sub-filter 1) and the FOG/MEMS gyro sub-filter (sub-filter 2) are respectively formed into two independent subsystems.
It should be noted that fig. 1 is a general structure of the federal filter, in which the state quantity X corresponding to the common system is directly inputted to the main filter, and fig. 3 is a schematic structural diagram of the filter provided by the present invention, in which the designed federal filter has no filtering element and directly introduces the error posture and covariance of two sub-filters, and the state quantity corresponding to the common system is fed back to the input end of each sub-filter through the output posture matrix, which is also an improvement of the present invention.
The principle of each sub-filter is as follows:
(1) FOG and MEMS accelerometer sub-filter 1;
measuring angular velocity of FOG under carrier coordinate system
Subtracting a pass transformation matrix
The converted earth angular rate under the carrier coordinate system obtains the angular rate of the carrier
Then, the FOG attitude matrix is solved by utilizing a quaternion method
Then eliminating the attitude error estimated by the main filter to obtain the output of the public system
Simultaneously, the output of the MEMS accelerometer is measured
Subtracting a pass transformation matrix
Converted gravitational acceleration in a carrier coordinate system, wherein G n The gravity acceleration under the navigation coordinate system is input into the sub-filter 1;
(2) FOG and MEMS gyroscope filter 2;
because the high-precision MEMS inertial set is adopted by the invention, in order to enable the output of the MEMS gyroscope to be more accurate, the angular speed of the MEMS gyroscope compensated by the grey prediction model under the carrier coordinate system is output
Subtracting a pass transformation matrix
The converted earth angular rate under the carrier coordinate system obtains the angular rate of the carrier
Then the output of the MEMS gyroscope is output
Subtracting the output of FOG
As a measure of the quantity of the sub-filter 2.
The main filter of the Federal filter combines the error states of the sub-filter 1 and the sub-filter 2
And covariance P 1 、P 2 Introducing and carrying out optimal fusion to obtain global optimal estimation
And global covariance matrix P g ；
The main filter estimates the global optimum according to the information conservation principle and the information distribution principle
And global covariance matrix P g Feeding back to the sub-filter 1 and the sub-filter 2, and performing data fusion on the output of the FOG and the MEMS inertial measurement unit; wherein the covariance P g Part is introduced into one of the sub-filters and the remaining part is introduced into the other sub-filter.
The state equation and the measurement equation of the sub-filter 1 are as follows:
in the formula, X 1 (t) represents the state quantity at time t of the 1 st sub-filter,
representing the state quantity estimator at time t of the 1 st sub-filter;
δ Θ (t) represents an attitude angle estimation error at time t, and δ Θ (t) ([ δ ψ (t) ] δ θ (t) ] δ Φ (t) ]] T δ ψ (t) represents a heading angle estimation error at time t, δ θ (t) represents a pitch angle estimation error at time t, δ φ (t) represents a roll angle estimation error at time t;
b f (t) zero offset of the fiber optic gyroscope expressed as x-axis, y-axis and z-axis at time t, b f (t)＝[b fx (t) b fy (t) b fz (t)] T ，b fx (t) x-axis fiber optic gyroscope zero bias at time t, b fy (t) zero offset of y-axis fiber-optic gyroscope at time t, b fz (t) represents the zero offset of the z-axis fiber optic gyroscope at time t;
the adopted uniaxial FOG is inclined in structure, and δ a (t) represents the acceleration measurement error of the MEMS accelerometer at the time t;
F 1 (t) represents the state transition matrix at time t,
the attitude matrix of the fiber-optic gyroscope representing time t, F a (t) diagonal matrix of accelerometer cut-off frequencies at time t, F a (t)＝diag(-c ax (t),-c ay (t),-c az (t)), w (t) represents a systematic random error w (t) at time t ═ w Θ (t)w b (t)w a (t)] T ，w Θ (t) random error, w, of corresponding error angle at time t b (t) Gyro zero-bias random error at time t, w a (t) represents random errors of the accelerometer at the time t, and the errors are all zero mean white noise, namely the following conditions are satisfied:
E[w(t)]＝0,Cov[w(t)w T (τ)]＝E[w(t)w T (τ)]＝qδ(t-τ)
wherein q represents the covariance strength of w (t);
Z 1 (t) represents the accelerometer measurement error at time t, τ represents any time other than time t, and Z 1 (t) ═ δ f, which is:
where δ f denotes the MEMS accelerometer measurement error, G n ＝[0 0 -g] T Representing a gravity acceleration vector, g representing a gravity acceleration; f. of b Representing acceleration values measured by a MEMS accelerometer, including a gravity acceleration vector G b The acceleration a of the carrier, and the measurement noise v of the accelerometer a ；
An estimate representing the accelerometer output, expressed as:
an estimate of the gravitational acceleration vector is represented,
an estimated value representing the acceleration of motion of the carrier;
[δΘ×]and [ G ] n ×]Respectively representing an attitude error angle and an antisymmetric matrix formed by gravity vectors in a navigation system, and represented as:
the state equation and the measurement equation of the sub-filter 2 are as follows:
in the formula, X 2 (t) represents the state quantity at time t of the 2 nd sub-filter,
K m (t) MEMS Gyro Scale factor, K, at time t m (t)＝[K mx (t) K my (t) K mz (t)] T ，K mx (t) x-axis MEMS gyro scale factor, K, expressed as time t my (t) y-axis MEMS gyro scale factor, K, expressed as time t mz (t) a z-axis MEMS gyro scaling factor expressed as time t;
F 2 (t) represents the state transition matrix at time t,
representing the attitude matrix of the MEMS gyroscope at the time t;
Z 2 (t) the difference between the MEMS gyroscope and the FOG at the time t is represented as a measurement error,
H 2 (t) is represented as a measurement matrix,
is shown as
Inverse matrix of (I) 3×3 Expressed as a 3 × 3 identity matrix;
(3) optimal fusion algorithm for main filter
The local estimation value of each sub-filter and the covariance matrix thereof are input into the main filter and are subjected to data fusion with the estimation value of the main filter to obtain the global optimal estimation
And its covariance matrix P g . The data fusion process can be expressed as:
it is to be emphasized that: the assumption here is that the sub-filters are not correlated, i.e. P ij 0(i ≠ j), which is a prerequisite for designing a federal filter. And finally, feeding back the error state and the covariance matrix of each sub-filter and the main filter according to an 'information distribution' principle, and resetting the error state and the covariance matrix of each sub-filter and the main filter by using a global filtering solution. Information distribution follows the principle of "information conservation", namely:
wherein, beta m Distributing coefficients for the main filter information; beta is a i Coefficients are assigned to the information of each sub-filter. Because the main filter of the federal filter has no filtering link, namely: beta is a m 0. Since there are only two sub-filters, beta i Here, take beta 1 Each sub-filter being tuned by beta 1 Value of such that the filter outputsThe navigation calculation precision is further improved.
Because the state quantities of the two sub-filters both contain delta theta, the state quantity in the main filter selects the common state quantity, and the covariance matrix P of the main filter g A common portion of the covariance matrix of each sub-filter is also selected.
In order to ensure the error tracking effect of the federal filter, the measurement variance R of the two subsystems is also adaptively designed, namely:
in the formula, R 1 Represents the measured variance, R, of the filter 1 2 Represents the measured variance, C, of the filter 2 ra Denotes the adaptive coefficient, C, of the filter 1 rm Denotes the adaptive coefficient of the filter 2, where C is selected ra ＝0.03，C rm 4. In addition, consider a global covariance matrix P g Is a sub-covariance matrix P 1 、P 2 The fusion of (2) cannot accurately reflect the error state of each sub-filter. Therefore, the difference values of the MEMS accelerometer and the MEMS gyroscope and FOG are also introduced into the covariance matrix, so that the covariance matrix P 1 、P 2 And carrying out self-adaptive adjustment according to the error state. Covariance matrix P 1 、P 2 Respectively satisfy:
in the formula, C pa Denotes the adaptive coefficient, C, of the filter 1 pm Denotes the adaptive coefficient of the filter 2, where C is selected pa ＝9，C pm ＝0.001。
In order to verify the method designed by the invention, the signal output of each sensor and the real situation of an inertial navigation algorithm are simulated, and original data are generated according to the noise of the MEMS inertial device and the fiber-optic gyroscope. The MEMS inertial group adopts ADIS16445 produced by ADI company and adopts FOG with the precision of 0.01 degree/s for structural configuration. And (3) preprocessing the MEMS gyroscope such as denoising, calibration compensation and the like, and compensating each error through a grey prediction model, wherein the zero deviation stability of each axis is within 10 degrees/h. Similarly, the zero bias stability of the MEMS accelerometer is not higher than 1mg after data preprocessing.
To simulate the attitude angle of the carrier in the stationary state, raw data were set such that the attitude angle Θ was ═ 0 ° 0 ° 0 °] T Static data of time. The original data are solved by using an inertial navigation attitude algorithm, and the designed Federal filter is simulated by using Matlab, so that the attitude angle solving results of the MEMS inertial measurement unit with the inclined fiber-optic gyroscope and the pure MEMS inertial measurement unit are shown in FIG. 4.
As can be seen from FIG. 4, due to the poor zero offset stability of the MEMS gyroscope, the error of the calculated attitude angle of the pure MEMS inertial navigation is large, but the configuration of the attitude angle of the pure MEMS inertial navigation and the FOG improves the attitude angle of the MEMS inertial navigation to a certain extent.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (6)
1. A data fusion method suitable for an MEMS inertial measurement unit with an inclined fiber-optic gyroscope is characterized in that the method is designed aiming at a single-axis FOG and a redundant structure of the MEMS inertial measurement unit, wherein a sensitive axis of the single-axis FOG and three measuring axes of the MEMS inertial measurement unit are arranged in equal included angles; the MEMS inertial set comprises: integrating a three-axis MEMS gyroscope and a three-axis MEMS accelerometer which are mutually orthogonal;
the method comprises the following steps:
s1: carrying out error compensation on the MEMS gyroscope by utilizing a grey prediction model;
the method specifically comprises the following steps:
let the angular velocity output value of the MEMS gyroscope be x (0) (i) I 1,2, … n, incrementally added to generate a new sequence of 1-AGO:
wherein x is (1) (1)＝x (0) (1) Outputting a value of the first angular velocity of the MEMS gyroscope; x is a radical of a fluorine atom (1) (k) Expressing the k-th angular velocity predicted value, and obtaining an adaptive residual gray prediction model:
in the formula, a represents a gray prediction model development coefficient, and b represents a gray prediction model control parameter;
the values of parameters a and b are estimated by the least squares method:
[a b] T ＝(B T B) -1 B T y n
in the formula, B represents a coefficient,
(x (1) (1)…x (1) (n)) represents the predicted angular velocity value, y n Representing angular velocity values of gyro output, y n ＝[x (0) (1),x (0) (2),…,x (0) (n)] T ；
Obtaining a predicted fitting value by using a gray reduction model 1-IAGO so as to obtain an MEMS gyroscope angular velocity output new sequence after error compensation:
in the formula (I), the compound is shown in the specification,
representing the k +1 th angular velocity estimate,
represents the k +1 th angular velocity predicted value,
represents the predicted value of the k-th angular velocity,
the 1 st estimate of the angular velocity is shown,
representing the 1 st angular velocity predicted value;
s2: and combining the single-axis FOG with the MEMS gyroscope and the MEMS accelerometer respectively, filtering by adopting a partial feedback type federal filter, adjusting the feedback coefficient of the partial feedback type federal filter, and performing data fusion on the output of the single-axis FOG and the MEMS inertial group.
2. The data fusion method applicable to the MEMS inertial measurement unit with the tilted fiber-optic gyroscope, according to claim 1, wherein the step S2 specifically includes:
s21: the uniaxial FOG and the MEMS accelerometer are jointly input to the sub-filter 1;
angular velocity measurement of uniaxial FOG in carrier coordinate system
Subtracting a pass transformation matrix
The converted earth angular rate under the carrier coordinate system obtains the angular rate of the carrier
Then, the quaternion method is used for solving the attitude matrix of the uniaxial FOG
Then eliminating the attitude error estimated by the main filter to obtain the output of the public system
Simultaneously, the output of the MEMS accelerometer is measured
Subtracting a pass transformation matrix
Converted acceleration of gravity in a carrier coordinate system, wherein G n The gravity acceleration under the navigation coordinate system is input into the sub-filter 1;
s22: the uniaxial FOG and the MEMS gyroscope after error compensation are combined and input to a sub-filter 2;
MEMS gyroscope compensated by gray prediction model and outputting angular speed of MEMS gyroscope in carrier coordinate system
Subtracting a pass transformation matrix
The converted earth angular rate under the carrier coordinate system obtains the angular rate of the carrier
Then the output of the MEMS gyroscope is transmittedGo out
Subtracting the output of a single-axis FOG
As a measure of the quantity of the sub-filter 2;
s23: the main filter of the Federal filter combines the error states of the sub-filter 1 and the sub-filter 2
And covariance P 1 、P 2 Introducing and carrying out optimal fusion to obtain global optimal estimation
And global covariance matrix P g ；
The main filter estimates the global optimum according to the information conservation principle and the information distribution principle
And global covariance matrix P g Feeding back to the sub-filter 1 and the sub-filter 2, and performing data fusion on the output of the uniaxial FOG and the MEMS inertial measurement unit; wherein the covariance P g Part is introduced into one of the sub-filters and the remaining part is introduced into the other sub-filter.
3. The data fusion method suitable for the MEMS inertial measurement unit with the tilted fiber-optic gyroscope, according to claim 1, further comprising: after S22, the measured variances of the two sub-filters are adaptively designed:
in the formula, R 1 Represents the measured variance, R, of the filter 1 2 Display filterMeasurement variance, C, of the wave filter 2 ra Denotes the adaptive coefficient, C, of the filter 1 rm Represents the adaptive coefficients of the filter 2;
in addition, consider a global covariance matrix P g Is a sub-covariance matrix P 1 、P 2 The fusion of (1) cannot accurately reflect the error state of each sub-filter; therefore, introducing the MEMS accelerometer and MEMS gyroscope and uniaxial FOG difference values into the covariance matrix, so that the covariance matrix P 1 、P 2 Also according to the error state, making self-adaptive adjustment, and making covariance matrix P 1 、P 2 Respectively satisfy:
in the formula, C pa Denotes the adaptive coefficient, C, of the filter 1 pm The adaptive coefficients of the filter 2 are represented.
4. The data fusion method suitable for the MEMS inertial measurement unit with the inclined fiber-optic gyroscope, as claimed in claim 3, wherein C is ra ＝0.03，C rm ＝4。
5. The data fusion method suitable for the MEMS inertial measurement unit with the inclined fiber-optic gyroscope, as claimed in claim 3, wherein C is pa ＝9，C pm ＝0.001。
6. The data fusion method suitable for the MEMS inertial set with the tilted fiber-optic gyroscope, as claimed in claim 1, wherein the included angle of the uniaxial FOG sensitive axis and the three measurement axes of the MEMS inertial set which are installed at equal included angles is 54.75 °.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811004559.6A CN109086250B (en) | 2018-08-30 | 2018-08-30 | Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811004559.6A CN109086250B (en) | 2018-08-30 | 2018-08-30 | Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109086250A CN109086250A (en) | 2018-12-25 |
| CN109086250B true CN109086250B (en) | 2022-08-05 |
Family
ID=64840257
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811004559.6A Active CN109086250B (en) | 2018-08-30 | 2018-08-30 | Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109086250B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109931935B (en) * | 2019-02-22 | 2021-04-06 | 河海大学 | Navigation system based on distance and environmental characteristics and parameter perturbation solution method thereof |
| US11656081B2 (en) * | 2019-10-18 | 2023-05-23 | Anello Photonics, Inc. | Integrated photonics optical gyroscopes optimized for autonomous terrestrial and aerial vehicles |
| CN113804185A (en) * | 2021-08-27 | 2021-12-17 | 中国人民解放军96901部队24分队 | Novel inertial navigation system based on MEMS array |
| CN114526729B (en) * | 2022-01-14 | 2023-11-17 | 重庆邮电大学 | Course optimization method of MEMS inertial positioning system based on redundancy technology |
| CN115014391A (en) * | 2022-05-31 | 2022-09-06 | 中南大学 | Method for correcting MEMS inertial navigation error based on FOG |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103557864A (en) * | 2013-10-31 | 2014-02-05 | 哈尔滨工程大学 | Initial alignment method for micro electro mechanical system (MEMS) strap-down inertial navigation adaptive square-root cubature Kalman filtering (SCKF) |
| CN103954304A (en) * | 2014-05-21 | 2014-07-30 | 北京航天自动控制研究所 | Zero offset short and long-term change value testing method applied to MEMS (Micro-electromechanical Systems) inertia unit |
| CN104374405A (en) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering |
| CN104656112A (en) * | 2015-01-28 | 2015-05-27 | 重庆大学 | Personal locating method and personal locating device based on surface electromyogram signal and MEMS inertial measurement unit combination |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050240347A1 (en) * | 2004-04-23 | 2005-10-27 | Yun-Chun Yang | Method and apparatus for adaptive filter based attitude updating |
-
2018
- 2018-08-30 CN CN201811004559.6A patent/CN109086250B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103557864A (en) * | 2013-10-31 | 2014-02-05 | 哈尔滨工程大学 | Initial alignment method for micro electro mechanical system (MEMS) strap-down inertial navigation adaptive square-root cubature Kalman filtering (SCKF) |
| CN103954304A (en) * | 2014-05-21 | 2014-07-30 | 北京航天自动控制研究所 | Zero offset short and long-term change value testing method applied to MEMS (Micro-electromechanical Systems) inertia unit |
| CN104374405A (en) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering |
| CN104656112A (en) * | 2015-01-28 | 2015-05-27 | 重庆大学 | Personal locating method and personal locating device based on surface electromyogram signal and MEMS inertial measurement unit combination |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109086250A (en) | 2018-12-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109086250B (en) | Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope | |
| Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
| Nemec et al. | Intelligent real-time MEMS sensor fusion and calibration | |
| CN110887481B (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
| Crocoll et al. | Model‐aided navigation for a quadrotor helicopter: A novel navigation system and first experimental results | |
| CN110567461B (en) | Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope | |
| CN112683269B (en) | MARG attitude calculation method with motion acceleration compensation | |
| Ghasemi‐Moghadam et al. | Attitude determination by combining arrays of MEMS accelerometers, gyros, and magnetometers via quaternion‐based complementary filter | |
| CN110567492A (en) | Low-cost MEMS inertial sensor system-level calibration method | |
| Sarbishei | On the accuracy improvement of low-power orientation filters using IMU and MARG sensor arrays | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| Al-Fahoum et al. | Design of a modified Madgwick filter for quaternion-based orientation estimation using AHRS | |
| Sun et al. | A robust indirect Kalman filter based on the gradient descent algorithm for attitude estimation during dynamic conditions | |
| CN113175926B (en) | Self-adaptive horizontal attitude measurement method based on motion state monitoring | |
| CN110736459B (en) | Angular deformation measurement error evaluation method for inertial quantity matching alignment | |
| Dichev et al. | A gyro-free system for measuring the parameters of moving objects | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| Xing et al. | Offline calibration for MEMS gyroscope g-sensitivity error coefficients based on the Newton iteration and least square methods | |
| CN112326162B (en) | Wing elastic deformation measuring method for airborne distributed POS | |
| Kaswekar et al. | Sensor fusion based vibration estimation using inertial sensors for a complex lightweight structure | |
| CN104655132A (en) | Method for estimating body elastic deformation angle on basis of accelerometer | |
| Wu et al. | Real-time magnetic disturbance determination for micro air vehicles via gravity and global navigation satellite system measurements | |
| Zhang et al. | Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors | |
| Örtel et al. | Integrated motion measurement illustrated by a cantilever beam | |
| Qi et al. | A state estimator of UAV using time-delayed position and IMU data |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |