Gyroscope, accelerometer and magnetometer fusion calibration method

Abstract

The invention belongs to the field of sensors, and provides a gyroscope, accelerometer and magnetometer fusion calibration method, which comprises the following steps: acquiring X-axis, Y-axis and Z-axis data of an accelerometer, a gyroscope and a magnetometer of equipment; based on the X-axis data, the Y-axis data and the Z-axis data of the accelerometer, whether the equipment meets a preset first static condition or not is judged, if yes, the three-axis offset of the gyroscope is obtained, and compensation is conducted on output data of the gyroscope; judging whether the equipment meets a preset second static condition or not based on the XYZ three-axis data of the gyroscope, judging whether the equipment meets a preset horizontal condition or not based on the XYZ three-axis data of the accelerometer, calculating an acceleration compensation value if the equipment meets the preset horizontal condition at the same time, and compensating the output of the accelerometer based on the acceleration compensation value; integrating the XYZ three-axis data of the gyroscope to obtain the rotation angle of the gyroscope, fitting the XYZ three-axis data of the magnetometer acquired in real time and the rotation angle obtained by integration to describe an arc to obtain an isosceles triangle, calculating to obtain the magnetic field disturbance variable of the magnetometer, and compensating the output of the magnetometer based on the magnetic field disturbance variable; according to the invention, the calibration problem of the gyroscope, the accelerometer and the magnetometer in the prior art can be solved.

Description

Technical Field

[0001] The present invention relates to the field of sensors, and in particular to a fusion calibration method for a gyroscope, an accelerometer and a magnetometer. Background Art

[0002] The fusion data of gyroscopes, accelerometers and magnetometers (magnetometers) are widely used in many fields. Fusion data refers to the data of three sensors. Based on fusion data, more accurate and stable posture and position information can be provided to meet the needs of various complex scenarios. It is widely used in: navigation and positioning; virtual reality (VR) and augmented reality (AR); sports and fitness; spatial audio; smart phones and wearable devices.

[0003] To have accurate and more stable attitude and position information, the gyroscope / accelerometer / magnetometer needs to provide accurate data, and the attitude and position information is calculated through the sensor fusion algorithm. This requires that the data obtained from the sensor is accurate enough, but the gyroscope will have initial drift error and temperature drift error, the accelerometer has initial offset error, and the magnetometer has external hard magnetic and soft magnetic interference. Therefore, these three sensors must be calibrated during use to ensure the accuracy of the data. The current traditional calibration method is to calibrate the gyroscope once before leaving the factory, the accelerometer once before leaving the factory, and the magnetometer calibrates the deviation by flipping the magnetic data in actual application. The above calibration method will have the following problems:

[0004] 1. The gyroscope's temperature changes and accumulated errors during use cause the X / Y / Z data deviation to become larger and larger;

[0005] 2. The magnetometer calibration is slow and requires relatively large movements, which affects the experience;

[0006] 3. The accelerometer's offset changes due to changes in mechanical structure and stress. Summary of the invention

[0007] In view of the above technical problems, the present invention provides a gyroscope, accelerometer and magnetometer fusion calibration method to solve the calibration problem of the gyroscope, accelerometer and magnetometer in the prior art.

[0008] Other features and advantages of the present disclosure will become apparent from the following detailed description, or may be learned in part by the practice of the present disclosure.

[0009] The present disclosure provides a fusion calibration method for a gyroscope, an accelerometer, and a magnetometer, the method comprising:

[0010] Get the XYZ three-axis data of the device's accelerometer, gyroscope, and magnetometer;

[0011] Based on the XYZ three-axis data of the accelerometer, determine whether the device meets the preset first static condition, and if so, obtain the three-axis offset of the gyroscope, and compensate the output data of the gyroscope based on the three-axis offset;

[0012] Based on the XYZ three-axis data of the gyroscope, determine whether the device meets the preset second static condition; based on the XYZ three-axis data of the accelerometer, determine whether the device meets the preset horizontal condition; if the device meets both the second static condition and the horizontal condition, calculate the acceleration compensation value, and compensate the output of the accelerometer based on the acceleration compensation value;

[0013] The XYZ three-axis data of the gyroscope are integrated to obtain the rotation angle of the gyroscope, and an arc is fitted to describe the XYZ three-axis data of the magnetometer acquired in real time and the rotation angle obtained by integration to obtain an isosceles triangle, so as to calculate the magnetic field interference amount of the magnetometer, and compensate the output of the magnetometer based on the magnetic field interference amount.

[0014] Furthermore, the first static condition includes:

[0015] Within a preset time, the X-axis data of the accelerometer is less than 0.05 m / s2, the Y-axis data is less than 0.05 m / s2, and the Z-axis data is less than 0.05 m / s2.

[0016] Furthermore, when acquiring the three-axis offset, the standard static data is subtracted from the XYZ three-axis data of the gyroscope to obtain the three-axis offset.

[0017] Furthermore, the second static condition includes:

[0018] Within a preset time, the X-axis data of the gyroscope is greater than -0.01 rad / s and less than 0.01 rad / s, the Y-axis data is greater than -0.01 rad / s and less than 0.01 rad / s, and the Z-axis data is greater than -0.01 rad / s and less than 0.01 rad / s.

[0019] Furthermore, the level conditions include:

[0020] Within the preset time, the X-axis data of the accelerometer is greater than -0.03m / s2 and less than 0.03m / s2, the Y-axis data is greater than -0.03m / s2 and less than 0.03m / s2, and the Z-axis data is greater than -9.77m / s2 and less than 9.83m / s2.

[0021] Furthermore, when calculating the acceleration compensation value, the standard data is subtracted from the current XYZ three-axis data of the accelerometer to obtain the acceleration compensation value.

[0022] Furthermore, when integrating the XYZ three-axis data of the gyroscope, the calculation formula is:

[0023] theta=θ0+ω·Δt;

[0024] theta is the rotation angle, θ0 is the initial angle, ω is the angular velocity, and Δt is the time interval.

[0025] The technical solution disclosed in this disclosure has the following beneficial effects:

[0026] The present disclosure can improve the measurement accuracy and stability of the sensor by fusing the calibration of three sensors and complementing each other's deficiencies, thereby improving the overall measurement accuracy and stability; reducing noise and interference; fusing the data of multiple sensors to verify each other, and when a sensor data is abnormal, it can be corrected or replaced by the data of other sensors, thereby improving the fault tolerance and robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 The present invention is a flow chart of a fusion calibration method of a gyroscope, an accelerometer and a magnetometer in an embodiment of the present specification. DETAILED DESCRIPTION

[0028] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in a variety of forms and should not be construed as being limited to the examples set forth herein; on the contrary, these embodiments are provided so that the present disclosure will be more comprehensive and complete, and the concepts of the example embodiments are fully conveyed to those skilled in the art. The described features, structures, or characteristics may be combined in one or more embodiments in any suitable manner. In the following description, many specific details are provided to provide a full understanding of the embodiments of the present disclosure. However, those skilled in the art will appreciate that the technical solutions of the present disclosure may be practiced while omitting one or more of the specific details, or other methods, components, devices, steps, etc. may be adopted. In other cases, known technical solutions are not shown or described in detail to avoid obscuring various aspects of the present disclosure.

[0029] In addition, the accompanying drawings are only schematic illustrations of the present disclosure. The same reference numerals in the figures represent the same or similar parts, and thus their repeated description will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities, which do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software form, or implemented in one or more hardware modules or integrated circuits, or implemented in different networks and / or processor devices and / or microcontroller devices.

[0030] like Figure 1As shown, this specification provides a gyroscope, accelerometer and magnetometer fusion calibration method, the method comprising steps S101-104:

[0031] In step S101, the XYZ three-axis data of the accelerometer, gyroscope and magnetometer of the device are obtained.

[0032] Among them, the XYZ three-axis data of the accelerometer refers to the acceleration of the device along the XYZ three-axis directions; the XYZ three-axis data of the gyroscope refers to the speed of the device rotating along the XYZ three-axis; the XYZ three-axis data of the magnetometer refers to the magnetic field strength sensed in the XYZ three-axis directions.

[0033] In step S102, based on the XYZ three-axis data of the accelerometer, it is determined whether the device meets the preset first static condition. If yes, the three-axis offset of the gyroscope is obtained, and the output data of the gyroscope is compensated based on the three-axis offset.

[0034] Among them, the XYZ three-axis data of the accelerometer is used to determine whether the device meets the first static condition. If it meets the first static condition, the gyroscope calibration is performed. The first static condition includes: within a preset time, the absolute value of the X-axis data of the accelerometer is less than 0.05m / s2, the absolute value of the Y-axis data is less than 0.05m / s2, and the absolute value of the Z-axis data is less than 0.05m / s2. When obtaining the three-axis offset, the XYZ three-axis data of the gyroscope is subtracted from the standard static data to obtain the three-axis offset.

[0035] Specifically, the data of the three axes of acceleration x, y, and z can be obtained from the physical chip, and the data jitter judgment can be performed in the data within 0.1s: if |x| < 0.05m / s2; |y| < 0.05m / s2; |z| < 0.05m / s2 within 0.1s, and this condition is met, it can be judged that the device is in a static state at this time, and the gyroscope can be calibrated, and the standard static gyroscope data is subtracted from the gyroscope data to obtain Delta x / Delta y / Delta z. The Delta x / Delta y / Delta z is the offset of the gyroscope caused by the influence of temperature and the like. As a compensation value, it is compensated to the gyroscope data, and the output value is the data after calibration compensation; here, the acceleration and gyroscope data are fused for calibration. Through step S102, when the device is started, the sensor is calibrated using the static state. During the operation of the device, when the static state is detected, the offset of the gyroscope is updated in real time to reduce the impact of long-term drift.

[0036] In step S103, based on the XYZ three-axis data of the gyroscope, it is determined whether the device meets the preset second still condition, and based on the XYZ three-axis data of the accelerometer, it is determined whether the device meets the preset horizontal condition. If the device meets both the second still condition and the horizontal condition, an acceleration compensation value is calculated, and the output of the accelerometer is compensated based on the acceleration compensation value.

[0037] The XYZ three-axis data of the gyroscope and the XYZ three-axis data of the accelerometer are used to determine whether the device is in a stationary state and a horizontal state. If so, the accelerometer is calibrated. The second stationary condition means that the angular velocity of the gyroscope is close to zero and the device is not rotating. The horizontal condition means that the X and Y accelerations are close to 0, and Z is close to the gravity acceleration, that is, the device is placed horizontally. When calculating the acceleration compensation value, the current XYZ three-axis data of the accelerometer is subtracted from the standard data to obtain the acceleration compensation value.

[0038] Specifically, the second static condition includes: within a preset time, the X-axis data of the gyroscope is greater than -0.01rad / s and less than 0.01rad / s, the Y-axis data is greater than -0.01rad / s and less than 0.01rad / s, and the Z-axis data is greater than -0.01rad / s and less than 0.01rad / s. The horizontal condition includes: within a preset time, the X-axis data of the accelerometer is greater than -0.03m / s2 and less than 0.03m / s2, the Y-axis data is greater than -0.03m / s2 and less than 0.03m / s2, and the Z-axis data is greater than -9.77m / s2 and less than 9.83m / s2.

[0039] In step S104, the XYZ three-axis data of the gyroscope is integrated to obtain the rotation angle of the gyroscope, and the XYZ three-axis data of the magnetometer acquired in real time and the rotation angle obtained by integration are fitted to describe an arc to obtain an isosceles triangle, so as to calculate the magnetic field interference amount of the magnetometer, and the output of the magnetometer is compensated based on the magnetic field interference amount.

[0040] Among them, in this step, the rotation angle is calculated by integrating the gyroscope, and it is fitted with the magnetometer data to analyze the magnetic field interference and compensate the magnetometer data. When integrating, theta=θ0+ω·Δt is calculated; theta is the rotation angle, θ0 is the initial angle, ω is the angular velocity, and Δt is the time interval. Then, the XYZ three-axis data of the magnetometer is combined with the rotation angle data of the gyroscope to fit the rotation trajectory (arc trajectory) of the device; the change trajectory of the magnetometer data can form an isosceles triangle, and this feature can be used to analyze the interference of the magnetic field.

[0041] In the prior art, the calibration method for the magnetometer generally uses the magnetic field xyz value alone for calibration, which requires a large amount of magnetic data, and the movement amplitude and calibration time are very long. However, this step integrates the gyroscope data, and the calibration speed and efficiency will be greatly improved.

[0042] It can be seen from the above embodiments that the present invention can improve the measurement accuracy and stability of the sensor by fusing the calibration of three sensors and complementing each other's deficiencies, thereby improving the overall measurement accuracy and stability; reducing noise and interference; fusing the data of multiple sensors to verify each other, and when a sensor data is abnormal, it can be corrected or replaced by the data of other sensors, thereby improving the fault tolerance and robustness of the system.

[0043] Through the description of the above implementation, it is easy for those skilled in the art to understand that the example implementation described here can be implemented by software, or by software combined with necessary hardware. Therefore, the technical solution according to the implementation of the present disclosure can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.) or on a network, including several instructions to enable a computing device (which can be a personal computer, a server, a terminal device, or a network device, etc.) to execute the method according to the exemplary implementation of the present disclosure.

[0044] In addition, the above-mentioned figures are only schematic illustrations of the processes included in the method according to the exemplary embodiment of the present disclosure, and are not intended to be limiting. It is easy to understand that the processes shown in the above-mentioned figures do not indicate or limit the time sequence of these processes. In addition, it is also easy to understand that these processes can be performed synchronously or asynchronously, for example, in multiple modules.

[0045] Those skilled in the art will readily appreciate other embodiments of the present disclosure after considering the specification and practicing the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present disclosure that follow the general principles of the present disclosure and include common knowledge or customary technical means in the art that are not disclosed in the present disclosure. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of the present disclosure are indicated by the claims.

Claims

1. A fusion calibration method for a gyroscope, an accelerometer and a magnetometer, characterized in that: The method comprises: Get the XYZ three-axis data of the device's accelerometer, gyroscope, and magnetometer; Based on the XYZ three-axis data of the accelerometer, determine whether the device meets the preset first static condition, and if so, obtain the three-axis offset of the gyroscope, and compensate the output data of the gyroscope based on the three-axis offset; Based on the XYZ three-axis data of the gyroscope, determine whether the device meets the preset second static condition; based on the XYZ three-axis data of the accelerometer, determine whether the device meets the preset horizontal condition; if the device meets both the second static condition and the horizontal condition, calculate the acceleration compensation value, and compensate the output of the accelerometer based on the acceleration compensation value; The XYZ three-axis data of the gyroscope are integrated to obtain the rotation angle of the gyroscope, and an arc is fitted to describe the XYZ three-axis data of the magnetometer acquired in real time and the rotation angle obtained by integration to obtain an isosceles triangle, so as to calculate the magnetic field interference amount of the magnetometer, and compensate the output of the magnetometer based on the magnetic field interference amount.

2. A gyroscope, accelerometer and magnetometer fusion calibration method according to claim 1, characterized in that: The first stationary condition includes: Within a preset time, the X-axis data of the accelerometer is less than 0.05 m / s2, the Y-axis data is less than 0.05 m / s2, and the Z-axis data is less than 0.05 m / s2.

3. The fusion calibration method of a gyroscope, an accelerometer and a magnetometer according to claim 1, characterized in that: When acquiring the three-axis offset, the standard static data is subtracted from the XYZ three-axis data of the gyroscope to obtain the three-axis offset.

4. The fusion calibration method of a gyroscope, an accelerometer and a magnetometer according to claim 1, characterized in that: The second stationary condition includes: Within a preset time, the X-axis data of the gyroscope is greater than -0.01 rad / s and less than 0.01 rad / s, the Y-axis data is greater than -0.01 rad / s and less than 0.01 rad / s, and the Z-axis data is greater than -0.01 rad / s and less than 0.01 rad / s.

5. The fusion calibration method of a gyroscope, an accelerometer and a magnetometer according to claim 1, characterized in that: The level requirements include: Within the preset time, the X-axis data of the accelerometer is greater than -0.03m / s2 and less than 0.03m / s2, the Y-axis data is greater than -0.03m / s2 and less than 0.03m / s2, and the Z-axis data is greater than -9.77m / s2 and less than 9.83m / s2.

6. A gyroscope, accelerometer and magnetometer fusion calibration method according to claim 1, characterized in that: When calculating the acceleration compensation value, the standard data is subtracted from the current XYZ three-axis data of the accelerometer to obtain the acceleration compensation value.

7. The fusion calibration method of a gyroscope, an accelerometer and a magnetometer according to claim 1, characterized in that: When integrating the XYZ three-axis data of the gyroscope, the calculation formula is: theta=θ0+ω·Δt; theta is the rotation angle, θ0 is the initial angle, ω is the angular velocity, and Δt is the time interval.

Images