地磁，干扰及校准，椭球拟合

地磁，干扰及校准，椭球拟合

参考 https://www.zhihu.com/column/c_1150063812093825024

 

IMU，AHRS 加速度计，陀螺仪，地磁计概念

来源 https://zhuanlan.zhihu.com/p/56073859

临时加进来的，前面已经进行了不少理论学习，这一篇注重介绍一些现实中的传感器，没有公式，与前后之间的关系也不大

加速度计，陀螺仪，地磁计

加速度是常用的惯性传感器，它测量线性加速度+加速度。即所谓的比力。

加速计测量三轴所感受到的加速度，所以当加速度计水平静止放置在地球上时候，读数为0,0,1G. 当加速度计收到X轴正方向1G 加速的时候，读数为1,0,1G

这里有一点小tricky. 重力是垂直向下的，但是正面水平放置，度数是0.0.1G ,为啥不是0.0,-1G. 这是因为 加速度计感受到的是外面对他的作用力(把加速度计想弹簧模型)。而不是重力场。

加速度计模型

陀螺仪

陀螺仪测量三轴角速度，说白了就是测量绕每个轴的旋转，也叫角速度传感器，也是一种惯性器件。

加速度计+陀螺仪就叫IMU(惯性测量单元)，所谓的6轴传感器

地磁场传感器

让我们再把地磁计加上，他测量三个轴的磁场强度(地磁场+附近的其他干扰磁场(比如磁铁，金属物体等等))

所谓9轴传感器，或者叫MARG(magnetic, angular rate)， 如果再配上一个单片机就有处理能力，配合合适的算法就可以叫做一个AHRS(attitude heading reference system)

三轴磁传感器感应周围的磁场：一个理想的磁传感器可以描述为下面数学模型：

•  地磁场和周围环境的磁场的总和
•  传感器零偏
•  传感器噪声

例 MPU6050 就是一个最典型的的6轴传感器，内含3个互相垂直的加速度计和三轴互相垂直的陀螺仪。坐标表示如下。

地球磁场

地球磁场的几何分布是非常不规则的，总体来说磁力线游地磁南极出发，回归到地磁北极:

注意磁力线的方向: 由地磁南极出发到地磁北极

对于北半球来说，地磁场方向斜向下，对于南半球，地磁场方向斜向上。

一个典型的北半球地磁场

Intensity 那条线就是3轴磁传感器的读值，是一个三维空间向量，Intensity的模就是磁场强度 = 

1. Declination(磁偏角):地磁场和地理北向的夹角。
2. Inclination(磁倾角):地磁场与水平方向的夹角。
3. 单位换算： 常用单位： 微特斯拉(uT), 毫高斯(mGauss)
4. 1 uT = 10 mGauss ，地磁场的范围：250 - 650 mGauss 或 25 - 64 uT

在NED坐标系下，

惯性系地磁场矢量计算公式

其中

•  为磁偏角
•  为磁倾角
•  为地磁场大小

地磁场倾角速查：

Magnetic Declination on Map​www.magnetic-declination.com/

某地详细地磁参数查询(NOAA):

NCEI Geomagnetic Calculators​www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml?#igrfwmm

例，通过NOAA网站查的北京的地磁场详细参数为：

可见北京地区总磁场强度为~54uT, 垂直分量:47uT, 水平分量:28uT, 倾角59°，偏角:-7°

参考

1. GNSS与惯性及多传感器组合导航系统原理-第二版
2. https://hal.inria.fr/hal-016501

 

 

NXP传感器融合笔记09(地磁，干扰及校准，椭球拟合)

来源 https://zhuanlan.zhihu.com/p/98325286

地球磁场简介

地球上某点的地磁场是一个三维空间矢量，从地磁南极出发到地磁北极。强度大约为23-66uT. 这个磁场强度在生活中属于非常小的量级。如果离一块小磁铁很近，那么磁铁产生的磁场可以轻轻松松超过地磁场上千倍。除了显而易见的磁铁，生活中的一些其他物品，包括你想到的想不到的，比如手机，电机，电子产品，耳机，内嵌金属的家具，办公桌，带电导线，甚至建筑钢架结构, 衣服里钥匙等等等等都会对地磁场产生严重干扰，总之：室内磁场分布非常复杂，地磁场在室内受干扰非常严重。

关于地磁场的一些更多知识和地磁传感器的基本概念，可以参看之前的笔记：

杨熙：NXP传感器融合笔记04(IMU，AHRS 加速度计，陀螺仪，地磁计概念)11 赞同 · 0 评论文章

美国某地的地磁场具体参数，可以通过NOAA网站查询：

可以看出，这个地方的地磁场大部分 分量是朝下的哦。。

硬磁和软磁干扰

硬磁干扰

硬磁干扰其实就相当于磁传感器的零偏，但是这个零偏不是由传感器自身引起。如果没有硬磁干扰，我们让磁传感器旋转所有可能的角度，把数据记录下来，会得到一个球，而且球心在0,0,0 原点，但由于硬磁干扰的影响。总会测得一个bias(球的圆心不在原点)，这个bias就是硬磁干扰。

软磁干扰

软磁是由于传感器周围的磁性物质扭曲磁场得到，如果向上面那样让传感器旋转然后采集点，软磁干扰表现为将球变成椭球：

注意无论是硬磁还是软磁干扰，都指的是在传感器坐标系下，换句话说，干扰源和传感器在同一个设备里，一起运动，旋转。如果干扰源不再传感器坐标系下而在固定坐标系下，那么就属于空间磁干扰，是无法校准补偿的！(比如室内的固定干扰源，桌子椅子，钢筋等)。 这也是为啥室内地磁电子罗盘不准的原因

总结一下硬磁干扰和软磁干扰对于磁场的畸变影响：

无干扰，硬磁干扰，软磁干扰

以及他们对磁传感器采样结果的影响：

完美的圆(无干扰)，偏心的圆(只有硬磁干扰)，偏心椭圆(硬磁+软磁 )

数学知识- 椭圆/圆拟合

这块一部分时后来加进来，地磁校准和圆/椭圆/球/椭球拟合这个数学问题息息相关，所以这里打算着重大书特书一下有关的数据知识，主要是看到一篇非常好的英文文章：

http://ztrw.mchtr.pw.edu.pl/en/least-squares-circle-fit/ 这节内容基本翻译自这篇文章

最小二乘圆拟合

机器视觉，包括本章说要解决的地磁校准问题都会可以归结于圆/椭圆或者椭球/椭球的数据拟合问题。这里有两本比较老但非常不错的论文可以参考：

• Gander W. Golub G.H. Strebel R. Least–Squares Fitting of Circles and Ellipses BIT Numerical Mathematics, 34(4) pp. 558–578, 1994
• Coope L.D. Circle fitting by linear and nonlinear least squares Journal of Optimization Theory and Applications, 76(2) pp. 381–388, 1993

圆拟合问题的提出：

给定一些数据点(X,Y) 求圆形坐标和圆半径，给出的数据点要大于未知数的个数。

比如给出的数据点记作：P

数据点

代数法求解

代数法求解原理简单直接，我们把圆方程写为代数形式：

设待求向量为

则有：  其中B：

 

一般情况下，我们需要加一个约束来转换成最小二层问题：  ，由此可得最小二层问题：

最终转换为圆形和半径：

matlab代码：

clear;
clc;
close all;


P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7];
B = [(P.*P)*[1 1]', P(:,1), P(:,2)];
b = -ones(length(P), 1);
res = (B'*B)^(-1)*B'*b;

xc = -res(2)/(2*res(1));
yc = -res(3)/(2*res(1));

c = sqrt(1 - res(1)^2 - res(2)^2 - res(3)^2);
r = sqrt((res(2)^2 + res(3)^2)/(4*res(1)^2) - c/res(1));

plot(P(:,1), P(:,2), '*')
 viscircles([xc, yc],r);
 axis equal

最小二乘法求解

这个结果可以说不怎么好，实际上，这种算法只是求得了圆形到这些数据点距离最近的一个圆，并没有考虑这些点的几何关系，所以拟合效果不佳，如果数据点比较少(比如上面这个例子)，那么效果会更差。

几何法求解

几何求解法直接优化(最小化) 各数据点与圆心所形成的向量的长度与圆半径的差值平方和，如上图所示。换成数学语言：设  ， 

则有： 

或者写做： 

如上所述，定义cost function:  . 现在，要解决的问题变成了非线性问题，不能用传统的最小二乘来解决了，我们采用高斯牛顿迭代来搞定它!

高斯牛顿迭代

高斯牛法采用迭代方法来实现优化：

实际上就是梯度下降法

其中  是雅克比矩阵的逆，  是残差。当方程数多于未知数时，系统变成超定问题，雅克比矩阵逆通过伪逆求得： 

雅克比矩阵怎么求呢，实际就是按每个变量求偏导数所组成的矩阵，不再赘述：

雅克比矩阵

matlab代码：

clear;
clc;
close all;


P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7];

%给定初值
xc = 5.3794;
yc = 7.2532;
r = 3.0370;
res = [xc; yc; r];

max_iters = 20;

max_dif = 10^(-6); % max difference between consecutive results
for i = 1 : max_iters
    J = Jacobian(res(1), res(2), P);
    R = Residual(res(1), res(2), res(3), P);
    prev = res;
    res = res - (J'*J)^(-1)*J'*R;
    dif = abs((prev - res)./res); 
    if dif < max_dif
        fprintf('Convergence met after %d iterations\n', i);
        break;
    end
end
if i == max_iters
    fprintf('Convergence not reached after %d iterations\n', i);
end

xc = res(1);
yc = res(2);
r = res(3);

plot(P(:,1), P(:,2), '*')
 viscircles([xc, yc],r);
 axis equal
 
function J = Jacobian(xc, yc, P) 
    len = size(P);
    r = sqrt((xc - P(:,1)).^2 + (yc - P(:,2)).^2);
    J = [(xc - P(:,1))./r, (yc - P(:,2))./r, -ones(len(1), 1)];
end


function R = Residual(xc, yc, r, P)
    R = sqrt((xc - P(:,1)).^2 + (yc - P(:,2)).^2) - r;
end

效果：

这次效果不错

这次虽然效果好，但是高斯牛顿迭代只会找到局部最优解，并且严重依赖于初始值，如果初始值给的不好，有可能得不到最优解甚至发散。我们来看看还有没有什么别的骚操作：

线性化的几何解法

之前我们的代价函数定位为：

所以这是一个非线性问题，只能采用非线性优化的方式来解。现在我们稍加改动，把它变成一个线性问题：定义代价函数为：

展开可得：

注意到我们要求是圆形坐标  和圆半径  ,整理可得：

之后就是很tricky的一步了，我们做如下变量替换：

那么代价函数就可以写成z的线性函数的形式：

下面就可以使用最小二层来解决了，进而求得圆形坐标和圆半径：

matlab代码：

clear;
clc;
close all;


P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7]';
n= length(P);


plot(P(1,:), P(2,:), '*');

%build deisgn matrix
A = [ P(1,:); P(2,:); ones(1,n)]';
b = sum(P.*P, 1)';

ls solution
a= (A'*A)^(-1)*A'*b;

xc = 0.5*a(1);
yc = 0.5*a(2);
R  =  sqrt((a(1)^2+a(2)^2)/4+a(3));
R

viscircles([xc, yc],R);
axis equal

结论

这三种方法各有千秋，通常，当数据点比较少时，三种方法出来的效果差别很大，当数据点足够多时，三种方法拟合出来的圆差不多。

三种方法比较

地磁3D校准模型

既然地磁场在实际应用中会经常受到硬磁和软磁干扰，那我们就要想办法校准补偿他。数学模型如下：

其实数学模型就是这个(所有的文献论文，大家都用这个模型咯)

其中

•  ： 校准之后的磁场(3纬矢量)
•  :软磁干扰矩阵
•  ：磁传感器原始数据
•  :硬磁bias(3维矢量)

校准模型根据待求的参数个数也分为几种：

1. 4参数校准法：只估计硬磁干扰和磁场强度，软磁矩阵默认为单位阵。
2. 7参数校准法：4参数校准法再加上软磁矩阵的主对角线元素(每个传感器轴的比例因子)
3. 10参数校准法： 把软磁干扰假设为对称矩阵，求解出对称矩阵的所有元素

地磁3D校准原理

校准的数学原理其实很简单： 任何校准好的传感器坐标系地磁场的大小应该是恒定不变的。就这一条， 用数学公式说就是:  . 全部靠这条公理来实现校准参数估计：

把展开，并且令 

可得 

后面就可以用最小二层法来求解矩阵A。

3D地磁校准方法

这里简单的讲下拟合球面的方法(，只是球面哦，不是椭球, 之前已经讲了如何拟合一个圆，这里只不过拓展为球，参考AN5019)

根据式子  展开可得：

写成矩阵形式：

令： 

则，可以转化为一个最小二乘问题(其实和上面的圆拟合第三种方法是一样的道理)。

matlab代码

close all;                          % close all figures
clear;                              % clear all variables
clc;                                % clear the command terminal

%% simulate data 
c = [-0.5; 0.2; 0.1]; % ellipsoid center
r = [1; 1; 1]; % semiaxis radii

[x,y,z] = ellipsoid(c(1),c(2),c(3),r(1),r(2),r(3),6);
D = [x(:),y(:),z(:)];

% add noise:
n = length(D);
noiseIntensity = 0.05; %噪声强度 
D = D + randn(n,3) * noiseIntensity;


%% matlab internal fitting 

[A,b,expmfs] = magcal(D, 'eye')
%fprintf( 'away from cetner %.5g\n', norm( b' - c) );
C = (D-b)*A; % calibrated data

figure(1)
plot3(D(:,1),D(:,2),D(:,3),'LineStyle','none','Marker','X','MarkerSize',2)
hold on
grid(gca,'on')
plot3(C(:,1),C(:,2),C(:,3),'LineStyle','none','Marker', ...
            'o','MarkerSize',2,'MarkerFaceColor','r') 
axis equal
xlabel('uT'); ylabel('uT');zlabel('uT') 
legend('Uncalibrated Samples', 'Calibrated Samples','Location', 'southoutside')
title("Uncalibrated vs Calibrated" + newline + "Magnetometer Measurements")
axis equal
hold off

其中magcal是matlab2019 sensor fusion toolbox自带的函数，算法和本文章描述的一样，可以直接看里面的magcal源代码。

番外

除了采用椭球拟合方法来校准地磁场外，其实还有另外一种方法，叫做辅助向量法或者点击不变法。其基本思路就是如果除了地磁测量值外 还知道另外一个传感器坐标系下的向量(比如静止时候加速度计测得的重力场)，那么地磁场和重力场的点积应该是不变的(点积不就是两个向量的模乘以cos夹角嘛)。利用这点也可以构成校准算法。 当然还可以结合点积不变法和椭球拟合法进行更为高级的校准。这里放一个以前弄的基于MCU的直接计算硬磁软磁干扰。 利用椭球拟合 和 点积不变法 相结合的方法。只需要很少采样点，而且不需要静止采样。即可计算出 软磁干扰矩阵 和 硬磁bias。

MCU计算硬磁软磁干扰

 

参考

• PNI(专门搞地磁的公司的白皮书): https://www.pnicorp.com/white-papers/
• Calibration and Alignment of Tri-Axial Magnetometers for Attitude Determination
• https://search.proquest.com/docview/1512028629
• http://ztrw.mchtr.pw.edu.pl/en/least-squares-circle-fit/
• http://cseweb.ucsd.edu/~mdailey/Face-Coord/ellipse-specific-fitting.pdf
• 基于递推最小二乘法的地磁测量误差校正方法
• pronenewbits/Arduino_AHRS_System
• https://ww2.mathworks.cn/matlabcentral/fileexchange/22684-ellipse-fit-direct-method
• https://blog.csdn.net/yandld/ar

 

================= End