Angular rate estimation method and system based on multi-gyroscope signal set membership sequential fusion

Abstract

The invention discloses an angular rate estimation method and system based on multi-gyroscope signal set membership sequential fusion, belongs to the technical field of navigation, and is used for solving the problem that the existing angular rate estimation method cannot be used in a scene with high real-time requirement, such as navigation. The angular rate estimation method based on multi-gyroscope signal set membership sequential fusion comprises the following steps: step 1, obtaining measurement information of N MEMS gyroscopes through measurement receiving equipment; step 2, based on an MEMS gyroscope measurement model, establishing a multi-gyroscope fusion model through a fusion center, and setting an initial state of the multi-gyroscope fusion model based on measurement information in the step 1; and step 3, determining an update model of a time update feasible set and a measurement update feasible set of the multi-gyroscope fusion model. According to the angular rate estimation method based on multi-gyroscope signal set membership sequential fusion, the problem that convex optimization is involved in the existing multi-MEMS gyroscope signal fusion process is avoided, the calculation complexity is optimized, and the angular rate estimation method can be used in navigation scenes with real-time requirements.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to an angular rate estimation method and system based on the sequential fusion of multiple gyroscope signals. Background Art

[0002] Among angular rate measurement devices, MEMS gyroscopes (MEMS) offer numerous advantages, including small size, low cost, and ease of integration. Consequently, they have experienced rapid growth in both civilian and military applications in recent years. However, compared to traditional mechanical and optical gyroscopes, their accuracy is relatively low, limiting their application in aerospace, precision-guided weapons, and other fields. Consequently, research groups both domestically and internationally are striving to improve the accuracy of MEMS gyroscopes. Multi-gyroscope redundancy technology can maintain the advantages of MEMS gyroscopes while effectively improving their accuracy in a short period of time without significantly increasing costs. This approach utilizes multiple gyroscopes to simultaneously detect the same angular rate signal. Fusion techniques are then used to combine the outputs from these gyroscopes, resulting in an angular rate estimate superior to that of a single gyroscope. Numerous researchers have demonstrated the significant advantages of this approach in improving MEMS gyroscope performance.

[0003] The core of multi-gyro redundancy technology lies in a multi-sensor fusion estimation algorithm, which typically fuses its outputs using the Kalman filter (KF) and its related extended algorithms. However, these traditional estimation methods often impose strict requirements on the noise distribution. In practical applications, the statistical characteristics of gyro noise are often quite complex and change in real time, making it difficult to satisfy probability distribution assumptions or determine the statistical characteristics. This can lead to estimation bias. In particular, the Kalman filter's high dependence on statistical characteristics and high sensitivity can amplify bias and cause estimator instability.

[0004] In recent years, fusion methods and their applications based on set membership estimation have gained increasing attention. This approach overcomes the shortcomings of traditional state estimation methods by requiring only bounded noise, without specifying its specific distribution within the bounds or knowing its statistical properties. The RCC-OBE algorithm, based on set membership estimation theory, has been successfully used for multi-MEMS gyroscope signal fusion. However, this method involves a convex optimization problem and has high computational complexity, making it unsuitable for real-time applications such as navigation. Summary of the Invention

[0005] The purpose of the present invention is to overcome the problems in the prior art and provide an angular rate estimation method and system based on the sequential fusion of multiple gyroscope signals. The method avoids the problems of convex optimization involved in the existing fusion process of multiple MEMS gyroscope signals, optimizes the computational complexity, and can be used in navigation scenarios with real-time requirements. While ensuring the fusion accuracy, the computational efficiency is improved by reducing the measurement dimension.

[0006] A method for estimating angular rate based on sequential fusion of multiple gyroscope signals comprises the following steps:

[0007] Step 1: Obtain measurement information of N MEMS gyroscopes through a measurement receiving device;

[0008] Step 2: Based on the MEMS gyroscope measurement model, a multi-gyroscope fusion model is established through the fusion center, and the initial state of the multi-gyroscope fusion model is set based on the measurement information in step 1;

[0009] Step 3: Determine the update model of the time update feasible set and the measurement update feasible set of the multi-gyro fusion model;

[0010] Step 4: At the fusion center, the state of the multi-gyroscope fusion model is updated in time and measurement to achieve collective sequential fusion of the measurement information of N MEMS gyroscopes, so that the initial state is updated to the target state, and the target state is estimated to obtain the angular rate estimate of the target state.

[0011] Preferably, step 2 specifically includes the following steps:

[0012] Step 2.1: Fuse the measurement information of N MEMS gyroscopes. The discrete equation of the output of the i-th MEMS gyroscope is expressed as:

[0013]

[0014] Where, the subscript k represents the kth moment, ω(k) is the true angular rate, and b i (k) is the noise w k-1 The angular rate of the drive is a random walk, and a(k) is the angular acceleration; is the state transfer matrix, T is the sampling period of the MEMS gyroscope, and I6 is the 6th-order unit matrix; is the process noise matrix; z i,k is the 1D measurement value of the i-th MEMS gyroscope at time k; v i,k is the measurement noise of the i-th MEMS gyroscope at time k; x k =[b1(k),b2(k),…,b N (k),ω(k),a(k)] represents the state vector; H i,k is the measurement matrix of the i-th MEMS gyroscope, w k-1 =[w1(k-1),w2(k-1),…,w N (k-1),n ω (k-1), j(k-1)] T , is the process noise, n ω(k-1) is the angular random walk, j(k-1) is the angular acceleration;

[0015] Process noise w k-1 and measurement noise v i,k Assume that the distribution is unknown but bounded:

[0016]

[0017] Among them, Q k-1 is a known positive definite matrix, representing the k-time process noise set shape matrix, Represents Q k-1 The inverse matrix, R i,k > 0, indicating the squared boundary of the measurement noise of the i-th gyroscope at time k; Indicates w k-1 The transpose of W k-1 represents the set of process noise; V i,k represents the set of measurement noise.

[0018] Step 2.2: The initial state of the multi-gyro fusion model is set to belong to the following ellipsoid set E0:

[0019]

[0020] in is the center of the ellipsoid, P0 is the shape matrix that defines the initial ellipsoid, and x0 represents the initial state.

[0021] Preferably, the step 3 specifically includes the following steps:

[0022] Step 3.1, determine the time to update the ellipsoid set;

[0023] The goal of time update is to get the time update ellipsoid set in It means that the k-time prediction ellipsoid set is obtained by time update using the information before k-1 time, and the center of the ellipsoid set is The shape matrix is ​​σ k|k-1 P k|k-1 , σ k|k-1 is the upper bound of the system's Lyapunov function, and P k|k-1 Together they determine the shape of the state prediction ellipsoid at time k; G k-1 is the process noise matrix;

[0024] Step 3.2: Determine the feasible state set of multi-sensor measurement information;

[0025] According to formula (1) and (3), we can calculate x k Belongs to the following set:

[0026] Xk ={x k :(z i,k -H i,k x k ) 2 ≤R i,k ,i=1,2,…N} (5)

[0027] Among them, x k represents the state vector; z i,k is the 1-dimensional measurement value of the i-th MEMS gyroscope at time k; H i,k is the measurement matrix of the i-th MEMS gyroscope;

[0028] Step 3.3, determine the ellipsoid set in the measurement update phase of the fusion center;

[0029] Update the ellipsoid set E through time k|k-1 and X in formula (5) k Intersection obtains:

[0030]

[0031] in,

[0032] Preferably, the step 4 specifically includes the following steps:

[0033] Step 4.1: When the state at time k-1 is given hour, Represents the k-1 time state ellipsoid set, the center of the ellipsoid set is The shape matrix is ​​σ k-1 P k-1 , σ k-1 is the upper bound of the system's Lyapunov function, and P k-1 Together they determine the shape of the state prediction ellipsoid at time k-1.

[0034] Step 4.2, time update:

[0035]

[0036] σ k|k-1 =σ k-1 (7)

[0037]

[0038] Among them, the parameter p k ∈(0,+∞) is used to optimize the ellipsoid The size of the parameter is optimized using the minimum trace criterion, and p is obtained. k The expression for the optimal value is as follows:

[0039]

[0040] F k-1 is the state transfer matrix, Q k-1 is a known positive definite matrix, representing the k-time process noise set shape matrix, G k-1 is the process noise matrix;

[0041] Step 4.3, measurement update is:

[0042] make P k,0 =P k|k-1 ,σ k,0 =σ k|k-1 ;

[0043] For i∈{1,2,…N}, perform sequential recursion as follows:

[0044]

[0045] in, Parameter q k,i ∈[0,+∞) is used to optimize the ellipsoid set The size of q k,i The optimal value of is obtained by solving the following equation:

[0046]

[0047] when hour, is the optimal value;

[0048] when When Eq. (13) has no solution, we take q k,i =0 is the optimal value, and the result of this iteration is equal to the result of the time update phase, indicating that no measurement update is performed in this round;

[0049] Step 4.4, take P k =P k,N ,σ k =σ k,N , we get the ellipsoid set Complete the measurement update process, for the fusion center;

[0050] Step 4.5, based on Fusion Center That is the state x in formula (1) k The estimated value of is the result of the multi-gyroscope set sequential fusion at the current state, where This is the estimated angular rate at the current moment.

[0051] An angular rate estimation system based on the sequential fusion of multi-gyroscope signal sets to implement the angular rate estimation method includes a measurement module, a model construction module, and a target state estimation module; the measurement module is used to measure the measurement information of N MEMS gyroscopes; the model construction module is used to construct a multi-gyroscope fusion model and obtain an updated model of the multi-gyroscope fusion model; the target state estimation module is used to obtain the fused measurement information of the N MEMS gyroscopes, perform time updates and measurement updates at the fusion center, estimate the target state, and obtain the angular velocity estimate value at the current state.

[0052] Compared with the prior art, the present invention has the following beneficial effects:

[0053] The present invention provides an angular rate estimation method based on the sequential fusion of multiple gyroscope signals, which avoids the problem of convex optimization involved in the existing fusion process of multiple MEMS gyroscope signals, optimizes the computational complexity, and can be used in navigation scenarios with real-time requirements. It improves computational efficiency by reducing the measurement dimension while ensuring fusion accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 The multi-MEMS gyroscope fusion test device of the present invention;

[0055] Figure 2 is a schematic diagram of a temperature-controlled turntable of the present invention;

[0056] Figure 3 is the angular rate output by a single MEMS gyroscope of the present invention;

[0057] Figure 4 is the output error of the angular rate output by a single MEMS gyroscope of the present invention;

[0058] Figure 5 The angular rate is the fusion output of multiple MEMS gyroscopes of the present invention;

[0059] Figure 6 for Figure 5 A partial magnified view of the middle trough;

[0060] Figure 7 It is the output error of the angular rate output by the fusion of multiple MEMS gyroscopes of the present invention. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical solution and advantages of the embodiment of the present invention clearer, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all the embodiments. Based on the described embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention. Figure 1 -Attached Figure 7 , describe this plan in detail.

[0062] Example 1:

[0063] Example 1 provides an angular rate estimation method based on the sequential fusion of multi-gyroscope signal sets, which improves the computational efficiency by reducing the measurement dimension while ensuring the fusion accuracy. The angular rate estimation method based on the sequential fusion of multi-gyroscope signal sets is introduced below.

[0064] 1. Obtain measurement information of N MEMS gyroscopes through the measurement receiving device;

[0065] 2. Establish a multi-gyro fusion model;

[0066] First, based on the MEMS gyroscope measurement model, a multi-gyroscope fusion model is established through the fusion center; the true angular rate ω of the MEMS gyroscope is represented by the noise n ω The random walk driven by the gyroscope is performed; the measurement information of N MEMS gyroscopes is fused, and the discrete equation of the output of the i-th MEMS gyroscope can usually be expressed as:

[0067]

[0068] Where, the subscript k represents the kth moment, ω(k) is the true angular rate, and b i (k) is the noise w k-1 The angular rate of the drive is a random walk, and a(k) is the angular acceleration; is the state transfer matrix, T is the sampling period of the MEMS gyroscope, and I6 is the 6th-order unit matrix; is the process noise matrix, I6 is the 6th order unit matrix; z i,k is the 1D measurement value of the i-th MEMS gyroscope at time k; v i,k is the measurement noise of the i-th MEMS gyroscope at time k; x k =[b1(k),b2(k),…,b N (k),ω(k),a(k)] represents the state vector; H i,k is the measurement matrix of the i-th MEMS gyroscope; w k-1=[w1(k-1),w2(k-1),…,w N (k-1),n ω (k-1), j(k-1)] T , is the process noise, n ω (k-1) is the angular random walk, j(k-1) is the angular acceleration;

[0069] Process noise w k-1 and measurement noise v i,k Assume that the distribution is unknown but bounded:

[0070]

[0071] Among them, Q k-1 is a known positive definite matrix, representing the k-time process noise set shape matrix, Indicates Q k-1 The inverse matrix, R i,k > 0, indicating the squared boundary of the measurement noise of the i-th gyroscope at time k; Indicates w k-1 The transpose of W k-1 represents the set of process noise; V i,k represents the set of measurement noise.

[0072] Accordingly, the initial state of the multi-gyro fusion model is set to belong to the following ellipsoid set E0:

[0073]

[0074] in is the center of the ellipsoid, P0 is the shape matrix that defines the initial ellipsoid, and x0 represents the initial state.

[0075] 3. Member Sequential Fusion

[0076] The measurement information of N MEMS gyroscopes is fused. At time k, the measurement information of N MEMS gyroscopes is obtained by fusion. Then, at the fusion center, the initial state of the multi-gyroscope fusion model is updated in time and measurement, so that the initial state is updated to the target state, thereby estimating the target state.

[0077] The goal of the time update is to find the ellipsoid in It means that the k-time prediction ellipsoid set is obtained by time update using the information before k-1 time, and the center of the ellipsoid set is The shape matrix is ​​σ k|k-1 P k|k-1 , σ k|k-1 As the upper bound of the system's Lyapunov function, and P k|k-1Together they determine the shape of the state prediction ellipsoid at time k; x is calculated according to equations (1) and (3): k Belongs to the following set:

[0078] X k ={x k :(z i,k -H i,k x k ) 2 ≤R i,k ,i=1,2,…N} (5)

[0079] Among them, x k represents the state vector; z i,k is the 1-dimensional measurement value of the i-th MEMS gyroscope at time k; H i,k is the measurement matrix of the i-th MEMS gyroscope.

[0080] Therefore, the goal of the fusion update phase of the centralized fusion center is to find the feasible set bounding ellipsoid set The ellipsoid set contains the intersection of the time-updated ellipsoid set and the state feasible set determined by multi-sensor measurement information, that is, the target is

[0081] The main idea of ​​the sequential filtering algorithm is that after executing the time update process at the current moment, the fusion center sequentially uses the measurement values ​​of each sensor to update the state estimate at the current moment, and finally obtains a fusion estimate based on global measurement information.

[0082] In the set membership framework, sequential filter fusion is a process of sequentially intersecting the time-updated ellipsoid set with the sensor measurement ellipsoid set. hour, Represents the k-1 time state ellipsoid set, the center of the ellipsoid set is The shape matrix is ​​σ k-1 P k-1 ,σ k-1 is the upper bound of the system's Lyapunov function, and P k-1 Together they determine the shape of the state prediction ellipsoid at time k-1. The update process of sequential filter fusion at time k-1 can be described as follows:

[0083] 1) Time update:

[0084]

[0085] σ k|k-1 =σ k-1 (7)

[0086]

[0087] Among them, the parameter pk ∈(0,+∞) is used to optimize the ellipsoid The size of the minimum trace criterion is used for parameter optimization, and p is obtained. k The expression for the optimal value is as follows:

[0088]

[0089] F k-1 is the state transfer matrix, Q k-1 is a known positive definite matrix, representing the k-time process noise set shape matrix, G k-1 is the process noise matrix;

[0090] 2) Measurement update: First, let P k,0 =P k|k-1 ,σ k,0 =σ k|k-1 ;

[0091] For i∈{1,2,…N}, perform sequential recursion as follows:

[0092]

[0093] in, Parameter q k,i ∈[0,+∞) is used to optimize the ellipsoid set The size of q k,i The optimal value of is obtained by solving the following equation:

[0094]

[0095] when hour, is the optimal value;

[0096] Note that When Equation (13) has no solution, we take q k,i =0 is the optimal value; the result of this iteration is equal to the result of the time update phase, which also means that no measurement update is performed in this round; this selective update mechanism is conducive to the reasonable use of measurement information and reduces the amount of calculation.

[0097] The derivation process of formulas (10), (11) and (12) is as follows:

[0098] When the i-1th measurement is given, the ellipsoid set is updated When , the i-th measurement updates the ellipsoid set Conforms to the following formula:

[0099]

[0100] From formula (14), we can further obtain: The elements in

[0101]

[0102] Pick The above formula can be transformed into

[0103]

[0104] Comparing with formula (14), we can get:

[0105]

[0106] Finally, take P k =P k,N ,σ k =σ k,N , we get the ellipsoid set Complete the measurement update process.

[0107] 4. When applying

[0108] Fusion Center As the result of the sequential fusion of multiple gyroscopes at the current state, This is the estimated angular rate at the current moment.

[0109] 5. Experimental analysis

[0110] The experiment adopts a 6-gyro solution: 6 gyro chips ADXRS300 are welded on a circuit board, and the corresponding functional circuits are designed and equipped. Then a data acquisition system is built to collect the output signals of the 6 gyros at the same time and fuse them. The multi-MEMS gyro fusion test device built in the experiment is as follows: Figure 1 As shown in the figure, the bandwidth of a single MEMS gyroscope is set to 40 Hz, so in order to satisfy the Nyquist theorem, the sampling frequency is set to 200 Hz.

[0111] During the test, place the test device on a horizontal temperature-controlled turntable. Figure 2 shown.

[0112] The temperature is set to a constant 25°C. After the gyro is powered on, it runs for 30 minutes. During this period, the turntable remains stationary. Then, data collection begins. The turntable parameters are set so that it performs a swing motion with an amplitude of 10° and a period of 2s. Therefore, the input rate of the MEMS gyroscope is ω = 10πsin(πt)° / s. Then, according to the above requirements, 10s of MEMS gyroscope output data are collected. The output and output error of a single MEMS gyroscope (taking MEMS gyroscope 1 as an example) are shown as follows: Figure 3 and Figure 4 shown.

[0113] After obtaining the output data of the MEMS gyroscope array, the method described in Section 1 is used to model the MEMS gyroscope array. On this basis, the set-membership sequential fusion filtering algorithm described in Section 2 is used to fuse the output data of multiple MEMS gyroscopes. At the same time, Kalman filtering and RCC-OBE algorithm are used as comparative fusion methods. The fusion results are shown in Figure 2. Figure 5 As shown, the output error of the MEMS gyroscope after fusion is as follows Figure 6 shown.

[0114] To quantitatively analyze the performance of several methods, we used the root mean square error (RMSE) and signal-to-noise ratio (SNR) to measure their denoising effectiveness. The definitions of RMSE and SNR indicate that, after denoising the same signal, the smaller the RMSE and the larger the SNR, the better the denoising effect. Furthermore, to demonstrate the computational efficiency of this method, the runtime of the algorithm in this experiment is presented, as shown in Table 1.

[0115] Table 1

[0116]

[0117] from Figure 3-Figure 5 As can be seen from Table 1, all three experimental methods effectively improved the accuracy of the MEMS gyroscope, which first verifies the effectiveness of multi-gyroscope fusion technology. The calculation results show that the set-membership sequential filter fusion designed in this invention outperforms the other two methods in terms of both signal-to-noise ratio and root mean square error. In particular, compared with the similar RCC-OBE algorithm, although the accuracy improvement is not significant, the computing speed is greatly improved, fully meeting the real-time requirements of navigation applications.

[0118] Example 2:

[0119] Embodiment 2 is an angular rate estimation system based on the sequential fusion of multi-gyroscope signal sets to implement the angular rate estimation method described in embodiment 1, including a measurement module, a model construction module and a target state estimation module; the measurement module is used to measure the measurement information of N MEMS gyroscopes; the model construction module is used to construct a multi-gyroscope fusion model and obtain an updated model of the multi-gyroscope fusion model; the target state estimation module is used to obtain the fused measurement information of N MEMS gyroscopes, perform time updates and measurement updates at the fusion center, estimate the target state, and obtain the angular velocity estimation value at the current state.

[0120] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are merely preferred examples of the present invention and are not intended to limit the present invention. Various changes and modifications may be made to the present invention without departing from the spirit and scope of the present invention, and such changes and modifications fall within the scope of the present invention. The scope of protection claimed in the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for estimating angular rate based on the sequential fusion of multiple gyroscope signals, characterized in that: The following steps are involved: Step 1: Obtain measurement information of N MEMS gyroscopes through a measurement receiving device; Step 2: Based on the MEMS gyroscope measurement model, a multi-gyroscope fusion model is established through the fusion center, and the initial state of the multi-gyroscope fusion model is set based on the measurement information in step 1; Step 3: Determine the update model of the time update feasible set and the measurement update feasible set of the multi-gyro fusion model; Step 4: At the fusion center, the state of the multi-gyroscope fusion model is updated in time and measurement to achieve collective sequential fusion of the measurement information of N MEMS gyroscopes, so that the initial state is updated to the target state, and the target state is estimated to obtain the angular rate estimate of the target state.

2. The angular rate estimation method based on the sequential fusion of multiple gyroscope signals according to claim 1, characterized in that: Step 2 specifically includes the following steps: Step 2.1: Fuse the measurement information of N MEMS gyroscopes to obtain the discrete equation of the output of the i-th MEMS gyroscope; the process noise w k-1 and measurement noise v i,k Assume that the distribution is unknown but bounded noise; Step 2.2: The initial state of the multi-gyro fusion model is set to belong to the following ellipsoid set E0: in is the center of the ellipsoid, P0 is the shape matrix that defines the initial ellipsoid, and x0 represents the initial state.

3. The angular rate estimation method based on the sequential fusion of multiple gyroscope signals according to claim 2, characterized in that: The step 3 specifically includes the following steps: Step 3.1: Determine the time to update the ellipsoid set E according to the discrete equation k|k-1 ; Step 3.2: Based on the discrete equation and the measurement noise v i,k Determine the state feasible set x of multi-sensor measurement information k ; Step 3.3: Update the ellipsoid set E according to time k|k-1 The state feasible set x of multi-sensor measurement information k Intersection obtains: Among them, ε k The ellipsoid set in the measurement update phase for the fusion center.

4. The angular rate estimation method based on the sequential fusion of multiple gyroscope signals according to claim 3, wherein: The step 4 specifically includes the following steps: Step 4.1, according to the ellipsoid set ε k Set the state to predict the shape of the ellipsoid; Step 4.2: Determine the time to update P based on the shape of the state prediction ellipsoid k ; Step 4.3: Update P based on the shape and time of the predicted ellipsoid k Determine the measurement update q k，i ; Step 4.4: Update P according to time k and measurement update q k，i Get the ellipsoid set Complete the measurement update process, is the fusion center, σ k is the upper bound of the system’s Lyapunov function; Step 4.5: Based on the ellipsoid set Get the estimated value x of the fusion center k , which is the result of the sequential fusion of multiple gyroscopes at the current state, and the estimated angular rate at the current moment is obtained.

5. An angular rate estimation system based on the sequential fusion of multiple gyroscope signals, characterized in that: To implement the angular rate estimation method according to any one of claims 1 to 4, comprising: A measurement module, used to measure measurement information of N MEMS gyroscopes; A model building module is used to build a multi-gyro fusion model and obtain an updated model of the multi-gyro fusion model; The target state estimation module is used to obtain the fused measurement information of N MEMS gyroscopes, perform time updates and measurement updates at the fusion center, estimate the target state, and obtain the angular velocity estimate at the current state.

Images