Error correction method applied to IMU (Inertial Measurement Unit)

Abstract

The invention discloses an error correction method applied to an IMU (Inertial Measurement Unit), and the method specifically comprises the following steps: S1, initializing filter parameters, and collecting the measurement data of the IMU; s2, performing sliding window processing on the measured data, and calculating a fluctuation range of the data in a window; s3, judging whether impact is detected or not according to the fluctuation range output in S2, if so, entering S4, and if not, ending; s4, updating state estimation and an error covariance matrix in filter parameters, and entering S5; s5, updating a process noise covariance and a measurement noise covariance; s6, according to diagonal elements of the measured noise covariance matrix, judging whether the diagonal elements meet a set threshold value, and if not, continuing circulation; if yes, filtering at the moment is ended, and measurement data of the collected IMU is output.

Description

Technical Field

[0001] The present invention relates to the technical field of data processing, and in particular to an error correction method applied to an IMU. Background Art

[0002] An Inertial Measurement Unit (IMU) is a key sensor that can measure the linear acceleration and angular velocity of an object in three-dimensional space. IMU usually contains three mutually perpendicular accelerometers and three mutually perpendicular gyroscopes, and has the characteristics of high precision, low noise, wide dynamic range, and high-frequency sampling. It is miniaturized and integrated, can maintain reliability in harsh environments, and has automatic calibration functions. These features make IMU widely used in aerospace, automotive navigation, robotics, motion tracking, and virtual reality and augmented reality systems.

[0003] When an inertial measurement unit (IMU) measures the motion of an object, due to internal errors in the sensor and environmental influences, its measurement data will accumulate errors over time, which is called drift, resulting in deviations in position and velocity calculations. The sources of IMU errors are mainly divided into: static error (zero bias, zero bias stability), dynamic error (proportional factor, nonlinearity, cross coupling) and noise (high-frequency error, white noise bandwidth, random walk). In order to reduce these effects, fusion algorithms and regular calibration methods are often used to improve the stability and accuracy of the IMU system, especially in applications over a long period of time or in dynamic environments.

[0004] When IMU faces shock (such as sudden acceleration change or vibration), the accelerometer and gyroscope inside the IMU may produce temporary noise or offset, which will increase the measurement error and affect its measurement accuracy. In order to reduce the impact of these shocks, anti-vibration measures and data filtering technology are usually used in the design to improve the stability and reliability of IMU in complex environments.

[0005] However, traditional filtering methods, such as ordinary Kalman filters and low-pass filters, although they perform well in processing linear systems and fixed noise environments, have limitations in processing nonlinear systems and dynamic noise environments. They are difficult to cope with transient shocks and changes in noise characteristics in IMU data, resulting in inaccurate state estimation. The adaptive extended Kalman filter (AEKF) is an extension of the Kalman filter and is used to process nonlinear systems. It linearizes the nonlinear system and thus solves the nonlinear problem to a certain extent. However, the EKF (Extended Kalman Filter) still assumes that the process noise and measurement noise are fixed, which is unrealistic in practical applications because the noise characteristics often change over time. Summary of the invention

[0006] In view of the problem of low accuracy of IMU measurement data in the prior art, the present invention proposes an error correction method applied to IMU.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] An error correction method applied to an IMU specifically comprises the following steps:

[0009] S1: Initialize filter parameters and collect IMU measurement data;

[0010] S2: Perform sliding window processing on the measured data and calculate the fluctuation range of the data within the window;

[0011] S3: Determine whether an impact is detected based on the fluctuation range of the output of S2. If so, enter S4, otherwise end;

[0012] S4: Update the state estimation and error covariance matrix in the filter parameters and enter S5;

[0013] S5: Update process noise covariance and measurement noise covariance;

[0014] S6: According to the diagonal elements of the measurement noise covariance matrix, determine whether it meets the set threshold, if not, continue the loop; if so, end the filtering at this moment and output the IMU measurement data.

[0015] Preferably, in S1, the filter parameters include state estimation Error covariance matrix P0, initial process noise covariance Q0, measurement noise covariance R0, adaptive parameters θ and γ.

[0016] Preferably, in S1, the measurement data of the IMU is:

[0017] z k =h(x k )+v k , x k =f(x k-1 ,u k )+ω k (1)

[0018] In formula (1), z k represents the measurement data at time k; h(x k ) indicates that the state x k The process of converting to measured data; k represents the measurement noise at time k; x k represents the filter state at time k; x k-1 represents the filter state at time k-1; u k represents the control input at time k; ω k represents the process noise at time k; f represents the state transfer function from time k-1 to time k.

[0019] Preferably, S2 includes: defining a window length N and a threshold β, performing sliding window processing on the measured data z at each time point t, and calculating the fluctuation range of the data in the window:

[0020]

[0021] In formula (2), Δ(t) represents the difference between the maximum and minimum values ​​of the measured data from time t to tN, that is, the fluctuation range; N represents the window length; z(i) represents the observed data at time i between time t and tN.

[0022] Preferably, in S3, whether an impact is detected is determined according to the following formula:

[0023] Δ(t)>β

[0024]

[0025] In formula (3), I(t) represents the impact detection result at time t, 1 represents the presence of impact, and 0 represents the absence of impact; Δ(t) represents the data fluctuation range at time t; β represents the impact threshold.

[0026] Preferably, the S4 includes:

[0027] S4-1: Update the state estimate;

[0028] First, calculate the Jacobian matrix H of the measurement function k :

[0029]

[0030] In formula (4), H k The Jacobian matrix representing the measurement function; Indicates that the subscript meets;

[0031] Then according to the Jacobian matrix H k Calculate the Kalman gain:

[0032]

[0033] In formula (5), K k represents the Kalman gain at time k; P k|k-1 represents the predicted value based on the error covariance matrix at time k; T represents the transposed matrix; R k represents the measurement noise covariance at time k;

[0034] Finally, the state estimate is updated according to the Kalman gain:

[0035]

[0036] In formula (6), represents the estimated value of the state estimation parameter at time k after the update; represents the predicted value of the filter state estimation parameter at time k; z k represents the measurement data at time k; h represents the measurement function;

[0037] S5-2: Update the error covariance matrix:

[0038] P k|k =(IK k H k ) k|k-1 (7)

[0039] In formula (7), P k|k represents the error covariance matrix at time k after the update; K k represents the Kalman gain at time k; H k The Jacobian matrix of the measurement function; P k|k-1 represents the predicted value based on the error covariance matrix at time k; I represents the identity matrix.

[0040] Preferably, in S6, the updating method of the process noise covariance and the measurement noise covariance is:

[0041]

[0042]

[0043] In formula (8), Q k+1represents the process noise covariance at time k+1; θ represents the adaptive parameter; Q k represents the process noise covariance at time k; z k represents the measurement data at time k; represents the predicted value of the filter state estimation parameter at time k; h represents the measurement function; T represents the transposed matrix;

[0044] In formula (9), R k+1 represents the measurement noise covariance at time k+1; γ represents the adaptive parameter; R k represents the measurement noise covariance at time k.

[0045] Preferably, the step S6 further includes adjusting noise adjustment factors θ and γ:

[0046]

[0047] In formula (10), θ and γ represent noise adjustment factors; θ0 and γ0 represent initial noise adjustment factors; and k represents time k.

[0048] In summary, due to the adoption of the above technical solution, compared with the prior art, the present invention has at least the following beneficial effects:

[0049] The present invention adaptively adjusts the process noise and the measurement noise covariance according to the measurement residual (the difference between the measured value and the predicted value) in the updating step, and dynamically adjusts the noise adjustment factor through a time-related model, thereby enhancing the adaptability and stability of the filter and improving the accuracy of the IMU measurement data.

[0050] At the same time, when an impact is detected, the state is updated and the noise is adjusted, and when no impact is detected, the update is skipped to maintain the efficiency of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 Schematic diagram of an error correction method applied to an IMU according to an exemplary embodiment of the present invention. DETAILED DESCRIPTION

[0052] The present invention is further described in detail below in conjunction with the examples and specific implementation methods. However, this should not be understood as the scope of the above subject matter of the present invention being limited to the following examples, and all technologies realized based on the content of the present invention belong to the scope of the present invention.

[0053] In the description of the present invention, it is necessary to understand that the terms "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship, are based on the orientation or position relationship shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0054] like Figure 1 As shown, the present invention provides an error correction method applied to IMU, which specifically includes the following steps:

[0055] S1: Initialize filter parameters and collect IMU measurement data.

[0056] In this embodiment, the filter parameters include state estimation Error covariance matrix P0, initial process noise covariance Q0, measurement noise covariance R0, adaptive parameters (ie, noise adjustment factors) θ and γ.

[0057] The measurement data collected by IMU is:

[0058] z k =h(x k )+v k , x k =f(x k-1 ,u k )+ω k (1)

[0059] In formula (1), z k represents the measurement data at time k; h(x k ) indicates that the state x k The process of converting to measured data; k represents the measurement noise at time k; x k represents the filter state at time k; x k-1 represents the filter state at time k-1; u k represents the control input at time k; ω k represents the process noise at time k; f represents the state transfer function from time k-1 to time k.

[0060] S2: Perform sliding window processing on the IMU measurement data and calculate the fluctuation range of the data within the window.

[0061] In a large amount of impact test data, the experiment shows that the impact phenomenon occurs when the accelerometer data change rate reaches more than 3g within 0.05 seconds at an IMU output frequency of 100Hz.

[0062] Therefore, the present invention defines the window length N(5) and the threshold β(3g), and for each time point t, performs sliding window processing on the measured data z to calculate the fluctuation range of the data in the window:

[0063]

[0064] In formula (2), Δ(t) represents the difference between the maximum and minimum values ​​of the measured data from time t to tN, that is, the fluctuation range; N represents the window length; z(i) represents the observed data at time i between time t and tN.

[0065] S3: Predict the current state based on the IMU measurement data.

[0066] In this embodiment, the present invention designs an adaptive Kalman filter system for processing IMU data. In this system, the state of the previous moment is used. and the control quantity u k Predict the state at the current time t Then check whether a collision occurs. If so, enter the subsequent Kalman filtering process to filter out the error. If not, skip the subsequent filtering process and enter the filtering at the next moment to maintain efficient calculation.

[0067] S3-1: In this embodiment, the state transfer function f(x,u) is used to predict the state at the next moment

[0068]

[0069] In formula (3), Represents the predicted value of the filter at time k; represents the estimated value of the filter at time k-1; u k represents the control input at time k; f represents the state transfer function, which is a mathematical model that describes how the system transfers from the state at one moment to the state at the next moment, and is constructed based on the actual usage model of the system.

[0070] S3-2: Nonlinearity of the previous moment Find partial derivatives and calculate the Jacobian matrix F of the state transfer function k :

[0071]

[0072] In formula (4), F k Represents the Jacobian matrix of the state transfer function at time k; Represents the subscript, corresponding to the state of the parameter in the state transfer function during calculation.

[0073] S3-3: Predict the error covariance matrix P of the next moment k|k-1 , which can be used to calculate the Kalman gain:

[0074] P k|k-1 =F k P k-1|k-1 F k T +Q k (5)

[0075] In formula (5), P k|k-1 represents the predicted value based on the error covariance matrix at time k; F k represents the Jacobian matrix of the state transfer function at time k; P k-1|k-1 represents the estimated value of the error covariance matrix at time k-1; T represents the matrix transpose; Q k represents the process noise covariance at time k.

[0076] S4: Determine whether an impact is detected based on the fluctuation range of the output of S2. If so, enter S5. If not, skip the subsequent process and end the filtering at this moment.

[0077] In this embodiment, the window length N(5) and the threshold β(3g) are defined, and whether an impact is detected is determined according to the following formula:

[0078] Δ(t)>β

[0079]

[0080] In formula (6), I(t) represents the impact detection result at time t, 1 represents the presence of impact, and 0 represents the absence of impact; Δ(t) represents the data fluctuation range at time t; β represents the impact threshold.

[0081] S5: Update the state estimation and error covariance matrix in the filter parameters and enter S6.

[0082] S5-1: Update the state estimate.

[0083] First, calculate the Jacobian matrix H of the measurement function k :

[0084]

[0085] In formula (7), H k The Jacobian matrix representing the measurement function; Indicates that the subscript meets;

[0086] Then according to the Jacobian matrix H k Calculate the Kalman gain:

[0087]

[0088] In formula (8), K k represents the Kalman gain at time k; P k|k-1 represents the predicted value based on the error covariance matrix at time k; T represents the transposed matrix; R k represents the measurement noise covariance at time k;

[0089] Finally, the state estimate is updated according to the Kalman gain:

[0090]

[0091] In formula (9), represents the estimated value of the state estimation parameter at time k after the update; represents the predicted value of the filter state estimation parameter at time k; z k represents the measurement data at time k; h represents the measurement function, which is usually used to describe how to obtain observations from the state space of the system. Its specific form depends on the specific application and problem.

[0092] S5-2: Update the error covariance matrix.

[0093] P k|k =(IK k H k ) k|k-1 (10)

[0094] In formula (10), P k|k represents the error covariance matrix at time k after the update; K k represents the Kalman gain at time k; H k The Jacobian matrix of the measurement function; P k|k-1 represents the predicted value based on the error covariance matrix at time k; I represents the identity matrix.

[0095] S6: Update the process noise covariance and the measurement noise covariance.

[0096]

[0097]

[0098] In formula (11), Q k+1 represents the process noise covariance at time k+1; θ represents the adaptive parameter; Q k represents the process noise covariance at time k; z k represents the measurement data at time k; represents the predicted value of the filter state estimation parameter at time k; h represents the measurement function; T represents the transposed matrix;

[0099] In formula (12), R k+1 represents the measurement noise covariance at time k+1; γ represents the adaptive parameter; R k represents the measurement noise covariance at time k.

[0100] In this embodiment, in order to enhance the adaptability and stability of the filter, the noise adjustment factors θ and γ are dynamically adjusted through a time-dependent model:

[0101]

[0102] In formula (13), θ and γ represent noise adjustment factors; θ0 and γ0 represent initial noise adjustment factors; and k represents time k.

[0103] S7: According to the diagonal elements of the measurement noise covariance matrix, determine whether it meets the set threshold. If not, continue the loop, that is, repeat S5-S7; if so, end the filtering at this moment and output the predicted value of the IMU.

[0104] In this embodiment, when an impact is detected, state update and noise adjustment are performed, and when no impact is detected, update is skipped, thereby maintaining high efficiency and improving calculation efficiency.

[0105] In this embodiment, the noise covariance matrix R k+1 It is calculated by formula (12) based on matrix operation, and its diagonal elements can be directly observed after calculation. k+1 matrix.

[0106] Those skilled in the art will appreciate that the above-mentioned embodiments are specific examples for implementing the present invention, and in actual applications, various changes may be made thereto in form and detail without departing from the spirit and scope of the present invention.

Claims

1. An error correction method applied to IMU, characterized in that: The specific steps include: S1: Initialize filter parameters and collect IMU measurement data; S2: Perform sliding window processing on the measured data and calculate the fluctuation range of the data within the window; S3: Determine whether an impact is detected based on the fluctuation range of the output of S2. If so, enter S4, otherwise end; S4: Update the state estimation and error covariance matrix in the filter parameters and enter S5; S5: Update process noise covariance and measurement noise covariance; S6: According to the diagonal elements of the measurement noise covariance matrix, determine whether it meets the set threshold, if not, continue the loop; if so, end the filtering at this moment and output the IMU measurement data.

2. The error correction method for IMU according to claim 1, characterized in that: In S1, the filter parameters include state estimation Error covariance matrix P0, initial process noise covariance Q0, measurement noise covariance R0, adaptive parameters θ and γ.

3. The error correction method applied to IMU according to claim 1, characterized in that: In S1, the measurement data of the IMU is: z k =h(x k )+v k ,x k =f(x k-1 ,u k )+ω k (1) In formula (1), z k represents the measurement data at time k; h(x k ) indicates that the state x k The process of converting to measured data; k represents the measurement noise at time k; x k represents the filter state at time k; x k-1 represents the filter state at time k-1; u k represents the control input at time k; ω k represents the process noise at time k; f represents the state transfer function from time k-1 to time k.

4. The error correction method for IMU according to claim 1, characterized in that: S2 includes: defining a window length N and a threshold β, performing sliding window processing on the measured data z at each time point t, and calculating the fluctuation range of the data in the window: In formula (2), Δ(t) represents the difference between the maximum and minimum values ​​of the measured data from time t to tN, that is, the fluctuation range; N represents the window length; z(i) represents the observed data at time i between time t and tN.

5. The error correction method applied to IMU according to claim 1, characterized in that: In S3, whether an impact is detected is determined according to the following formula: In formula (3), I(t) represents the impact detection result at time t, 1 represents the presence of impact, and 0 represents the absence of impact; Δ(t) represents the data fluctuation range at time t; β represents the impact threshold.

6. The error correction method for IMU according to claim 1, characterized in that: The S4 includes: S4-1: Update the state estimate; First, calculate the Jacobian matrix H of the measurement function k : In formula (4), H k The Jacobian matrix representing the measurement function; Indicates that the subscript meets; Then according to the Jacobian matrix H k Calculate the Kalman gain: In formula (5), K k represents the Kalman gain at time k; P k|k-1 represents the predicted value based on the error covariance matrix at time k; T represents the transposed matrix; R k represents the measurement noise covariance at time k; Finally, the state estimate is updated according to the Kalman gain: In formula (6), represents the estimated value of the state estimation parameter at time k after the update; represents the predicted value of the filter state estimation parameter at time k; z k represents the measurement data at time k; h represents the measurement function; S5-2: Update the error covariance matrix: In formula (7), P k|k represents the error covariance matrix at time k after the update; K k represents the Kalman gain at time k; H k The Jacobian matrix of the measurement function; P k|k-1 represents the predicted value based on the error covariance matrix at time k; I represents the identity matrix.

7. The error correction method for IMU according to claim 1, characterized in that: In S6, the updating method of the process noise covariance and the measurement noise covariance is: In formula (8), Q k+1 represents the process noise covariance at time k+1; θ represents the adaptive parameter; Q k represents the process noise covariance at time k; z k represents the measurement data at time k; represents the predicted value of the filter state estimation parameter at time k; h represents the measurement function; T represents the transposed matrix; In formula (9), R k+1 represents the measurement noise covariance at time k+1; γ represents the adaptive parameter; R k represents the measurement noise covariance at time k.

8. The error correction method for IMU according to claim 1, characterized in that: The S6 also includes adjusting the noise adjustment factors θ and γ: In formula (10), θ and γ represent noise adjustment factors; θ0 and γ0 represent the initial noise adjustment factors; k represents the kth moment.

Images