A method for automatic magnetic field calibration based on nine-axis IMU

Abstract

The present invention provides a magnetic field automatic calibration method based on a nine-axis IMU, comprising the following steps: data acquisition, data filtering, attitude solution, magnetometer tilt compensation and model optimization, ellipse parameter solution based on SVD decomposition, and heading angle calculation. The present invention proposes a magnetic field automatic calibration method based on a nine-axis IMU, which calibrates the "hard iron" and "soft iron" errors of the magnetic field, improves the drawbacks of the least squares method, automatically screens data, simplifies the model, reduces the amount of calculation of the calibration model, and ensures the correctness of parameter estimation; compensates for the error caused by the change of the attitude of the magnetometer carrier platform; analyzes the collected data, intelligently triggers the calibration conditions, and does not require human participation in complex spatial motion actions to achieve automatic calibration of the magnetometer.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning, and in particular to a magnetic field automatic calibration method based on a nine-axis IMU. Background Art

[0002] Navigation system is an important technology in the booming development of artificial intelligence. It has been widely used in automobile driving, navigation, aviation, intelligent robots, pedestrian positioning, etc., which involves the combined use of multiple sensors. Magnetometer (electronic compass) is an important component. It is used alone or in combination with other sensors to measure the navigation direction. Magnetometer is a sensor that uses Hall elements to measure the earth's magnetic field. Its function is similar to that of a compass. Since the direction of the geomagnetic field is basically the same except for the north and south poles, from south to north, the magnetometer plays the role of measuring this direction, providing important support for human navigation activities.

[0003] However, in actual use, there are still some problems with using magnetometers for navigation. The strength of the earth's magnetic field is relatively weak, only about 0.5 Gauss, while the magnetic field strength is about 4 Gauss at a distance of 2 cm from an ordinary mobile phone speaker, and the magnetic field near a mobile phone motor reaches 6 Gausss. It can be seen that the earth's magnetic field is very susceptible to interference from electronic equipment; not only that, the current generated during the operation of live equipment and metals in the environment, such as iron, nickel and cobalt, will also interfere with the magnetic field, causing magnetometer measurement errors. Generally speaking, the so-called magnetic field interference refers to the phenomenon that the earth's magnetic field in the local area where the magnetic sensor is placed deviates due to the presence of magnetic materials or materials that can affect the local magnetic field strength. Magnetic field interference can cause errors in the heading angle measured by the magnetometer, and even lead to completely opposite results in the heading measurement.

[0004] Considering the serious consequences of magnetic field interference in the navigation environment, the magnetic field near the magnetometer must be calibrated when using it. The principle of calibration is: rotate the magnetometer in all directions in space. If there is no magnetic field interference nearby, the magnetometer three-axis output point set R = {x, y, z} will form a sphere with a radius of about 0.5 Guass centered on the coordinate origin (zero point) in space. If there is magnetic field interference nearby, the zero point will be offset, and this error is called "hard iron" interference; at the same time, it is no longer a "sphere" but becomes an "ellipsoid", and the error caused by this is called "soft iron" interference. To calibrate the magnetometer is to correct the "hard iron" and "soft iron" interference. Since the error caused by the "hard iron" interference is much greater than the "soft iron" error, in many applications where the accuracy requirement is not high, only the "hard iron" error is calibrated, that is, only the zero point offset of the sphere is calculated.

[0005] At present, the general methods of magnetic field calibration are: 1. Plane calibration method; 2. Stereo 8-shaped calibration method; 3. Ten-sided calibration method; 4. Least square fitting method. Among them, methods 1-3 are all calibrated by rotating the magnetometer in a fixed posture (method 1, place the magnetometer horizontally and rotate it 360 degrees; method 2, hold the magnetometer and move it around the "8" in space, this method is currently widely used in mobile phones; method 3, use the magnetometer to pose 10 fixed postures). Through these methods, the maximum and minimum values ​​of the magnetic field strength of each axis of the magnetometer are obtained, and the arithmetic average is taken as the "zero point offset" of each axis of the magnetometer; method 4 is to place the magnetometer in a constant magnetic field with a known magnetic field strength, collect magnetometer data, establish an error model, and then use the least squares idea to estimate the magnetometer calibration parameters. The problems of the prior art are as follows: Problem 1: Although methods 1-3 are convenient and simple, they can only estimate the "zero offset" and cannot calibrate the "soft iron" error. In many navigation applications, they will cause large errors. Method 4 requires a large amount of data that meets the requirements. On the one hand, the amount of calculation is relatively large and the convergence is slow, which is difficult to implement in some real-time application solutions or low-end embedded systems. On the other hand, if the movement span of the collected data is not large, such as only a small angle movement, the model estimation will fail. Problem 2: Methods 1-4 require human participation to perform specific actions. First, they are not smart enough, and second, they are difficult to implement in many actual navigation applications. For example, in the magnetic field calibration of unmanned vehicles, it is difficult to flip the vehicle in multiple directions for calibration. If it is done, it is very troublesome and time-consuming. For example, in the application of indoor navigation and positioning of pedestrians, the magnetic field during the movement is often changing. When performing tasks, it is impossible for humans to participate in the magnetic field calibration frequently. Problem 3: The purpose of magnetic field calibration is to calculate the heading. When actually measuring the heading after calibration, it needs to be placed horizontally. If the magnetometer is tilted, it will cause a large measurement error. In practical applications, the attitude change of the magnetometer carrier is often relatively large, so the measured heading will introduce a large error during the attitude change process. Summary of the invention

[0006] In view of the defects in the prior art, the present invention proposes a magnetic field automatic calibration method based on a nine-axis IMU, which calibrates the "hard iron" and "soft iron" errors of the magnetic field, improves the disadvantages of the least squares method, automatically screens data, simplifies the model, reduces the calculation amount of the calibration model, and ensures the correctness of parameter estimation; compensates for the error caused by the posture change of the magnetometer carrier platform; and realizes automatic calibration of the magnetometer by analyzing the collected data and intelligently triggering the calibration conditions, without the need for human participation in complex spatial motion actions.

[0007] The present invention provides a method for automatic magnetic field calibration based on a nine-axis IMU, comprising the following steps:

[0008] S1. Data acquisition: using a nine-axis IMU to collect relevant data of the carrier platform, wherein the nine-axis IMU is composed of a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer;

[0009] S2, data filtering, low-pass filtering the data of the three-axis accelerometer, and band-pass filtering the data of the three-axis gyroscope;

[0010] S3, attitude solution, based on the data of the three-axis accelerometer and the three-axis gyroscope, calculate the pitch angle and roll angle of the carrier platform;

[0011] S4, magnetometer tilt compensation and model optimization, using the pitch angle and roll angle obtained in step S3, the data of the three-axis magnetometer is converted from the carrier coordinate system to the navigation coordinate system, so as to convert the ellipsoid image formed by the rotation of the three-axis magnetometer in all directions in space into a plane ellipse;

[0012] S5, solving the ellipse parameters based on SVD decomposition, according to the ellipse target optimization function, through deformation and variable transformation, the ellipse obtained in step S4 is converted into a homogeneous equation system, and the solution of the equation system is obtained by SVD decomposition;

[0013] S6, calculating the heading angle, the solution of the equation group obtained in step S5 is used to obtain calibration parameters, the magnetometer data is corrected according to the calibration parameters, and then the heading angle is calculated.

[0014] Optionally, the specific steps of posture solution in step S3 are as follows:

[0015] S31, attitude update, substitute the three-axis angular velocity measured by the three-axis gyroscope in step S1 into the quaternion differential equation for solution, and update the quaternion by the gyroscope integral according to the Longo Kutta method. Suppose the data measured by the three-axis gyroscope is g=(gx,gy,gz), and the sampling time is t, then:

[0016]

[0017] Where w = sqrt(gx*gx+gy*gy+gz*gz), q′ is the updated quaternion, q = [q0 q1 q2 q3] T is the quaternion of the previous moment;

[0018] S32, error correction, the error is corrected using the EKF method, assuming the corrected quaternion vector is err_q, the corrected quaternion is q, then the corrected true quaternion is:

[0019] q = q′ + err_q (2);

[0020] S33, solving attitude angle

[0021] After the corrected quaternion is obtained according to step S32, the required posture is solved and converted into Euler angles. The calculation formula is:

[0022] pitch=atan(2q1q2+2q3q0,1-2q0q0-2q1q1)

[0023] roll=asin(-2q0q2+2q3q1)

[0024] yaw=-astan(2q3q2+2q0q1,1-2q1q1-2q2q2) (3)

[0025] Among them, pitch represents the pitch angle, roll represents the roll angle, and yaw represents the yaw angle.

[0026] Optionally, the specific steps of magnetometer tilt compensation in step S4 are as follows:

[0027] Assume that the output of the three-axis magnetometer at time k is The output is the coordinates of the carrier coordinate system, and the heading is calculated according to the magnetometer formula:

[0028]

[0029] Among them, D is the magnetic declination, which varies from region to region and can be obtained by looking up the table. y , B x are the projections of the geomagnetic field in the horizontal direction, which are decomposed into the orthogonal components of the horizontal y-axis and x-axis in the navigation coordinate system. The attitude information calculated in step S23 is converted to the navigation system to obtain B y , B x , the calculation formula is:

[0030]

[0031]

[0032] Optionally, the method for model optimization in step S4 is as follows:

[0033] Ideally, when the magnetometer is in an ideal geomagnetic field, the output model satisfies:

[0034]

[0035] Among them, M MAG (m x ,m y ,m z ) is the magnetometer measurement data, G is the ideal geomagnetic field, which is about 0.5Guass, and can be obtained by looking up the table.

[0036] Due to the error of the quantization factor and the existence of zero bias, the output model of the magnetometer satisfies:

[0037] (k x (m x +b x )) 2 +(k y (m y +b y )) 2 +(k z (m z +b z )) 2 =G 2 (7)

[0038] Among them, k x ,k y ,k z is the scale factor, also known as the “soft iron” error, b x ,b y ,b z is the zero bias, or “hard iron” error,

[0039] The pitch and roll angles obtained by attitude solution are used for tilt compensation, and the output model of the magnetometer is simplified to:

[0040] (k x 'B x +b x ′) 2 +(k y 'B y +b y ′) 2 =G′ 2 (8)

[0041] make

[0042] B x ′=k x 'B x +b x '

[0043] B y ′=k y 'B y +b y ′ (9)

[0044] Then the following optimization model can be obtained:

[0045]

[0046] Optionally, the method for solving the ellipse parameters based on SVD decomposition in step S5 is as follows:

[0047] By transforming equation (10) and changing variables, we can obtain the linear homogeneous equation system:

[0048] AgV=0 (11)

[0049] in

[0050]

[0051]

[0052] Vector V is the solution to the homogeneous equation, which can be obtained by SVD decomposition. It should be noted that vector V is not a direct solution to equation group (10), but needs to be transformed as follows:

[0053]

[0054]

[0055]

[0056]

[0057]

[0058] Obtain the magnetometer calibration parameter k x ′,k y ′,b x ′,b y ’, and then substitute it into (10) to obtain the calibrated data.

[0059] It can be seen from the above technical solution that the beneficial effects of the present invention are as follows: the present invention provides a method for automatic magnetic field calibration based on a nine-axis IMU, comprising the following steps: data acquisition, data filtering, attitude solution, magnetometer tilt compensation and model optimization, ellipse parameter solution based on SVD decomposition, and heading angle calculation. The present invention proposes a method for automatic magnetic field calibration based on a nine-axis IMU, which calibrates the "hard iron" and "soft iron" errors of the magnetic field, improves the drawbacks of the least squares method, automatically screens data, simplifies the model, reduces the amount of calculation of the calibration model, and ensures the correctness of parameter estimation; compensates for the error caused by the change in the attitude of the magnetometer carrier platform; by analyzing the collected data, intelligently triggering the calibration conditions, and realizing automatic calibration of the magnetometer without the need for human participation in complex spatial motion actions. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for the specific embodiments or the description of the prior art. In all the drawings, similar elements or parts are generally identified by similar reference numerals. In the drawings, the elements or parts are not necessarily drawn according to the actual scale.

[0061] Figure 1 It is the algorithm block diagram of the present invention;

[0062] Figure 2 This is a comparison chart of the data before and after the magnetometer magnetic field calibration.

[0063] Reference numerals:

[0064] 1-the trajectory of sample data distribution after calibration, 2-the trajectory of sample data distribution before calibration. DETAILED DESCRIPTION

[0065] The following embodiments of the technical solution of the present invention are described in detail in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and are therefore only used as examples, and cannot be used to limit the protection scope of the present invention.

[0066] Referring to the figure, this embodiment provides a method for automatic magnetic field calibration based on a nine-axis IMU, characterized in that it includes the following steps:

[0067] S1. Data acquisition: using a nine-axis IMU to collect relevant data of the carrier platform, wherein the nine-axis IMU is composed of a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer;

[0068] S2, data filtering, low-pass filtering the data of the three-axis accelerometer, and band-pass filtering the data of the three-axis gyroscope;

[0069] S3, attitude solution, based on the data of the three-axis accelerometer and the three-axis gyroscope, calculate the pitch angle and roll angle of the carrier platform; the specific steps of attitude solution are as follows:

[0070] S31, attitude update, substitute the three-axis angular velocity measured by the three-axis gyroscope in step S1 into the quaternion differential equation for solution, and update the quaternion by the gyroscope integral according to the Longo Kutta method. Suppose the data measured by the three-axis gyroscope is g=(gx,gy,gz), and the sampling time is t, then:

[0071]

[0072] Where w = sqrt(gx*gx+gy*gy+gz*gz), q′ is the updated quaternion, q = [q0 q1 q2 q3] T is the quaternion of the previous moment;

[0073] S32, error correction. Since the attitude angle calculated by the gyroscope integration alone will drift, it needs to be corrected by the accelerometer. The correction methods include complementary filtering and Kalman filtering. In order to estimate the state more accurately, the present invention adopts the EKF method (extended Kalman filtering) to correct the error. Let the corrected quaternion vector be err_q, and the corrected quaternion be q. Then the corrected true quaternion is:

[0074] q = q′ + err_q (2);

[0075] S33, solving attitude angle

[0076] After the corrected quaternion is obtained according to step S32, the required posture is solved and converted into Euler angles. The calculation formula is:

[0077] pitch=atan(2q1q2+2q3q0,1-2q0q0-2q1q1)

[0078] roll=asin(-2q0q2+2q3q1)

[0079] yaw=-astan(2q3q2+2q0q1,1-2q1q1-2q2q2) (3)

[0080] Where pitch represents the pitch angle, roll represents the roll angle, and yaw represents the yaw angle. Note that the calculated yaw angle is a relative angle, has no absolute initial value, is generally initialized to zero, and is not used in the attitude tilt compensation of the present invention.

[0081] S4, magnetometer tilt compensation and model optimization, using the pitch angle and roll angle obtained in step S3, the data of the three-axis magnetometer is converted from the carrier coordinate system to the navigation coordinate system, so as to convert the ellipsoid image formed by the rotation of the three-axis magnetometer in all directions in space into a plane ellipse;

[0082] The specific steps of magnetometer tilt compensation are as follows:

[0083] Assume that the output of the three-axis magnetometer at time k is The output is the coordinates of the carrier coordinate system, and the heading is calculated according to the magnetometer formula:

[0084]

[0085] Among them, D is the magnetic declination, which varies from region to region and can be obtained by looking up the table. y , B xare the projections of the geomagnetic field in the horizontal direction, which are decomposed into the orthogonal components of the horizontal y-axis and x-axis in the navigation coordinate system. It can be seen that the tilt compensation method of the magnetometer is to use the attitude information calculated in step S23 and convert it to the navigation system to obtain B y , B x , the calculation formula is:

[0086]

[0087]

[0088] It can be seen that the calibration problem of the magnetometer can be transformed into the calibration problem of the horizontal dual-axis magnetic force through inclination compensation. This greatly simplifies the algorithm difficulty and calculation amount in the subsequent convex optimization process.

[0089] The method of model optimization is as follows:

[0090] Ideally, when the magnetometer is in an ideal geomagnetic field, the output model satisfies:

[0091]

[0092] Among them, M MAG (m x ,m y ,m z ) is the magnetometer measurement data, G is the ideal geomagnetic field, which is about 0.5Guass, and can be obtained by looking up the table.

[0093] In practical applications, due to the error of the quantization factor and the existence of zero bias, the output model of the magnetometer satisfies:

[0094] (k x (m x +b x )) 2 +(k y (m y +b y )) 2 +(k z (m z +b z )) 2 =G 2 (7)

[0095] Among them, k x ,k y ,k z is the scale factor, also known as the “soft iron” error, b x ,b y ,b z is the zero bias, or “hard iron” error,

[0096] In this embodiment, the pitch and roll angles obtained by attitude solution are used for tilt compensation, and the output model of the magnetometer is simplified as follows:

[0097] (k x 'B x +b x ′) 2 +(k y 'B y +b y ′) 2 =G′ 2 (8)

[0098] Thus, the calibration of the magnetometer is to find k x ′,k y ′,b x ′,b y ’The process of these 4 unknown quantities.

[0099] make

[0100] B x ′=k x 'B x +b x '

[0101] B y ′=k y 'B y +b y ′ (9)

[0102] Then the following optimization model can be obtained:

[0103]

[0104] S5, solving the ellipse parameters based on SVD decomposition, according to the ellipse target optimization function, through deformation and variable transformation, the ellipse obtained in step S4 is converted into a homogeneous equation system, and the solution of the equation system is obtained by SVD decomposition;

[0105] The method for solving the ellipse parameters based on SVD decomposition is as follows:

[0106] By transforming equation (10) and changing variables, we can obtain the linear homogeneous equation system:

[0107] AgV=0 (11)

[0108] in

[0109]

[0110]

[0111] Vector V is the solution to the homogeneous equation, which can be obtained by SVD decomposition. It should be noted that vector V is not a direct solution to equation group (10), but needs to be transformed as follows:

[0112]

[0113]

[0114]

[0115]

[0116]

[0117] Obtain the magnetometer calibration parameter k x ′,k y ′,b x ′,b y ’, and then substitute it into (10) to obtain the calibrated data.

[0118] S6, calculating the heading angle, the solution of the equation group obtained in step S5 is used to obtain calibration parameters, the magnetometer data is corrected according to the calibration parameters, and then the heading angle is calculated.

[0119] The present invention proposes a magnetic field automatic calibration method based on a nine-axis IMU, which calibrates the "hard iron" and "soft iron" errors of the magnetic field, improves the disadvantages of the least square method, automatically screens data, simplifies the model, reduces the calculation amount of the calibration model, and ensures the correctness of parameter estimation; compensates for the error caused by the posture change of the magnetometer carrier platform; and realizes automatic calibration of the magnetometer by analyzing the collected data and intelligently triggering the calibration conditions without the need for human participation in complex spatial motion actions.

[0120] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be included in the scope of the claims and specification of the present invention.

Claims

1. A magnetic field automatic calibration method based on nine-axis IMU, characterized in that: The steps include: S1. Data acquisition: using a nine-axis IMU to collect relevant data of the carrier platform, wherein the nine-axis IMU is composed of a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer; S2, data filtering, low-pass filtering the data of the three-axis accelerometer, and band-pass filtering the data of the three-axis gyroscope; S3, attitude solution, based on the data of the three-axis accelerometer and the three-axis gyroscope, calculate the pitch angle and roll angle of the carrier platform; The specific steps of posture solution in step S3 are as follows: S31, attitude update, the three-axis angular velocity measured by the three-axis gyroscope in step S1 is substituted into the quaternion differential equation for solution, and the quaternion is updated by the gyroscope integral according to the Longo Kutta method. Assume that the data measured by the three-axis gyroscope is g=(gx, gy, gz), and the sampling time is t, then: Where w = sqrt(gx*gx+gy*gy+gz*gz), q′ is the updated quaternion, q = [q0 q1 q2 q3] T is the quaternion of the previous moment; S22, error correction, the error is corrected using the EKF method, assuming the corrected quaternion vector is err_q, the corrected quaternion is q″, then the corrected true quaternion is: q″=q′+err_q (2) S33, solving attitude angle After the corrected quaternion is obtained according to step S32, the required posture is solved and converted into Euler angles. The calculation formula is: Among them, pitch represents the pitch angle, roll represents the roll angle, and yaw represents the yaw angle; S4, magnetometer tilt compensation and model optimization, using the pitch angle and roll angle obtained in step S3, the data of the three-axis magnetometer is converted from the carrier coordinate system to the navigation coordinate system, so as to convert the ellipsoid image formed by the rotation of the three-axis magnetometer in all directions in space into a plane ellipse; The specific steps of magnetometer tilt compensation in step S4 are as follows: Assume that the output of the three-axis magnetometer at time k is The output is the coordinates of the carrier coordinate system, and the heading is calculated according to the magnetometer formula: Where D is the magnetic declination, obtained by looking up the table, and B y , B x are the projections of the geomagnetic field in the horizontal direction, which are decomposed into orthogonal components of the horizontal y-axis and x-axis in the navigation coordinate system; Using the attitude information calculated in step S23, convert it to the navigation system and obtain B y , B x , the calculation formula is: B y , B x The value at time k; The method of model optimization in step S4 is as follows: Ideally, when the magnetometer is in an ideal geomagnetic field, the output satisfies: Among them, M MAG (m x , m y , m z ) is the magnetometer measurement data, G is the ideal geomagnetic field; Due to the error of the quantization factor and the existence of zero bias, the output model of the magnetometer satisfies: (k x (m x +b x )) 2 +(k y (m y +b y )) 2 +(k z (m z +b z )) 2 =G 2 (7) Among them, k x , k y , k z is the scale factor, i.e. the soft iron error, b x , b y , b z is the zero bias, i.e. the hard iron error; The pitch and roll angles obtained by attitude solution are used for tilt compensation, and the output model of the magnetometer is simplified to: (k′ x B x +b′ x ) 2 +(k′ y B y +b′ y ) 2 =G′ 2 (8) make The following optimization model is obtained: S5, solving the ellipse parameters based on SVD decomposition, according to the ellipse target optimization function, through deformation and variable transformation, the ellipse obtained in step S4 is converted into a homogeneous equation system, and the solution of the equation system is obtained by SVD decomposition; The method for solving the ellipse parameters based on SVD decomposition in step S5 is as follows: By transforming equation (10) and changing variables, we can obtain the linear homogeneous equation system: AgV=0 (11) in V=[a,b,c,d,e] T (12) in Vector V is the solution to the homogeneous equation, which is obtained by SVD decomposition. Vector V is not a direct solution to the equation system (10) and needs to be transformed as follows: Obtain the magnetometer calibration parameter k′ x , k′ y , b′ x , b′ y Then, substitute into (10) to obtain the calibrated data; S6, calculating the heading angle, the solution of the equation group obtained in step S5 is used to obtain calibration parameters, the magnetometer data is corrected according to the calibration parameters, and then the heading angle is calculated.

Images