电子罗盘(2)：磁传感器的误差来源、硬磁干扰的校准（3个参数）、实验验证

理想情况

理想情况下，磁传感器的测量结果是完全准确的，随着磁传感器的不同位置，三轴的读数为地磁场强度在三轴的投影：
$$\begin{gathered} \text{h}=\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right] \\ {h}_x^2+h_y^2+h_z^2=B^2 \end{gathered}$$
因此，磁传感器在不同角度的测量值，落在一个球面$x^2+y^2+z^2=B^2$上，球心是原点，半径是B。

误差来源

主要分为磁传感器内部和外部因素造成的误差。

内部

主要是磁传感器灵敏度

1. 三轴灵敏度不同，

   例如，10uT的磁感应强度，分别用三个轴来测量，测量结果为：9uT，10uT，11uT。

2. 三轴不正交

   例如，某个磁场下，理想的读数是：[10uT,0,0]，但实际上是[10uT,1uT,0]，因为XY两轴不是完全正交，原来只在X轴有读数，在非理想情况下，Y轴上也有了一定读数。

3. 零偏

   在完全无磁场时也有一个非零读数。

几种误差对读数的影响示意图如下：（来自文末参考资料）

外部

1. 硬磁干扰

   硬磁干扰由传感器附近的永磁体等的磁场引起，造成的影响类似于零偏。

2. 软磁干扰

   软磁干扰是传感器附近的铁磁材料改变了原磁场的强度和方向。

详见磁传感器PCB设计指导 Layout Recommendations for PCBs Using a Magnetometer Sensor (nxp.com.cn)

误差模型

假设理想的测量结果为：
$${\mathbf{h}}_0=\left[\begin{array}{c}{h}_{0x}\\ h_{0y}\\ h_{0z}\end{array}\right]$$
首先，外部的磁场由于软磁和硬磁干扰而发生了改变，先考虑外部的干扰：
$${\mathbf{h}}_{\mathbf{m}}={\mathbf{I}}_{3\times 3}{\mathbf{h}}_0+{\mathbf{F}}_{\mathbf{h}}\mathbf{i}$$
I是3x3矩阵，用于表示软磁干扰，${\mathbf{F}}_{\mathbf{s}}\mathbf{i}$为硬磁干扰造成的偏置。

考虑内部误差后，测量结果变为：
$$\mathbf{h}={\mathbf{S}}_{3\times 3}{\mathbf{N}}_{3\times 3}{\mathbf{h}}_{\mathbf{m}}+{\mathbf{F}}_{\mathbf{o}\mathbf{s}}={\mathbf{S}}_{3\times 3}{\mathbf{N}}_{3\times 3}({\mathbf{I}}_{3\times 3}{\mathbf{h}}_0+{\mathbf{F}}_{\mathbf{h}}\mathbf{i})+{\mathbf{F}}_{\mathbf{o}\mathbf{s}}$$

其中，S、N为3x3矩阵，S表示三轴灵敏度差异，N表示三轴非正交的影响。${\mathbf{F}}_{\mathbf{o}\mathbf{s}}$为3x1向量，表示零偏。

化简后，测量值h与实际值h0之间的关系可以表示为：
$$\begin{gathered} \mathbf{h}=\mathbf{W}{\mathbf{h}}_0+\mathbf{V} \\ {\mathbf{h}}_0={\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V}) \end{gathered}$$
W是3x3矩阵，V是3x1向量。其中，W是一个对称矩阵（具体），因此只含6个系数。加上V的3个系数，总共需要获得9个系数才能实现校准。

但是，在某些场合只有硬磁干扰，而软磁干扰不明显，则此时$\mathbf{W}=\mathbf{E}$，$\mathbf{h}={\mathbf{h}}_0+\mathbf{V}$就只需要获得V的3个系数。

硬磁干扰的校准（3个参数）

使用的模型

如果只有硬磁干扰，则理论测量结果h0可表示为：
$${\mathbf{h}}_0={\mathbf{W}}^{-1}(\mathbf{h}-\mathbf{V})=\mathbf{h}-\mathbf{V}$$
理论测量结果落在球面上：
$${\mathbf{h}}_0^{\mathbf{T}}{\mathbf{h}}_0=(\mathbf{h}-\mathbf{V}{)}^T(\mathbf{h}-\mathbf{V})=B^2$$
其中，
$$\text{h}=\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right],\text{V}=\left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right]$$
展开后：
$$h_x^2+h_y^2+h_z^2-2V_x{h}_x-2V_y{h}_y-2V_y{h}_y+V_x^2+V_y^2+V_z^2-B^2=0$$
整理下：
$$[h_x^2+h_y^2+h_z^2]-\left[\begin{array}{cccc}{h}_x& h_y& h_z& 1\end{array}\right]\left[\begin{array}{c}2V_x\\ 2V_y\\ 2V_z\\ B^2-(V_x^2+V_y^2+V_z^2)\end{array}\right]=0$$

最小二乘法

获得多组测量数据h(1),h(2)… h(m)，并整理：
$$\left[\begin{array}{c}{h}_x^2[1]+h_y^2[1]+h_z^2[1]\\ h_x^2[2]+h_y^2[2]+h_z^2[2]\\ ...\\ h_x^2[m]+h_y^2[m]+h_z^2[m]\end{array}\right]-\left[\begin{array}{cccc}{h}_x[1]& h_y[1]& h_z[1]& 1\\ h_x[2]& h_y[2]& h_z[2]& 1\\ ...& ...& ...& ...\\ h_x[m]& h_y[m]& h_z[m]& 1\end{array}\right]\left[\begin{array}{c}2V_x\\ 2V_y\\ 2V_z\\ B^2-(V_x^2+V_y^2+V_z^2)\end{array}\right]=\left[\begin{array}{c}ϵ[1]\\ ϵ[2]\\ ...\\ ϵ[m]\end{array}\right]$$
用符号表示：
$$\mathbf{Y}-\text{X}\beta =ϵ$$
根据使用的模型，需要求一组系数$\beta$，使得：
$$min({ϵ}^{\mathbf{T}}ϵ)=(\mathbf{Y}-\mathbf{X}\beta {)}^T(\mathbf{Y}-\mathbf{X}\beta )$$
根据最小二乘法，
$$\beta ={\left({\mathbf{X}}^{T}X\right)}^{-1}{\mathbf{X}}^T\mathbf{Y}=\left[\begin{array}{c}2V_x\\ 2V_y\\ 2V_z\\ B^2-(V_x^2+V_y^2+V_z^2)\end{array}\right]$$
根据$\beta$，可以方便地求出偏置V和磁感应强度的估计值。
$$\begin{gathered} V_x=\frac{\beta [1]}{2} \\ {V}_y=\frac{\beta [2]}{2} \\ {V}_z=\frac{\beta [3]}{2} \\ {B}_{estimate}=\sqrt{\beta [4{]}^2+V_x^2+V_y^2+V_z^2} \end{gathered}$$

实测结果

用手头的一个磁传感器，采集了传感器朝向各个位置时的约600组数据。做了下简单验证。

最小二乘法的计算结果为：
$$\beta =\left[\begin{array}{c}51.78\\ -122.84\\ 16.33\\ -3305.63\end{array}\right]$$
转换为零偏和磁感应强度B：
$$\begin{gathered} V_x=25.89,V_y=-61.42,V_z=8.17 \\ {B}_{estimate}=34.70\mu T \end{gathered}$$
说明传感器样机受到一定的硬磁干扰，XYZ三轴都有一定的。其中Y轴最严重。

实际当地的磁感应强度是49uT左右，但是因为电子罗盘应用中，最后是通过计算tan(-y/x)获得角度，所以只需要相对大小，而不怎么关注数据的绝对大小。

把校准后的数据和拟合结果画在同一个图上：

从XY两轴看，数据点基本上都在拟合的圆内。偏置基本被消除。

从YZ两轴看，偏置基本被消除，但是数据点有不少在圆外，说明可能还有一些软磁干扰。

对比校准前后的数据，红色为校准后，蓝色为校准前。可以看到原数据基本在一个球面上，球心距离原点较远。而校准后的数据，球心在原点，说明零偏基本上被消除了。

总结

1. 磁传感器的误差来源：传感器内部的零偏、灵敏度、正交；外界环境的硬磁和软磁干扰
2. 只存在硬磁干扰的情况下，可以方便地使用最小二乘法进行校准，这时只需要校准3个参数
3. 用实验数据进行了验证

代码和资料

MATLAB代码

load('ramdom0502_2.mat')  % 包含mag_x,mag_y,mag_z 3个列向量，都是598*1
close all
% 原始数据
scatter3(mag_x,mag_y,mag_z,10)
hold on 

Y = mag_x.^2 + mag_y.^2 + mag_z.^2;
ones_vector = ones(length(mag_x),1);
X = [mag_x,mag_y,mag_z,ones_vector];

beta = inv(X'*X) * X' * Y;  % 最小二乘法
% 把beta的4个值转换成零偏和B
offset_x = 0.5 * beta(1);
offset_y = 0.5 * beta(2);
offset_z = 0.5 * beta(3);
B_est = sqrt(beta(4) + offset_x^2 + offset_y^2 + offset_z^2);

% 减去零偏后的数据
scatter3(mag_x-offset_x,mag_y-offset_y,mag_z-offset_z,10)  % 设置每个点大小为10
xlabel('X/\mu T')
ylabel('Y/\mu T')
zlabel('Z/\mu T')
hold on 
% 画球面
[u,v,w] = sphere(56);
K = B_est;
% 校准后的球面
shape1 = surf(K*u,K*v,K*w);
set(shape1, 'FaceAlpha', 0.2)
shading flat
axis equal  % 设置坐标轴单位长度相等，看起来更接近球形

% 原始数据的球面
shape2 = surf(K*u+offset_x,K*v+offset_y,K*w+offset_z);
set(shape2, 'FaceAlpha', 0.2)
shading flat
axis equal

参考资料

NXP的应用手册 AN4246，4参数校准 https://www.nxp.com.cn/docs/en/application-note/AN4246.pdf

磁传感器PCB设计指导 Layout Recommendations for PCBs Using a Magnetometer Sensor (nxp.com.cn)

Teslabs Engineering - A way to calibrate a magnetometer

样机

实验数据用很久以前买的开发板+传感器采集，用面包板+跳线连了下电源和I2C。

串口工具：MobaXterm

传感器：九轴传感器MPU9250+气压计BMP280，淘宝买的

MCU：Nano V3.0开发板， Atmega328P，也是淘宝买的

驱动：不用自己写驱动，在Arduino中搜一下现成的，我用的是MPU9250_WE。

基于例程简单修改下就行，正确配置传感器，NANO负责读取和从串口发送测量数据，数据都是在MATLAB中离线处理的。