CN106342175B - A kind of data fusion method that improves Gyro Precision - Google Patents

Abstract

The invention discloses a kind of data fusion method that improves Gyro Precision, belong to field of sensor measurement. The method adopts the responsive unidirectional angular speed of multiple gyro composition array of specification of the same type, design the optimal estimation wave filter based on simplifying noise model, carry out the fused filtering of multiple angle rate signals, set up the relevant covariance matrix of measurement noise by Noise Correlation analysis, solve the analytic solutions of Li Kadi equation, obtain angular speed and estimate Kalman Filtering for Discrete recurrence equation, realized the optimal estimation of angular speed, improve precision. Effect of the present invention is: 1) adopt the method for multisensor array data fusion to improve Gyro Precision, broken through conventional method and carried high-precision bottleneck. 2) simplification of gyroscope error model, has reduced filtering dimension, has improved system real time. 3) Analytical Solution of filtering estimate variance steady-state value, has simplified the enforcement of wave filter greatly, only needs the circulation of a recurrence equation just can realize the optimal estimation of angular speed.

Description

A kind of data fusion method that improves Gyro Precision

Technical field

The present invention relates to a kind of data fusion method that improves Gyro Precision, belong to field of sensor measurement.

Background technology

It is excellent that micromechanical gyro based on micro-electromechanical technology has that volume is little, low in energy consumption, price is low, the life-span is long and can be mass etc.Point, is very suitable for building microminiaturized, inertial navigation and guidance system cheaply. But, the precision of micromechanical gyro and biographySystem gyro is compared and is differed far away, has seriously limited the application in Aero-Space high-end field. By improve gyro sensitive structure,Processing technology, interface testing circuit etc. improve the conventional method of signal to noise ratio, are difficult to improve by a relatively large margin the precision of micromechanical gyro,And cannot fundamentally improve the drift characteristic of gyro. Adopt multisensor Data Fusion technology, utilize optimal estimation Theoretical DesignBlending algorithm can be realized the estimation compensation of gyroscopic drift, utilizes sensor self to reach the self compensation calibration of random error, improvesPrecision. Utilize multiple gyro composition arrays, devise optimum is estimated filtering algorithm, and to gyro array, multiple metrical information filtering addsPower is processed, and obtains a high-precision gyro signal, is referred to as " virtual gyro ". The patent No. is U.S. of US2003/0187623State's patent " High accuracy inertial sensors from inexpensive components, 2003 " has proposed multiple singleThe method that improves Gyro Precision is processed in the array signal filtering of gyro composition. The method is by the micromechanical gyro of multiple general precisionsComposition array, in algorithm design is modeled as gyro signal three of true angular speed, angle random walk ARW and offset driftsPoint, wherein offset drift is driven by speed random walk RRW noise; Classify true angular speed and gyro offset drift as stateIt is estimated, utilize the correlation between noise signal to set up system noise and measurement noise covariance matrix, solve continuous-form card GermaniaFiltering equations obtains the analytic solutions of filtering estimate variance, and design Kalman filter is carried out filtering to multiple measuring values, is inputtedThe optimal estimation of angular speed, thus Gyro Precision improved. Although the method can increase substantially the precision of micromechanical gyro,Also come with some shortcomings: first, for the micromechanical gyro of low precision, its random error main manifestations is that angle random walk is whiteNoise, especially very little for the applied environment speed random walk noise proportion in short-term, be modeled as true angular speed,Angle random walk ARW and offset drift three parts have increased the dimension of wave filter, have increased computation burden, are unfavorable for virtualGyro real time data processing. Secondly the number that, increases gyro array is by the complexity that causes the Li Kadi differential equation to be resolved solvingIncrease, be unfavorable for the enforcement of filtering algorithm. In addition, patent " the Virtual Realization side of gyro that application number is 200610104564.5Method, 2006 ", equally gyroscope error model is modeled as to true angular speed, angle random walk ARW and offset drift three parts,In algorithm design, adopt calculus of finite differences to set up dynamic filter model, the multiple measurement signals of gyro array are carried out to calculus of differences and disappearExcept the impact of true angular speed on modeling, estimate gyro offset drift, proofread and correct each gyro output signal, then multiple to after proofreading and correctGyro signal is averaged, and realizes the estimation of dynamic angular speed. Also there is certain deficiency in this calculus of finite differences: first, filtering is estimatedObject be to obtain the estimated value of true angular speed, and in difference processing, eliminated useful true angle rate signal, causeThe reduction of estimated accuracy. Secondly, this error model is unfavorable for reducing gyro angle random walk noise.

Summary of the invention

In order to overcome the above-mentioned defect of prior art, the present invention proposes a kind of raising Gyro Precision based on simplifying noise modelData fusion method, adopts N gyro of specification of the same type to form the responsive unidirectional angular speed of array, carries out multiple angles speedThe filtering of rate signal is synthetic, realizes the optimal estimation of true angular speed, improves precision.

The data fusion method principle of the raising Gyro Precision that the present invention proposes, as Fig. 1, specifically comprises the following steps:

The first step: gather gyro array angle rate signal. Adopt array detection same sensitive direction same of N gyro compositionIndividual angle rate signal, sets gyro signal sampling period T, is converted to N angle rate signal y by A/D₁，y₂，…，y_N。

Second step: ask for gyro angle random walk ARW noise variance q_n. The angle rate signal that utilizes the first step to gather y₁，y₂，…，y_N, adopt Allan variance method to analyze N Gyro Random error, obtain each gyro angle random walk ARWNoise variance, is averaged and obtains q_n。

The 3rd step: ask for gyro array noise correlation coefficient ρ. The angle rate signal y that utilizes the first step to gather₁，y₂，…，y_N, logicalCross correlation analysis, ask for the correlation coefficient ρ between gyro array angle random walk ARW noise.

The 4th step: ask for relevant covariance matrix R. The ARW noise variance q that utilizes second step and the 3rd step to obtain_nAnd relevantCoefficient ρ, obtains the relevant covariance matrix R of ARW noise:

The 5th step: design data fusion card Thalmann filter. The design of Kalman filter is the core that gyro array data merges,Comprise following sub-step:

Sub-step 1: set up gyro array data and merge state-space model

Gyro signal is modeled as to true angular speed ω and angle random walk noise n sum, for the array of N gyro composition,Have:

y＝H·ω+v(1)

$y=\left[\begin{array}{c}{y}_1\\ \·\\ \·\\ \·\\ y_N\end{array}\right],v=\left[\begin{array}{c}{n}_1\\ \·\\ \·\\ \·\\ n_N\end{array}\right],H={\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]}_{N\×1}---\left(2\right)$

Wherein y is the vector of multiple gyro measuring value compositions, and v is the vector of angle random walk noise composition, and H is for measuring squareBattle array. ω is modeled as to random walk processWherein n_ωFor true driven at such angular rates white noise. List ω in state vectorIt is estimated, sets up gyro array data fusion state-space model as follows:

$\left\{\begin{array}{c}X\left(t\right)=\mathrm{\ω}\\ \stackrel{\·}{X}\left(t\right)=0\·X\left(t\right)+w\left(t\right)=n_{\mathrm{\ω}}\\ y\left(t\right)=H\·X\left(t\right)+v\left(t\right)\end{array}\right.---\left(3\right)$

X in tool (t) is state vector, and y (t) is measuring value vector, and system noise vector w (t) and measurement noise vector v (t) are respectivelyFor thering is the white Gaussian noise of zero-mean, that is: E[w (t)]=0, E[w (t) w^T(t+τ)]＝q_ωδ(τ)，E[v(t)]＝0， E[v(t)v^T(t+τ)]＝Rδ(τ)，q_ωFor white noise n_ωVariance, R is the relevant covariance square of ARW noise that the 4th step obtainsBattle array, δ (τ) is Kronecker δ function.

Sub-step 2: solve filtering estimate variance steady-state value

The data fusion state-space model of setting up for sub-step 1, true angular speed estimates that continuous Kalman filter equation is:

$\stackrel{\·}{\hat{X}}\left(t\right)=K\left(t\right)\[y\left(t\right)-H\hat{X}\left(t\right)\]---\left(4\right)$

K(t)＝P(t)H^TR^-1 (5)

$\stackrel{\·}{P}\left(t\right)=q_{\mathrm{\ω}}-P\left(t\right)H^T{R}^{-1}HP\left(t\right)---\left(6\right)$

WhereinK (t) and P (t) are respectively estimated value, filtering gain matrix and the filtering estimate variance of t moment state vector, H^TFor the transposed matrix of matrix H, R^-1For the inverse matrix of matrix R, equation (6) is referred to as Li Kadi equation. By to sub-step1 set up state-space model Analysis on Observability know the complete Observable of filtering system, along with the growth of filtering time t, P (t) byThe impact that is not subject to initial value gradually also reaches steady-state value P (∞), just can obtain steady-state filtering gain square as long as therefore obtain P (∞)Battle array K (∞), finally obtains the optimal estimation of state vector, simplifies the enforcement of wave filter, improves computational speed.

The Hamiltonian matrix M of Li Kadi equation is:

$M=\left[\begin{array}{cc}0& H^T{R}^{-1}H\\ q_{\mathrm{\ω}}& 0\end{array}\right]---\left(7\right)$

The solution of Li Kadi equation is: P (t)=A (t) B^-1(t), wherein matrix A (t) and B (t) meet linear first-order differential equation:

$\left[\begin{array}{c}\stackrel{\·}{B}\left(t\right)\\ \stackrel{\·}{A}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}0& H^T{R}^{-1}H\\ q_{\mathrm{\ω}}& 0\end{array}\right]\left[\begin{array}{c}B\left(t\right)\\ A\left(t\right)\end{array}\right]---\left(8\right)$

Solving equation (8):

$\left[\begin{array}{c}B\left(t\right)\\ A\left(t\right)\end{array}\right]=e^{Mt}\left[\begin{array}{c}I\\ P_0\end{array}\right]---\left(9\right)$

Wherein I is unit matrix, P₀For initial filtering estimate variance, defined variable C=H^TR^-1H, solves e^Mt:

$e^{Mt}=\left[\begin{array}{cc}\frac{1}{2}{e}^{\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}& \frac{\sqrt{C/q_{\mathrm{\ω}}}}{2}{e}^{\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}\\ \frac{\sqrt{q_{\mathrm{\ω}}/C}}{2}{e}^{\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}& \frac{1}{2}{e}^{\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}{e}^{-\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}& -\frac{\sqrt{C/q_{\mathrm{\ω}}}}{2}{e}^{-\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}\\ -\frac{\sqrt{q_{\mathrm{\ω}}/C}}{2}{e}^{-\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}& \frac{1}{2}{e}^{-\sqrt{{\mathrm{Cq}}_{\mathrm{\ω}}}t}\end{array}\right]---\left(10\right)$

Therefore, filtering estimate variance steady-state value P (∞) is:

$P\left(\mathrm{\∞}\right)=(\sqrt{q_{\mathrm{\ω}}/C}+P_0){(1+\sqrt{C/q_{\mathrm{\ω}}}{P}_0)}^{-1}---\left(11\right)$

Sub-step 3: solve angular speed and estimate Kalman Filtering for Discrete recurrence equation

The filtering estimate variance steady-state value P (∞) that utilizes sub-step 2 to obtain, calculates steady-state filtering gain matrix K (∞):

$K_{\mathrm{\∞}}=P\left(\mathrm{\∞}\right)H^T{R}^{-1}=(\sqrt{q_{\mathrm{\ω}}/C}+P_0){(1+\sqrt{C/q_{\mathrm{\ω}}}{P}_0)}^{-1}{H}^T{R}^{-1}={\mathrm{GH}}^T{R}^{-1}---\left(12\right)$

Wherein variableAngular speed estimates that continuous Kalman filter equation is:

$\stackrel{\·}{\hat{X}}\left(t\right)=K_{\mathrm{\∞}}\[y\left(t\right)-H\hat{X}\left(t\right)\]=-GC\stackrel{\·}{\hat{X}}\left(t\right)+{\mathrm{GH}}^T{R}^{-1}y\left(t\right)---\left(13\right)$

To continuous filtering equation (13) discretization, angular speed estimates that Kalman Filtering for Discrete recurrence equation is:

$\hat{X}(k+1)=e^{-GCT}\hat{X}\left(k\right)+\frac{1}{C}(1-e^{-GCT})H^T{R}^{-1}y\left(k\right)---\left(14\right)$

WhereinWithBe respectively the angular speed estimated value in k+1 and k moment, y (k) is the output of k moment gyro arrayAngular speed value. Completed the design of Kalman filter to this.

The 6th step: angle rate signal filtering is estimated. Set initial filtering estimate variance P₀=0, white noise n_ωVariance q_ωValueBe 100～1000 (°/h)²Between arbitrary real number, initial state vector estimated valueFor in the first step, obtain k timeCarve gyro angle rate signal y₁，y₂，…，y_NVectorial y (k)=[y of composition₁，y₂，…，y_N]^T, utilize the angular speed estimated value in k momentAnd the 5th the angular speed that obtains of sub-step 3 in step estimate Kalman Filtering for Discrete recurrence equation (14), when recursion obtains k+1The angular speed estimated value of carvingReturn to the first step and gather k+1 moment gyro array angle rate signal y (k+1), utilizeAnd the 5th sub-step 3 in step estimate the angular speed estimated value in k+2 momentCycle calculations successively.

Beneficial effect of the present invention: 1) utilize multiple gyro composition arrays, adopt the method for data fusion to improve Gyro Precision, prominentBreak the bottleneck of conventional method raising micromechanical gyro precision. 2) simplification of gyroscope error model, has reduced filtering dimension, improvesSystem real time. 3) Analytical Solution of filtering estimate variance steady-state value, has simplified the enforcement of wave filter greatly, only needs oneThe circulation of recurrence equation just can realize the optimal estimation of angular speed, has avoided direct employing Kalman filtering discrete equation to filteringThe renewal of estimate variance and filtering gain is calculated.

Brief description of the drawings

Fig. 1 is that Gyro Precision improves data fusion method schematic diagram

Fig. 2 is that Gyro Precision improves data fusion method flow chart

Detailed description of the invention

The present embodiment adopts 6 micromechanical gyro ADXRS300 composition gyro arrays, realizes a high-precision virtual gyro and isSystem, consults Fig. 1, Fig. 2, and the data fusion method of the present embodiment specifically comprises the steps:

The first step: gather gyro array angle rate signal. Adopt the same angle of the array detection same sensitive direction of 6 gyrosRate signal, sets sampling period T=0.01s, is converted to 6 gyro angle rate signal y by A/D₁，y₂，…，y₆。

Second step: ask for gyro angle random walk ARW noise variance q_n. The angle rate signal that utilizes the first step to gather y₁，y₂，…，y₆, adopt Allan variance method to analyze each Gyro Random error, obtain each gyro angle random walk ARW and make an uproarSound variance, is averaged and obtains q_n＝370.3²(°/h)²。

The 3rd step: ask for gyro array noise correlation coefficient ρ. The angle rate signal y that utilizes the first step to gather₁，y₂，…，y₆, logicalCross correlation analysis, obtain correlation coefficient ρ=0.01 between gyro angle random walk ARW noise.

The 4th step: ask for relevant covariance matrix R. The ARW noise variance q that utilizes second step and the 3rd step to obtain_nAnd relevantCoefficient ρ, obtains the relevant covariance matrix R of ARW noise:

The 5th step: design data fusion card Thalmann filter. Gyro signal is modeled as to true angular speed ω and angle random walkNoise n sum, true angular speed ω is modeled as random walk processWherein n_ωFor average is that 0 variance is q_ωTrueDriven at such angular rates white noise. List ω in state vector it estimated, set up true angular speed optimal estimation Kalman filter and be:

$\hat{X}(k+1)=e^{-GCT}\hat{X}\left(k\right)+\frac{1}{C}(1-C^{-GCT})H^T{R}^{-1}y\left(k\right)$

WhereinWithBe respectively the angular speed estimated value in k+1 and k moment, y (k) is the output of k moment gyro arrayAngular speed vector, R^-1For the inverse matrix of covariance matrix R, H^TFor measurement matrixTransposed matrix, T is gyroSignal sampling period, variableC＝H^TR^-1H，P₀For initial filtering estimation sidePoor.

The 6th step: angle rate signal filtering is estimated. Set initial filtering estimate variance P₀=0, white noise n_ωVariance q_ω＝100(°/h)², the estimated value of initial state vectorFor the k moment gyro angle rate signal obtaining in the first step y₁，y₂，…，y₆Vectorial y (k)=[y of composition₁，y₂，…，t₆]^T, utilize the angular speed estimated value in k momentAnd the 5th step neutronThe angular speed that step 3 obtains is estimated Kalman Filtering for Discrete recurrence equation (14), and recursion obtains the angular speed estimated value in k+1 momentReturn to the first step and gather k+1 moment gyro angle rate signal y (k+1), utilizeAnd the 5th sub-step in stepRapid 3 estimate the angular speed estimated value in k+2 momentCycle calculations successively.

By experiment, 6 angle random walk noises and speed random walk noise are respectively the microcomputer of 370.3 °/h and 4.14 °/hTool gyro ADXRS300, after Data Fusion Filtering, obtains a high-precision virtual gyro, wherein angle random walkNoise and speed random walk noise are reduced to respectively 1.179 °/h and 0.1274 °/h, and precision is greatly improved.

Claims (4)

1. a data fusion method that improves Gyro Precision, is characterized in that: comprise the steps:

The first step: gather gyro array angle rate signal: adopts the same of array detection same sensitive direction that N gyro formIndividual angle rate signal, sets gyro signal sampling period T, is converted to N angle rate signal y by A/D₁，y₂，…，y_N；

Second step: ask for gyro angle random walk ARW noise variance q_n: the angle rate signal that utilizes the first step to gather y₁，y₂，…，y_N, adopt Allan variance method to analyze N Gyro Random error, obtain each gyro angle random walk ARWNoise variance, is averaged and obtains q_n；

The 3rd step: ask for gyro array noise correlation coefficient ρ: the angle rate signal y that utilizes the first step to gather₁，y₂，…，y_N, logicalCross correlation analysis, ask for the correlation coefficient ρ between gyro array angle random walk ARW noise;

The 4th step: ask for relevant covariance matrix R: the ARW noise variance q that utilizes second step and the 3rd step to obtain_nAnd relevantCoefficient ρ, obtains the relevant covariance matrix R of ARW noise:

The 5th step: design data fusion card Thalmann filter: gyro signal is modeled as to true angular speed ω and angle random walkNoise n sum, true angular speed ω is modeled as random walk processWherein n_ωFor average is that 0 variance is q_ωTrueDriven at such angular rates white noise; List ω in state vector it is estimated, set up angular speed and estimate Kalman Filtering for Discrete recurrence equationFor:

$\hat{X}(k+1)=e^{-GCT}\hat{X}\left(k\right)+\frac{1}{C}(1-e^{-GCT})H^T{R}^{-1}y\left(k\right)---\left(1\right)$

WhereinWithBe respectively the angular speed estimated value in k+1 and k moment, y (k) is the output of k moment gyro arrayAngular speed vector, R^-1For the inverse matrix of covariance matrix R, H^TFor measurement matrixTransposed matrix, T is gyroSignal sampling period, variableC＝H^TR^-1H，P₀For initial filtering estimation sidePoor;

The 6th step: angle rate signal filtering is estimated: set initial filtering estimate variance P₀, white noise n_ωVariance q_ω, initialState vector estimated valueFor the k moment gyro angle rate signal y obtaining in the first step₁，y₂，…，y_NThe vector of composition y(k)＝[y₁，y₂，…，y_N]^T, utilize the angular speed estimated value in k momentAnd the 5th the angular speed that obtains of step estimate discrete Ka ErGraceful filtering recurrence equation (1), recursion obtains the angular speed estimated value in k+1 momentReturn to the first step and gather the k+1 momentGyro array angle rate signal y (k+1), utilizesAnd the 5th Kalman filter in step estimate the angular speed in k+2 momentEstimated valueCycle calculations successively, realizes angular speed optimal estimation, improves precision.

2. a data fusion method for raising Gyro Precision as claimed in claim 1, is characterized in that: in described the 6th stepInitial filtering estimate variance P₀Be set as P₀＝0。

3. a data fusion method for raising Gyro Precision as claimed in claim 1, is characterized in that: in described the 6th stepWhite noise n_ωVariance q_ωValue be 100～1000 (°/h)²Between arbitrary real number.

4. a data fusion method for raising Gyro Precision as claimed in claim 1, is characterized in that: in described the 6th stepInitial state vector estimated valueBe set as