Inertial measurement unit (IMU) noise suppression method combining Kalman filtering and proportion integration differentiation (PID) control
By combining Kalman filtering and PID control methods, IMU data is preprocessed, fusion compensation and optimization, which solves the problem that IMU data noise affects attitude resolution accuracy, and achieves higher-precision attitude resolution.
Patent Information
- Application Number
- CN202411796091.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-12-06
- Publication Date
- 2025-03-21
AI Technical Summary
IMU data are susceptible to body vibration noise and cumulative errors, resulting in reduced attitude resolution accuracy and reliability. The existing methods have problems such as poor flexibility and limited accuracy improvement.
Using the combined Kalman filtering and PID control method, the output data of the IMU is pre-processed, the PID control algorithm fusion compensation and the Kalman filter optimization are optimized to suppress noise and improve the attitude resolution accuracy.
It effectively suppresses the noise of the IMU during the attitude resolution process, improves the attitude resolution accuracy, and provides a more accurate and reliable attitude solution for inertial navigation and motion control systems.
Smart Images
Figure CN119669666A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of inertial measurement unit noise reduction methods, and in particular to an IMU noise suppression method combining Kalman filtering and PID control. Background Art
[0002] An inertial measurement unit (IMU) is a device that measures the attitude angle and acceleration of an object, usually composed of an accelerometer and a gyroscope. With its advantages of small size, light weight, low price, independence from the external environment, and high accuracy of position information in a short period of time, IMU is widely used in medical equipment and wearable devices.
[0003] The inertial measurement unit measures the acceleration and angular velocity of an object through an accelerometer and a gyroscope, which is used to solve the object's attitude information. However, IMU data is easily affected by body vibration noise and cumulative errors, thereby reducing the accuracy and reliability of attitude solution. Therefore, an efficient method is needed to suppress noise and improve the accuracy of solution. Existing methods mostly use a single filtering algorithm or hardware calibration method, which has problems such as poor flexibility and limited accuracy improvement. Therefore, the present invention provides an IMU noise suppression method combined with Kalman filtering and PID control, which has important theoretical value and practical significance in attitude solution. Summary of the invention
[0004] To address the above problems, the present invention proposes an IMU noise suppression method combining Kalman filtering and PID control to reduce noise interference and improve the accuracy of object attitude calculation. The specific steps are as follows:
[0005] S1, preprocessing the output data of the accelerometer and gyroscope in the IMU;
[0006] S2, using PID control algorithm to fuse and compensate the output data of accelerometer and gyroscope;
[0007] S3, using Kalman filter to optimize the fused data;
[0008] S4, performing object posture calculation based on the optimized data and outputting high-precision posture calculation results;
[0009] Furthermore, in the first step, the three-axis acceleration values of the accelerometer and the three-axis angular velocity values of the gyroscope in the IMU output data are collected at a frequency of not less than 100 Hz to ensure the real-time and accuracy of the collected data. The accelerometer and gyroscope data are preprocessed, including unitization and zero bias correction.
[0010] Furthermore, in the second step, the PID control algorithm is used to dynamically adjust the error vector using three parameters of proportion, integration and differentiation, so as to achieve real-time fusion compensation of the output data of the accelerometer and gyroscope.
[0011] PID control algorithm parameter K p , K i , K d It is set according to the error vector between the attitude after gyroscope integration and the attitude obtained by accelerometer. p is the proportional term, K i is the integral term, K d is the differential term. The principle diagram of IMU data fusion based on PID control is shown in the attached Figure 2 .
[0012] Furthermore, in the third step, the fused data is optimized using a Kalman filter. The Kalman filter process includes two stages: state prediction and measurement update. The state estimate and covariance are updated recursively. The update equation is as follows:
[0013]
[0014] P k- =AP k-1 A T +Q(2)
[0015] k k =P k- H T (HP k- H T +R) -1 (3)
[0016]
[0017]
[0018] Where: A is the state transfer matrix, k k is the Kalman filter gain matrix; Z k is the measurement value; H is the measurement matrix; is the prior estimated covariance at time k; P k is the posterior estimated covariance; is the prior estimate at time k; is the a posteriori estimate; Q is the process noise covariance, and R is the measurement noise covariance. The principle diagram of IMU error suppression by combining Kalman filtering and PID control is shown in the attached figure. Figure 3 .
[0019] Furthermore, in the fourth step, the object attitude is solved based on the optimized data, and the attitude solution result is used to calculate the pitch angle, roll angle and heading angle through the quaternion method. The solution formula is as follows:
[0020]
[0021] θ=-arcsin(2q 1 q 3 -2q 0 q 2 )(7)
[0022]
[0023] Where: ψ represents the heading angle; θ represents the pitch angle; represents the roll angle. 0 ,q 1 ,q 2 ,q 3 is the quaternion coefficient. The quaternion coefficient is updated using the first-order Runge-Kutta method. The update formula is as follows:
[0024]
[0025]
[0026]
[0027]
[0028] In the formula, ω x is the angular velocity of the gyroscope x-axis; ω y is the angular velocity of the y-axis of the gyroscope; ω z is the angular velocity of the gyroscope z-axis; T is the measurement period.
[0029] The present invention discloses an IMU noise suppression method combining Kalman filtering and PID control, which collects and preprocesses the output data of a gyroscope and an accelerometer in the IMU output data, fuses and compensates the output data of the accelerometer and the gyroscope using a PID control algorithm, and then optimizes the fused data using a Kalman filter, thereby effectively suppressing the noise of the IMU in the attitude solution process, improving the attitude solution accuracy, and providing a more accurate and reliable attitude solution solution for certain inertial navigation and motion control systems. BRIEF DESCRIPTION OF THE DRAWINGS
[0030] Figure 1 The present invention is a flow chart of an IMU noise suppression method combining Kalman filtering and PID control.
[0031] Figure 2This is a principle block diagram of IMU data fusion based on PID control of the present invention.
[0032] Figure 3 This is a block diagram of the principle of IMU error suppression using combined Kalman filtering and PID control of the present invention. DETAILED DESCRIPTION
[0033] This experiment uses the IMU model MPU6050 and uses STM32F103ZET6 to read the output data of the accelerometer and gyroscope. At room temperature, the sampling frequency is 100Hz to collect the three-axis acceleration value A of the accelerometer. XOUT , A YOUT , A ZOUT , and the three-axis angular velocity value ω of the gyroscope x ,ω y ,ω z , and the process of normalizing the read accelerometer data and converting it into a unit vector is as follows:
[0034]
[0035] A X =A XOUT / A norm (2)
[0036] A Y =A YOUT / A norm (3)
[0037] A Z =A ZOUT / A norm (4)
[0038] The classical PID control theory is used to fuse and compensate the data of the gyroscope and accelerometer in the IMU. First, the error between the gyroscope and the accelerometer is calculated.
[0039] The gravity vector in the geographic coordinate system is converted from the geographic coordinate system to the carrier coordinate system using quaternions. That is, the gravity unit vector converted from the current quaternion in the carrier coordinate system is equivalent to the gravity vector calculated using the gyroscope integrated attitude. The calculation formula is as follows:
[0040] V X =2(q 1 q 3 -q 0 q 2 )(5)
[0041] V Y =2(q 0 q 1 +q 2q 3 )(6)
[0042] V Z =q 0 2 -q 1 2 -q 2 2 +q 3 2 (7)
[0043] Where: q 0 ,q 1 ,q 2 ,q 3 Represents the quaternion coefficient; V X 、V Y 、V Z The gravity vector representing the attitude after gyroscope integration is solved. The quaternion coefficients are updated using the first-order Runge-Kutta method, and the calculation formula is as follows:
[0044]
[0045]
[0046]
[0047]
[0048] In the formula, ω x is the angular velocity of the gyroscope x-axis; ω y is the angular velocity of the y-axis of the gyroscope; ω z is the angular velocity of the gyroscope z-axis; T is the measurement period.
[0049] Since the accelerometer and gyroscope calculate attitude in different ways, there is a certain error between them. It is necessary to find the error vector between the attitude after gyroscope integration and the attitude calculated by the accelerometer. The calculation formula is as follows:
[0050] e X =(A Y V Z -A Z V Y )(12)
[0051] e Y =(A Z V X -A X V Z )(13)
[0052] e Z =(A X VY -A Y V X )(14)
[0053] Where: e X 、e Y 、e Z are the attitude solution errors on the three axes of the accelerometer and gyroscope. X 、e Y 、e Z The PID control algorithm is used to compensate for the output of the gyroscope.
[0054] Then use the Kalman filter to optimize the fused data to optimize the output data and suppress noise. The Kalman filter update equation is as follows:
[0055]
[0056]
[0057]
[0058]
[0059]
[0060] Where: A is the state transfer matrix, k k is the Kalman filter gain matrix; Z k is the measurement value; H is the measurement matrix; is the prior estimated covariance at time k; P k is the posterior estimated covariance; is the prior estimate at time k; is the a posteriori estimate; Q is the process noise covariance, and R is the measurement noise covariance.
[0061] The object attitude is solved based on the optimized data, and the attitude solution results are used to calculate the pitch angle, roll angle and heading angle through the quaternion method. The solution formula is as follows:
[0062]
[0063] θ=-arcsin(2q 1 q 3 -2q 0 q 2 )(twenty one)
[0064]
[0065] Where: ψ represents the heading angle; θ represents the pitch angle; Indicates the roll angle.0 ,q 1 ,q 2 ,q 3 are the quaternion coefficients.
[0066] The embodiments of the present invention are described in detail above. The present invention is not limited to the specific details in the above embodiments. Within the technical concept of the present invention, the technical solutions of the present invention can be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should be included in the scope of the claims of the present invention.
Claims
1. An IMU noise suppression method combining Kalman filtering and PID control, characterized in that: The specific steps are as follows: S1. Preprocess the output data of the accelerometer and gyroscope in the IMU. S2. Use PID control algorithm to fuse and compensate the output data of accelerometer and gyroscope. S3. Use Kalman filter to optimize the fused data. S4. Perform object posture calculation based on the optimized data and output high-precision posture calculation results.
2. The IMU noise suppression method of combining Kalman filtering and PID control according to claim 1, characterized in that: In the first step, the three-axis acceleration values of the accelerometer and the three-axis angular velocity values of the gyroscope in the IMU output data are collected, and the accelerometer and gyroscope data are preprocessed, including unitization operation and zero bias correction.
3. The IMU noise suppression method of combining Kalman filtering and PID control according to claim 1, characterized in that: In the second step, the PID control algorithm uses three parameters, namely, proportional, integral and differential, to dynamically adjust the error vector, so as to achieve real-time fusion compensation of the output data of the accelerometer and the gyroscope.
4. The IMU noise suppression method of combining Kalman filtering and PID control according to claim 1, characterized in that: In the third step, the Kalman filter process includes two stages: state prediction and measurement update, and the state estimate and covariance are updated recursively. The update equation is as follows: Where: A is the state transfer matrix, k k is the Kalman filter gain matrix; Z K is the measurement value; H is the measurement matrix; is the prior estimated covariance at time k; P k is the posterior estimated covariance; is the prior estimate at time k; is the a posteriori estimate; Q is the process noise covariance; R is the measurement noise covariance.
5. The IMU noise suppression method combining Kalman filtering and PID control according to claim 1, characterized in that: In the fourth step, the attitude solution result is used to calculate the pitch angle, roll angle and heading angle by using the quaternion method, wherein the quaternion coefficients are updated by the first-order Runge-Kutta method.