Array MEMS gyroscope high-precision data fusion method

Abstract

The invention provides a high-precision data fusion method for an array MEMS (Micro Electro Mechanical System) gyroscope, which comprises the following steps of: designing a first-stage Kalman filter to carry out local optimal estimation on each sensor before multi-sensor data enters a fusion center by adopting a multi-stage distributed data fusion technology based on improved Kalman filtering; a first-stage estimation value of a single gyroscope in the array is obtained, then processed information is sent to a second-stage fusion center to be processed, and finally an optimized high-precision angular velocity output value is obtained. Meanwhile, the angular velocity is estimated by using the steady-state value of the filtering gain in the Kalman filtering process, so that the calculation of the gain and the mean square error in each step is avoided, the complexity and the operand in the processing process are optimized, and the real-time processing of the gyroscope array is facilitated. The method has remarkable effects of reducing the random error of the MEMS gyroscope and improving the detection precision of the MEMS gyroscope.

Description

Technical Field

[0001] The invention belongs to the technical field of sensors, and in particular relates to a high-precision data fusion method for array-type MEMS gyroscopes. Background Art

[0002] The micromechanical gyroscope (MEMS gyroscope) is a high-tech product and one of the most important branches in the field of contemporary micromechanical electronic systems (MEMS). It is produced based on micromachining manufacturing technology. Its working principle is to use the Coriolis effect to convert external rotational motion into relative displacement of the internal plates of the gyroscope. Compared with mechanical gyroscopes and optical gyroscopes, MEMS gyroscopes have the following outstanding features: (1) Small size and light weight. MEMS technology is a high-tech field for planning, manufacturing, improving, measuring and controlling micron / nano materials. Therefore, the size of MEMS gyroscopes can reach the micron / nano order of magnitude, which is convenient for integration with other functional chips and can be directly embedded in circuit boards with main control functions. (2) High reliability. Since high-durability and impact-resistant materials are selected during the manufacturing process and integration methods are adopted, MEMS gyroscopes can work normally and exert their functional characteristics in most cases. (3) Low manufacturing cost. High-precision optical gyroscopes are expensive due to the difficulty in obtaining manufacturing materials, while MEMS gyroscopes are manufactured using cheap basic materials such as silicon, and generally cost less than a thousand yuan, with a minimum cost of several yuan. They can meet the needs of mass production in military equipment and civilian fields, and have a wide range of applicability.

[0003] MEMS gyroscopes have been widely used in inertial navigation, aerospace, automobiles and electronic equipment due to the above-mentioned significant and unique advantages. However, low-cost MEMS gyroscopes are affected by the inability to achieve high-level processing and manufacturing technology and high-demand design solutions in the current design and production process. They have not made significant breakthroughs in performance. Due to the influence of their bias stability, nonlinearity and temperature stability, they show a common defect of low detection accuracy, which makes them unable to meet the needs of many high-precision applications. At present, the main methods to improve the accuracy of MEMS gyroscopes are as follows: one is to use a consistent signal processing method to process the module integrating multiple angular velocity sensitive units when manufacturing and processing MEMS gyroscopes, but this method requires a high level of manufacturing and processing technology and circuit design technology; the other is to use the advantages of MEMS gyroscopes to select multiple MEMS sensors with lower accuracy to form an array, detect the same target, collect multiple sensor data, and perform data fusion processing to output a set of high-precision and high-reliability data to achieve precise detection of moving targets. Because the final output data does not come from a single gyroscope, but the optimized angular velocity output signal obtained by fusion of the outputs of multiple gyroscopes, it is also called "virtual gyroscope technology." Taking into account the R&D cost and time of these two methods, constructing a gyroscope array to improve the accuracy and reliability of MEMS gyroscopes has become a widely used method.

[0004] Multi-sensor information fusion is a key technology in the research of array MEMS gyroscopes. It is a process of detecting, correlating, combining and estimating multi-source data to improve the accuracy of state estimation. The random error of MEMS gyroscopes is one of the main factors restricting their detection accuracy. By adopting multi-sensor data fusion technology, the measurement values ​​of the gyroscope array are analyzed and identified, the optimal filter is designed to estimate the size of each gyroscope error, and the measurement information is compensated and corrected to obtain a high-precision estimate of the input angular rate. In the process of multi-sensor information fusion, there are two main structures according to the different ways of processing the original data: centralized fusion and distributed fusion. Most of the traditional array MEMS gyroscope research adopts a centralized fusion method based on Kalman filtering, that is, the data of each sensor is directly sent to the fusion center to obtain the final high-precision estimate through Kalman filtering for optimal estimation. The centralized fusion has a simple structure and low information loss, but it has the disadvantages of high requirements for system communication and heavy calculation burden on the fusion center. Especially when the number of gyroscopes in the array increases, the complexity of the fusion algorithm will increase significantly. At the same time, in the Kalman recursive estimation process, the gain and mean square error are calculated and updated at each step, which is computationally intensive and complex, and is not suitable for real-time processing in practical applications. The final optimization effect of the traditional gyro array accuracy is limited, and it is still difficult to meet the needs of high-precision practical engineering applications such as inertial navigation. Summary of the invention

[0005] The purpose of the embodiments of the present application is to provide a method, an apparatus, and an electronic device to solve the technical problems existing in the related art that a single MESM gyroscope has large random errors, low detection accuracy, and is difficult to meet the requirements of high-precision engineering applications.

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

[0007] (1) A gyroscope array is formed by combining several MEMS gyroscopes of the same model with parallel sensitive axes to measure the same angular velocity signal;

[0008] (2) The array gyroscope is placed in a horizontal static state and continuously sampled at a certain sampling frequency to repeatedly obtain multiple sets of sampled data. The sampled data is used to perform Allan variance analysis on each MEMS gyroscope in the array to calculate the random walk noise n of each gyroscope angle. ai The variance Q ai and rate random walk noise n bi The variance Q bi , the variance of the angle random walk noise and the variance of the rate random walk noise obtained by Allan variance analysis of multiple groups of data are averaged as the final parameters;

[0009] (3) A random walk error model is used to model the angular velocity measurement value; and a first-level Kalman filter design is performed, that is, based on 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) Discretize the continuous state equation and measurement equation constructed in step (3) to obtain the one-step transfer matrix Φ of the discrete Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , measurement sequence Y k , the measurement matrix H k And the discretized covariance matrix of the system noise is Q k , the discretized covariance matrix of the measurement noise R k ; Use the results obtained from the discretization process to perform Kalman filter calculations on each gyroscope in the array; i,k Perform recursive estimation, define the rate extraction vector e, and extract the first-level Kalman filter results of each gyroscope;

[0011] (5) The random walk error modeling derived in step (3) is extended to the array form; that is, the random walk error model of the MEMS gyroscope in array form is obtained, and the rate random walk and true angular velocity of the gyroscope array are used to construct the system state quantity and establish the state equation; the data of each gyroscope in the array after the first-level Kalman filter is used as the measurement value Z(t), and the angle random walk noise of the gyroscope 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 measurement equation of the gyroscope array;

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

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

[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 of the array gyro after the first stage filtering to obtain a high-precision angular velocity estimate; define the matrix S = Q k 1 / 2 A, for the state quantity X k Transform, ξ(k) = S -1 X k (k), using the Kalman gain steady-state value in discrete form The measurement sequence Z(k) performs recursive minimum variance estimation on the transformed state quantity ξ(k). To select the vector, is the filter output, i.e., the optimal estimate of the input angular velocity of the gyroscope array at the kth sampling point.

[0015] Furthermore, the same angular velocity signal is measured in step (1), specifically, the following steps are used: a plurality of MEMS gyroscopes are divided into groups of two, and the MEMS gyroscopes in the same group are installed in opposite directions along the sensitive axis to construct an array gyroscope and perform angular velocity detection; the output ends of the two gyroscopes in the same group are respectively connected to the positive and negative input ends of the differential input A / D conversion chip, and the angular velocity measurement values ​​y1(t), y2(t), y3(t), ..., y of each group of gyroscopes in the array are obtained through A / D conversion. N (t).

[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 the random walk model is selected to perform error modeling on the MEMS gyroscope and generalized to the array form, wherein the noise part includes the angle random walk noise ARW and the rate random walk noise RRW; the random walk error model for a single MEMS gyroscope is:

[0017]

[0018] Where y(t) is the output value of the MEMS gyroscope, ω(t) is the true angular velocity, which is regarded as a random walk noise driven by the true angular velocity white noise, that is, The real angular velocity drives the white noise n ω The variance is Q ω ; b(t) is the gyro bias, driven by white noise W(t), corresponding to rate random walk (RRW), n(t) is the observation noise, corresponding to angle random walk (ARW);

[0019] Then the random walk error model in 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 can be abbreviated as:

[0022]

[0023] Where i is the gyroscope serial number, N is the number of gyroscopes, and y i is the actual output angular velocity of the i-th gyroscope, ω is the real angular velocity of the environment, and b i is the ith gyroscope bias, which is determined by the rate random walk white noise n bi Driven, the variance is n ai is the measurement noise of the i-th gyroscope, which is the angle random walk white noise, and its variance is

[0024] Furthermore, in step (3), the system state quantity is constructed by using the rate random walk and the true angular velocity, specifically:

[0025] For a single gyro in an array:

[0026]

[0027] The state equation of the system constructed in this way is:

[0028]

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

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

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

[0032]

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

[0034] Furthermore, the step (4) uses the result obtained by the discretization process to perform Kalman filter calculation on each gyroscope in the array; i,k Do recursive estimation, define the rate extraction vector e, and extract the first-level Kalman filter results of each gyroscope; specifically: use the obtained system measurement sequence Y k , the measurement matrix H k, the one-step transfer matrix Φ of the Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , the discretized covariance matrix Q of the system noise k , the discretized covariance matrix of the measurement noise R k , perform Kalman filter calculation on each gyroscope in the array according to the recursive calculation process; the specific filtering process is: the calculation system predicts the mean square error in one step and system one-step state prediction Complete the time update to get the system filter gain Then get the state estimate for the new time and the estimated mean square error P k =(IK k H k ) k / k-1 , complete the measurement update; take the new state and mean square error as the initial value of the calculation at the next moment, and i,k =[b i,k ,ω i,k ] T (i=1,2…N) to make a recursive estimate, define the rate extraction vector e=[0,1], and use eX i,k =ω i,k The first-level Kalman filter results of each gyroscope can be obtained.

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

[0036]

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

[0038] set up:

[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 can be shortened to:

[0041]

[0042] Design the second-level Kalman filter: According to the established array form random error model, construct the system state quantity:

[0043]

[0044] The state equation of the system constructed in this way is:

[0045]

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

[0047] The gyroscope data in the array after the first stage Kalman filter is used 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 gyroscope after the first-stage filtering; the angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed as follows:

[0048]

[0049] 1 of them N represents an N-dimensional column vector whose elements are all 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) and V(t) satisfy the following expressions:

[0050]

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

[0052] Furthermore, the step (6) is specifically as follows: discretizing the constructed continuous state equation and measurement equation:

[0053]

[0054] Where:

[0055]

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

[0057]

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

[0059] Furthermore, the steady-state value calculation process of the Kalman gain in step (7) is as follows: First, define a matrix of (N+1)×(N+1) Then perform eigenvalue decomposition on L, that is, L = ADA T And make its last eigenvalue 0 and It is derived from matrices A and D as follows:

[0060]

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

[0062] The steady-state value of the Kalman gain in continuous time form is expressed 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] Furthermore, the step (8) adopts the Kalman gain steady-state value based on the discrete form The constant gain Kalman filter recursive estimation process is: define the matrix S = Q 1 / 2 A, for the state quantity X k Transform, ξ(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 quantity and the high-precision angular velocity extraction after fusion are completed by the following equation:

[0068]

[0069] in is the minimum variance estimate of ξ(k), The last row of is set to 0, which corresponds to the pure integrator state; To select the vector, is the filter output, i.e., the optimal estimate of the input angular velocity of the gyroscope array at the kth sampling point.

[0070] A second aspect of the present invention: an electronic device, comprising:

[0071] One or more MEMS gyroscope arrays

[0072] One or more processors are used to read and process the 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 high-precision data fusion method for array MEMS gyroscopes.

[0075] The present invention includes the following beneficial effects: reducing the random error of MEMS gyroscope and significantly improving the detection accuracy of MEMS gyroscope; using the steady-state value of Kalman gain to estimate the angular velocity, avoiding the calculation of gain and mean square error at each step, optimizing the complexity and amount of operation of the processing process, and being more conducive to the real-time processing of the gyroscope array. The present invention constructs a gyroscope array and performs data fusion. Before the multi-sensor data enters the fusion center, the Kalman filter is used to perform local optimal estimation on each sensor, obtain the first-level estimation value of a single gyroscope, and then the processed information is sent to the second-level fusion center for processing, and the final high-precision estimation value is obtained to reduce the random error of MEMS and significantly improve the detection accuracy.

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

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

[0078] Figure 1 This is a flow chart of a high-precision data fusion method of a distributed multi-level array MEMS gyroscope based on an improved Kalman filter of the present invention;

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

[0080] Figure 3 It is a block diagram of a discrete constant gain Kalman filter of the present invention;

[0081] Figure 4 This is a test result diagram of the effect comparison of the fusion method proposed in the present invention (gyroscope static output data comparison);

[0082] Figure 5 This is a comparison test result diagram of the fusion method proposed in the present invention (comparison of gyro Allan variance analysis results). DETAILED DESCRIPTION

[0083] Here, exemplary embodiments are described in detail, and examples thereof are shown in the accompanying drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application.

[0084] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0085] The technical solution of the present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0086] like Figure 1 The present invention provides a high-precision data fusion method for an array MEMS gyroscope to reduce the random error of the MEMS gyroscope and obtain a high-precision angular velocity value. The specific steps are as follows:

[0087] Step 1: Usually, MEMS gyroscopes produced in the same batch have similar zero bias instability due to reasons such as materials and production processes. Therefore, several MEMS gyroscopes are grouped in pairs, and the MEMS gyroscopes in the same group are installed in opposite directions along the sensitive axis to form a gyroscope array for angular velocity detection to improve the output signal-to-noise ratio. The output ends of the two gyroscopes in the same group are respectively connected to the positive and negative input ends of the differential input A / D conversion chip, and the angular velocity measurement values ​​y1(t), y2(t), y3(t), ..., y of each gyroscope in the array are obtained through A / D conversion. N (t) In one embodiment of the present invention, eight MEMS gyroscopes are selected on both sides to form a MEMS gyroscope array.

[0088] Step 2: Continuously sample the array gyroscope at a certain sampling frequency for a period of time (more than 2 hours) in a horizontal static state, repeat the process to obtain array sampling data, pre-process the sampling data according to the 3σ criterion, and remove outliers in the data. Use the sampling data to perform Allan variance analysis on each MEMS gyroscope in the array, draw a double logarithmic curve of the Allan standard deviation (σ(τ)) and the correlation time (τ), and use the least squares method to fit and calculate the random walk noise n of each gyroscope angle. ai Variance and rate random walk noise n bi Variance The variance of the angle random walk noise and the variance of the rate random walk noise obtained by Allan variance analysis of multiple groups of data are averaged as the final parameters used.

[0089] Step 3: Use the random walk model to model the error of the angular velocity measurement:

[0090]

[0091] Where y(t) is the output value of the MEMS gyroscope, ω(t) is the true angular velocity, and the true value should be 0 in the static state. However, due to the influence of factors such as the external environment, it is actually impossible to be 0. It is regarded as a random walk noise driven by the true angular velocity white noise, that is, The real angular velocity drives the white noise n ω The variance of is. b(t) is the gyro 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-level Kalman filter design. Based on the random walk error model established in step 3, the system state quantity is constructed using rate random walk and true angular velocity:

[0093] For a single gyro in an array:

[0094]

[0095] The state equation of the system constructed in this way is:

[0096]

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

[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] The system noise covariance matrix The measurement noise covariance matrix r = [Q ai ].

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

[0103]

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

[0105]

[0106] And the discretized covariance matrix of the system noise is obtained as The discretized covariance matrix of the measurement noise R k =[Q ai (k)],Q bi (k) is the variance of the random walk noise of the ith gyro rate, Q ω (k) is the covariance matrix of the real angular velocity driving white noise, Q ai (k) is the random walk noise variance of the i-th gyroscope angle.

[0107] Step 6: Use the system measurement sequence Y obtained in step 5 k , the measurement matrix H k , the one-step transfer matrix Φ of the Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , the discretized covariance matrix Q of the system noise k , the discretized covariance matrix of the measurement noise R k ,according to Figure 2 The recursive calculation process shown in the figure performs Kalman filter calculation on each gyroscope in the array. The specific filtering process is: the calculation system predicts the mean square error in one step and system one-step state prediction Complete the time update to get the system filter gain Then get the state estimate for the new time and the estimated mean square error P k =(IK k H k ) k / k-1 , complete the measurement update. Take the new state and mean square error as the initial value of the calculation at the next moment, and i,k =[b i,k ,ω i,k ] T (i=1,2…N) to make a recursive estimate, define the rate extraction vector e=[0,1], and use eX i,k =ω i,kThe first-level Kalman filter results of each gyroscope can be obtained.

[0108] Step 7: Extend the random walk error model derived in step 3 to the array form:

[0109]

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

[0111] set up:

[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 formula (8) can be simplified as:

[0114]

[0115] Step 8: Second-level Kalman filter design. Based on the array form random error model established in step 7, construct the system state quantity:

[0116]

[0117] The state equation of the system constructed in this way is:

[0118]

[0119] Where F(t) = 0·I N+1 , G(t)=I N+1 , I N is the N-dimensional identity matrix.

[0120] The gyroscope data in the array after the first stage Kalman filter is used 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 gyroscope after the first stage filtering. The angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed as follows:

[0121]

[0122] 1 of them N represents an N-dimensional column vector whose elements are all 1. Parameter W(t) = [n b1 ,n b2 ,…,n bN ,n ω ] T ,V(t)=[n a1 ,n a2 ,…,n aN ] T . W(t) and V(t) satisfy:

[0123]

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

[0125] Step 9: As in step 5, discretize the constructed continuous state equation and measurement equation:

[0126]

[0127] Where:

[0128]

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

[0130]

[0131] The discretized covariance matrix of the system noise is: The discretized covariance matrix of the measurement noise R k =[Q a (k)],Q b (k) is the rate random walk noise vector n b The covariance matrix of , the diagonal elements are the rate random walk noise n calculated in step 2 bi Variance Q ω (k) is the covariance matrix of the real angular velocity driving white noise, Q a (k) is the angle random walk noise vector n a The covariance matrix of , the diagonal elements are the angle random walk noise n calculated in step 2 ai Variance

[0132] Step 10: In the second-stage Kalman filter, the steady-state value of the Kalman gain is used instead of the gain value updated in real time at each step. If the state space model is a steady-state system, the filter gain K after the Kalman filter is stabilized is k→∞ 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 estimated mean square error P is

[0133] P=(IK ∞ H)ΦPΦ T +ΓQΓ T

[0134] =(IK ∞ H)ΦPΦ T +Q(17)

[0135] The steady-state value of the Kalman gain is K ∞ =PH T R -1 , Substituting into formula (17), we get:

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

[0137]

[0138] According to the analytical solution of the Riccati differential equation (RDE), the steady-state value of the estimated mean square error P is calculated by equation (19), and then the steady-state value of the Kalman gain is derived. The calculation of the steady-state value of the Kalman gain in actual implementation is as follows: First, define the matrix (N+1)×(N+1) Then perform eigenvalue decomposition on L, that is, L = ADA T And make its last eigenvalue 0, then and Take it out from A and D as follows:

[0139]

[0140] Where v is the column vector of the last column, is an N×N matrix, In practical cases when finite precision arithmetic is used, the smallest eigenvalue is never exactly zero, in which case the smallest eigenvalue is used to represent zero for the purpose of eigenvalue sorting.

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

[0142]

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

[0144]

[0145] Where T s is the discretization period.

[0146] Step 11: Use the discrete form of the Kalman filter gain steady-state value derived in step 9 Instead of the gain value updated at each step. Press Figure 3 The discrete constant gain Kalman filter block diagram shown in the figure performs data fusion on the array gyro 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 the matrix S = Q 1 / 2 A, for the state quantity X k Transform, ξ(k) = S -1 X k (k), X k =[b1(k),b2(k),…,b N (k),ω(k)] T , which 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 quantity and the high-precision angular velocity extraction after fusion are completed by the following equation:

[0147]

[0148] in is the minimum variance estimate of ξ(k), The last row of (corresponding to the pure integrator state) must be set to exactly 0. To select the vector, is the filter output, i.e., the optimal estimate of the input angular velocity of the gyroscope array at the kth sampling point.

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

[0150] After the array MEMS gyroscope data is fused using the method proposed in the present invention, the angle random walk error of the fused virtual gyroscope data is reduced to Bias instability reduced to 0.068361° / h, rate random walk reduced to 0.030816° / h 3 / 2 , the bias stability is reduced to 0.1052° / h, and the standard deviation is reduced to 5.5847×10 -5 ° / s.

[0151] In order to further evaluate the effect of the distributed multi-level array MEMS gyroscope multi-sensor information fusion method based on improved Kalman filtering proposed in the present invention, the method proposed in the present invention is tested and compared with the centralized single-layer basic Kalman filtering and the centralized single-layer constant gain Kalman filtering using the same set of test data. Figure 4 , Figure 5 As shown, the method proposed in the present invention can significantly reduce the random error of the MEMS gyroscope and improve the detection accuracy of the MEMS gyroscope.

[0152] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the contents disclosed herein. The present application is intended to cover any variations, uses or adaptations of the present application, which follow the general principles of the present application and include common knowledge or customary technical means in the art that are not disclosed in the present application.

[0153] It should be understood that the present application is not limited to the exact construction that has been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof.

Claims

1. A high-precision data fusion method for array MEMS gyroscopes, characterized in that: The following steps are involved: (1) A gyroscope array is formed by combining several MEMS gyroscopes of the same model with parallel sensitive axes to measure the same angular velocity signal; (2) The array gyroscope is placed in a horizontal static state and continuously sampled at a certain sampling frequency to repeatedly obtain multiple sets of sampled data. The sampled data is used to perform Allan variance analysis on each MEMS gyroscope in the array to calculate the random walk noise n of each gyroscope angle. ai Variance and rate random walk noise n bi Variance The variance of the angle random walk noise and the variance of the rate random walk noise obtained by Allan variance analysis of multiple groups of data are averaged as the final parameters; (3) Using the random walk error model to model the random walk error of the angular velocity measurement value; The first-level Kalman filter design is carried out, that is, based on 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) Discretize the continuous state equation and measurement equation constructed in step (3) to obtain the one-step transfer matrix Φ of the discrete Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , measurement sequence Y k , the measurement matrix H k And the discretized covariance matrix of the system noise is Q k , the discretized covariance matrix of the measurement noise R k ; Use the results obtained from the discretization process to perform Kalman filter calculations on each gyro in the array; i,k Perform recursive estimation, define the rate extraction vector e, and extract the first-level Kalman filter results of each gyroscope; (5) The random walk error modeling derived in step (3) is extended to the array form; that is, the random walk error model of the MEMS gyroscope in array form is obtained, and the rate random walk and true angular velocity of the gyroscope array are used to construct the system state quantity and establish the state equation; the data of each gyroscope in the array after the first-level Kalman filter is used as the measurement value Z(t), and the angle random walk noise of the gyroscope 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 measurement equation of the gyroscope array; (6) Discretize the constructed continuous state equation and measurement equation to obtain the one-step transfer matrix Φ of the second-stage Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , measurement sequence Z k , the measurement matrix H k And the discretized covariance matrix of the system noise is Q k , the discretized covariance matrix of the measurement noise R k ; (7) The steady-state value of the Kalman gain is derived to replace the gain value updated in real time at each step. If the state space model is a steady-state system, the filter gain K after the Kalman filter is stabilized is k→∞ is also a constant value; first define the matrix (N+1)×(N+1) Then perform eigenvalue decomposition on L, that is, L = ADA T And make its last eigenvalue 0, and Obtained by A and D processing; Calculate the steady-state value K of the Kalman gain in continuous time ∞ , after discretization, calculate the steady-state value of the discrete Kalman gain (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 of the array gyro after the first stage filtering to obtain a high-precision angular velocity estimate; define the matrix S = Q k 1 / 2 A, for the state quantity X k Transform, ξ(k) = S -1 X k (k), using the Kalman gain steady-state value in discrete form The measurement sequence Z(k) performs recursive minimum variance estimation on the transformed state quantity ξ(k). To select the vector, is the filter output, i.e., the optimal estimate of the input angular velocity of the gyroscope array at the kth sampling point.

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

3. The high-precision data fusion method of 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 the MEMS gyroscope. The random walk model is selected to perform error modeling on the MEMS gyroscope and generalized to the array form, wherein the noise part includes the angle random walk noise ARW and the rate random walk noise RRW; the random walk error model for a single MEMS gyroscope is: Where y(t) is the output value of the MEMS gyroscope, ω(t) is the true angular velocity, which is regarded as a random walk noise driven by the true angular velocity white noise, that is, The real angular velocity drives the white noise n ω The variance is Q ω ; b(t) is the gyro bias, driven by white noise W(t), corresponding to rate random walk (RRW), n(t) is the observation noise, corresponding to angle random walk (ARW); Then the random walk error model in array form is: 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 can be abbreviated as: Where i is the gyroscope serial number, N is the number of gyroscopes, and y i is the actual output angular velocity of the i-th gyroscope, ω is the real angular velocity of the environment, and b i is the ith gyroscope bias, which is determined by the rate random walk white noise n bi Driven, the variance is n ai is the measurement noise of the i-th gyroscope, which is the angle random walk white noise, and its variance is 4. The high-precision data fusion method of array MEMS gyroscope according to claim 1, characterized in that: In step (3), the system state quantity is constructed by using the rate random walk and the true angular velocity, which is specifically: For a single gyro in an array: The state equation of the system constructed in this way is: The actual output value of each gyroscope is regarded as the measured value Y(t) = Y i (t)(i=1,2,…,N), the angular velocity random walk noise is regarded as the measurement noise, and the measurement equation can be constructed as follows: Y(t)=H(t)X(t)+V(t)=[1 1]·X i (t)+n ai ; Parameter W(t) = [n bi ,n ω ] T ,V(t)=[n ai ]; According to Kalman filter theory, W(t) and V(t) satisfy: The system noise covariance matrix The measurement noise covariance matrix r = [Q ai ].

5. The high-precision data fusion method of array MEMS gyroscope according to claim 1, characterized in that: The step (4) uses the result obtained by the discretization process to perform Kalman filter calculation on each gyroscope in the array; i,k Do recursive estimation, define the rate extraction vector e, and extract the first-level Kalman filter results of each gyroscope; specifically: use the obtained system measurement sequence Y k , the measurement matrix H k , the one-step transfer matrix Φ of the Kalman filter k,k-1 , the system noise driving matrix Γ k-1 , the discretized covariance matrix Q of the system noise k , the discretized covariance matrix of the measurement noise R k , perform Kalman filter calculation on each gyroscope in the array according to the recursive calculation process; The specific filtering process is: Calculate the system one-step prediction mean square error and system one-step state prediction Complete the time update to get the system filter gain Then get the state estimate for the new time and the estimated mean square error P k =(IK k H k ) k / k-1 , complete the measurement update; take the new state and mean square error as the initial value of the calculation at the next moment, and i,k =[b i,k ,ω i,k ] T (i=1,2…N) to make a recursive estimate, define the rate extraction vector e=[0,1], and use eX i,k =ω i,k The first-level Kalman filter results of each gyroscope can be obtained.

6. The high-precision data fusion method of array MEMS gyroscope according to claim 3, characterized in that: Step (5) is to generalize the random walk error modeling to the array form: Where i is the gyroscope serial number, N is the number of gyroscopes, and y i is the actual output angular velocity of the i-th gyroscope, ω is the real angular velocity of the environment, and b i is the ith gyroscope bias, which is determined by the rate random walk white noise n bi Driven, the variance is n ai is the i-th gyroscope measurement noise, which is expressed as angle random walk white noise with a variance of set up: 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 abbreviated as: Design the second-level Kalman filter: According to the established array form random error model, construct the system state quantity: The state equation of the system constructed in this way is: Where F(t) = 0·I N+1 , G(t)=I N+1 , I N is the N-dimensional identity matrix; The gyroscope data in the array after the first stage Kalman filter is used 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 gyroscope after the first-stage filtering; the angle random walk noise is regarded as the measurement noise, and the measurement equation is constructed as follows: 1 of them N represents an N-dimensional column vector whose elements are all 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) and V(t) satisfy the following expressions: The system noise covariance matrix The measurement noise covariance matrix R = [Q a ].

7. The high-precision data fusion method of array MEMS gyroscope according to claim 1, characterized in that: The step (6) is specifically: discretizing the constructed continuous state equation and measurement equation: Where: T is the discretization period, and W k , V k satisfy: The discretized covariance matrix of the system noise is: The discretized covariance matrix of the measurement noise R k =[Q a (k)],Q b (k) is the rate random walk noise vector n b The covariance matrix of bi Variance Q ω (k) is the covariance matrix of the real angular velocity driving white noise, Q a (k) is the angle random walk noise vector n a The covariance matrix of , the diagonal elements are the angle random walk noise n calculated in step 2 ai Variance 8. The high-precision data fusion method of array MEMS gyroscope according to claim 1, characterized in that: The steady-state value calculation process of the Kalman gain in step (7) is as follows: First, define the matrix (N+1)×(N+1) Then perform eigenvalue decomposition on L, that is, L = ADA T And make its last eigenvalue 0 and It is derived from matrices A and D as follows: Where v is the column vector of the last column, is an N×N matrix, is a (N+1)×(N) matrix; The steady-state value of the Kalman gain in continuous time form is expressed as: The steady-state value of the Kalman gain in discrete form is: Where T s is the discretization period.

9. The high-precision data fusion method of array MEMS gyroscope according to claim 1, characterized in that: In step (8), the Kalman gain steady-state value based on the discrete form is adopted The constant gain Kalman filter recursive estimation process is: define the matrix S = Q 1 / 2 A, for the state quantity X k Transform, ξ(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 quantity and the high-precision angular velocity extraction after fusion are completed by the following equation: in is the minimum variance estimate of ξ(k), The last row of is set to 0, which corresponds to the pure integrator state; To select the vector, is the filter output, i.e., the optimal estimate of the input angular velocity of the gyroscope array at the kth sampling point.

10. An electronic device, characterized in that: include: One or more MEMS gyroscope arrays One or more processors are used to read and process the MEMS gyro array data; A 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 high-precision data fusion method for array MEMS gyroscopes as described in any one of claims 1-8.

Images