地磁,干扰及校准,椭球拟合
地磁,干扰及校准,椭球拟合
参考 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的模就是磁场强度 =
- Declination(磁偏角):地磁场和地理北向的夹角。
- Inclination(磁倾角):地磁场与水平方向的夹角。
- 单位换算: 常用单位: 微特斯拉(uT), 毫高斯(mGauss)
- 1 uT = 10 mGauss ,地磁场的范围:250 - 650 mGauss 或 25 - 64 uT
在NED坐标系下,
惯性系地磁场矢量计算公式
其中
为磁偏角
为磁倾角
为地磁场大小
地磁场倾角速查:
某地详细地磁参数查询(NOAA):
例,通过NOAA网站查的北京的地磁场详细参数为:
可见北京地区总磁场强度为~54uT, 垂直分量:47uT, 水平分量:28uT, 倾角59°,偏角:-7°
参考
- GNSS与惯性及多传感器组合导航系统原理-第二版
- https://hal.inria.fr/hal-016501
NXP传感器融合笔记09(地磁,干扰及校准,椭球拟合)
来源 https://zhuanlan.zhihu.com/p/98325286
地球磁场简介
地球上某点的地磁场是一个三维空间矢量,从地磁南极出发到地磁北极。强度大约为23-66uT. 这个磁场强度在生活中属于非常小的量级。如果离一块小磁铁很近,那么磁铁产生的磁场可以轻轻松松超过地磁场上千倍。除了显而易见的磁铁,生活中的一些其他物品,包括你想到的想不到的,比如手机,电机,电子产品,耳机,内嵌金属的家具,办公桌,带电导线,甚至建筑钢架结构, 衣服里钥匙等等等等都会对地磁场产生严重干扰,总之:室内磁场分布非常复杂,地磁场在室内受干扰非常严重。
关于地磁场的一些更多知识和地磁传感器的基本概念,可以参看之前的笔记:
美国某地的地磁场具体参数,可以通过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维矢量)
校准模型根据待求的参数个数也分为几种:
- 4参数校准法:只估计硬磁干扰和磁场强度,软磁矩阵默认为单位阵。
- 7参数校准法:4参数校准法再加上软磁矩阵的主对角线元素(每个传感器轴的比例因子)
- 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。
参考
- 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
【推荐】国内首个AI IDE,深度理解中文开发场景,立即下载体验Trae
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 阿里最新开源QwQ-32B,效果媲美deepseek-r1满血版,部署成本又又又降低了!
· SQL Server 2025 AI相关能力初探
· AI编程工具终极对决:字节Trae VS Cursor,谁才是开发者新宠?
· 开源Multi-agent AI智能体框架aevatar.ai,欢迎大家贡献代码
· Manus重磅发布:全球首款通用AI代理技术深度解析与实战指南
2021-03-26 ITU R-REC-S 系列建议书分类
2020-03-26 一些QT开源项目
2020-03-26 Qt Quick WebGL在Qt 5.12中正式发布