A kalman filter-based dynamic compensation method for zero bias of Z-axis of MEMS gyroscope

Abstract

The application discloses a kind of MEMS gyroscopes Z axis zero offset dynamic compensation method based on Kalman filtering, it is related to the field of MEMS gyroscopes Z axis zero offset dynamic compensation.MEMS sensor is stationary, obtains Z axis angular velocity measurement value and respectively calculates average E (0) And covariance COV (0), with Offset=E (0) As the factory value of compensation value;In actual measurement, read new carrier attitude data from IMU;Judge whether the carrier is static, if the new carrier record data jump range is greater than threshold value, then determine the moment is dynamic, k value is unchanged, k indicates the kth successful acquisition static data, return last step;By dynamic capture Z axis zero offset and using Kalman algorithm to dynamic compensation of zero offset, obtain the compensation value of new MEMS gyroscope Z axis angular velocity Rz, update Offset.Improve the precision of gyroscope, reduce the error of gyroscope Z axis, while reducing the measurement error of heading angle YAW.

Description

[0001] This application is the following application.

[0002] The application number is: 201810743466.9

[0003] Application date: July 9, 2018

[0004] The application is titled: A Dynamic Compensation Method for Zero Bias of the Z-Axis in a MEMS Gyroscope Based on Kalman Filtering.

[0005] Application for division Technical Field

[0006] This invention relates to the field of dynamic zero-bias compensation for the Z-axis of MEMS gyroscopes, and more specifically to a dynamic zero-bias compensation method for the Z-axis of MEMS gyroscopes based on Kalman filtering. Background Technology

[0007] MEMS gyroscope-based IMU modules all suffer from zero bias. While the zero bias of the X and Y axes can be calibrated using Kalman filtering algorithms based on the accelerometer measurements of the X and Y axes, the Z axis cannot be calibrated using this method. Therefore, the Z-axis measurements of MEMS-based IMU modules on the market are generally inaccurate, resulting in significant deviations in the heading angle measurement. This deviation is largely due to the zero bias of the Z axis.

[0008] This invention improves the accuracy of MEMS gyroscopes and reduces the Z-axis error by dynamically capturing the Z-axis bias and using the Kalman algorithm to dynamically compensate for the bias, while also reducing the measurement error of the heading angle (YAW). Summary of the Invention

[0009] In view of this, the present invention provides a dynamic compensation method for zero bias of MEMS gyroscope Z-axis based on Kalman filtering, which can improve the accuracy of MEMS gyroscope, reduce the Z-axis error of gyroscope, and reduce the measurement error of heading angle YAW.

[0010] To achieve the above objectives, the present invention adopts the following technical solution:

[0011] A dynamic compensation method for zero bias in the Z-axis of a MEMS gyroscope based on Kalman filtering includes the following steps:

[0012] S1: Before the product leaves the factory, the MEMS sensor is left to stand still, 1000 Z-axis angular velocity measurements are obtained, and the average value E(0) and covariance COV(0) are calculated respectively. Offset = E(0) is used as the factory value of the compensation value.

[0013] S2: In actual testing, new carrier attitude data is read from the IMU;

[0014] S3: Determine whether the carrier is static. If the data jump range of the new carrier is greater than the threshold, then the time is determined to be dynamic. The value of k remains unchanged, where k represents the kth time that static data is successfully acquired. Return to S2.

[0015] S4: By dynamically capturing the Z-axis zero bias and using the Kalman algorithm to dynamically compensate for the zero bias, a new compensation value for the Z-axis angular velocity Rz of the MEMS gyroscope is obtained, and the Offset is updated.

[0016] Optionally, S3 also includes: if the fluctuation range of the new carrier recorded data is less than the threshold, then the moment is considered to be still static, the average value of the new carrier Rz is calculated and denoted as E(k), and its covariance is calculated and denoted as COV(k), where k represents the kth time that static data is successfully acquired. If static carrier attitude data can be successfully acquired again, then k = k + 1.

[0017] Optionally, the static judgment criteria and threshold settings specifically include:

[0018] Read the carrier attitude data from the IMU and record 1000 sets of data, including Ax, Ay, Az, and Rz, where Ax, Ay, and Az represent the acceleration along the x-axis, y-axis, and z-axis, respectively.

[0019] The thresholds are: Tax = M*[max(Ax) – min(Ax)], Tay = M*[max(Ay) – min(Ay)], Taz = M*[max(Az) – min(Az)], Trz = M*[max(Rz) – min(Rz)], where max and min correspond to the maximum and minimum values ​​of the vector, respectively. Tax, Tay, Taz, and Trz represent the x-axis acceleration threshold, y-axis acceleration threshold, z-axis acceleration threshold, and z-axis rotational speed threshold, respectively. M is the threshold scaling factor, which is set according to the working environment of the carrier.

[0020] If the measured jumps of the three-axis accelerations Ax, Ay, Az and the Z-axis rotational speed Rz of the MEMS gyroscope are all less than the threshold, it is determined to be static; otherwise, it is dynamic. Since it is difficult to ensure that the external noise signal is in a low state in practical applications, the jump range is appropriately increased.

[0021] Optionally, the calculation using the Kalman algorithm in step S4 includes the following steps:

[0022] The initial values ​​are assigned as follows: where P(0|0)=COV(0), X(0|0)=E(0), Q(k)=0.0001, Q(k) is set according to the specific situation, R(k)=COV(k), and the compensation value X(k|k) at different times k is obtained through iterative calculation.

[0023] X(k|k-1)=X(k-1|k-1);

[0024] Where k represents the determination that the k-th measurement meets the static conditions, and this is referred to as time k. At this time, X(k-1|k-1) is the optimal compensation value of Rz at time k-1, i.e., Offset, and X(k|k-1) is the estimated compensation value of Rz at time k obtained by using the optimal compensation value of Rz at time k-1.

[0025] P(k|k-1)=P(k-1|k-1)+Q(k);

[0026] P(k-1|k-1) is the covariance of the optimal compensation value of Rz at time k-1; P(k|k-1) is the covariance of the estimated compensation value at time k; and Q(k) is the variance of the estimated noise at time k.

[0027] X(k|k)=X(k|k-1)+Kg(k)(E(k)–X(k|k-1));

[0028] Kg(k)=P(k|k-1) / (P(k|k-1)+R(k));

[0029] P(k|k)=(1–Kg(k))P(k|k-1);

[0030] Offset = X(k|k);

[0031] R(k) is the variance of the measurement noise at time k, Kg(k) is the Kalman gain at time k, P(k|k) is the covariance of the optimal compensation value at time k, and X(k|k) represents the optimal compensation value of Rz at time k obtained by weighting the measured average value E(k) and the estimated value X(k|k-1) of Rz at time k through the Kalman gain value. Finally, X(k|k) is assigned to Offset to complete the update of the Rz compensation value. At this time, Offset is the final compensation value obtained.

[0032] As can be seen from the above technical solution, compared with the prior art, the present invention provides a dynamic compensation method for Z-axis zero bias of MEMS gyroscope based on Kalman filtering. By dynamically capturing the Z-axis zero bias and using the Kalman algorithm to dynamically compensate for the zero bias, the accuracy of MEMS gyroscope is improved, the Z-axis error of gyroscope is reduced, and the measurement error of heading angle YAW is reduced. Attached Figure Description

[0033] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0034] Figure 1 This is a diagram showing the heading angle values ​​measured by the MEMS gyroscope without compensation in an embodiment of the present invention.

[0035] Figure 2 This is a diagram showing the heading angle value obtained by performing simple compensation after measuring the slope using a MEMS gyroscope in an embodiment of the present invention.

[0036] Figure 3 This is a logic diagram of the compensation process in an embodiment of the present invention;

[0037] Figure 4 This is a schematic diagram illustrating the selection range of Tax, taking the A-axis acceleration Ax as an example in an embodiment of the present invention;

[0038] Figure 5 This is a comparison chart of the YAW value after compensation using the algorithm and the YAW value without compensation in an embodiment of the present invention;

[0039] Figure 6 This is a synchronous comparison diagram of the YAW value and the MTi-300AHRS value after compensation using the algorithm in this embodiment of the invention;

[0040] Figure 7 This is a graph showing the difference between the YAW value and the MTi-300AHRS after compensation using the algorithm in this embodiment of the invention.

[0041] Figure 8 This is a map of compensation values ​​updated using the Kalman filter algorithm in an embodiment of the present invention. Detailed Implementation

[0042] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] Through long-term tracking and measurement of MEMS gyroscopes, it was found that the zero bias of MEMS gyroscopes exhibits short-term stability and small long-term fluctuations. Taking one typical measurement as an example, the heading angle value measured without compensation is as follows: Figure 1 As shown.

[0044] After measuring the slope, simple compensation is performed to obtain the heading angle value, as shown below. Figure 2 As shown, by Figure 1 It can be seen that simple compensation cannot effectively solve the zero bias problem of the Z-axis.

[0045] This invention employs a dynamic adaptive Kalman compensation algorithm. By capturing static or near-static moments of the carrier, the Kalman algorithm is used to calculate and compensate for the zero bias of the Z-axis, ensuring that the compensated zero bias is always close to the true value, thus greatly improving the measurement accuracy of the MEMS gyroscope.

[0046] The logic diagram of the compensation process is as follows: Figure 3 As shown, a dynamic compensation method for zero bias of the Z-axis of a MEMS gyroscope based on Kalman filtering mainly includes:

[0047] Step S1: When the IMU is in a static state, acquire the carrier attitude data, and calculate the compensation value of the MEMS gyroscope Z-axis angular velocity Rz using the acquired carrier attitude data as the initial compensation value Offset;

[0048] Step S2: In the actual test, new carrier attitude data is read from the IMU;

[0049] Step S3: Determine whether the carrier is static;

[0050] Step S4: Calculate and obtain the compensation value of the new MEMS gyroscope Z-axis angular velocity Rz using the Kalman algorithm, update the Offset, and then jump to step S2 to form a loop.

[0051] Further, step S1 specifically includes:

[0052] When the IMU is in a static or near-static state

[0053] Initialization: Before the product leaves the factory, the MEMS sensor is left to stand still for 5 seconds to obtain the data of the carrier's z-axis rotation speed Rz. The average value of the carrier's Rz during this process is calculated and denoted as E(0). Offset = E(0) is used as the compensation value of the current MEMS gyroscope's z-axis rotation speed Rz at the time of leaving the factory. The variance of the 1000 Rz measurements is denoted as COV(0).

[0054] Further, step S3 specifically includes:

[0055] Step S31: If the new carrier attitude data fluctuation range is less than the threshold, it is considered that the moment is still static. Calculate the average value of the new carrier Rz, denoted as E(k), and calculate its covariance, denoted as COV(k). k represents the kth time that static data is successfully acquired. If static carrier attitude data can be successfully acquired again, then k = k + 1.

[0056] Step S32: If the new carrier attitude data jumps within a range greater than the threshold, then the moment is determined to be dynamic, the k value remains unchanged, and the process jumps back to step S2.

[0057] Furthermore, the static judgment criteria and threshold settings specifically include:

[0058] Read the carrier attitude data from the IMU and record 1000 sets of data, including Ax, Ay, Az, and Rz, where Ax, Ay, and Az represent the acceleration along the x-axis, y-axis, and z-axis, respectively.

[0059] The thresholds are: Tax = M*[max(Ax) – min(Ax)], Tay = M*[max(Ay) – min(Ay)], Taz = M*[max(Az) – min(Az)], Trz = M*[max(Rz) – min(Rz)], where max and min correspond to the maximum and minimum values ​​of the vector, respectively. Tax, Tay, Taz, and Trz represent the x-axis acceleration threshold, y-axis acceleration threshold, z-axis acceleration threshold, and z-axis rotational speed threshold, respectively. M is the threshold scaling factor, which can be set according to the carrier's working environment.

[0060] If the measured jumps of the three-axis accelerations Ax, Ay, and Az, and the Z-axis rotational speed Rz of the MEMS gyroscope are all less than the threshold, it is determined to be static; otherwise, it is dynamic. Since it is difficult to guarantee that external noise signals are kept to a minimum in practical applications, the jump range is appropriately increased. Taking the A-axis acceleration Ax as an example, the schematic diagram is as follows... Figure 4 As shown, M = 1.5 here.

[0061] Based on the characteristics of MEMS gyroscopes, such as short-term stability and small long-term fluctuations in bias, a univariate Kalman filter algorithm is applied for bias compensation self-updating:

[0062] For linear systems, the normal Kalman filter algorithm is as follows:

[0063] X(k) = AX(k-1) + BU(k) + W(k);

[0064] Z(k) = HX(k) + V(k);

[0065] X(k) is the system state at time k, and U(k) is the control input to the system at time k. A and B are system parameters. Z(K) is the measured value at time k, and H is the parameter of the measurement system. W(k) and V(k) represent the process noise and measurement noise, respectively, and their covariances are Q and R, respectively.

[0066] X(k|k-1)=AX(k-1|k-1)+BU(k)(1);

[0067] In equation (1), X(k|k-1) is the result predicted using the previous state, X(k-1|k-1) is the optimal result of the previous state, and U(k) is the control variable of the current state.

[0068] P(k|k-1)=AP(k-1|k-1)A'+Q(k)(2);

[0069] In equation (2), P(k|k-1) is the covariance corresponding to X(k|k-1), and P(k-1|k-1) is the covariance corresponding to X(k-1|k-1).

[0070] X(k|k)=X(k|k-1)+Kg(k)(Z(k)–HX(k|k-1))(3);

[0071] Kg=P(k|k-1)H' / (HP(k|k-1)H'+R(k))(4);

[0072] Where Kg is the Kalman gain, and X(k|k) is the Kalman filtering result at time k, i.e., the optimal estimate.

[0073] P(k|k)=(1–Kg(k)H)P(k|k-1)(5);

[0074] P(k|k) is the updated covariance at time k.

[0075] Formulas (1)-(5) above are the five major formulas of Kalman filtering. In this patent application, there is no system control quantity, and the observed value is the system state. U(k) = 0, H = 1. Assuming that the compensation value of the previous time is the same as the prediction value of the next time, then A = 1. The initial value of the covariance is P(0|0) = COV(0), where k represents the gyroscope speed value of the 1000 points in the kth time being judged as static. Then Z(k) = E(k), where E(k) is the average value of the 1000 Rz measurements taken in the kth time. That is, the average value E of the 1000 points of the Z-axis gyroscope value under static condition is the system state value at time k. In the iterative calculation, R(k) = COV(k), where COV(k) represents the covariance of the 1000 Rz values ​​at that time. At this time, the Kalman gain is simplified to Kg = P(k|k-1) / (P(k|k-1) + R(k)). Based on the above description, the Kalman filter calculation process in this patent is simplified to the following formula:

[0076] Furthermore, the calculation using the Kalman algorithm described in step S4 includes the following specific steps:

[0077] The initial values ​​are assigned as follows: where P(0|0)=COV(0), X(0|0)=E(0), Q(k)=0.0001 (Q(k) can be set according to the specific situation, R(k)=COV(k), and the compensation value X(k|k) at different times k can be obtained through iterative calculation;

[0078] X(k|k-1)=X(k-1|k-1);

[0079] Where k represents the determination that the k-th measurement satisfies the static condition (hereafter referred to as time k), at which time X(k-1|k-1) is the optimal compensation value of Rz at time k-1, i.e., Offset, and X(k|k-1) is the estimated compensation value of Rz at time k obtained through the optimal compensation value of Rz at time k-1.

[0080] P(k|k-1)=P(k-1|k-1)+Q(k);

[0081] P(k-1|k-1) is the covariance of the optimal compensation value of Rz at time k-1; P(k|k-1) is the covariance of the estimated compensation value at time k; and Q(k) is the variance of the estimated noise at time k.

[0082] X(k|k)=X(k|k-1)+Kg(k)(E(k)–X(k|k-1));

[0083] Kg(k)=P(k|k-1) / (P(k|k-1)+R(k));

[0084] P(k|k)=(1–Kg(k))P(k|k-1);

[0085] Offset = X(k|k);

[0086] R(k) represents the variance of the measurement noise at time k, Kg(k) represents the Kalman gain at time k, P(k|k) represents the covariance of the optimal compensation value at time k, and X(k|k) represents the optimal compensation value of Rz at time k obtained by weighting the measured average value E(k) and the estimated value X(k|k-1) of Rz at time k using the Kalman gain value. Finally, X(k|k) is assigned to Offset to complete the update of the Rz compensation value. At this point, Offset is the final compensation value obtained.

[0087] Furthermore, the carrier attitude data mentioned in step S1 consists of 1000 consecutive MEMS gyroscope Z-axis angular velocity Rz measurements.

[0088] The YAW value compensated using the algorithm is compared with the uncompensated YAW value, as shown in the comparison chart. Figure 5 As shown.

[0089] The YAW value compensated using this algorithm was compared synchronously with the MTi-300AHRS. This comparison shows that the algorithm effectively reduces the error caused by the zero bias of the MEMS gyroscope. Figure 6-7 As shown, in this process, the compensation value updated by the Kalman filter algorithm is as follows: Figure 8 As shown.

[0090] Experiments have shown that for carriers with intermittent motion, this adaptive dynamic Kalman filter compensation algorithm can effectively correct the zero bias of the Z-axis of the MEMS gyroscope intermittently, thereby effectively improving the measurement accuracy of the Z-axis heading angle of the MEMS gyroscope.

[0091] Based on the characteristics of MEMS gyroscopes, such as short-term stability and long-term minute fluctuations in zero bias, this patent combines an adaptive dynamic zero bias compensation algorithm with a univariate Kalman filter to form an adaptive compensation system that can automatically adjust compensation parameters. The adaptive dynamic compensation can gradually adjust the zero bias compensation value over time, so that the compensation value changes automatically with the IMU state. Kalman filtering has the characteristic of accurate estimation. Combining Kalman filtering with adaptive dynamic compensation makes the system's compensation value closer to the true compensation value while being automatically updated, making the compensation result more accurate and thus improving the measurement accuracy of the IMU.

[0092] It can achieve at least the following beneficial effects:

[0093] A dynamic compensation method for Z-axis zero bias of a MEMS gyroscope based on Kalman filtering mainly includes: Step S1: When the IMU is in a static state, acquire the carrier attitude data and calculate the compensation value Offset of the MEMS gyroscope Z-axis angular velocity Rz using the acquired carrier attitude data; Step S2: In actual measurement, read new carrier attitude data from the IMU; Step S3: Determine whether the carrier is static; Step S4: Calculate and obtain the new compensation value of the MEMS gyroscope Z-axis angular velocity Rz using the Kalman algorithm, update the Offset, and then jump back to step S2 to form a loop. This patent improves the accuracy of the MEMS gyroscope and reduces the Z-axis error of the gyroscope by dynamically capturing the Z-axis zero bias and using the Kalman algorithm to dynamically compensate for the zero bias, while also reducing the measurement error of the heading angle YAW.

[0094] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0095] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A dynamic compensation method for zero bias of the Z-axis of a MEMS gyroscope based on Kalman filtering, characterized in that, Includes the following steps: S1: Before the product leaves the factory, the MEMS sensor is left to stand still, 1000 Z-axis angular velocity measurements are obtained, and the average value E(0) and covariance COV(0) are calculated respectively. Offset = E(0) is used as the factory value of the compensation value. S2: In actual testing, new carrier attitude data is read from the IMU; S3: Determine whether the carrier is static. If the data jump range of the new carrier is greater than the threshold, then the time is determined to be dynamic. The value of k remains unchanged, where k represents the kth time that static data is successfully acquired. Return to S2. S4: By dynamically capturing the Z-axis zero bias and using the Kalman algorithm to dynamically compensate for the zero bias, a new compensation value for the Z-axis angular velocity Rz of the MEMS gyroscope is obtained, and the Offset is updated.

2. The dynamic compensation method for zero bias of MEMS gyroscope Z-axis based on Kalman filtering according to claim 1, characterized in that, S3 also includes that if the fluctuation range of the new carrier recorded data is less than the threshold, then it is considered that the moment is still static. The average value of the new carrier Rz is calculated and denoted as E(k), and its covariance is calculated and denoted as COV(k). k represents the kth time that static data is successfully acquired. If static carrier attitude data can be successfully acquired again, then k = k + 1.

3. A dynamic compensation method for zero bias of the Z-axis of a MEMS gyroscope based on Kalman filtering according to claim 1 or 2, characterized in that, The static judgment criteria and threshold settings specifically include: Read the carrier attitude data from the IMU and record 1000 sets of data, including Ax, Ay, Az, and Rz, where Ax, Ay, and Az represent the acceleration along the x-axis, y-axis, and z-axis, respectively. The thresholds are: Tax = M*[max(Ax) – min(Ax)], Tay = M*[max(Ay) – min(Ay)], Taz = M*[max(Az) – min(Az)], Trz = M*[max(Rz) – min(Rz)], where max and min correspond to the maximum and minimum values ​​of the vector, respectively. Tax, Tay, Taz, and Trz represent the x-axis acceleration threshold, y-axis acceleration threshold, z-axis acceleration threshold, and z-axis rotational speed threshold, respectively. M is the threshold scaling factor, which is set according to the working environment of the carrier. If the measured jumps of the three-axis accelerations Ax, Ay, Az and the Z-axis rotational speed Rz of the MEMS gyroscope are all less than the threshold, it is determined to be static; otherwise, it is dynamic. Since it is difficult to ensure that the external noise signal is in a low state in practical applications, the jump range is increased.

4. The dynamic compensation method for zero bias of MEMS gyroscope Z-axis based on Kalman filtering according to claim 1, characterized in that, The calculation using the Kalman algorithm described in step S4 involves the following steps: The initial values ​​are assigned as follows: where P(0|0)=COV(0), X(0|0)=E(0), Q(k)=0.0001, Q(k) is set according to the specific situation, R(k)=COV(k), and the compensation value X(k|k) at different times k is obtained through iterative calculation. X(k|k-1)=X(k-1|k-1); Where k represents the determination that the k-th measurement meets the static conditions, and this is referred to as time k. At this time, X(k-1|k-1) is the optimal compensation value of Rz at time k-1, i.e., Offset, and X(k|k-1) is the estimated compensation value of Rz at time k obtained from the optimal compensation value of Rz at time k-1. P(k|k-1)=P(k-1|k-1)+Q(k); P(k-1|k-1) is the covariance of the optimal compensation value of Rz at time k-1; P(k|k-1) is the covariance of the estimated compensation value at time k; and Q(k) is the variance of the estimated noise at time k. X(k|k)=X(k|k-1)+Kg(k)(E(k)–X(k|k-1)); Kg(k)=P(k|k-1) / (P(k|k-1)+R(k)); P(k|k)=(1–Kg(k))P(k|k-1); Offset = X(k|k); R(k) is the variance of the measurement noise at time k, Kg(k) is the Kalman gain at time k, P(k|k) is the covariance of the optimal compensation value at time k, and X(k|k) represents the optimal compensation value of Rz at time k obtained by weighting the measured average value E(k) and the estimated value X(k|k-1) of Rz at time k through the Kalman gain value. Finally, X(k|k) is assigned to Offset to complete the update of the Rz compensation value. At this time, Offset is the final compensation value obtained.

Images