Implementation method of vehicle-mounted six-axis gyroscope

Abstract

The invention provides an implementation method of a vehicle-mounted six-axis gyroscope. The implementation method comprises a gyroscope data initialization and zero offset calibration step; a data processing step; a six-axis data fusion and attitude estimation step: using a Kalman filtering algorithm to receive the three-axis acceleration value and the three-axis angular velocity value provided by the data processing step as input, and continuously updating and optimizing state estimation of the system by combining prediction based on a known motion model and observation based on a sensor measurement value; fusing the filtered acceleration and angular velocity data through a six-axis fusion algorithm, and calculating attitude information of the object by adopting a quaternion representation method; after the attitude information represented by the quaternion is obtained, a three-axis Euler angle is further solved, and attitude data is provided for attitude monitoring of the vehicle and an intelligent driving system; and a serial port data output step. The invention aims to improve the navigation precision and reliability, improve the safety of vehicle driving, accurately judge the vehicle state and update the vehicle body posture and the driving condition in real time.

Description

Technical Field

[0001] The present invention relates to the technical field of automotive inertial navigation, and particularly relates to a method for implementing an in-vehicle six-axis gyroscope. Background Art

[0002] With the continuous pursuit of safety and navigation accuracy in the automotive industry, especially with the development of autonomous driving and autonomous navigation technologies, the demand for in-vehicle gyroscopes is increasing day by day. As a sensor that can accurately measure the angular velocity and attitude of an object, by combining an accelerometer and an angular velocity sensor, a gyroscope can monitor important data such as vehicle acceleration, deceleration, vehicle speed, and heading in real time, helping to achieve precise inertial navigation. Its characteristics of miniaturization, high integration, and high precision make it an important tool for improving vehicle driving stability and safety. Especially in the case of GPS signal loss, it can provide key navigation information. However, there are still some problems and disadvantages in the existing in-vehicle gyroscopes, such as large influence of environmental factors on navigation accuracy, insufficient accuracy in road condition monitoring, untimely safety accident warning, and insufficient accuracy in vehicle body attitude monitoring.

[0003] GPS signal loss problem: Existing technologies rely on GPS signals for positioning and navigation. However, in places where GPS signals are unstable or lost, such as underground tunnels, viaducts, and dense urban areas, existing navigation systems may have positioning errors or be unable to perform effective navigation.

[0004] Insufficient accuracy in road condition monitoring: Current vehicle road condition monitoring systems often rely on cameras, radars, and sensors, etc. However, in a rapidly changing driving environment, these systems may be difficult to provide comprehensive road condition information in real time and accurately. Especially for subtle vehicle body changes during high-speed driving, traditional methods are difficult to accurately capture.

[0005] Insufficient safety accident warning: Existing technologies may lack high-precision dynamic monitoring when dealing with dangerous driving behaviors such as traffic accidents, sharp turns, and sudden braking, resulting in the inability to effectively warn of accidents at the initial stage, thus increasing safety risks.

[0006] Accuracy problem in vehicle body attitude monitoring: For the monitoring of vehicle body attitude (such as roll, rollover risk, etc.), existing technologies are often not accurate enough. Especially in the case of severe collisions or sharp turns, there is a lack of accurate assessment of vehicle attitude changes, thus affecting the driver's ability to predict and respond to dangers. Summary of the Invention

[0007] To solve the problems of insufficient navigation accuracy, low road condition monitoring accuracy, untimely safety accident warning, and insufficient vehicle body attitude monitoring accuracy in the prior art, the purpose of the present invention is to provide a method for implementing an in-vehicle six-axis gyroscope, aiming to improve navigation accuracy and reliability, enhance the safety of vehicle driving, accurately judge the vehicle state, and update the vehicle body attitude and driving conditions in real time.

[0008] The present invention achieves the above purpose through the following technical solutions:

[0009] A method for implementing an in-vehicle six-axis gyroscope, the method comprising the following steps:

[0010] Gyroscope data initialization and zero-bias calibration step: Collect the original acceleration and angular velocity data in a stationary state, and calibrate the zero-bias to eliminate the static deviation;

[0011] Data processing step: Collect the real-time acceleration and angular velocity data, and preprocess the real-time data; Apply a low-pass filter to filter out high-frequency noise from the preprocessed data, and use a weighted average technique to process the filtered data; Apply the Kalman filtering technique for real-time state estimation;

[0012] Six-axis data fusion and attitude estimation step: Apply the Kalman filtering algorithm to receive the three-axis acceleration values (Ax, Ay, Az) and three-axis angular velocity values (Wx, Wy, Wz) provided by the data processing step as inputs, and continuously update and optimize the state estimation of the system by combining the prediction based on the known motion model and the observation based on the sensor measurement values; Fuse the filtered acceleration and angular velocity data through a six-axis fusion algorithm, and use the quaternion representation method to calculate the attitude information of the object; After obtaining the attitude information represented by the quaternion, further calculate the three-axis Euler angles to provide attitude data for the vehicle attitude monitoring and intelligent driving system;

[0013] Serial port data output step: Transmit the processed attitude data to an external device in real time through the serial port.

[0014] According to the method for implementing an in-vehicle six-axis gyroscope provided by the present invention, before the gyroscope data initialization and zero-bias calibration step, an initialization step is further included:

[0015] Execute the self-check process of the six-axis gyroscope, including detecting the integrity, response speed, and sensitivity of each sensor element inside the six-axis gyroscope;

[0016] Dynamically adjust the data acquisition rate and range setting of the six-axis gyroscope according to the actual operation requirements of the vehicle and the performance parameters of the six-axis gyroscope, so as to achieve the optimal balance between the accuracy and real-time performance of data acquisition;

[0017] Use a filtering algorithm to perform zero-bias calibration on a six-axis gyroscope. This algorithm is used to automatically identify and compensate for errors generated by environmental factors or internal deviations in the six-axis gyroscope in a stationary state, and obtain accurate zero-bias data;

[0018] During the initialization process, preliminarily verify the output data of the six-axis gyroscope. By comparing the expected value with the measured value, confirm whether the output of the six-axis gyroscope is stable and accurate;

[0019] Record the initialization parameters and calibration results of the six-axis gyroscope to form an initialization log file.

[0020] According to an implementation method of a vehicle-mounted six-axis gyroscope provided by the present invention, continuously collect raw data of three-axis acceleration and angular velocity within a determined stationary time period;

[0021] Calculate the average value of the collected acceleration and angular velocity data in each axis. This average value represents the zero-bias value of the six-axis gyroscope in a stationary state; for the accelerometer, calculate Ax_bias, Ay_bias, Az_bias; for the gyroscope, calculate Wx_bias, Wy_bias, Wz_bias;

[0022] Compare the calculated zero-bias value with a preset ideal zero-bias value or factory calibration value to determine the deviation amount in each axis;

[0023] Generate calibration parameters or a calibration matrix according to the deviation amount, which are used to perform real-time correction on the subsequent collected acceleration and angular velocity data to eliminate static deviation;

[0024] Store the calibration parameters or the calibration matrix in the non-volatile memory of the system, and automatically load and apply them when the system is powered on and initialized each time.

[0025] According to an implementation method of a vehicle-mounted six-axis gyroscope provided by the present invention, after initial calibration, determine the measurement range of the accelerometer and gyroscope in the six-axis gyroscope. This measurement range should cover the maximum acceleration and angular velocity values encountered during application; by adjusting the measurement range setting or configuration register inside the IMU, adjust the measurement ranges of the accelerometer and gyroscope to the determined ranges; after adjusting the measurement ranges, re-perform data collection in a stationary state, continuously collect raw data of three-axis acceleration and angular velocity after adjusting the measurement ranges; recalculate the average value of the acceleration and angular velocity data after adjusting the measurement ranges in each axis as the new zero-bias value; generate corresponding updated calibration parameters or a calibration matrix according to the new zero-bias value, which are used to perform real-time correction on the subsequent collected acceleration and angular velocity data after adjusting the measurement ranges to eliminate the static deviation introduced by the measurement range adjustment.

[0026] According to a method for implementing a vehicle-mounted six-axis gyroscope provided by the present invention, the preprocessing of real-time data includes:

[0027] Perform preliminary cleaning on the raw data and set the threshold range to remove abnormal data points that exceed the normal range;

[0028] Perform zero point calibration operation, that is, calculate and record the average value of angular velocity and acceleration on each axis during the period of time when the sensor is determined to be in a completely stationary state. These average values ​​are regarded as the zero point offset of the sensor;

[0029] Subtract the corresponding zero point offset from the raw data collected subsequently to perform zero point correction to ensure that the reading of the sensor in a static state is close to the theoretical zero value, thereby eliminating the influence of zero point offset on measurement accuracy;

[0030] Apply smoothing filtering techniques to the zero-corrected data;

[0031] Evaluate the quality of preprocessed data by calculating statistical indicators such as the standard deviation and signal-to-noise ratio of the data, and compare it with known reference values ​​or historical data to verify whether the preprocessing effect meets expectations;

[0032] The preprocessed data and related zero point offset, filtering parameters and other information are stored in the system database.

[0033] According to a method for implementing a vehicle-mounted six-axis gyroscope provided by the present invention, after the data preprocessing stage, a low-pass filter is selected as a signal processing tool, and the type of the low-pass filter is determined according to the IMU data characteristics;

[0034] Set the filter cutoff frequency, which should be lower than the highest frequency of the valid signal and higher than the lowest frequency of the expected noise;

[0035] According to the selected filter type and cutoff frequency, the specific parameters of the filter are calculated and determined, and the pre-processed data is input into the configured low-pass filter for filtering, filtering out high-frequency noise components higher than the cutoff frequency, and outputting a smooth signal containing low-frequency effective dynamic information;

[0036] Verify the filtered data and evaluate whether the filtering effect meets the expected requirements by comparing the spectral characteristics, signal-to-noise ratio and other indicators of the data before and after filtering.

[0037] According to a method for implementing a vehicle-mounted six-axis gyroscope provided by the present invention, the filtered data is processed using a weighted average technique, comprising:

[0038] After low-pass filtering, a series of filtered angular velocity and acceleration data are obtained;

[0039] Set a weighted average time window that contains a certain number of the latest data points;

[0040] Assign a weight value to each data point within the time window. This weight value is determined based on the acquisition time of the data point. The most recently acquired data point is given the maximum weight, while earlier acquired data points are gradually given smaller weights;

[0041] Calculate the weighted average, that is, multiply each data point within the time window by its corresponding weight value, then sum all the products, and divide by the sum of the weight values to obtain the weighted average data;

[0042] Use the data after weighted average processing as the angular velocity and acceleration values at the current moment for subsequent attitude estimation;

[0043] As new data is continuously acquired, update the data points and corresponding weight values within the time window, and repeat the step of calculating the weighted average to achieve real-time weighted average processing.

[0044] According to an implementation method of an in-vehicle six-axis gyroscope provided by the present invention, the application of Kalman filtering technology for real-time state estimation includes:

[0045] Define the state vector and observation vector of the dynamic system. The state vector contains the motion state parameters to be estimated, and the observation vector is provided with the measured values of angular velocity and acceleration by the six-axis gyroscope;

[0046] Establish the state transition equation and observation equation of the dynamic system. The state transition equation is used to describe the variation law of the system state over time, and the observation equation is used to describe the relationship between the measured value and the system state;

[0047] Initialize the state estimation value and error covariance matrix of the Kalman filter;

[0048] Within each time step, predict the next state estimation value and error covariance matrix of the system according to the state transition equation, constituting the prediction step of the Kalman filter;

[0049] Obtain the measured values of the six-axis gyroscope at the current time step, and calculate the residual between the measurement prediction value and the actual measured value according to the observation equation;

[0050] Calculate the covariance matrix of the residual, that is, the measurement noise matrix, which is used to reflect the uncertainty of the measured value;

[0051] Use the residual, the measurement noise matrix, and the error covariance matrix in the prediction step to calculate the Kalman gain. The Kalman gain is used to determine the weight of the measured value when updating the state estimation value;

[0052] Update the state estimate and the error covariance matrix according to the Kalman gain and the residual, constituting the update step of the Kalman filter;

[0053] Among them, the state estimate is continuously updated within each time step to track the state change of the system in real time.

[0054] According to an implementation method of an in-vehicle six-axis gyroscope provided by the present invention, the specific implementation of adopting the Kalman filter and the quaternion representation method in the six-axis data fusion and attitude estimation step includes the following steps:

[0055] Define the state vector of the system, which contains the quaternion (q0, q1, q2, q3) describing the attitude of the object, and at the same time define the observation vector, which is composed of the three-axis acceleration values (Ax, Ay, Az) and the three-axis angular velocity values (Wx, Wy, Wz) provided by the six-axis gyroscope;

[0056] According to the motion model and state estimate of the object, use the state transition equation to predict the state vector and the error covariance matrix at the current moment;

[0057] In the observation update stage, use the measurement values provided by the six-axis gyroscope to calculate the residual between the measurement prediction value and the actual measurement value through the observation equation, and calculate the measurement noise matrix;

[0058] Combine the prediction result, the residual, the measurement noise matrix and the error covariance matrix, calculate the Kalman gain, and use the Kalman gain to update the state vector and the error covariance matrix to obtain the state estimate;

[0059] In the state vector, use the quaternion representation method to describe the attitude of the object. Using the updated quaternion state, calculate the three-axis Euler angles of the object through the conversion formula from quaternion to Euler angle as the final attitude estimation result.

[0060] According to an implementation method of an in-vehicle six-axis gyroscope provided by the present invention, in the state vector, use the quaternion (q0, q1, q2, q3) to represent the attitude of the object, where q0 is the real part and q1, q2, q3 are the imaginary parts; among them, the update of the quaternion is realized through the prediction and observation steps of the Kalman filter;

[0061] After obtaining the updated quaternion state (q0, q1, q2, q3), use the conversion formula from quaternion to Euler angle to calculate the three-axis Euler angles of the object, including the roll angle φ, the pitch angle θ, and the yaw angle ψ). The specific conversion formula is as follows:

[0062] The calculation formula for the roll angle φ (Roll) is: φ = atan2(2(q0q1 + q2q3), 1 - 2(q12 + q22))

[0063] The calculation formula for the pitch angle θ is: θ = asin(2(q0q2 - q3q1))

[0064] The calculation formula for the yaw angle ψ is: ψ = atan2(2(q0q3 + q1q2), 1 - 2(q22 + q32))

[0065] Among them, atan2 is a two-parameter arctangent function used to return the corresponding angle at a given coordinate point; asin is an arcsine function used to return the corresponding angle for a given sine value.

[0066] Thus, compared with the prior art, the present invention has the following remarkable beneficial effects:

[0067] 1. Improve navigation accuracy and reliability: Through the six-axis gyroscope IMU of the present invention, even in the case of GPS signal loss or interference, the heading trajectory of the vehicle can still be accurately calculated, ensuring reliable navigation of the vehicle in various complex environments.

[0068] 2. Enhance safety: By continuously monitoring the acceleration, deceleration, vehicle speed, heading, and body attitude of the vehicle, especially using the gyroscope to monitor dangerous situations such as sharp turns, sudden brakes, and rollovers, the present invention can effectively warn of potential safety risks and provide immediate safety feedback and emergency measure suggestions for the driver.

[0069] 3. Accurately judge the vehicle state: Through the differential average filtering method of acceleration and angular velocity, the present invention can accurately judge the dynamic changes of the vehicle in traffic accidents, such as severe collisions, sharp turns, or body tilts, helping to evaluate the risk of rollover or overturning, thereby providing important data support for vehicle safety management.

[0070] 4. Real-time update of body attitude and driving conditions: By combining the calibration of gyroscope data, the acquisition of IMU zero bias, and various smooth attitude functions, the present invention can real-time update and monitor the driving attitude of the vehicle, especially in complex or extreme situations, providing more accurate vehicle driving state data for the driver and fleet management personnel, further ensuring driving safety.

[0071] 5. The present invention uses Kalman filtering technology for real-time state estimation, which can quickly respond to changes in the vehicle's motion state and provide timely attitude data support for the intelligent driving system.

[0072] 6. The six-axis data fusion and attitude estimation steps of the present invention combine the data of the accelerometer and the gyroscope, and perform fusion processing through a filtering algorithm and a six-axis fusion algorithm, making full use of the complementarity of multi-sensors and improving the comprehensiveness and accuracy of attitude estimation. The present invention uses the quaternion representation method to calculate the attitude information of an object, avoiding the gimbal lock problem in the traditional Euler angle representation method and enhancing the stability of attitude estimation.

[0073] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 FIG. is a flowchart of an embodiment of a method for implementing a vehicle-mounted six-axis gyroscope according to the present invention.

[0075] Figure 2 FIG. is a schematic flow diagram of an embodiment of a method for implementing a vehicle-mounted six-axis gyroscope according to the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0076] To make the objectives, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the present invention. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art without making creative efforts based on the embodiments in the present invention fall within the protection scope of the present invention.

[0077] Reference to "embodiment" herein means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the present application. The phrase appears in various places in the specification and does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive of other embodiments. Those skilled in the art will explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0078] See Figure 1 and Figure 2 , the present invention provides a method for implementing a vehicle-mounted six-axis gyroscope, the method comprising the following steps:

[0079] S1 - Gyroscope data initialization and zero-bias calibration step: Collect the original acceleration and angular velocity data in a stationary state, and perform calibration zero-bias to eliminate the static deviation;

[0080] S2 - Data processing step: Collect the real-time acceleration and angular velocity data, and perform preprocessing on the real-time data; Apply a low-pass filter to filter out high-frequency noise from the preprocessed data, and use a weighted average technique to process the filtered data; Apply the Kalman filter technique for real-time state estimation;

[0081] S3 - Six - axis Data Fusion and Attitude Estimation Step: Apply the Kalman filter algorithm to receive the three - axis acceleration values (Ax, Ay, Az) and three - axis angular velocity values (Wx, Wy, Wz) provided by the data processing step as inputs. By combining the prediction based on the known motion model and the observation based on the sensor measurement values, continuously update and optimize the state estimation of the system; fuse the filtered acceleration and angular velocity data through the six - axis fusion algorithm, and use the quaternion representation method to calculate the attitude information of the object; after obtaining the attitude information represented by the quaternion, further solve the three - axis Euler angles to provide attitude data for the vehicle's attitude monitoring and intelligent driving system.

[0082] S4 - Serial Port Data Output Step: Real - time transmit the processed attitude data to external devices through the serial port.

[0083] In this embodiment, before the gyroscope data initialization and zero - bias calibration step, there is also an initialization step. Through hardware self - check, interface configuration, and data processing module initialization, ensure that all components of the system work properly. This step specifically includes:

[0084] First, ensure that the IMU is accurately connected to the vehicle's control system, ensure that the power supply and communication interfaces of the IMU can work properly, and avoid failures during the data acquisition process.

[0085] Execute the self - check process of the six - axis gyroscope, including detecting the integrity, response speed, and sensitivity of each sensor element inside the six - axis gyroscope; according to the actual operating requirements of the vehicle and the performance parameters of the six - axis gyroscope, dynamically adjust the data acquisition rate and range settings of the six - axis gyroscope to achieve the optimal balance between the accuracy and real - time performance of data acquisition.

[0086] Use the filtering algorithm to perform zero - bias calibration on the six - axis gyroscope. This algorithm is used to automatically identify and compensate for the errors generated by environmental factors or internal biases of the six - axis gyroscope in the stationary state, and obtain accurate zero - bias data.

[0087] During the initialization process, conduct a preliminary verification of the output data of the six - axis gyroscope. By comparing the expected values with the measured values, confirm whether the output of the six - axis gyroscope is stable and accurate; record the initialization parameters and calibration results of the six - axis gyroscope to form an initialization log file.

[0088] Specifically, the six - axis gyroscope IMU (Inertial Measurement Unit) is an important sensor device, widely used in fields such as aerospace, automotive, robotics, virtual reality, and smartphones. It can provide real - time information about the motion state of an object, including acceleration, angular velocity, and attitude, etc.

[0089] The six-axis IMU consists of two main sensors, a three-axis accelerometer and a three-axis gyroscope. The three-axis accelerometer is responsible for measuring the linear acceleration of an object in three axes (X, Y, Z). It calculates the acceleration by detecting the force acting on the sensor. According to Newton's second law (F = ma), the velocity and displacement of the object can be deduced. The three-axis gyroscope, on the other hand, measures the angular velocity of the object around three axes (i.e., the angular displacement per unit time), reflecting its rotation state, and obtains the rotation information of the object by measuring the angular displacement per unit time.

[0090] After completing the above preparations, start real-time acquisition of the data output by the IMU, including acceleration and angular velocity information, which are the basis for subsequent analysis. Using the acquired acceleration and angular velocity information, calculate the three Euler angles (roll angle, pitch angle, yaw angle) to describe the direction and attitude of the object. The roll angle represents the rotation angle of the object around the front-back axis, the pitch angle represents the rotation angle of the object around the left-right axis, and the yaw angle represents the rotation angle of the object around the vertical axis.

[0091] In the above steps of gyroscope data initialization and zero bias calibration, during the determined stationary time period, continuously acquire the raw data of three-axis acceleration and angular velocity; calculate the average value of the acquired acceleration and angular velocity data in each axis, and this average value represents the zero bias value of the six-axis gyroscope in the stationary state; for the accelerometer, calculate Ax_bias, Ay_bias, Az_bias; for the gyroscope, calculate Wx_bias, Wy_bias, Wz_bias; compare the calculated zero bias value with the preset ideal zero bias value or the factory calibration value to determine the deviation amount in each axis; according to the deviation amount, generate calibration parameters or a calibration matrix for real-time correction of the subsequent acquired acceleration and angular velocity data to eliminate the static deviation; store the calibration parameters or the calibration matrix in the non-volatile memory of the system and automatically load and apply them when the system is powered on and initialized each time.

[0092] After the initial calibration, determine the measurement range of the accelerometer and gyroscope in the six-axis gyroscope, and this measurement range should cover the maximum acceleration and angular velocity values encountered during the application process; by adjusting the measurement range settings or configuration registers inside the IMU, adjust the measurement ranges of the accelerometer and gyroscope to the determined range; after adjusting the measurement range, re-perform data acquisition in the stationary state, continuously acquire the raw data of three-axis acceleration and angular velocity after adjusting the measurement range; recalculate the average value of the acceleration and angular velocity data after adjusting the measurement range in each axis as the new zero bias value; according to the new zero bias value, generate the corresponding updated calibration parameters or calibration matrix for real-time correction of the acceleration and angular velocity data acquired later after adjusting the measurement range to eliminate the static deviation introduced due to the adjustment of the measurement range.

[0093] It can be seen that after the six-axis gyroscope obtains the three-axis acceleration and three-axis angular velocity data, the accuracy of the data usually needs to be optimized through calibration due to possible sensor errors or external environmental interference. The calibration process first collects the original acceleration and angular velocity data in a stationary state and performs zero bias calibration to eliminate static errors. Finally, the system adjusts the appropriate range according to the specific application requirements and performs zero bias calibration again to ensure accurate measurement of the sensor within the entire range. Through this series of calibration operations, the six-axis gyroscope can provide more accurate and reliable attitude estimation and navigation information.

[0094] In the data processing step, the real-time data is preprocessed, including:

[0095] The raw data is preliminarily cleaned, and abnormal data points beyond the normal range are eliminated by setting a threshold range; a zero-point correction operation is performed, that is, during the time period when the sensor is determined to be in a completely stationary state, the average values ​​of the angular velocity and acceleration on each axis are calculated and recorded, and these average values ​​are regarded as the zero-point offset of the sensor; the corresponding zero-point offset is subtracted from the raw data collected subsequently, and zero-point correction is performed to ensure that the reading of the sensor in a stationary state is close to the theoretical zero value, thereby eliminating the influence of the zero-point offset on the measurement accuracy; smoothing filtering technology is applied to the data after zero-point correction; the quality of the preprocessed data is evaluated, and statistical indicators such as the standard deviation and signal-to-noise ratio of the data are calculated, and the comparison with known reference values ​​or historical data is used to verify whether the preprocessing effect meets the expectations; the preprocessed data and related zero-point offset, filtering parameters and other information are stored in the system database.

[0096] After the data preprocessing stage, a low-pass filter is selected as a signal processing tool, and the type of low-pass filter is determined according to the characteristics of the IMU data; the cutoff frequency of the filter is set, which should be lower than the highest frequency of the effective signal and higher than the lowest frequency of the expected noise; according to the selected filter type and cutoff frequency, the specific parameters of the filter are calculated and determined, and the preprocessed data is input into the configured low-pass filter for filtering to filter out high-frequency noise components higher than the cutoff frequency and output a smooth signal containing low-frequency effective dynamic information; the filtered data is verified, and by comparing the spectral characteristics, signal-to-noise ratio and other indicators of the data before and after filtering, it is evaluated whether the filtering effect meets the expected requirements.

[0097] In this embodiment, the filtered data is processed using a weighted average technique, including:

[0098] After low-pass filtering, a series of filtered angular velocity and acceleration data are obtained; a weighted average time window is set, which contains a certain number of the latest data points; a weight value is assigned to each data point within the time window, and this weight value is determined according to the acquisition time of the data point. The latest acquired data point is given the maximum weight, while the earlier acquired data points are gradually given smaller weights; the weighted average is calculated, that is, each data point within the time window is multiplied by its corresponding weight value, then all the products are added together, and then divided by the sum of the weight values to obtain the weighted average data; the weighted average processed data is used as the angular velocity and acceleration values at the current moment for subsequent attitude estimation.

[0099] Among them, as new data is continuously acquired, the data points and corresponding weight values within the time window are updated, and the step of calculating the weighted average is repeatedly executed to achieve real-time weighted average processing.

[0100] In this embodiment, the Kalman filtering technology is applied for real-time state estimation, including:

[0101] Define the state vector and observation vector of the dynamic system. The state vector contains the motion state parameters to be estimated, and the observation vector provides the measured values of angular velocity and acceleration by a six-axis gyroscope; establish the state transition equation and observation equation of the dynamic system. The state transition equation is used to describe the variation law of the system state over time, and the observation equation is used to describe the relationship between the measured values and the system state; initialize the state estimation value and error covariance matrix of the Kalman filter; within each time step, predict the next state estimation value and error covariance matrix of the system according to the state transition equation, which constitutes the prediction step of the Kalman filter; obtain the measured values of the six-axis gyroscope at the current time step, and calculate the residual between the measurement prediction value and the actual measured value according to the observation equation; calculate the covariance matrix of the residual, that is, the measurement noise matrix, which is used to reflect the uncertainty of the measured values; use the residual, the measurement noise matrix, and the error covariance matrix in the prediction step to calculate the Kalman gain, and the Kalman gain is used to determine the weight of the measured value when updating the state estimation value; update the state estimation value and error covariance matrix according to the Kalman gain and the residual, which constitutes the update step of the Kalman filter.

[0102] Among them, the state estimation value is continuously updated within each time step to track the state change of the system in real time.

[0103] Specifically, when processing the data collected by an IMU (Inertial Measurement Unit), the angular velocity and acceleration measured by sensors such as gyroscopes and accelerometers are often affected by various errors, such as measurement errors, noise, and drift. In order to improve the data quality and enhance the reflection of the true motion state of the vehicle, a series of data processing techniques are usually adopted, such as low-pass filtering, weighted average, and Kalman filtering.

[0104] First, in the data acquisition phase, raw angular velocity and acceleration data are obtained from sensors. This data may be interfered with by high-frequency noise, affecting its accuracy and stability. Therefore, data preprocessing is crucial. By cleaning the raw data, removing obvious noise, and performing zero-point correction, it is ensured that the readings of the sensors are close to the theoretical values in the stationary state, thus improving the data quality.

[0105] After data preprocessing, a low-pass filter is first used to process the data. Appropriate filter parameters are selected, and an appropriate cut-off frequency is set to filter out high-frequency noise above the cut-off frequency and retain the effective signals below the cut-off frequency. Through this process, the data becomes smoother, reducing the interference of sensor noise and environmental factors on the measurement results. Low-pass filtering can effectively remove unnecessary high-frequency components while retaining the effective low-frequency dynamic information, thus more accurately reflecting the true motion state of the vehicle.

[0106] Next, the weighted average technique is used to further process the data after low-pass filtering. In the weighted average process, by setting different weight values, the importance of the latest data is emphasized, making the currently acquired data account for a larger proportion in the final calculation. This method helps to reduce the influence of historical data on the results and more accurately reflects the real-time dynamic changes. The weighted average further smooths the data and reduces the influence of noise on the final results, thus improving the reliability of attitude estimation.

[0107] Finally, the Kalman filtering technique is applied to the real-time state estimation of the dynamic system. The Kalman filter continuously updates the estimation of the previous state and combines the current measurement values to gradually improve the estimation accuracy. It can effectively process data with noise and track the state changes of the system in real time, thus improving the prediction accuracy of the motion state. The Kalman filter plays a crucial role in this process. Especially when facing dynamic changes and noise interference, it can provide a more stable and accurate output.

[0108] By combining these three methods of low-pass filtering, weighted average, and Kalman filtering, the data quality can be significantly improved, and a more stable and reliable data sequence can be obtained. These finely processed data provide a solid foundation for subsequent attitude analysis and motion state inference, helping to achieve more accurate dynamic tracking and analysis.

[0109] In this embodiment, the specific implementation of using the Kalman filter and quaternion representation method in the six-axis data fusion and attitude estimation step includes the following steps:

[0110] Define the state vector of the system, which contains the quaternion (q0, q1, q2, q3) describing the object's attitude. At the same time, define the observation vector, which consists of the three-axis acceleration values (Ax, Ay, Az) and the three-axis angular velocity values (Wx, Wy, Wz) provided by the six-axis gyroscope;

[0111] According to the object's motion model and state estimation, use the state transition equation to predict the state vector and error covariance matrix at the current moment;

[0112] In the observation update stage, use the measurement values provided by the six-axis gyroscope, calculate the residual between the measurement prediction value and the actual measurement value through the observation equation, and calculate the measurement noise matrix;

[0113] Combine the prediction result, the residual, the measurement noise matrix, and the error covariance matrix, calculate the Kalman gain, and use the Kalman gain to update the state vector and the error covariance matrix to obtain the state estimation;

[0114] In the state vector, use the quaternion representation method to describe the object's attitude. Using the updated quaternion state, solve the three-axis Euler angles of the object through the conversion formula from quaternion to Euler angles as the final attitude estimation result.

[0115] In the state vector, use the quaternion (q0, q1, q2, q3) to represent the object's attitude, where q0 is the real part and q1, q2, q3 are the imaginary parts; among them, the update of the quaternion is realized through the prediction and observation steps of the Kalman filter;

[0116] After obtaining the updated quaternion state (q0, q1, q2, q3), use the conversion formula from quaternion to Euler angles to solve the three-axis Euler angles of the object, including the roll angle φ, the pitch angle θ, and the yaw angle ψ). The specific conversion formula is as follows:

[0117] The calculation formula for the roll angle φ (Roll) is: φ = atan2(2(q0q1 + q2q3), 1 - 2(q1^2 + q2^2))

[0118] The calculation formula for the pitch angle θ (Pitch) is: θ = asin(2(q0q2 - q3q1))

[0119] The calculation formula for the yaw angle ψ (Yaw) is: ψ = atan2(2(q0q3 + q1q2), 1 - 2(q2^2 + q3^2))

[0120] Among them, atan2 is a two-parameter arctangent function used to return the corresponding angle at a given coordinate point; asin is an arcsine function used to return the angle corresponding to a given sine value.

[0121] Specifically, the Kalman filter in this embodiment updates the state estimation of the system by continuously combining prediction and observation based on the three-axis acceleration values (Ax, Ay, Az) and three-axis angular velocity values (Wx, Wy, Wz) provided by the IMU. First, the current state is predicted using the known motion model and the previous state, and then the prediction result is corrected through observation updates using the measured values of the sensors, and the error covariance is calculated to evaluate the uncertainty of the system. Through this method, the Kalman filter can effectively improve the response speed and accuracy of the system to the dynamic environment. At the same time, the fused data can be used to calculate the attitude information of the object, usually represented by quaternions, to avoid the gimbal lock problem in the traditional Euler angle representation method, thereby enhancing the stability of the attitude estimation.

[0122] Quaternion is an effective way to represent three-dimensional rotation, which can overcome the problem of loss of rotational degrees of freedom in Euler angle representation. In practical applications, the angles between the vehicle and the x-axis and y-axis can be calculated from quaternion data. The x-axis usually represents the forward direction of the vehicle, and by the angle with the x-axis, the uphill, downhill or horizontal driving state of the vehicle can be determined. Specifically, when the angle is greater than 20°, it means the vehicle is going uphill; when the angle is less than -20°, it means the vehicle is going downhill; and when the angle is between -20° and 20°, the vehicle is in a horizontal driving state. At the same time, the y-axis usually represents the direction perpendicular to the ground, and by the angle with the y-axis, the risk of vehicle roll can be judged. When the angle is greater than 10°, it indicates that the left side of the vehicle is tilted; when the angle is less than -10°, it means the right side of the vehicle is tilted. These angle analyses provide important bases for vehicle attitude monitoring, helping to optimize driving decisions and improve driving safety.

[0123] During the processing, the Kalman filter is used to filter the data, reduce the interference of high-frequency noise, and retain the low-frequency signals, so as to obtain a more stable attitude estimation. At the same time, the six-axis fusion algorithm and the quaternion method can effectively fuse the data of the gyroscope and the accelerometer, and calculate the three-axis Euler angles to avoid the gimbal lock problem. Finally, after the post-processing and verification stages, the fusion results will be evaluated and verified, and the algorithm will be continuously optimized according to the actual application feedback to ensure the accuracy and reliability of the output results. This series of processes ensures the accuracy and stability of vehicle attitude estimation, providing strong support for the intelligent driving system.

[0124] In the above serial port data output step, the processed IMU data (acceleration, angular velocity, Euler angles, quaternions) are transmitted to external devices through UART, and the real-time, reliability and integrity of the data are ensured.

[0125] The above embodiments are only preferred embodiments of the present invention, and the scope of protection of the present invention cannot be limited thereby. Any non-substantial changes and substitutions made by those skilled in the art based on the present invention fall within the scope of protection required by the present invention.

Claims

1. A method for implementing a vehicle-mounted six-axis gyroscope, characterized in that: The method comprises the following steps: Gyroscope data initialization and zero bias calibration steps: collect raw acceleration and angular velocity data in a stationary state, and calibrate the zero bias to eliminate static deviation; Data processing steps: collect real-time acceleration and angular velocity data, pre-process the real-time data; use a low-pass filter to filter out high-frequency noise from the pre-processed data, and use weighted average technology to process the filtered data; use Kalman filtering technology to perform real-time state estimation; Six-axis data fusion and attitude estimation step: Apply the Kalman filter algorithm to receive the three-axis acceleration values ​​(Ax, Ay, Az) and three-axis angular velocity values ​​(Wx, Wy, Wz) provided by the data processing step as input, and continuously update and optimize the system state estimation by combining predictions based on known motion models and observations based on sensor measurements; fuse the filtered acceleration and angular velocity data through the six-axis fusion algorithm, and use quaternion representation to calculate the object's attitude information; after obtaining the attitude information represented by the quaternion, further solve the three-axis Euler angle to provide attitude data for the vehicle's attitude monitoring and intelligent driving system; Serial port data output steps: transmit the processed posture data to the external device in real time through the serial port.

2. The method according to claim 1, characterized in that Before the gyroscope data initialization and zero bias calibration steps, an initialization step is also included: Execute the self-test process of the six-axis gyroscope, including testing the integrity, response speed and sensitivity of each sensor element inside the six-axis gyroscope; According to the actual operation requirements of the vehicle and the performance parameters of the six-axis gyroscope, the data acquisition rate and range settings of the six-axis gyroscope are dynamically adjusted to achieve the optimal balance between the accuracy and real-time performance of data acquisition; The six-axis gyroscope is calibrated for zero bias using a filtering algorithm. The algorithm is used to automatically identify and compensate for the errors caused by environmental factors or internal deviations in the six-axis gyroscope when it is stationary, and obtain accurate zero bias data. During the initialization process, the output data of the six-axis gyroscope is preliminarily verified. By comparing the expected value with the measured value, it is confirmed whether the output of the six-axis gyroscope is stable and accurate; Record the initialization parameters and calibration results of the six-axis gyroscope to form an initialization log file.

3. The method according to claim 2, characterized in that: During a certain static period, the original data of three-axis acceleration and angular velocity are continuously collected; Calculate the average value of the collected acceleration and angular velocity data in each axis, which represents the zero bias value of the six-axis gyroscope in a static state; for the accelerometer, calculate Ax_bias, Ay_bias, Az_bias; for the gyroscope, calculate Wx_bias, Wy_bias, Wz_bias; Compare the calculated zero offset value with the preset ideal zero offset value or the factory calibration value to determine the deviation amount in each axial direction; According to the deviation, a calibration parameter or a calibration matrix is ​​generated for real-time correction of the subsequently collected acceleration and angular velocity data to eliminate the static deviation; The calibration parameters or calibration matrix are stored in the non-volatile memory of the system and automatically loaded and applied each time the system is powered on and initialized.

4. The method according to claim 3, characterized in that: After the initial calibration, the range of the accelerometer and gyroscope in the six-axis gyroscope is determined, and the range should cover the maximum acceleration and angular velocity values ​​encountered during the application process; the range of the accelerometer and gyroscope is adjusted to the determined range by adjusting the range setting or configuration register inside the IMU; after adjusting the range, re-execute data collection in a stationary state, and continuously collect the three-axis acceleration and angular velocity raw data after the range adjustment; recalculate the average value of the acceleration and angular velocity data in each axis after the range adjustment as a new zero bias value; based on the new zero bias value, generate the corresponding updated calibration parameters or calibration matrix, which is used to perform real-time correction on the acceleration and angular velocity data subsequently collected after the range adjustment, so as to eliminate the static deviation introduced by the range adjustment.

5. The method according to claim 1, characterized in that The preprocessing of real-time data includes: Perform preliminary cleaning on the raw data and set the threshold range to remove abnormal data points that exceed the normal range; Perform zero point calibration operation, that is, calculate and record the average value of angular velocity and acceleration on each axis during the period of time when the sensor is determined to be in a completely stationary state. These average values ​​are regarded as the zero point offset of the sensor; Subtract the corresponding zero point offset from the raw data collected subsequently to perform zero point correction to ensure that the reading of the sensor in a static state is close to the theoretical zero value, thereby eliminating the influence of zero point offset on measurement accuracy; Apply smoothing filtering techniques to the zero-corrected data; Evaluate the quality of preprocessed data by calculating statistical indicators such as the standard deviation and signal-to-noise ratio of the data, and compare it with known reference values ​​or historical data to verify whether the preprocessing effect meets expectations; The preprocessed data and related zero point offset, filtering parameters and other information are stored in the system database.

6. The method according to claim 5, characterized in that: After the data preprocessing stage, a low-pass filter is selected as a signal processing tool, and the type of low-pass filter is determined according to the characteristics of the IMU data; Set the filter cutoff frequency, which should be lower than the highest frequency of the valid signal and higher than the lowest frequency of the expected noise; According to the selected filter type and cutoff frequency, the specific parameters of the filter are calculated and determined, and the pre-processed data is input into the configured low-pass filter for filtering, filtering out high-frequency noise components higher than the cutoff frequency, and outputting a smooth signal containing low-frequency effective dynamic information; Verify the filtered data and evaluate whether the filtering effect meets the expected requirements by comparing the spectral characteristics, signal-to-noise ratio and other indicators of the data before and after filtering.

7. The method according to claim 6, characterized in that The method of using a weighted average technique to process the filtered data includes: After low-pass filtering, a series of filtered angular velocity and acceleration data are obtained; Set a weighted average time window that contains a certain number of the latest data points; Assign a weight value to each data point in the time window. The weight value is determined by the time when the data point was collected. The most recently collected data point is given the largest weight, while the earlier collected data points are gradually given smaller weights. Calculate the weighted average, that is, multiply each data point in the time window by its corresponding weight value, then add all the products and divide them by the sum of the weight values ​​to get the weighted average data; The weighted averaged data is used as the angular velocity and acceleration values ​​at the current moment for subsequent attitude estimation; As new data is continuously collected, the data points and corresponding weight values ​​within the time window are updated, and the steps of calculating the weighted average are repeated to achieve real-time weighted average processing.

8. The method according to any one of claims 1 to 7, characterized in that: The application of Kalman filtering technology to perform real-time state estimation includes: Define the state vector and observation vector of the dynamic system. The state vector contains the motion state parameters that need to be estimated, and the observation vector is provided by the six-axis gyroscope to measure the angular velocity and acceleration. Establish the state transfer equation and observation equation of the dynamic system. The state transfer equation is used to describe the change of the system state over time, and the observation equation is used to describe the relationship between the measured value and the system state. Initialize the state estimate and error covariance matrix of the Kalman filter; In each time step, the next state estimate and error covariance matrix of the system are predicted according to the state transfer equation, which constitutes the prediction step of Kalman filtering; Get the six-axis gyroscope measurement value of the current time step, and calculate the residual between the measurement prediction value and the actual measurement value according to the observation equation; Calculate the covariance matrix of the residual, i.e. the measurement noise matrix, which is used to reflect the uncertainty of the measurement value; The Kalman gain is calculated using the residual, the measurement noise matrix, and the error covariance matrix in the prediction step. The Kalman gain is used to determine the weight of the measurement value when updating the state estimate. According to the Kalman gain and residual, the state estimate and the error covariance matrix are updated to form the update step of the Kalman filter; Among them, the state estimation value is continuously updated in each time step to track the state changes of the system in real time.

9. The method according to claim 8, characterized in that The specific implementation of using Kalman filtering and quaternion representation in the six-axis data fusion and attitude estimation step includes the following steps: Define the state vector of the system, which contains the quaternion (q0, q1, q2, q3) describing the object's posture, and define the observation vector, which consists of the three-axis acceleration values ​​(Ax, Ay, Az) and three-axis angular velocity values ​​(Wx, Wy, Wz) provided by the six-axis gyroscope; According to the object's motion model and state estimation, the state vector and error covariance matrix at the current moment are predicted using the state transfer equation; In the observation update phase, the measurement values ​​provided by the six-axis gyroscope are used to calculate the residual between the measurement prediction value and the actual measurement value through the observation equation, and the measurement noise matrix is ​​calculated; Combine the prediction results, residuals, measurement noise matrix and error covariance matrix to calculate the Kalman gain, and use the Kalman gain to update the state vector and error covariance matrix to obtain the state estimate; In the state vector, quaternion representation is used to describe the posture of the object. Using the updated quaternion state and the quaternion to Euler angle conversion formula, the three-axis Euler angle of the object is solved as the final posture estimation result.

10. The method according to claim 9, characterized in that: In the state vector, quaternion (q0, q1, q2, q3) is used to represent the posture of the object, where q0 is the real part and q1, q2, q3 are the imaginary parts; the update of the quaternion is realized through the prediction and observation steps of the Kalman filter; After obtaining the updated quaternion state (q0, q1, q2, q3), the three-axis Euler angle of the object is solved using the quaternion to Euler angle conversion formula, including the roll angle φ, pitch angle θ, and yaw angle ψ. The specific conversion formula is as follows: The calculation formula of the roll angle φ(Roll) is: φ=atan2(2(q0q1+q2q3),1-2(q12+q22)) The calculation formula of the pitch angle θ(Pitch) is: θ=asin(2(q0q2-q3q1)) The calculation formula of the yaw angle ψ (Yaw) is: ψ = atan2 (2 (q0q3 + q1q2), 1-2 (q22 + q32)) where atan2 is a two-parameter inverse tangent function, which is used to return the corresponding angle at a given coordinate point; asin is the inverse sine function, which is used to return the angle corresponding to a given sine value.

Images