High-precision data fusion method for array MEMS gyroscope

Abstract

The application provides a high-precision data fusion method for an array type MEMS gyroscope, which adopts a multi-stage distributed data fusion technology based on improved Kalman filtering; before multi-sensor data enters a fusion center, a first-stage Kalman filter is designed to perform local optimal estimation on each sensor, so that a first-stage estimated value of a single gyroscope in the array is obtained; then, the processed information is sent into a second-stage fusion center for processing, so that an optimized high-precision angular velocity output value is finally obtained. Meanwhile, in the Kalman filtering process, the steady-state value of the filtering gain is used for the estimation of the angular velocity, so that the calculation of the gain and the mean square deviation at each step is avoided, the complexity and the operation amount of the optimization process are optimized, and the real-time processing of the gyroscope array is more favorable. The method has a significant effect on reducing the random error of the MEMS gyroscope and improving the detection precision of the MEMS gyroscope.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of sensors, and particularly relates to a high-precision data fusion method for an array type MEMS gyroscope. BACKGROUND

[0002] A micromechanical gyroscope (MEMS gyroscope) is a high-tech product and is one of the particularly important branches in the field of contemporary micro-mechanical electronic systems (MEMS). The MEMS gyroscope is produced based on micro-mechanical processing manufacturing technology, and its working principle is to convert external rotational motion into the relative displacement of the internal polar plate of the gyroscope by means of the Coriolis force effect. Compared with mechanical gyroscopes and optical gyroscopes, the MEMS gyroscope has the following outstanding features: (1) small size and light weight. MEMS technology is a high-tech field of planning, manufacturing, perfecting, measuring and dominating micron / nanometer materials, so the MEMS gyroscope can reach the micron / nanometer order of magnitude in size, which is convenient for integration with other functional chips and can be directly embedded into a circuit board with master control function. (2) High reliability. Since durable and impact-resistant materials are selected in the manufacturing process, and integration means is adopted, the MEMS gyroscope can normally work and play its functional characteristics in most cases. (3) Low manufacturing cost. High-precision optical gyroscopes are high in cost due to the difficulty in obtaining manufacturing materials, while the MEMS gyroscope is manufactured by using inexpensive basic materials such as silicon, and is generally below 1,000 yuan, and the minimum is several yuan. The MEMS gyroscope can meet mass production in military equipment demand and civil fields, and has wide applicability.

[0003] MEMS gyroscope has been widely used in inertial navigation, aerospace, automotive and electronic devices due to its above-mentioned advantages. However, the low-cost MEMS gyroscope cannot meet the needs of many high-precision applications due to its low detection precision caused by the influence of bias stability, nonlinearity and temperature stability. Currently, there are two main methods to improve the precision of MEMS gyroscope. One is to use consistent signal processing method to process the module integrated with multiple angular velocity sensitive units during the manufacturing process of MEMS gyroscope, but this method requires high-level manufacturing process and circuit design technology. The other is to use multiple low-precision MEMS sensors to form an array to detect the same target, collect multiple sensor data, process the data through data fusion, and output a set of high-precision and high-credibility data to realize precise detection of the moving target. Because the final output data does not come from a single gyroscope, but the optimized angular velocity output signal obtained by fusing the output of multiple gyroscopes, it is also called "virtual gyroscope technology". Considering the development cost and time of the two methods, constructing a gyroscope array to improve the precision and reliability of MEMS gyroscope has become a widely used method.

[0004] Multi-sensor information fusion is a key technology in array MEMS gyroscope research, which is a process of detecting, correlating, combining and estimating multiple source data to improve the accuracy of state estimation. Random error of MEMS gyroscope is one of the main factors that restrict its detection precision. By analyzing and identifying the measurement values of the gyroscope array, the optimal filter is designed to estimate the size of each error of the gyroscope, and the measurement information is compensated and corrected, so that the high-precision estimation value of the input angular rate can be obtained. In the process of multi-sensor information fusion, there are mainly two structures according to different ways of processing original data: centralized fusion and distributed fusion. In the traditional array MEMS gyroscope research, the centralized fusion method based on Kalman filter is mostly used, that is, the data of each sensor is directly sent to the fusion center to obtain the final high-precision estimation value through Kalman filter. The centralized fusion structure is simple and has small information loss, but it has the disadvantages of high communication requirement of the system, heavy calculation burden of the fusion center, etc. Especially when the number of gyroscopes in the array increases, the complexity of the fusion algorithm will be significantly improved. At the same time, in the Kalman recursive estimation process, the gain and mean square deviation are calculated and updated at each step, which has large amount of calculation and complex calculation, and is not suitable for real-time processing in actual application process. The final optimization effect of the precision of traditional gyroscope array is limited, and it is still difficult to meet the needs of high-precision practical engineering applications such as inertial navigation. SUMMARY

[0005] The purpose of the embodiments of the present application is to provide a method and device, and an electronic device, to solve the technical problem of large single MESM gyroscope random error, low detection precision, and difficulty in meeting high-precision engineering application requirements in the related art.

[0006] According to a first aspect of the present application, a high-precision data fusion method of an array MEMS gyroscope is provided, comprising the following steps:

[0007] (1) A plurality of MEMS gyroscopes with the same model and parallel sensitive axes for measuring the same angular velocity signal are arranged to form a gyroscope array, i.e., an array gyroscope;

[0008] (2) The array gyroscope is placed in a horizontal static state for continuous sampling at a certain sampling frequency, and a plurality of groups of sampling data are repeatedly obtained. Allan variance analysis is performed on each MEMS gyroscope in the array using the sampling data, and the variance Q ai of the angle random walk noise n ai and the variance Q bi of the rate random walk noise n bi of each gyroscope are calculated. The mean value of the variance of the angle random walk noise and the variance of the rate random walk noise obtained by Allan variance analysis on the plurality of groups of data is used as the final parameter;

[0009] (3) A random walk error model is used to model the random walk error of the angular velocity measurement value. A first-level Kalman filter is designed, i.e., according to the established random walk error model, the rate random walk and the true angular velocity are used to construct the system state quantity, and the system state equation is established. The actual output value of each gyroscope is regarded as the measurement value, and the angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed;

[0010] (4) The continuous state equation and the measurement equation constructed in step (3) are discretized to obtain a one-step transition matrix Φ k,k-1 of the discrete Kalman filter, a system noise driving matrix Γ k-1 , a measurement sequence Y k , a measurement matrix H k , a system noise discretization covariance matrix Q k , and a measurement noise discretization covariance matrix R k . The results obtained by the discretization are used to perform Kalman filter calculation on each gyroscope in the array. The state quantity X i,k is recursively estimated, a rate extraction vector e is defined, and the first-level Kalman filter result of each gyroscope is extracted;

[0011] (5) Extend the random walk error model derived in step (3) to the array form; that is, obtain the array form of MEMS gyroscope random walk error model, and use the rate random walk and real angular velocity of the gyroscope array to construct the system state variables and establish the state equation; take the data of each gyroscope in the array after the first stage Kalman filter as the measurement value Z(t), and regard the angle random walk noise of the gyroscope array as the measurement noise, construct the measurement equation, and design the second stage Kalman filter according to the state equation and measurement equation of the gyroscope array.

[0012] (6) Discretize the constructed continuous state equation and measurement equation to obtain the one-step transition matrix Φ of the second-stage Kalman filter. k,k-1 System noise driving matrix Γ k-1 Measurement sequence Z k Measurement matrix H k The discretized covariance matrix of the system noise is obtained as Q. k Discretized covariance matrix R of measurement noise k ;

[0013] (7) Derive the steady-state value of the Kalman gain to replace the real-time updated gain value at each step. If the state-space model is a time-invariant system, then the Kalman filter gain K after stabilization is... k→∞ It is also a constant value; first, define an (N+1)×(N+1) matrix. Then, eigenvalue decomposition is performed on L, i.e., L = ADA. T And make its last feature value 0, and Obtained by A and D processing; calculate the steady-state value K of the Kalman gain in continuous-time form. ∞ After discretization, the steady-state value of the discrete form of the Kalman gain is calculated.

[0014] (8) Using the discrete form of the Kalman gain steady-state value derived in step (7) Instead of updating the gain value at each step, a constant-gain Kalman filter is used to fuse the data from the array gyroscopes after the first-stage filtering to obtain a high-precision angular velocity estimate; the matrix S = Q is defined. k 1 / 2 A, regarding the state variable X k Perform the transformation, ξ(k)=S -1 X k (k), using the discrete form of the Kalman gain steady-state value The measurement sequence Z(k) is used to perform a recursive minimum variance estimate of the transformed state variable ξ(k). To select a vector, This is the filter output, which is the optimal estimate of the input angular velocity by the gyroscope array at the k-th sampling point.

[0015] Further, the same angular velocity signal is measured in step (1), specifically: an array gyroscope is constructed by grouping several MEMS gyroscopes into two groups each, and the MEMS gyroscopes in the same group are installed in opposite directions along the sensitive axis direction; the output ends of the two gyroscopes in the same group are respectively connected to the positive and negative input ends of a differential input A / D conversion chip, and the angular velocity measurement values y1(t), y2(t), y3(t), …, y N (t) of each group of gyroscopes in the array are obtained through A / D conversion.

[0016] Specifically, the error modeling in step (3) is based on the analysis of the signal characteristics and main error sources of the MEMS gyroscope, and a random walk model is selected for error modeling of the MEMS gyroscope and extended to an array form, wherein the noise part includes angle random walk noise ARW and rate random walk noise RRW; the random walk error model of a single MEMS gyroscope is:

[0017]

[0018] where y(t) is the output value of the MEMS gyroscope, ω(t) is the real angular velocity, and the random walk noise driven by the real angular velocity white noise is where the variance of the real angular velocity driven white noise n ω is Q ω ; b(t) is the gyroscope bias, driven by white noise W(t), corresponding to the rate random walk (RRW), and n(t) is the observation noise, corresponding to the angle random walk (ARW);

[0019] The random walk error model of the array form is:

[0020]

[0021] When y = [y1, y2, …, y N ] T , b = [b1, b2, …, b N ] T , n a = [n a1 , n a2 , …, n aN ] T , n b = [n b1 , n b2 , …, n bN ] T , it is abbreviated as:

[0022]

[0023] where i is the gyroscope number, N is the number of gyros, y i is the actual output angular velocity of the i-th gyro, ω is the real angular velocity of the environment, b i is the bias of the i-th gyro, and n bi is the rate random walk white noise driven by the bias, and the variance is n ai is the measurement noise of the i-th gyro, i.e., the angle random walk white noise, and the variance is

[0024] Further, the step (3) uses the rate random walk and the real angular velocity to construct the system state quantity, specifically:

[0025] For a single gyro in the array:

[0026]

[0027] The state equation of the system thus constructed is:

[0028]

[0029] The actual output value of each gyro is regarded as the measurement value Y(t) = Y i (t) (i = 1, 2,..., N), and the angular velocity random walk noise is regarded as the measurement noise, and the measurement equation can be constructed as:

[0030] Y(t) = H(t)X(t) + V(t) = [1 1] · X i (t) + n ai ;

[0031] The parameter W(t) = [n bi , n ω ] T , V(t) = [n ai ]; according to the Kalman filtering theory, W(t), V(t) satisfy:

[0032]

[0033] where the system noise covariance matrix The measurement noise covariance matrix r = [Q ai ].

[0034] Further, the step (4) uses the obtained results of the discretization processing to perform Kalman filtering calculation on each gyro in the array; the state quantity X i,k is recursively estimated, a rate extraction vector e is defined, and the first-level Kalman filtering result of each gyro is obtained; specifically, the obtained system measurement sequence Y k , the measurement matrix H k is used to perform the first-level Kalman filtering calculation., the one-step transition matrix of Kalman filter Φ k,k-1 , the system noise driving matrix Γ k-1 , the system noise discretization covariance matrix Q k , the measurement noise discretization covariance matrix R k , the Kalman filter calculation of each gyroscope in the array is calculated according to the recursive calculation process; the specific filtering process is: the system one-step prediction mean square error is calculated and the system one-step state prediction The time update is completed, and the system filter gain is obtained Then the state estimation at the new time is obtained and the estimated mean square error P k =(I-K k H k )P k / k-1 , the measurement update is completed; the new state and mean square error are taken as the initial value of the calculation at the next time, and X i,k =[b i,k ,ω i,k ] T (i=1, 2…N) is recursively estimated, the rate extraction vector e=[0, 1] is defined, and the first level Kalman filter result of each gyroscope can be obtained by e.X i,k =ω i,k .

[0035] Further, step (5) is specifically to generalize the random walk error modeling to an array form:

[0036]

[0037] In the formula, i is the gyroscope number, N is the number of gyroscopes, y i is the actual output angular velocity of the i-th gyroscope, ω is the real angular velocity of the environment, b i is the bias of the i-th gyroscope, which is driven by the rate random walk white noise n bi , and the variance is n ai is the measurement noise of the i-th gyroscope, which is expressed as an angle random walk white noise, and the variance is

[0038] Let:

[0039] y=[y1,y2,…,y N ] T ,b=[b1,b2,…,b N ] T ,n a =[n a1 ,n a2 ,…,n aN ]T , n b = [n b1 , n b2 , …, n bN ] T ,

[0040] The above formula is simplified as:

[0041]

[0042] Second order Kalman filtering is performed: according to the established array form random error model, the system state quantity is constructed:

[0043]

[0044] The state equation of the system thus constructed is:

[0045]

[0046] Where F(t) = 0 I N+1 , G(t) = I N+1 , I N is an N-dimensional unit matrix;

[0047] The gyro data in the array after the first order Kalman filtering is taken as the measurement Z(t) = [Y f1 (t), Y f2 (t), …, Y fN (t)] T , Y fi (t) represents the angular velocity value of the i-th gyro after the first order filtering; the angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed as:

[0048]

[0049] Where 1 N represents an N-dimensional column vector with all elements being 1, the parameter W(t) = [n b1 , n b2 , …, n bN , n ω ] T , V(t) = [n a1 , n a2 , …, n aN ] T ; W(t), V(t) satisfy the following expression:

[0050]

[0051] Where the system noise covariance matrix The measurement noise covariance matrix R = [Q a ].

[0052] Further, the step (6) is specifically: discretization processing is made to the constructed continuous state equation and measurement equation:

[0053]

[0054] In the formula:

[0055]

[0056] T is a discretization period, and W k , V k satisfy:

[0057]

[0058] The system noise discretization covariance matrix is The measurement noise discretization covariance matrix R k = [Q a (k)], Q b (k) is the covariance matrix of the rate random walk noise vector n b , and the diagonal elements are the variance of the rate random walk noise n bi Q ω (k) is the covariance matrix of the true angular velocity driving white noise, Q a (k) is the covariance matrix of the angle random walk noise vector n a , and the diagonal elements are the variance of the angle random walk noise n ai calculated in step two

[0059] Further, the steady-state value calculation process of the Kalman gain in the step (7) is: first, define the (N+1) × (N+1) matrix Then, eigenvalue decomposition is made to L, that is, L = ADA T And make the last eigenvalue of A as 0, then And The matrix A, D are derived from the matrix as follows:

[0060]

[0061] Where v is the column vector of the last column, is an N × N matrix, is an (N+1) × (N) matrix;

[0062] Then, the steady-state value of the Kalman gain in the continuous time form is represented as:

[0063] ​

[0064] The steady-state value of the Kalman gain in discrete form is:

[0065]

[0066] where T s is the discretization period.

[0067] Further, the constant gain Kalman filter recursion estimation process based on the steady-state value of the Kalman gain in discrete form in step (8) is: Define matrix S = Q 1 / 2 A, transform the state variable X k , ξ(k) = S -1 X k (k), X k = [b1(k), b2(k), …, b N (k), ω(k)] T This transformation is used to isolate the pure integral action in order to take practical measures to ensure that no numerical rounding errors occur in implementation; the estimation of the state variable and the high-precision angular velocity extraction after fusion are completed by the following equation:

[0068]

[0069] where is the minimum variance estimation of ξ(k), the last row of which is set to 0, which corresponds to the pure integrator state; is the selection vector, is the filter output, that is, the optimal estimation value of the input angular velocity of the gyro array at the kth sampling point.

[0070] The second aspect of the application is an electronic device, comprising:

[0071] one or more groups of MEMS gyro arrays

[0072] one or more processors for reading and processing MEMS gyro array data;

[0073] a memory for storing one or more programs;

[0074] When the one or more programs are executed by the one or more processors, the one or more processors implement the array MEMS gyro high-precision data fusion method.

[0075] The present application comprises the following advantages: the random error of the MEMS gyroscope is reduced, and the detection precision of the MEMS gyroscope is improved significantly; the steady-state value of the Kalman gain is used for the estimation of the angular velocity, the calculation of the gain and the mean square deviation is avoided at each step, the complexity and the operation amount of the processing process are optimized, and the real-time processing of the gyroscope array is more favorable.

[0076] It should be understood that the foregoing general description and the following detailed description are only exemplary and explanatory and are not restrictive of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0077] The accompanying drawings incorporated in and forming a part of the specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the application.

[0078] Figure 1 The flow chart of the high-precision data fusion method of the distributed multi-stage array MEMS gyroscope based on the improved Kalman filter of the present application;

[0079] Figure 2 The flow chart of the discrete Kalman filter of the present application;

[0080] Figure 3 The block diagram of the discrete constant gain Kalman filter of the present application;

[0081] Figure 4 The effect comparison test result graph of the fusion method of the present application (gyroscope static output data comparison);

[0082] Figure 5 The effect comparison test result graph of the fusion method of the present application (gyroscope Allan variance analysis result comparison). DETAILED DESCRIPTION

[0083] The exemplary embodiments will be described in detail herein with reference to the attached drawings. The same reference numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all the embodiments consistent with the present application.

[0084] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting. As used in this application and the appended claims, the singular forms "a," "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will also be understood that the term "and / or," as used herein, refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0085] The technical solutions of the application are further described below with reference to the accompanying drawings and embodiments

[0086] As Figure 1 shown, the application provides a high-precision data fusion method for array MEMS gyroscopes to reduce the random error of MEMS gyroscopes and obtain high-precision angular velocity values. The specific steps are as follows:

[0087] Step one: MEMS gyroscopes produced in the same batch usually have similar zero bias instability due to material and production process reasons. Therefore, a plurality of MEMS gyroscopes are grouped into a group, the MEMS gyroscopes in the same group are installed in opposite directions along the sensitive axis to form a gyro array, and angular velocity detection is performed to improve the output signal-to-noise ratio. The output ends of the two gyroscopes in the same group are connected to the positive and negative input ends of a differential input A / D conversion chip, respectively, and the angular velocity measurement values y1(t), y2(t), y3(t),..., y N (t) of each gyroscope in the array are obtained through A / D conversion. In an embodiment of the application, 8 MEMS gyroscopes are selected to form a MEMS gyroscope array.

[0088] Step two: The array gyro is continuously sampled for a period of time (more than 2 hours) at a certain sampling frequency in a horizontal static state, the process is repeated to obtain array sampling data, the sampling data is preprocessed according to the 3σ rule to remove abnormal values in the data. The sampling data is used to perform Allan variance analysis on each MEMS gyroscope in the array, a double logarithmic curve graph of Allan standard deviation (σ(τ)) and correlation time (τ) is drawn, and the least square method is used to fit and calculate the variance of the angle random walk noise n ai of each gyroscope and the variance of the rate random walk noise n . bi The variances of the angle random walk noise and the rate random walk noise obtained by Allan variance analysis on a plurality of groups of data are averaged to obtain the final used parameters.

[0089] Step three: the random walk model is used to model the error of the angular velocity measurement value:

[0090]

[0091] ​Where y(t) is the MEMS gyroscope output value, and ω(t) is the true angular velocity. Under static conditions, the true value should be 0, but due to external environmental factors, it cannot actually be 0. Therefore, it is considered as random walk noise driven by white noise of the true angular velocity. The real angular velocity drives the white noise n ω The variance is given by . b(t) is the gyroscope bias, driven by white noise W(t), corresponding to rate random walk (RRW), and n(t) is the observation noise, corresponding to angle random walk (ARW).

[0092] Step 4: First-stage Kalman filter design. Based on the random walk error model established in Step 3, the system state variables are constructed using rate random walk and true angular velocity:

[0093] For a single gyroscope in the array:

[0094]

[0095] The state equation of the system thus constructed is:

[0096]

[0097] The actual output values ​​of each gyroscope are considered as the measured values ​​Y(t) = Y i If (t)(i=1,2,…,N), and the angular velocity random walk noise is considered as measurement noise, then the measurement equation can be constructed as:

[0098] Y(t)=H(t)X(t)+V(t)=[1 1]·X i (t)+n ai (4)

[0099] Parameter W(t) = [n bi ,n ω ] T V(t)=[n ai According to Kalman filtering theory, W(t) and V(t) satisfy:

[0100]

[0101] Where the system noise covariance matrix Measurement noise covariance matrix r = [Q ai ].

[0102] Step 5: Discretize the continuous state equation and measurement equation constructed in Step 4.

[0103]

[0104] Obtain the one-step transition matrix Φ of the Kalman filter. k,k-1=I2, system noise driving matrix Γ k-1 =T·I², where T is the discretization period. W k-1 =[n bi (k),n ω (k)] T V k =[n ai (k)] and satisfies and W k V k satisfy:

[0105]

[0106] The discretized covariance matrix of the system noise is obtained as follows: Discretized covariance matrix R of measurement noise k =[Q ai (k)],Q bi (k) represents the variance of the random walk noise at the i-th gyroscope rate, Q ω (k) is the covariance matrix of the white noise driven by the true angular velocity, Q ai (k) represents the variance of the random walk noise at the i-th gyroscope angle.

[0107] Step Six: Utilize the systematic measurement sequence Y obtained in Step Five k Measurement matrix H k The one-step transition matrix Φ of Kalman filtering k,k-1 System noise driving matrix Γ k-1 The system noise discretization covariance matrix Q k Discretized covariance matrix R of measurement noise k ,according to Figure 2 The recursive calculation process shown performs Kalman filtering calculations on each gyroscope in the array. Specifically, the filtering process involves calculating the system's one-step prediction mean square error. System one-step state prediction Complete the time update, and then obtain the system filter gain. Then obtain the state estimate for the new time step. and the estimated mean square error P k =(IK k H k )P k / k-1 The measurement update is completed. The new state and mean square error are used as the initial values ​​for the next calculation, and X is... i,k =[b i,k ,ω i,k ] T (i = 1, 2, ..., N) are recursively estimated, and the rate extraction vector is defined as e = [0, 1]. From eX... i,k =ω i,kThe first level Kalman filtering results of each gyroscope are obtained.

[0108] Step seven: The random walk error model derived in step three is extended to an array form:

[0109]

[0110] where i is the gyroscope number, N is the number of gyroscopes, y i is the actual output angular velocity of the i-th gyroscope, ω is the real angular velocity of the environment, b i is the bias of the i-th gyroscope, and n bi is the rate random walk white noise driven by the rate, with a variance of n ai is the measurement noise of the i-th gyroscope, which is expressed as an angle random walk white noise, with a variance of

[0111] Let:

[0112] y = [y1, y2, …, y N ] T , b = [b1, b2, …, b N ] T , n a = [n a1 , n a2 , …, n aN ] T , n b = [n b1 , n b2 , …, n bN ] T ,

[0113] Then equation (8) can be simplified as:

[0114]

[0115] Step eight: Second level Kalman filter design. According to the array form random error model established in step seven, the system state quantity is constructed:

[0116]

[0117] The state equation of the system thus constructed is:

[0118]

[0119] where F(t) = 0·I N+1 , G(t) = I N+1 , and I N is an N-dimensional unit matrix.

[0120] The gyro data in the array after the first Kalman filtering is taken as the measurement value Z(t) = [Y f1 (t), Y f2 (t), …, Y fN (t)] T ,Y fi (t) represents the angular velocity value of the i-th gyro after the first filtering. The angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed as:

[0121]

[0122] where 1 N represents an N-dimensional column vector with all elements being 1. The parameter W(t) = [n b1 ,n b2 ,…,n bN ,n ω ] T ,V(t) = [n a1 ,n a2 ,…,n aN ] T . W(t), V(t) satisfy:

[0123]

[0124] where the system noise covariance matrix The measurement noise covariance matrix R = [Q a ].

[0125] Step nine: as in step five, the constructed continuous state equation and measurement equation are discretized:

[0126]

[0127] In the formula:

[0128]

[0129] T is the discretization period, and W k , V k satisfy:

[0130]

[0131] The system noise discretization covariance matrix is The measurement noise discretization covariance matrix R k = [Q a (k)], Q b (k) is the covariance matrix of the rate random walk noise vector n b , and the diagonal elements are the variance of the rate random walk noise n bi calculated in step two Q ω (k) is the covariance matrix of the true angular rate driven white noise, Q a (k) is the covariance matrix of the angle random walk noise vector n a (k) is the covariance matrix of the angle random walk noise n ai (k) is the covariance matrix of the angle random walk noise n

[0132] Step ten: The steady state value of the Kalman gain is used in the second stage Kalman filter instead of the gain value updated in real time at each step. If the state space model is a constant system, the filter gain K k→∞ of the Kalman filter is also a constant value. The Kalman gain K(t) in the recursive Kalman filter depends on the state covariance P(t) propagated according to the Riccati differential equation (RDE). The steady state value of the estimation mean square error P is

[0133] P = (I - K ∞ H) ΦPΦ T + ΓQΓ T

[0134] = (I - K ∞ H) ΦPΦ T + Q (17)

[0135] The steady state value of the Kalman gain is K ∞ = PH T R -1 , which is substituted into equation (17) to obtain:

[0136] P = (I - PH T R -1 H) ΦPΦ T + Q (18)

[0137]

[0138] The steady state value of the estimation mean square error P is calculated from equation (19) according to the analytical solution of the Riccati differential equation (RDE), and the steady state value of the Kalman gain is further derived. The calculation of the steady state value of the Kalman gain in actual implementation is as follows: first, define the (N+1) x (N+1) matrix Then perform eigenvalue decomposition on L, i.e. L = ADA T and make the last eigenvalue of A equal to 0, then and are taken out from A and D in the following way:

[0139]

[0140] where v is the column vector of the last column, is an N x N matrix, In practice, for a (N+1)×(N) matrix, when using a finite-precision algorithm, the smallest eigenvalue will never be exactly zero. In this case, for the purpose of eigenvalue sorting, the smallest eigenvalue is used to represent zero.

[0141] The steady-state value of the Kalman gain in continuous-time form is expressed as:

[0142]

[0143] The steady-state value of the discrete form of the Kalman gain is:

[0144]

[0145] Where T s For discretized periods.

[0146] Step 11: Use the discrete form of the Kalman filter gain steady-state value derived in Step 9. Replaces the gain value updated at each step. (Press) Figure 3 The discrete constant-gain Kalman filter block diagram shown is used to fuse data from the array gyroscopes after the first-stage filtering to obtain a high-precision angular velocity estimate. The specific recursive estimation process of the constant-gain Kalman filter is as follows. Define matrix S = Q. 1 / 2 A, regarding the state variable X k Perform the transformation, ξ(k)=S -1 X k (k), X k =[b1(k),b2(k),…,b N (k),ω(k)] T This transformation is used to isolate the pure integral action, so that practical measures can be taken to ensure that no numerical rounding errors occur in the implementation. The estimation of the state variables and the extraction of the fused high-precision angular velocity are accomplished by the following equations:

[0147]

[0148] in For the minimum variance estimate of ξ(k), The last line (corresponding to the pure integrator state) must be set to 0 precisely. To select a vector, This is the filter output, which is the optimal estimate of the input angular velocity by the gyroscope array at the k-th sampling point.

[0149] In one embodiment of the present invention, the angular random walk error is greatest in the unprocessed raw gyroscope data. Minimum is The zero bias instability is maximum 28.9786° / h and minimum 9.7870° / h; the rate random walk error is maximum 98.7317° / h 3 / 2 , minimum 10.623098° / h 3 / 2 ; the zero bias stability is maximum 48.18° / h and minimum 28.15° / h; the standard deviation is maximum 0.0526° / s and minimum 0.0474° / s.

[0150] After the array MEMS gyroscope is fused by the method, the angle random walk error of the fused virtual gyroscope data is reduced to The zero bias instability is reduced to 0.068361° / h, and the rate random walk is reduced to 0.030816° / h 3 / 2 The zero bias stability is reduced to 0.1052° / h, and the standard deviation is reduced to 5.5847*10 -5 -4° / s.

[0151] In order to further evaluate the effect of the distributed multi-level array MEMS gyroscope multi-sensor information fusion method based on the improved Kalman filter, the same test data is used to compare the method with the centralized single-layer basic Kalman filter and the centralized single-layer constant gain Kalman filter. Figure 4 , Figure 5 As shown in the figure, the method can significantly reduce the random error of the MEMS gyroscope and improve the MEMS gyroscope detection precision.

[0152] Other embodiments of the application will be apparent to those skilled in the art from consideration of the specification and practice of the application disclosed herein. The specification and examples given herein are intended as illustrative only and not limiting of the present application. It will be understood by those skilled in the art that various modifications and changes can be made thereto without departing from the scope of the present application, which is set out in the claims below.

[0153] It should be understood that the application is not limited to the precise construction that has been described above and illustrated in the accompanying drawings, and that various modifications and changes can be made by those skilled in the art without departing from the scope of the application.

Claims

1. A high-precision data fusion method for an array MEMS gyroscope, characterized in that, Comprising the following steps: (1) a plurality of MEMS gyroscopes with the same model, the sensitive axis parallel to each other, measuring the same angular velocity signal, constitute a gyro array, namely array gyro; (2) Array gyroscope is placed in a horizontal static state to continuously sample at a certain sampling frequency, and multiple sets of sampling data are repeatedly obtained. Allan variance analysis is used on each MEMS gyroscope in the array to calculate the variance of the angle random walk noise n ai of each gyroscope and the variance of the rate random walk noise n bi of each gyroscope The average of the variance of the angle random walk noise and the variance of the rate random walk noise obtained by Allan variance analysis on multiple sets of data is used as the final parameter. (3) the random walk error model is used to model the angular velocity measurement value; And the first level Kalman filter design, that is, according to the established random walk error model, the rate random walk and the true angular velocity are used to construct the system state quantity, and the system state equation is established; The actual output value of each gyroscope is regarded as the measurement value, and the angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed; (4) Discretization is made to the continuous state equation and the measurement equation constructed in step (3) to obtain a one-step transfer matrix Φ of the discrete Kalman filter k,k-1 , a system noise driving matrix Γ k-1 , a measurement sequence Y k , a measurement matrix H k , a system noise discretization covariance matrix Q k , and a measurement noise discretization covariance matrix R k ; the obtained results of the discretization are used to make Kalman filter calculation on each gyroscope in the array; a state quantity X i,k is recursively estimated, a rate extraction vector e is defined, and the first-stage Kalman filter result of each gyroscope is extracted. (5) the random walk error modeling derived in step (3) is generalized to an array form; That is, the random walk error model of the array form MEMS gyroscope is obtained, and the rate random walk and the true angular velocity of the gyro array are used to construct the system state quantity, and the state equation is established; The array data of each gyroscope after the first level Kalman filter is taken as the measurement value Z(t), and the angle random walk noise of the gyro array is regarded as the measurement noise, and the measurement equation is constructed, and the second level Kalman filter is designed according to the state equation and the measurement equation of the gyro array; (6) Discretization of the constructed continuous state equation and measurement equation to obtain the one-step transfer matrix Φ of the second Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , the measurement sequence Z k , the measurement matrix H k , the system noise discretization covariance matrix Q k , and the measurement noise discretization covariance matrix R k ; (7) Derive the steady-state value of the Kalman gain to replace the gain value updated in real time at each step, if the state space model is a constant system, the filter gain K of the Kalman filter after stabilization k→∞ is also a constant value; first define the (N+1) x (N+1) matrix Then do eigenvalue decomposition on L, that is, L = ADA T And make the last eigenvalue of it 0, And Obtained by A, D processing; Computing a steady-state value of a Kalman gain in continuous time form K ∞ Computing a steady-state value of a Kalman gain in discrete form K (8) the steady-state value of the Kalman gain in discrete form derived from step (7) Instead of updating the gain value at each step, a constant gain Kalman filter is used to fuse the data from the first stage filtered array gyros to obtain high precision angular rate estimates; define the matrix S = Q k 1 / 2 A, the state variable X k is transformed, ξ(k) = S -1 X k (k), the steady-state value of the Kalman gain in discrete form is used The measurement sequence Z(k) is used to recursively estimate the transformed state variable ξ(k) in the minimum variance sense, is the selection vector, is the filter output, i.e. the optimal estimate of the input angular rate at the kth sampling point of the array gyros.

2. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, The same angular velocity signal is measured in the step (1), specifically: an array gyroscope is constructed in a form that a plurality of MEMS gyroscopes are divided into two groups, and the MEMS gyroscopes in the same group are installed in opposite directions along the sensitive axis direction, and angular velocity detection is performed; the output ends of the two gyroscopes in the same group are respectively connected to the positive and negative input ends of a differential input A / D conversion chip, and the angular velocity measurement values y1(t), y2(t), y3(t), …, y N (t) of each group of gyroscopes in the array are obtained through A / D conversion.

3. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, The error modeling in step (3) is based on the analysis of the signal characteristics and main error sources of MEMS gyroscope, and the random walk model is selected to model the error of MEMS gyroscope and generalized to an array form, wherein the noise part includes angle random walk noise ARW and rate random walk noise RRW; The random walk error model of a single MEMS gyroscope is: where y(t) is the MEMS gyroscope output value, ω(t) is the real angular velocity, and b(t) is the gyroscope bias, driven by white noise W(t) corresponding to the rate random walk (RRW), n(t) is the observation noise, corresponding to the angle random walk (ARW). where the real angular velocity drives white noise n ω with variance Q ω ; b(t) is the gyroscope bias, driven by white noise W(t) corresponding to the rate random walk (RRW), n(t) is the observation noise, corresponding to the angle random walk (ARW). The random walk error model of the array form is: Let y = [y1, y2, ..., y N ] T b = [b1, b2, ..., b N ] T n a =[n a1 ,n a2 ,…,n aN ] T n b =[n b1 n b2 ,…,n bN ] T Then it is abbreviated as: where i is the gyro index, N is the number of gyros, y i is the actual output angular rate of the ith gyro, ω is the true angular rate of the environment, b i is the bias of the ith gyro, which is modeled as a random walk white noise n bi driven by a bias rate, with variance n ai is the measurement noise of the ith gyro, which is modeled as a random walk white noise with variance 4. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, In step (3), the rate random walk and the true angular velocity are used to construct the system state quantity, specifically: For a single gyroscope in the array: The state equation of the system constructed by this is: The actual output values of the respective gyroscopes are regarded as measurement values Y(t) = Y i (t) (i = 1, 2, …, N), and the angular velocity random walk noise is regarded as measurement noise. The measurement equation can be constructed as: Y(t) = H(t) X(t) + V(t) = [1 1] · X i (t) + n ai ; The parameter W(t) = [n bi ,n ω ] T The parameter V(t) = [n ai ]; according to Kalman filtering theory, W(t), V(t) satisfy: where R is the system noise covariance matrix The measurement noise covariance matrix r = [Q ai ].

5. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, The step (4) performs Kalman filtering calculation on each gyroscope in the array by using the obtained result of the discretization processing; and the state quantity X i,k Recursive estimation is performed, a rate extraction vector e is defined, and a first-stage Kalman filtering result of each gyroscope is extracted. k A measurement matrix H k A one-step transition matrix Φ of Kalman filtering k,k-1 A system noise driving matrix Γ k-1 A system noise discretization covariance matrix Q k A measurement noise discretization covariance matrix R k The recursive calculation process is used to perform Kalman filtering calculation on each gyroscope in the array. The specific filtering process is: calculating system one-step prediction mean square error and system one-step state prediction completing time update, and then obtaining system filtering gain then obtaining state estimation of the new time and estimation mean square error P k =(I-K k H k )P k / k-1 , completing measurement update; taking the new state and mean square error as the initial value of the next time calculation, recursively estimating X i,k =[b i,k ,ω i,k ] T (i=1, 2…N), defining the rate extraction vector e=[0, 1], and obtaining the first level Kalman filtering result of each gyroscope by e.X i,k =ω i,k .

6. The high-precision data fusion method of an array MEMS gyroscope according to claim 3, characterized in that, According to step (5), the random walk error modeling is generalized to an array form: where i is the gyro index, N is the number of gyros, y i is the actual output angular rate of the ith gyro, ω is the real angular rate of the environment, b i is the bias of the ith gyro, which is driven by a rate random walk white noise n bi with variance n ai is the measurement noise of the ith gyro, which is represented as an angle random walk white noise with variance Let: y = [y1, y2, ..., y N ] T b = [b1, b2, ..., b N ] T n a =[n a1 ,n a2 ,…,n aN ] T n b =[n b1 n b2 ,…,n bN ] T Then the above formula can be simplified to: The second level Kalman filter design is performed: according to the established array form random error model, the system state quantity is constructed: The state equation of the system constructed by this is: where F(t) = 0 - I N+1 G(t) = I N+1 I N is the N-dimensional identity matrix; The gyro data in the array after the first Kalman filtering is taken as the measurement Z(t) = [Y f1 (t),Y f2 (t),…,Y fN (t)] T ,Y fi (t) represents the angular velocity value of the i-th gyro after the first filtering; the angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed as: where 1 N represents an N-dimensional column vector with all elements being 1, and the parameter W(t) = [n b1 ,n b2 ,…,n bN ,n ω ] T , V(t) = [n a1 ,n a2 ,…,n aN ] T ; W(t), V(t) satisfy the following expression: where the system noise covariance matrix The measurement noise covariance matrix R = [Q a ].

7. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, In step (6), the continuous state equation and the measurement equation constructed are discretized: In the formula: T is the discretization period, and W k , V k satisfies: The system noise discretized covariance matrix is The measurement noise discretized covariance matrix R k = [Q a (k)], Q b (k) is the covariance matrix of the rate random walk noise vector n b with diagonal elements being the variance of the rate random walk noise n bi Q ω (k) is the covariance matrix of the true angular velocity driven white noise, Q a (k) is the covariance matrix of the angle random walk noise vector n a with diagonal elements being the variance of the angle random walk noise n ai calculated in step two ​ 8. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, The steady-state value of the Kalman gain in the step (7) is calculated by first defining an (N+1) x (N+1) matrix L is then eigenvalue-decomposed, i.e. L = ADA T and making the last eigenvalue of A equal to zero and is derived from the matrices A, D in the following way: where v is the column vector of the last column, is an N x N matrix, is an (N + 1) x (N) matrix; The steady-state value of the Kalman gain in the continuous time form is represented as: The steady-state value of the Kalman gain in the discrete form is: where T s is the discretization period.

9. The high-precision data fusion method of an array MEMS gyroscope according to claim 1, characterized in that, The step (8) uses a constant gain Kalman filter recursion based on the steady state value of the discrete form of the Kalman gain S = Q 1 / 2 A, a transformation of the state variable X k (k) = S -1 X k (k), X k = [b1(k), b2(k),..., b N (k), ω(k)] T , which is used to isolate the pure integration effect in order to take practical measures to ensure that no numerical rounding error occurs in the implementation; the estimation of the state variable and the high-precision angular velocity extraction after fusion are completed by the following equation: where is the minimum variance estimate of ξ(k), The last row of is set to 0, which corresponds to the pure integrator state; is the selection vector, is the filter output, i.e., the optimal estimate of the input angular rate by the gyroscope array at the kth sample point.

10. An electronic device, comprising: Comprising: One or more groups of MEMS gyro arrays One or more processors for reading MEMS gyro array data and processing; Memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the array MEMS gyroscope high-precision data fusion method according to any one of claims 1-8.

Images