An Automatic Calibration Method for Electronic Compass Based on MEMS Sensors

Abstract

The present invention discloses an automatic calibration method for an electronic compass based on MEMS sensors. The method includes collecting the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions, and calculating the attitude angles of the carrier in the X-axis and Y-axis directions; obtaining the currently measured magnetic field intensity of the magnetometer of the electronic compass, and regularly calculating and updating the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions in the current attitude, calculating the attitude angle of the carrier in the Z-axis direction according to the theoretical magnetic field intensity, and calculating the magnetic field errors of the electronic compass in the X-axis, Y-axis, and Z-axis directions according to the relationship between the quaternion and the attitude angle; then calculating the comprehensive magnetic field error, and determining whether the comprehensive magnetic field error exceeds a set threshold. If it exceeds the set threshold, the magnetic field deviation is corrected. When the comprehensive magnetic field error exceeds the set threshold in the present invention, it can be automatically corrected; the problem that the electronic compass has inaccurate pointing after long-term use or being disturbed is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic calibration of electronic compasses, and particularly to an automatic calibration method for an electronic compass based on MEMS sensors. Background Art

[0002] Due to its characteristics such as small size, low power consumption, and low cost, MEMS has become increasingly popular in attitude measurement applications. However, it has problems such as output noise and zero drift that cannot be eliminated. As the service time increases, the error of its output attitude will also become larger. Existing electronic compasses generally adopt the scheme of IMU + magnetometer, and use the short-term accuracy of the gyroscope and the long-term accuracy of the accelerometer and magnetometer for fusion calculation. Due to the characteristics of MEMS, it is necessary to calibrate each sensor before use. Due to the complexity of calibration, it is often only possible to calibrate once at the factory. With the change of the application environment, especially the changes in temperature and geomagnetic field, the sensors will have errors, resulting in excessive heading errors of the electronic compass. Summary of the Invention

[0003] The purpose of the present invention is to provide an automatic calibration method for an electronic compass based on MEMS sensors in view of the deficiencies of the existing technology.

[0004] To achieve the above purpose, the present invention provides an automatic calibration method for an electronic compass based on MEMS sensors, including:

[0005] Collect the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions, and calculate the attitude angles of the carrier in the X-axis and Y-axis directions according to the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions, respectively, as follows:

[0006] θx = atan2(-ax, az)

[0007] θy = atan2(ay, ax * sin(θx) + az * cos(θx))

[0008] where θx and θy are the attitude angles of the carrier in the X-axis and Y-axis directions, respectively, and ax, ay, and az are the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions;

[0009] Obtain the currently measured magnetic field intensity of the magnetometer of the electronic compass, and regularly calculate and update the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions in the current attitude, respectively, as follows:

[0010] bx = cos(θx) * mx + sin(θx) * mz

[0011] by = cos(θy) * mx – sin(θx) * sin(θy) * my + cos(θx) * sin(θy) * mz

[0012] bz = sin(θy) * mx + sin(θx) * cos(θy) * my + cos(θx) * cos(θy) * mz

[0013] Wherein, bx, by, and bz are the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions respectively under the current attitude, and mx, my, and mz are the components of the measured magnetic field intensity of the electronic compass in the X-axis, Y-axis, and Z-axis directions;

[0014] According to the theoretical magnetic field intensities bx and by, the attitude angle θz of the carrier in the Z-axis direction is calculated as:

[0015] θz = atan2(by, bx)

[0016] According to the relationship between the quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions, calculate the magnetic field errors eMx, eMy, eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions;

[0017] According to the magnetic field errors eMx, eMy, eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions, calculate the comprehensive magnetic field error, and determine whether the comprehensive magnetic field error exceeds the set threshold. If it exceeds the set threshold, correct the magnetic field deviation, specifically as follows:

[0018]

[0019] Wherein, 、 、 are the magnetic fields in the corrected X-axis, Y-axis, and Z-axis directions respectively.

[0020] Furthermore, the relationship between the quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions is:

[0021] 。

[0022] Furthermore, the calculation methods of the magnetic field errors eMx, eMy, eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions are as follows:

[0023] 。

[0024] Furthermore, the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions are collected by a three-axis accelerometer arranged on the carrier.

[0025] Furthermore, the calculation method of the comprehensive magnetic field error is as follows:

[0026]

[0027] Among them, is the calculated comprehensive magnetic field error.

[0028] Furthermore, the set threshold is 10.

[0029] Furthermore, the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions under the current attitude are updated every ten minutes.

[0030] Beneficial effects: By collecting the three-axis accelerations of the carrier, calculating the pitch angle and roll angle of the carrier according to the three-axis accelerations of the carrier, calculating the theoretical magnetic field intensity regularly based on the currently measured magnetic field intensity of the magnetometer and the pitch angle and roll angle of the carrier, calculating the yaw angle according to the theoretical magnetic field intensity, and then calculating the magnetic field error by using the relationship between the quaternion and the attitude angle. When the comprehensive magnetic field error exceeds the set threshold, it can be automatically corrected; the magnetometer can be calibrated regularly during the use of the electronic compass, solving the problem that the electronic compass points inaccurately after long-term use or being disturbed. Brief Description of the Drawings

[0031] Figure 1 is a schematic flow chart of the automatic calibration method of the electronic compass based on MEMS sensors according to an embodiment of the present invention;

[0032] Figure 2 is a schematic diagram of the magnetic field intensity obtained by the magnetometer of the electronic compass before calibration;

[0033] Figure 3 is a schematic diagram of the magnetic field intensity obtained by the magnetometer of the electronic compass after calibration. Detailed Embodiment

[0034] The following further clarifies the present invention in conjunction with the drawings and specific embodiments. These embodiments are implemented on the premise of the technical solution of the present invention. It should be understood that these embodiments are only used to illustrate the present invention and not to limit the scope of the present invention.

[0035] As Figure 1 shown, an embodiment of the present invention provides an automatic calibration method of an electronic compass based on MEMS sensors, including:

[0036] Collect the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions, and calculate the attitude angles of the carrier in the X-axis and Y-axis directions according to the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions as follows:

[0037] θx = atan2(-ax, az)

[0038] θy = atan2(ay, ax * sin(θx) + az * cos(θx))

[0039] Wherein, θx and θy are respectively the attitude angles of the carrier in the X-axis and Y-axis directions, and ax, ay, and az are respectively the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions. The accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions can be collected by a triaxial accelerometer provided on the carrier. The attitude angle θx is the pitch angle of the carrier, and the attitude angle θy is the roll angle of the carrier.

[0040] Obtain the currently measured magnetic field intensity of the magnetometer of the electronic compass, and regularly calculate and update the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions in the current attitude, respectively, as follows:

[0041] bx = cos(θx) * mx + sin(θx) * mz

[0042] by = cos(θy) * mx – sin(θx) * sin(θy) * my + cos(θx) * sin(θy) * mz

[0043] bz = sin(θy) * mx + sin(θx) * cos(θy) * my + cos(θx) * cos(θy) * mz

[0044] Wherein, bx, by, and bz are respectively the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions calculated and updated in the current attitude, and mx, my, and mz are the components of the measured magnetic field intensity of the electronic compass in the X-axis, Y-axis, and Z-axis directions. It is preferable to calculate and update the theoretical magnetic field intensities of the carrier in the X-axis, Y-axis, and Z-axis directions in the current attitude every ten minutes.

[0045] Calculate the attitude angle θz of the carrier in the Z-axis direction according to the theoretical magnetic field intensities bx and by as:

[0046] θz = atan2(by, bx)

[0047] The above attitude angle θz is the yaw angle of the carrier.

[0048] According to the relationship between the quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions, calculate the magnetic field errors eMx, eMy, eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions. Specifically, the relationship between the above quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions is as follows:

[0049]

[0050] Based on the relationships between the above quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions, the methods for calculating the magnetic field errors eMx, eMy, and eMz are as follows:

[0051] 。

[0052] After calculating the magnetic field errors eMx, eMy, and eMz of the gyrocompass in the X-axis, Y-axis, and Z-axis directions, the comprehensive magnetic field error is calculated based on the magnetic field errors eMx, eMy, and eMz of the gyrocompass in the X-axis, Y-axis, and Z-axis directions, as follows:

[0053]

[0054] Among them, is the calculated comprehensive magnetic field error.

[0055] Then, it is judged whether the comprehensive magnetic field error exceeds the set threshold. If it exceeds the set threshold, the magnetic field deviation is corrected, as follows:

[0056]

[0057] Among them, 、 、 are the magnetic field intensity values in the corrected X-axis, Y-axis, and Z-axis directions respectively. The above set threshold is preferably 10, that is, when the comprehensive error > 10, the magnetic field deviation is corrected, otherwise no correction is made.

[0058] See Figure 2 and Figure 3 , Figure 2 and Figure 3 respectively show the magnetic field intensities in the X-axis, Y-axis, and Z-axis directions, with the unit of uT. Among them, Figure 2 shows the magnetic field intensity obtained by the magnetometer of the gyrocompass before calibration. It can be seen that its distribution is not a sphere, and the center of the sphere is not at the zero point. If this data is directly used, the calculated yaw angle will be incorrect. See Figure 3 , Figure 3 shows the corrected magnetic field intensity after automatic calibration using the present invention. Its center of the sphere is at the zero point position, and the points are basically on the spherical surface. At this time, using the magnetometer to calculate the yaw angle will be within the accuracy range.

[0059] The above is only the preferred embodiment of the present invention. It should be noted that for those of ordinary skill in the art, other parts not specifically described belong to the prior art or common general knowledge. Without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A method for automatic calibration of an electronic compass based on a MEMS sensor, characterized in that: include: Collect the acceleration of the carrier in the X-axis, Y-axis, and Z-axis directions, and calculate the carrier's attitude angles in the X-axis and Y-axis directions based on the carrier's acceleration in the X-axis, Y-axis, and Z-axis directions: θx=atan2(-ax,az) θy=atan2(ay,ax*sin(θx)+az*cos(θx)) Among them, θx and θy are the attitude angles of the carrier in the X-axis and Y-axis directions respectively, and ax, ay, and az are the accelerations of the carrier in the X-axis, Y-axis, and Z-axis directions respectively; Get the current measured magnetic field strength of the electronic compass magnetometer, and regularly calculate and update the theoretical magnetic field strength of the carrier in the X-axis, Y-axis, and Z-axis directions in the current posture: bx=cos(θx)*mx+sin(θx)*mz by=cos(θy)*mx–sin(θx)*sin(θy)*my+cos(θx)*sin(θy)*mz bz=sin(θy)*mx+sin(θx)*cos(θy)*my+cos(θx)*cos(θy)*mz Among them, bx, by, bz are the theoretical magnetic field strengths of the carrier in the X-axis, Y-axis, and Z-axis directions in the current posture, respectively, and mx, my, and mz are the components of the measured magnetic field strength of the electronic compass in the X-axis, Y-axis, and Z-axis directions; According to the theoretical magnetic field strengths bx and by, the attitude angle θz of the carrier in the Z-axis direction is calculated as: θz=atan2(by,bx) According to the relationship between the quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions, the magnetic field errors eMx, eMy, and eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions are calculated; The comprehensive magnetic field error is calculated according to the magnetic field errors eMx, eMy, and eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions, and it is determined whether the comprehensive magnetic field error exceeds the set threshold. If it exceeds the set threshold, the magnetic field deviation is corrected, as follows: ； in, , , They are the corrected magnetic fields in the X-axis, Y-axis, and Z-axis directions respectively; The relationship between the quaternions q1, q2, q3, q4 and the attitude angles θx, θy, θz of the carrier in the X-axis, Y-axis, and Z-axis directions is: ； The calculation method of the magnetic field errors eMx, eMy, and eMz of the electronic compass in the X-axis, Y-axis, and Z-axis directions is as follows: 。 2. The method for automatic calibration of an electronic compass based on a MEMS sensor according to claim 1, characterized in that: The acceleration of the carrier in the X-axis, Y-axis and Z-axis directions is collected by a three-axis accelerometer arranged on the carrier.

3. The method for automatic calibration of an electronic compass based on a MEMS sensor according to claim 1, characterized in that: The comprehensive magnetic field error is calculated as follows: ； in, is the calculated comprehensive magnetic field error.

4. The method for automatic calibration of an electronic compass based on a MEMS sensor according to claim 1, characterized in that: The set threshold is 10.

5. The method for automatic calibration of an electronic compass based on a MEMS sensor according to claim 1, characterized in that: The theoretical magnetic field strength of the carrier in the X-axis, Y-axis and Z-axis directions in the current posture is updated every ten minutes.

Images