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

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

理想情况

理想情况下，磁传感器的测量结果是完全准确的，随着磁传感器的不同位置，三轴的读数为地磁场强度在三轴的投影：
h = [ h x h y h z ] h x 2 + h y 2 + h z 2 = B 2 \textbf{h}=\left[ \begin{matrix} h_x\\ h_y \\ h_z \end{matrix} \right] \\ h_x^2 + h_y^2 + h_z^2 = B^2 h=⎣⎡​hx​hy​hz​​⎦⎤​hx2​+hy2​+hz2​=B2
因此，磁传感器在不同角度的测量值，落在一个球面 x 2 + y 2 + z 2 = B 2 x^2+y^2+z^2=B^2 x2+y2+z2=B2上，球心是原点，半径是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)

误差模型

假设理想的测量结果为：
h 0 = [ h 0 x h 0 y h 0 z ] \mathbf{h_0}=\left[ \begin{matrix} h_{0x}\\ h_{0y} \\ h_{0z} \end{matrix} \right] h0​=⎣⎡​h0x​h0y​h0z​​⎦⎤​
首先，外部的磁场由于软磁和硬磁干扰而发生了改变，先考虑外部的干扰：
h m = I 3 × 3 h 0 + F h i \mathbf{h_m}=\mathbf{I_{3\times3}} \mathbf{h_0} + \mathbf{F_hi} hm​=I3×3​h0​+Fh​i
I是3x3矩阵，用于表示软磁干扰， F s i \mathbf{F_si} Fs​i为硬磁干扰造成的偏置。

考虑内部误差后，测量结果变为：
h = S 3 × 3 N 3 × 3 h m + F o s = S 3 × 3 N 3 × 3 ( I 3 × 3 h 0 + F h i ) + F o s \mathbf{h} = \mathbf{S_{3\times3}} \mathbf{N_{3\times3}}\mathbf{h_m}+\mathbf{F_{os}}= \mathbf{S_{3\times3}} \mathbf{N_{3\times3}}(\mathbf{I_{3\times3}} \mathbf{h_0} + \mathbf{F_hi})+\mathbf{F_{os}} h=S3×3​N3×3​hm​+Fos​=S3×3​N3×3​(I3×3​h0​+Fh​i)+Fos​

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

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

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

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

使用的模型

如果只有硬磁干扰，则理论测量结果h0可表示为：
h 0 = W − 1 ( h − V ) = h − V \mathbf{h_0} = \mathbf{W^{-1}}(\mathbf{h}-\mathbf{V} )=\mathbf{h}-\mathbf{V} h0​=W−1(h−V)=h−V
理论测量结果落在球面上：
h 0 T h 0 = ( h − V ) T ( h − V ) = B 2 \mathbf{h_0^T}\mathbf{h_0} = (\mathbf{h}-\mathbf{V})^T(\mathbf{h}-\mathbf{V})=B^2 h0T​h0​=(h−V)T(h−V)=B2
其中，
h = [ h x h y h z ] , V = [ V x V y V z ] \textbf{h}=\left[ \begin{matrix} h_x\\ h_y \\ h_z \end{matrix} \right], \textbf{V}=\left[ \begin{matrix} V_x\\ V_y \\ V_z \end{matrix} \right] h=⎣⎡​hx​hy​hz​​⎦⎤​,V=⎣⎡​Vx​Vy​Vz​​⎦⎤​
展开后：
h x 2 + h y 2 + h z 2 − 2 V x h x − 2 V y h y − 2 V y h y + V x 2 + V y 2 + V z 2 − B 2 = 0 h_x^2+h_y^2+h_z^2- 2V_xh_x- 2V_yh_y- 2V_yh_y + V_x^2 +V_y^2 +V_z^2 -B^2 = 0 hx2​+hy2​+hz2​−2Vx​hx​−2Vy​hy​−2Vy​hy​+Vx2​+Vy2​+Vz2​−B2=0
整理下：
[ h x 2 + h y 2 + h z 2 ] − [ h x h y h z 1 ] [ 2 V x 2 V y 2 V z B 2 − ( V x 2 + V y 2 + V z 2 ) ] = 0 [h_x^2+h_y^2+h_z^2]- \left[ \begin{matrix} h_x & h_y & h_z & 1 \end{matrix} \right] \left[ \begin{matrix} 2V_x\\ 2V_y\\ 2V_z \\ B^2 - (V_x^2 +V_y^2 +V_z^2) \end{matrix} \right] = 0 [hx2​+hy2​+hz2​]−[hx​​hy​​hz​​1​]⎣⎢⎢⎡​2Vx​2Vy​2Vz​B2−(Vx2​+Vy2​+Vz2​)​⎦⎥⎥⎤​=0

最小二乘法

获得多组测量数据h(1),h(2)… h(m)，并整理：
[ 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 ] ] − [ 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 ] [ 2 V x 2 V y 2 V z B 2 − ( V x 2 + V y 2 + V z 2 ) ] = [ ϵ [ 1 ] ϵ [ 2 ] . . . ϵ [ m ] ] \left[ \begin{matrix} 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{matrix} \right] -\left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} 2V_x\\ 2V_y\\ 2V_z \\ B^2 - (V_x^2 +V_y^2 +V_z^2) \end{matrix} \right] = \left[ \begin{matrix} \epsilon[1]\\ \epsilon[2] \\ ... \\ \epsilon[m] \end{matrix} \right] ⎣⎢⎢⎡​hx2​[1]+hy2​[1]+hz2​[1]hx2​[2]+hy2​[2]+hz2​[2]...hx2​[m]+hy2​[m]+hz2​[m]​⎦⎥⎥⎤​−⎣⎢⎢⎡​hx​[1]hx​[2]...hx​[m]​hy​[1]hy​[2]...hy​[m]​hz​[1]hz​[2]...hz​[m]​11...1​⎦⎥⎥⎤​⎣⎢⎢⎡​2Vx​2Vy​2Vz​B2−(Vx2​+Vy2​+Vz2​)​⎦⎥⎥⎤​=⎣⎢⎢⎡​ϵ[1]ϵ[2]...ϵ[m]​⎦⎥⎥⎤​
用符号表示：
Y − X β = ϵ \mathbf{Y} - \textbf{X} \mathbf{\beta}=\mathbf{\epsilon} Y−Xβ=ϵ
根据使用的模型，需要求一组系数 β \beta β，使得：
m i n ( ϵ T ϵ ) = ( Y − X β ) T ( Y − X β ) min(\mathbf{\epsilon^T}\mathbf{\epsilon})=(\mathbf{Y} - \mathbf{X} \mathbf{\beta})^T(\mathbf{Y} - \mathbf{X} \mathbf{\beta}) min(ϵTϵ)=(Y−Xβ)T(Y−Xβ)
根据最小二乘法，
β = ( X T X ) − 1 X T Y = [ 2 V x 2 V y 2 V z B 2 − ( V x 2 + V y 2 + V z 2 ) ] \beta = \left( \mathbf{X}^TX\right)^{-1}\mathbf{X}^T\mathbf{Y}= \left[ \begin{matrix} 2V_x\\ 2V_y\\ 2V_z \\ B^2 - (V_x^2 +V_y^2 +V_z^2) \end{matrix} \right] β=(XTX)−1XTY=⎣⎢⎢⎡​2Vx​2Vy​2Vz​B2−(Vx2​+Vy2​+Vz2​)​⎦⎥⎥⎤​
根据 β \beta β，可以方便地求出偏置V和磁感应强度的估计值。
V x = β [ 1 ] 2 V y = β [ 2 ] 2 V z = β [ 3 ] 2 B e s t i m a t e = β [ 4 ] 2 + V x 2 + V y 2 + V z 2 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} Vx​=2β[1]​Vy​=2β[2]​Vz​=2β[3]​Bestimate​=β[4]2+Vx2​+Vy2​+Vz2​ ​

实测结果

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

最小二乘法的计算结果为：
β = [ 51.78 − 122.84 16.33 − 3305.63 ] \beta= \left[ \begin{matrix} 51.78\\ -122.84 \\ 16.33 \\ -3305.63 \end{matrix} \right] β=⎣⎢⎢⎡​51.78−122.8416.33−3305.63​⎦⎥⎥⎤​
转换为零偏和磁感应强度B：
V x = 25.89 , V y = − 61.42 , V z = 8.17 B e s t i m a t e = 34.70 μ T V_x =25.89, V_y=-61.42, V_z=8.17 \\ B_{estimate} = 34.70 \mu T Vx​=25.89,Vy​=−61.42,Vz​=8.17Bestimate​=34.70μT
说明传感器样机受到一定的硬磁干扰，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中离线处理的。