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

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

参考 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与惯性及多传感器组合导航系统原理-第二版

 

 

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

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

地球磁场简介

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

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

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

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

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

硬磁和软磁干扰

硬磁干扰

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

软磁干扰

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

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

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

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

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

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

数学知识- 椭圆/圆拟合

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

 这节内容基本翻译自这篇文章

最小二乘圆拟合

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

圆拟合问题的提出：

给定一些数据点(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计算硬磁软磁干扰
 

参考

 

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

 

posted @ 2022-03-26 23:48  lsgxeva  阅读(3787)  评论(0)  编辑  收藏  举报