CN112461224A - Magnetometer calibration method based on known attitude angle - Google Patents

Abstract

The invention provides a magnetometer calibration method based on a known attitude angle. Based on the assumption that the total magnetic field strength of a fixed position is unchanged and the projection component of the magnetic field strength of the fixed position in the same coordinate system is unchanged, the observed value of the magnetometer is projected to a local coordinate system by using a known attitude angle according to a magnetometer measurement model, and magnetometer calibration parameters are obtained through least square iterative solution. Compared with the prior calibration technology, the method has the advantages of simple algorithm and higher efficiency. Meanwhile, the calibration of the soft and hard magnetic effects of the magnetometer is realized, and the coordinate system of the magnetometer is aligned with the coordinate system of the inertial sensor, so that the information of the magnetometer and the inertial sensor is put in a common frame, and the magnetometer and the inertial sensor can play an important role in the fusion positioning of the multisource sensor and the sensor array.

Description

A Magnetometer Calibration Method Based on Known Attitude Angle

technical field

The invention belongs to the field of magnetometer calibration, in particular to a magnetometer calibration method when the attitude angle is known.

Background technique

A magnetometer is a sensor used to measure the magnetic induction intensity, which can provide a reference to magnetic north and is a key information source for attitude estimation in low-cost, high-performance navigation systems. Due to the limitation of the processing technology, the magnetometer will have problems such as zero offset error, quadrature axis coupling error, scale factor error, etc., which will affect the measurement accuracy of the magnetometer; and the magnetometer is susceptible to magnetic interference from the surrounding environment, which is divided into soft magnetic interference. and hard magnetic interference. Therefore, in order to measure more accurate magnetic field information, the magnetometer needs to be calibrated. In addition, in the multi-source fusion sensor positioning, putting the information of the magnetometer and the inertial sensor in a common frame is the basis to achieve high-precision positioning, but due to the installation process, soft magnetic interference and other reasons, the axis of the magnetometer and the inertial sensor It is difficult to achieve complete consistency, so the cross-alignment calibration of the magnetometer and the inertial sensor is a key point and an urgent problem to be solved.

At present, according to domestic and foreign literatures, ellipsoid fitting based on maximum likelihood estimation method is the most commonly used intrinsic calibration method for magnetometers. The fact that the measured value of is constant regardless of orientation. For cross-calibration between sensors, it is mainly done by relying on local gravity information, but this calibration method relies on accelerometer measurements.

To sum up, when the magnetometer is calibrated, how to quickly complete the internal calibration and cross-calibration of the magnetometer is a key problem that needs to be solved. The present invention proposes a simple and effective method of magnetometer calibration and cross-calibration with inertial sensors based on the least squares method, including but not limited to this application scenario, without relying on attitude and gravity information.

SUMMARY OF THE INVENTION

Aiming at the requirement of quick calibration of the magnetometer and the problem that the shaft system of the magnetometer is not aligned with the shaft system of the IMU inertial sensor, the present invention proposes a method to complete the intrinsic calibration of the magnetometer based on the principle of least squares when the attitude angle is known. The cross-calibration of the IMU inertial sensor can quickly calibrate the magnetometer quadrature axis coupling error, scale factor error, zero offset error, soft magnetic error, hard magnetic error, and realize the axis alignment of the magnetometer and the IMU inertial sensor. The invention does not need any external equipment and parameter setting, is simple and feasible, has good universality, can meet the requirements of rapid (about 20s) calibration on site, and has good calibration accuracy.

The present invention adopts the following technical scheme: an internal calibration of the magnetometer based on the least squares method and the alignment with the axis of the IMU inertial sensor are mainly completed by rotating the smart phone, and the geomagnetic field strength based on a certain position is Under the assumption that the three-axis magnetic field components in the local coordinate system are constant and the total magnetic field strength is constant, the magnetometer calibration parameters are obtained by modeling the measurement model of the magnetometer, using Taylor expansion and the least squares method; The protocol includes the following steps:

Step 1, select an arbitrary location in an outdoor open area, and query the earth magnetic field reference model according to the latitude and longitude coordinates of the location to obtain the reference value of the total magnetic field strength of the location;

Step 2, take the sensor measurement center as the rotation center, make it rotate multiple times around the X, Y, and Z axes of the carrier coordinate system, and obtain the attitude angle of each epoch during the calibration process (ie pitch angle, roll angle, heading angle) and magnetic field information;

Step 3: Establish a magnetometer measurement model, determine parameters to be estimated, perform Taylor expansion on the measurement model of the magnetometer, and use least squares to iteratively obtain parameters to be estimated.

Further, when modeling the measurement model of the magnetometer, it is considered that the magnetometer is subject to soft magnetic error, scale factor error, quadrature coupling error, zero offset error, hard iron error and sensor noise, and the magnetometer in the carrier coordinate system. The measurement model is as follows:

的乘积作为一个待估矩阵，记为

将b_M和b_HI之和作为一个待估矩阵，记为B_M，模型简化后如下式，待估参数为

和B_M；Perform mean filtering on the original observation data m _{b_init} of the magnetometer to reduce the noise n _M of the magnetometer to obtain m _b ;

The product of , as a matrix to be estimated, is denoted as

The sum of b _M and b _HI is taken as a matrix to be estimated, denoted as B _M , the model is simplified as follows, and the parameters to be estimated are

and _BM ;

表示载体坐标系下磁场强度的真实值；

表示磁力计坐标系到惯性传感器IMU坐标系的姿态旋转矩阵，为3×3方阵；C_SI表示软磁效应变化矩阵；C_NO是磁力计三轴由非正交转化为正交的变化矩阵，为3×3方阵，主对角线元素为0；S_M为比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；m_{b_init}表示磁力计原始观测数据；b_M表示磁力计零位偏置；b_HI表示硬磁效应误差；n_M为磁力计噪声矢量。Among them, b represents the carrier coordinate system b, the b system is based on the inertial sensor IMU phase center as the origin, the x-axis points to the forward direction of the carrier, the y-axis is perpendicular to the x-axis and points to the right side of the carrier, and the z-axis is perpendicular to the x-axis and the y-axis. form a right-handed system;

Represents the true value of the magnetic field strength in the carrier coordinate system;

Represents the attitude rotation matrix from the magnetometer coordinate system to the inertial sensor IMU coordinate system, which is a 3×3 square matrix; _CSI represents the soft magnetic effect change matrix; C _NO is the change matrix of the magnetometer three-axis transformation from non-orthogonal to orthogonal , is a 3×3 square matrix, and the main diagonal element is 0; S _M is the scale factor, which is a 3×3 square matrix, only the main diagonal element has data, and the other elements are 0; m _{b_init} represents the original magnetometer observation data; b _M is the magnetometer zero offset; b _HI is the hard magnetic effect error; n _M is the magnetometer noise vector.

之后迭代计算时

通过

求得，然后求得当地坐标系下的磁场强度真值

计算关系如下式：Further, when iterative calculation is performed on the magnetometer measurement model, m _b is firstly used as the reference true value of the magnetic field strength in the carrier coordinate system during the initial iterative calculation, that is,

After iterative calculation

pass

Obtained, and then obtained the true value of the magnetic field strength in the local coordinate system

The calculation relationship is as follows:

表示第k个历元载体坐标系到当地坐标系的姿态旋转矩阵，

表示第k个历元载体坐标系下磁场强度参考真值；n表示当地坐标n系，n系是以惯性传感器IMU相位中心为原点，x轴平行于当地水平面指向正北，y轴平行于当地水平面指向正东，z轴垂直于当地水平面向下，三者构成右手系；In the formula, k represents the k-th epoch, and j represents the total number of epochs;

Represents the attitude rotation matrix from the kth epoch carrier coordinate system to the local coordinate system,

Represents the true value of the magnetic field strength in the kth epoch carrier coordinate system; n represents the local coordinate n system, the n system takes the inertial sensor IMU phase center as the origin, the x-axis is parallel to the local horizontal plane and points to true north, and the y-axis is parallel to the local The horizontal plane points to the due east, and the z-axis is perpendicular to the local horizontal plane downward, and the three constitute a right-handed system;

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}，将其作为当地坐标系下的磁场真值，进而得到载体坐标系下磁场强度参考真值

M_{n_ture}及

计算过程如下：Under the assumption that the total magnetic intensity remains unchanged, the total magnetic field intensity ||M _{n_CGRF} || can be obtained according to _{Mn_CGRF} , and by calculating

proportional to ||M _{n_CGRF} ||, the

The data of each axis changes in equal proportion to obtain M _{n_ture} , which is used as the true value of the magnetic field in the local coordinate system, and then the reference true value of the magnetic field strength in the carrier coordinate system is obtained.

M _{n_ture} and

The calculation process is as follows:

表示当地坐标系到载体坐标系的姿态旋转矩阵；in,

Represents the attitude rotation matrix from the local coordinate system to the carrier coordinate system;

线性化，根据最小二乘法迭代求得待估参数，具体实现方式如下：Further, the first-order Taylor expansion will simplify the model

Linearization, the parameters to be estimated are iteratively obtained according to the least squares method, and the specific implementation is as follows:

为3×3方阵，待估参数B_M为3×1矩阵，具体表现为：Parameters to be estimated

is a 3×3 square matrix, and the parameter to be estimated B _M is a 3×1 matrix, which is specifically expressed as:

展开，the formula

expand,

There are a total of 12 parameters to be found, denoted as X,

X=[a ₁ a ₂ a ₃ a ₄ a ₅ a ₆ a ₇ a ₈ a ₉ b ₁ b ₂ b ₃ ] ^T

Since this formula is nonlinear, it is expanded according to Taylor's formula, discarding second-order terms and higher-order terms, and the superscript i represents the number of iterations, as follows:

where Δ represents the residual, let:

Let: x ⁱ =X ⁱ -X ^i-1 ,

Equation (7) is simplified to:

将式(8)写为残差方程的形式，make

Write equation (8) in the form of the residual equation,

The solution result of Equation (9) can be obtained by the least square method, each observation value is equal weight, P is the identity matrix, and we can obtain

x = (H ^T PH) ^-1 H ^T Pz (10)

X ⁱ =x ⁱ +X ^i-1 (11)

更新

重复该迭代过程直至收敛，得到参数结果。In the formula, X ⁱ is the m _b obtained by the mean filtering of the original observation data m _{b_in} of the magnetometer.

renew

This iterative process is repeated until convergence, and the parametric results are obtained.

Further, the initial value of the least squares method is obtained in the following way,

磁力计和惯性传感器坐标系之间的旋转角为小角度，故

接近单位阵；C_SI各个元素值都比较小；C_NO主对角线元素为0，其余元素也较小；S_M只有主对角线元素，各元素数值接近1；故

的初值为单位阵I_3×3；for

The rotation angle between the magnetometer and the inertial sensor coordinate system is a small angle, so

It is close to the unit matrix; the value of each element of _CSI is relatively small; the main diagonal element of _{C NO} is 0, and the other elements are also small; SM has only main diagonal elements, and the value of each element is close to 1; therefore

The initial value of the unit matrix I _{3 × 3} ;

将磁力计原始观测数据m_{b_init}作为第一次迭代计算时载体坐标系下的磁场真值，即

根据每个历元的

(

表示每个历元的载体坐标系到当地坐标系的姿态旋转矩阵)，求得当地坐标系下各轴的磁场强度的均值

作为当地坐标系下的磁场强度的真值。在基于磁强总强度不变的假设下，根据M_{n_CGRF}求得其磁场总强度||M_{n_CGRF}||。通过计算出

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}，将其作为当地坐标系下的磁场真值。进而得到载体坐标系下磁场强度参考真值

For the initial value of _BM , only the magnetometer is affected by the bias error, and by further simplifying the model, that is,

The original observation data m _{b_init} of the magnetometer is taken as the true value of the magnetic field in the carrier coordinate system during the first iterative calculation, namely

According to each epoch

(

Represents the attitude rotation matrix from the carrier coordinate system of each epoch to the local coordinate system), and obtains the mean value of the magnetic field strength of each axis in the local coordinate system

as the true value of the magnetic field strength in the local coordinate system. Under the assumption that the total magnetic intensity remains unchanged, the total magnetic field intensity ||M _{n_CGRF} || is obtained according to _{Mn_CGRF} . by calculating

proportional to ||M _{n_CGRF} ||, the

The data of each axis is changed proportionally to obtain M _{n_ture} , which is taken as the true value of the magnetic field in the local coordinate system. And then get the reference true value of the magnetic field strength in the carrier coordinate system

求差得到每个历元的零偏，再将其求平均得到B_M；Compare the original data m _{b_init} with

Calculate the difference to get the zero offset of each epoch, and then average it to get _BM ;

重复上述过程，最终求得B_M，将其作为泰勒展开中的初值。When the difference between the BM obtained in this iteration and the _BM obtained in the previous iteration is less than _0.1mGauss , the zero offset calibration is considered to be completed, otherwise it is updated.

Repeat the above process, and finally obtain B _M , which is used as the initial value in Taylor expansion.

The present invention has the following beneficial effects:

(1) The present invention completes the internal calibration of the magnetometer and the cross-calibration with the inertial sensor on the basis of obtaining the high-precision attitude angle, which can reduce the zero offset error, quadrature axis coupling error, scale factor error, soft magnetic interference and hard magnetic Interference and shaft misalignment error between magnetometer and inertial sensor, the calibration effect is better.

(2) The present invention is simple to operate, easy to implement, and has a short operation time. It only needs to take the center of the smartphone (ie, the carrier) as the rotation center, and rotate 720° around the X, Y, and Z axes of the carrier coordinate system to complete the calibration work. There is no requirement for the posture, and the calibration efficiency is high.

(3) The present invention realizes that the magnetometer coordinate system is aligned with the inertial sensor coordinate system, so that the information of the magnetometer and the inertial sensor is placed in a common frame, which can play an important role in the sensor array.

Description of drawings

Figure 1 is a schematic diagram of the mobile phone calibration action.

Figure 2 is a flow chart of iteratively solving the calibration parameters of the magnetometer.

Detailed ways

The technical solutions of the present invention will be further described below with reference to the accompanying drawings and embodiments.

Step 1, select a certain open position outdoors as the calibration position, obtain the longitude and latitude coordinates and the height of the position, obtain the magnetic field intensity M _{n_IGRF} of the position by querying the International Geomagnetic Reference Field IGRF (International Geomagnetic Reference Field) model;

Step 2, as shown in Figure 1, take the center of the smartphone (ie, the carrier) as the center of rotation, and rotate it around the X, Y, and Z axes of the carrier coordinate system by 720°;

(姿态旋转矩阵的作用是实现矢量由一个坐标系投影变换到另一个坐标系)；Step 3: Obtain the high-precision attitude rotation matrix from the carrier coordinate system of each epoch to the local coordinate system in the calibration process through the method of the patented "MEMS gyroscope automatic calibration method" through pseudo-position constraints and reverse smoothing

(The role of the attitude rotation matrix is to realize the projection transformation of the vector from one coordinate system to another coordinate system);

Step 4, establish a magnetometer measurement model, establish the relationship between the magnetic field intensity in the local coordinate system and the magnetic field intensity in the carrier coordinate system, and obtain calibration parameters by iterative calculation of the magnetometer measurement model, and the calculation process is shown in Figure 2;

Moreover, the implementation of step 4 includes the following sub-steps,

41) Model the measurement model of the magnetometer. Affected by the manufacturing process and machining accuracy, the magnetometer has quadrature coupling error, scale factor error, zero offset error and sensor noise. In addition, magnetometers are susceptible to magnetic interference from the surrounding environment. These interferences are divided into soft magnetic interference and hard magnetic interference. Their manifestations are the same as quadrature coupling error, scale factor error, and zero offset error. Therefore, it is necessary to complete the calibration of the above errors at the same time. In addition, there is a misalignment error between the magnetometer shaft system and the inertial sensor shaft system. The present invention considers that the carrier coordinate system is the same as the inertial sensor coordinate system axis, so the measurement model of the magnetometer in the carrier coordinate system is as follows:

表示载体坐标系下磁场强度的真实值；

表示磁力计坐标系到惯性传感器IMU坐标系的姿态旋转矩阵，为3×3方阵；C_SI表示软磁效应变化矩阵；C_NO是磁力计三轴由非正交转化为正交的变化矩阵，为3×3方阵，主对角线元素为0；S_M为比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；m_{b_init}表示磁力计原始观测数据，b_M表示磁力计零位偏置；b_HI表示硬磁效应误差；n_M为磁力计噪声矢量。In the formula, b represents the carrier coordinate system b, the b system is based on the inertial sensor IMU phase center as the origin, the x-axis points to the forward direction of the carrier, the y-axis is perpendicular to the x-axis and points to the right side of the carrier, and the z-axis is perpendicular to the x-axis and the y-axis. And form a right-handed system; n represents the local coordinate n system, the n system is based on the inertial sensor IMU phase center as the origin, the x-axis is parallel to the local horizontal plane and points to the north, the y-axis is parallel to the local horizontal plane and points to the east, and the z-axis is perpendicular to the local horizontal plane. Face down, the three form a right-hand system;

Represents the true value of the magnetic field strength in the carrier coordinate system;

Represents the attitude rotation matrix from the magnetometer coordinate system to the inertial sensor IMU coordinate system, which is a 3×3 square matrix; _CSI represents the soft magnetic effect change matrix; C _NO is the change matrix of the magnetometer three-axis transformation from non-orthogonal to orthogonal , is a 3×3 square matrix, and the main diagonal element is 0; S _M is the scale factor, which is a 3×3 square matrix, only the main diagonal element has data, and the other elements are 0; m _{b_init} represents the original magnetometer observation data, b _M is the magnetometer zero offset; b _HI is the hard magnetic effect error; n _M is the magnetometer noise vector.

42) Perform mean filtering on the original observation data m _{b_init} of the magnetometer to reduce the noise n _M of the magnetometer to obtain m _b .

的乘积作为一个待估矩阵，记为

用m_b代替m_{b_init}和n_M之和；将b_M和b_HI之和作为一个待估矩阵，记为B_M。磁力计测量模型简化后如下式，待估参数为

和B_M。43) Put the formula (1) in

The product of , as a matrix to be estimated, is denoted as

Use m _b to replace the sum of m _{b_init} and n _M ; take the sum of b _M and b _HI as a matrix to be estimated, denoted as B _M . The magnetometer measurement model is simplified as follows, and the parameters to be estimated are

and _BM .

之后迭代计算时

通过

求得，然后求得当地坐标系下的磁场强度真值

计算关系如下式：44) Take m _b as the reference true value of the magnetic field strength in the carrier coordinate system in the initial iterative calculation, namely

After iterative calculation

pass

Obtained, and then obtained the true value of the magnetic field strength in the local coordinate system

The calculation relationship is as follows:

表示第k个历元载体坐标系到当地坐标系的姿态旋转矩阵，

表示第k个历元载体坐标系下磁场强度参考真值。In the formula, k(k=1,2...j) represents the kth epoch, and j represents the total number of epochs;

Represents the attitude rotation matrix from the kth epoch carrier coordinate system to the local coordinate system,

Indicates the reference true value of the magnetic field strength in the kth epoch carrier coordinate system.

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}。将其作为当地坐标系下的磁场真值，进而得到载体坐标系下磁场强度参考真值

M_{n_ture}及

计算过程如下：45) Under the assumption that the total magnetic intensity remains unchanged, the total magnetic field intensity ||M _{n_CGRF} || is obtained according to _{Mn_CGRF} . By calculating

proportional to ||M _{n_CGRF} ||, the

The data of each axis is changed proportionally to obtain M _{n_ture} . Take it as the true value of the magnetic field in the local coordinate system, and then obtain the reference true value of the magnetic field strength in the carrier coordinate system

M _{n_ture} and

The calculation process is as follows:

表示当地坐标系到载体坐标系的姿态旋转矩阵；in,

Represents the attitude rotation matrix from the local coordinate system to the carrier coordinate system;

和B_M，具体实现方式如下，46) Perform first-order Taylor expansion on the simplified model (Equation 2) to complete the linearization of the model, and then use the least squares method to obtain the parameters to be estimated

and B _M , the specific implementation is as follows,

为3×3方阵，待估参数B_M为3×1矩阵，具体表现为：Parameters to be estimated

is a 3×3 square matrix, and the parameter to be estimated B _M is a 3×1 matrix, which is specifically expressed as:

展开，the formula

expand,

There are a total of 12 parameters to be found, denoted as X,

X=[a ₁ a ₂ a ₃ a ₄ a ₅ a ₆ a ₇ a ₈ a ₉ b ₁ b ₂ b ₃ ] ^T

Since this formula is nonlinear, it is expanded according to Taylor's formula, discarding second-order terms and higher-order terms, and the superscript i represents the number of iterations, as follows:

where Δ represents the residual, let:

Let: x ⁱ =X ⁱ -X ^i-1 ,

Equation (7) is simplified to:

将式(8)写为残差方程的形式，make

Write equation (8) in the form of the residual equation,

The solution result of Equation (9) can be obtained by the least squares method. The present invention considers that each observation value has equal weight, and P is the unit matrix.

x = (H ^T PH) ^-1 H ^T Pz (10)

X ⁱ =x ⁱ +X ^i-1 (11)

更新

重复步骤43)～46)，直至迭代收敛，得到待估参数结果。The value of ^Xi is m _b in step 42);

renew

Steps 43) to 46) are repeated until the iteration converges, and the parameter result to be estimated is obtained.

Further, the initial value of the least squares method is obtained as follows:

磁力计和惯性传感器坐标系之间的旋转角为小角度，故

接近单位阵，C_SI各个元素值都比较小，C_NO主对角线元素为0，其余元素也较小，S_M只有主对角线元素，各元素数值接近1，故

的初值为单位阵I_3×3；because

The rotation angle between the magnetometer and the inertial sensor coordinate system is a small angle, so

Close to the unit matrix, the value of each element of _CSI is relatively small, the main diagonal element of _{C NO} is 0, and the other elements are also small, SM has only the main diagonal element, and the value of each element is close to 1, so

The initial value of the unit matrix I _{3 × 3} ;

For B _M , its initial value is given as follows:

For the mobile phone magnetometer, the error caused by the zero bias is much larger than the error caused by the quadrature axis coupling and the scale factor. Therefore, when the Taylor expansion is performed to obtain the initial value, only the magnetometer is affected by the zero bias error, so the magnetometer is further simplified. The measurement model is:

根据每个历元的

求得当地坐标系下各轴的磁场强度的均值，作为当地坐标系下的磁场强度的真值，设共有j个历元的数据，计算过程如下：The original observation data m _{b_init} of the magnetometer is taken as the true value of the magnetic field in the carrier coordinate system during the first iterative calculation, namely

According to each epoch

The mean value of the magnetic field strength of each axis in the local coordinate system is obtained as the true value of the magnetic field strength in the local coordinate system. If there are j epochs of data in total, the calculation process is as follows:

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}，将其作为当地坐标系下的磁场真值，进而得到载体坐标系下磁场强度参考真值

M_{n_ture}及

计算过程如下：Under the assumption that the total magnetic intensity remains unchanged, the total magnetic field intensity ||M _{n_CGRF} || can be obtained according to _{Mn_CGRF} , and by calculating

proportional to ||M _{n_CGRF} ||, the

The data of each axis changes in equal proportion to obtain M _{n_ture} , which is used as the true value of the magnetic field in the local coordinate system, and then the reference true value of the magnetic field strength in the carrier coordinate system is obtained.

M _{n_ture} and

The calculation process is as follows:

求得每个历元的载体坐标系下的磁场强度，在只考虑零偏误差的条件下，将所有历元的载体坐标系下的磁场强度有差异的原因是由于存在零偏造成的，由于标定动作为绕轴旋转，通过平均即可得到载体坐标系下的磁场强度真值，计算过程如下：According to each epoch

The magnetic field strength in the carrier coordinate system of each epoch is obtained. Under the condition that only the zero offset error is considered, the reason why the magnetic field strength in the carrier coordinate system of all epochs is different is due to the existence of zero offset. The calibration action is to rotate around the axis, and the true value of the magnetic field strength in the carrier coordinate system can be obtained by averaging. The calculation process is as follows:

求差得到每个历元的零偏，再将其求平均得到B_M Combine the original data m _{b_init} with in the above formula

Calculate the difference to get the zero offset of each epoch, and then average it to get B _M

重复上述过程。最终求得B_M，将其作为泰勒展开中的初值。When the difference between the BM obtained in this iteration and the _BM obtained in the previous iteration is less than _0.1mGauss , the zero offset calibration is considered to be completed, otherwise it is updated according to formula (12).

Repeat the above process. Finally, B _M is obtained, which is used as the initial value in the Taylor expansion.

The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which the present invention pertains can make various modifications or additions to the described specific embodiments or substitute in similar manners, but will not deviate from the spirit of the present invention or go beyond the definitions of the appended claims range.

Claims (6)

1. a magnetometer calibration method based on known attitude angle, is characterized in that, comprises the steps: Step 1, select an arbitrary location in an outdoor open area, and query the earth magnetic field reference model according to the latitude and longitude coordinates of the location to obtain the reference value of the total magnetic field strength of the location; Step 2, take the sensor measurement center as the rotation center, make it rotate multiple times around the X, Y, and Z axes of the carrier coordinate system, and obtain the attitude angle and magnetic field information of each epoch in the calibration process; Step 3: Establish a magnetometer measurement model, determine parameters to be estimated, perform Taylor expansion on the measurement model of the magnetometer, and use least squares to iteratively obtain parameters to be estimated. 2. a kind of magnetometer calibration method based on known attitude angle according to claim 1 is characterized in that: when the measurement model of magnetometer is modeled, consider that magnetometer is subject to soft magnetic error, scale factor error, quadrature axis Coupling error, zero offset error, hard iron error and sensor noise, the measurement model of the magnetometer in the carrier coordinate system is as follows:

Perform mean filtering on the original observation data m _{b_init} of the magnetometer to reduce the noise n _M of the magnetometer to obtain m _b ;

The product of , as a matrix to be estimated, is denoted as

Take the sum of b _M and b _HI as a matrix to be estimated, denoted as B _M ; the model is simplified as follows, and the parameters to be estimated are

and _BM ;

Among them, b represents the carrier coordinate system b, the b system is based on the inertial sensor IMU phase center as the origin, the x-axis points to the forward direction of the carrier, the y-axis is perpendicular to the x-axis and points to the right side of the carrier, and the z-axis is perpendicular to the x-axis and the y-axis. form a right-handed system;

Represents the true value of the magnetic field strength in the carrier coordinate system;

Represents the attitude rotation matrix from the magnetometer coordinate system to the inertial sensor IMU coordinate system, which is a 3×3 square matrix; _CSI represents the soft magnetic effect change matrix; C _NO is the change matrix of the magnetometer three-axis transformation from non-orthogonal to orthogonal , is a 3×3 square matrix, and the main diagonal element is 0; S _M is the scale factor, which is a 3×3 square matrix, only the main diagonal element has data, and the other elements are 0; m _{b_init} represents the original magnetometer observation data; b _M is the magnetometer zero offset; b _HI is the hard magnetic effect error; n _M is the magnetometer noise vector. 3. a kind of magnetometer calibration method based on known attitude angle according to claim 2, is characterized in that: when magnetometer measurement model is carried out iterative calculation, at first with m _b as the magnetic field intensity under the carrier coordinate system during initial iterative calculation The reference truth value, i.e.

After iterative calculation

pass

Obtained, and then obtained the true value of the magnetic field strength in the local coordinate system

The calculation relationship is as follows:

In the formula, k represents the k-th epoch, and j represents the total number of epochs;

Represents the attitude rotation matrix from the kth epoch carrier coordinate system to the local coordinate system,

Represents the true value of the magnetic field strength in the kth epoch carrier coordinate system; n represents the local coordinate n system, the n system takes the inertial sensor IMU phase center as the origin, the x-axis is parallel to the local horizontal plane and points to true north, and the y-axis is parallel to the local The horizontal plane points to the due east, and the z-axis is perpendicular to the local horizontal plane downward, and the three constitute a right-handed system; Under the assumption that the total magnetic intensity remains unchanged, the total magnetic field intensity ||M _{n_CGRF} || can be obtained according to _{Mn_CGRF} , and by calculating

proportional to ||M _{n_CGRF} ||, the

The data of each axis changes in equal proportion to obtain M _{n_ture} , which is used as the true value of the magnetic field in the local coordinate system, and then the reference true value of the magnetic field strength in the carrier coordinate system is obtained.

M _{n_ture} and

The calculation process is as follows:

in,

Represents the attitude rotation matrix from the local coordinate system to the carrier coordinate system. 4. a kind of magnetometer calibration method based on known attitude angle according to claim 3, is characterized in that: will simplify model by first-order Taylor expansion

Linearization, the parameters to be estimated are iteratively obtained according to the least squares method, and the specific implementation is as follows: Parameters to be estimated

is a 3×3 square matrix, and the parameter to be estimated B _M is a 3×1 matrix, which is specifically expressed as:

the formula

expand,

There are a total of 12 parameters to be found, denoted as X, X=[a ₁ a ₂ a ₃ a ₄ a ₅ a ₆ a ₇ a ₈ a ₉ b ₁ b ₂ b ₃ ] ^T Since this formula is nonlinear, it is expanded according to Taylor's formula, discarding second-order terms and higher-order terms, and the superscript i represents the number of iterations, as follows:

where Δ represents the residual, let:

Let: x ⁱ =X ⁱ -X ^i-1 ,

Equation (7) is simplified to:

make

Write equation (8) in the form of the residual equation,

The solution result of Equation (9) can be obtained by the least square method, each observation value is equal weight, P is the identity matrix, and we can obtain x = (H ^T PH) ^-1 H ^T Pz (10) X ⁱ =x ⁱ +X ^i-1 (11) In the formula, X ⁱ is the m _b obtained by the mean filtering of the original observation data m _{b_init} of the magnetometer.

renew

This iterative process is repeated until convergence, and the parametric results are obtained. 5. a kind of magnetometer calibration method based on known attitude angle according to claim 4 is characterized in that: the initial value of least squares method is obtained by the following manner, for

The rotation angle between the magnetometer and the inertial sensor coordinate system is a small angle, so

Close to the unit matrix, the value of each element of _CSI is relatively small, the main diagonal element of _{C NO} is 0, and the other elements are also small, SM has only the main diagonal element, and the value of each element is close to 1, so

The initial value of the unit matrix I _{3 × 3} ; For the initial value of _BM , only the magnetometer is affected by the bias error, and by further simplifying the model, that is,

The original observation data m _{b_init} of the magnetometer is taken as the true value of the magnetic field in the carrier coordinate system during the first iterative calculation, namely

According to each epoch

Represents the attitude rotation matrix from the carrier coordinate system of each epoch to the local coordinate system, and obtains the mean value of the magnetic field strength of each axis in the local coordinate system

As the true value of the magnetic field strength in the local coordinate system; under the assumption that the total magnetic strength is constant, the total magnetic field strength ||M _{n_CGRF} || is obtained from _{Mn_CGRF} , and by calculating

proportional to ||M _{n_CGRF} ||, the

The data of each axis changes in equal proportion to obtain M _{n_ture} , which is used as the true value of the magnetic field in the local coordinate system, and then the reference true value of the magnetic field strength in the carrier coordinate system is obtained.

Compare the original data m _{b_init} with

Calculate the difference to get the zero offset of each epoch, and then average it to get _BM ;

When the difference between the BM obtained from this iteration and the _BM obtained from the previous iteration is less than _0.1mGauss , the zero-bias calibration is considered to be completed; otherwise, according to

renew

Repeat the above process, and finally obtain B _M , which is used as the initial value in Taylor expansion. 6. A magnetometer calibration method based on a known attitude angle according to claim 1, characterized in that: in step 2, the sensor is rotated 720 degrees around the X, Y, and Z axes of the carrier coordinate system, so that The attitude angle includes pitch angle, roll angle, and heading angle.

Images