地球磁場(chǎng)簡(jiǎn)介

地球上某點(diǎn)的地磁場(chǎng)是一個(gè)三維空間矢量，從地磁南極出發(fā)到地磁北極。強(qiáng)度大約為23-66uT. 這個(gè)磁場(chǎng)強(qiáng)度在生活中屬于非常小的量級(jí)。如果離一塊小磁鐵很近，那么磁鐵產(chǎn)生的磁場(chǎng)可以輕輕松松超過(guò)地磁場(chǎng)上千倍。除了顯而易見(jiàn)的磁鐵，生活中的一些其他物品，包括你想到的想不到的，比如手機(jī)，電機(jī)，電子產(chǎn)品，耳機(jī)，內(nèi)嵌金屬的家具，辦公桌，帶電導(dǎo)線，甚至建筑鋼架結(jié)構(gòu), 衣服里鑰匙等等等等都會(huì)對(duì)地磁場(chǎng)產(chǎn)生嚴(yán)重干擾，總之：室內(nèi)磁場(chǎng)分布非常復(fù)雜，地磁場(chǎng)在室內(nèi)受干擾非常嚴(yán)重。

關(guān)于地磁場(chǎng)的一些更多知識(shí)和地磁傳感器的基本概念，可以參看之前的筆記：楊熙：NXP傳感器融合筆記04(IMU，AHRS 加速度計(jì)，陀螺儀，地磁計(jì)概念)?zhuanlan.zhihu.com

美國(guó)某地的地磁場(chǎng)具體參數(shù)，可以通過(guò)NOAA網(wǎng)站查詢：可以看出，這個(gè)地方的地磁場(chǎng)大部分 分量是朝下的哦。。

硬磁和軟磁干擾

硬磁干擾

硬磁干擾其實(shí)就相當(dāng)于磁傳感器的零偏，但是這個(gè)零偏不是由傳感器自身引起。如果沒(méi)有硬磁干擾，我們讓磁傳感器旋轉(zhuǎn)所有可能的角度，把數(shù)據(jù)記錄下來(lái)，會(huì)得到一個(gè)球，而且球心在0,0,0 原點(diǎn)，但由于硬磁干擾的影響。總會(huì)測(cè)得一個(gè)bias(球的圓心不在原點(diǎn))，這個(gè)bias就是硬磁干擾。

軟磁干擾

軟磁是由于傳感器周?chē)拇判晕镔|(zhì)扭曲磁場(chǎng)得到，如果向上面那樣讓傳感器旋轉(zhuǎn)然后采集點(diǎn)，軟磁干擾表現(xiàn)為將球變成橢球：注意無(wú)論是硬磁還是軟磁干擾，都指的是在傳感器坐標(biāo)系下，換句話說(shuō)，干擾源和傳感器在同一個(gè)設(shè)備里，一起運(yùn)動(dòng)，旋轉(zhuǎn)。如果干擾源不再傳感器坐標(biāo)系下而在固定坐標(biāo)系下，那么就屬于空間磁干擾，是無(wú)法校準(zhǔn)補(bǔ)償?shù)?#xff01;(比如室內(nèi)的固定干擾源，桌子椅子，鋼筋等)。 這也是為啥室內(nèi)地磁電子羅盤(pán)不準(zhǔn)的原因

總結(jié)一下硬磁干擾和軟磁干擾對(duì)于磁場(chǎng)的畸變影響：無(wú)干擾，硬磁干擾，軟磁干擾

以及他們對(duì)磁傳感器采樣結(jié)果的影響：完美的圓(無(wú)干擾)，偏心的圓(只有硬磁干擾)，偏心橢圓(硬磁+軟磁 )

數(shù)學(xué)知識(shí)- 橢圓/圓擬合

這塊一部分時(shí)后來(lái)加進(jìn)來(lái)，地磁校準(zhǔn)和圓/橢圓/球/橢球擬合這個(gè)數(shù)學(xué)問(wèn)題息息相關(guān)，所以這里打算著重大書(shū)特書(shū)一下有關(guān)的數(shù)據(jù)知識(shí)，主要是看到一篇非常好的英文文章：

最小二乘圓擬合

機(jī)器視覺(jué)，包括本章說(shuō)要解決的地磁校準(zhǔn)問(wèn)題都會(huì)可以歸結(jié)于圓/橢圓或者橢球/橢球的數(shù)據(jù)擬合問(wèn)題。這里有兩本比較老但非常不錯(cuò)的論文可以參考：

圓擬合問(wèn)題的提出：

給定一些數(shù)據(jù)點(diǎn)(X,Y) 求圓形坐標(biāo)和圓半徑，給出的數(shù)據(jù)點(diǎn)要大于未知數(shù)的個(gè)數(shù)。

比如給出的數(shù)據(jù)點(diǎn)記作：P數(shù)據(jù)點(diǎn)

代數(shù)法求解

代數(shù)法求解原理簡(jiǎn)單直接，我們把圓方程寫(xiě)為代數(shù)形式：

設(shè)待求向量為

則有：

其中B：

一般情況下，我們需要加一個(gè)約束來(lái)轉(zhuǎn)換成最小二層問(wèn)題：

，由此可得最小二層問(wèn)題：

最終轉(zhuǎn)換為圓形和半徑：

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最小二乘法求解

這個(gè)結(jié)果可以說(shuō)不怎么好，實(shí)際上，這種算法只是求得了圓形到這些數(shù)據(jù)點(diǎn)距離最近的一個(gè)圓，并沒(méi)有考慮這些點(diǎn)的幾何關(guān)系，所以擬合效果不佳，如果數(shù)據(jù)點(diǎn)比較少(比如上面這個(gè)例子)，那么效果會(huì)更差。

幾何法求解

幾何求解法直接優(yōu)化(最小化) 各數(shù)據(jù)點(diǎn)與圓心所形成的向量的長(zhǎng)度與圓半徑的差值平方和，如上圖所示。換成數(shù)學(xué)語(yǔ)言：設(shè)

，

則有：

或者寫(xiě)做：

如上所述，定義cost function:

. 現(xiàn)在，要解決的問(wèn)題變成了非線性問(wèn)題，不能用傳統(tǒng)的最小二乘來(lái)解決了，我們采用高斯牛頓迭代來(lái)搞定它!

高斯牛頓迭代

高斯牛法采用迭代方法來(lái)實(shí)現(xiàn)優(yōu)化：實(shí)際上就是梯度下降法

其中

是雅克比矩陣的逆，

是殘差。當(dāng)方程數(shù)多于未知數(shù)時(shí)，系統(tǒng)變成超定問(wèn)題，雅克比矩陣逆通過(guò)偽逆求得：

雅克比矩陣怎么求呢，實(shí)際就是按每個(gè)變量求偏導(dǎo)數(shù)所組成的矩陣，不再贅述：雅克比矩陣

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

functionJ =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

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

end

效果：這次效果不錯(cuò)

這次雖然效果好，但是高斯牛頓迭代只會(huì)找到局部最優(yōu)解，并且嚴(yán)重依賴于初始值，如果初始值給的不好，有可能得不到最優(yōu)解甚至發(fā)散。我們來(lái)看看還有沒(méi)有什么別的騷操作：

線性化的幾何解法

之前我們的代價(jià)函數(shù)定位為：

所以這是一個(gè)非線性問(wèn)題，只能采用非線性優(yōu)化的方式來(lái)解。現(xiàn)在我們稍加改動(dòng)，把它變成一個(gè)線性問(wèn)題：定義代價(jià)函數(shù)為：

展開(kāi)可得：

注意到我們要求是圓形坐標(biāo)

和圓半徑

,整理可得：

之后就是很tricky的一步了，我們做如下變量替換：

那么代價(jià)函數(shù)就可以寫(xiě)成z的線性函數(shù)的形式：

下面就可以使用最小二層來(lái)解決了，進(jìn)而求得圓形坐標(biāo)和圓半徑：

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

結(jié)論

這三種方法各有千秋，通常，當(dāng)數(shù)據(jù)點(diǎn)比較少時(shí)，三種方法出來(lái)的效果差別很大，當(dāng)數(shù)據(jù)點(diǎn)足夠多時(shí)，三種方法擬合出來(lái)的圓差不多。三種方法比較

地磁3D校準(zhǔn)模型

既然地磁場(chǎng)在實(shí)際應(yīng)用中會(huì)經(jīng)常受到硬磁和軟磁干擾，那我們就要想辦法校準(zhǔn)補(bǔ)償他。數(shù)學(xué)模型如下：

其實(shí)數(shù)學(xué)模型就是這個(gè)(所有的文獻(xiàn)論文，大家都用這個(gè)模型咯)

其中 ： 校準(zhǔn)之后的磁場(chǎng)(3緯矢量)

:軟磁干擾矩陣

：磁傳感器原始數(shù)據(jù)

:硬磁bias(3維矢量)

校準(zhǔn)模型根據(jù)待求的參數(shù)個(gè)數(shù)也分為幾種：4參數(shù)校準(zhǔn)法：只估計(jì)硬磁干擾和磁場(chǎng)強(qiáng)度，軟磁矩陣默認(rèn)為單位陣。

7參數(shù)校準(zhǔn)法：4參數(shù)校準(zhǔn)法再加上軟磁矩陣的主對(duì)角線元素(每個(gè)傳感器軸的比例因子)

10參數(shù)校準(zhǔn)法： 把軟磁干擾假設(shè)為對(duì)稱矩陣，求解出對(duì)稱矩陣的所有元素

地磁3D校準(zhǔn)原理

校準(zhǔn)的數(shù)學(xué)原理其實(shí)很簡(jiǎn)單： 任何校準(zhǔn)好的傳感器坐標(biāo)系地磁場(chǎng)的大小應(yīng)該是恒定不變的。就這一條， 用數(shù)學(xué)公式說(shuō)就是:

. 全部靠這條公理來(lái)實(shí)現(xiàn)校準(zhǔn)參數(shù)估計(jì)：

把

展開(kāi)，并且令

可得

后面就可以用最小二層法來(lái)求解矩陣A。

3D地磁校準(zhǔn)方法

這里簡(jiǎn)單的講下擬合球面的方法(，只是球面哦，不是橢球, 之前已經(jīng)講了如何擬合一個(gè)圓，這里只不過(guò)拓展為球，參考AN5019)

根據(jù)式子

展開(kāi)可得：

寫(xiě)成矩陣形式：

令：

則，可以轉(zhuǎn)化為一個(gè)最小二乘問(wèn)題(其實(shí)和上面的圓擬合第三種方法是一樣的道理)。

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; %噪聲強(qiáng)度

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自帶的函數(shù)，算法和本文章描述的一樣，可以直接看里面的magcal源代碼。

番外

除了采用橢球擬合方法來(lái)校準(zhǔn)地磁場(chǎng)外，其實(shí)還有另外一種方法，叫做輔助向量法或者點(diǎn)擊不變法。其基本思路就是如果除了地磁測(cè)量值外 還知道另外一個(gè)傳感器坐標(biāo)系下的向量(比如靜止時(shí)候加速度計(jì)測(cè)得的重力場(chǎng))，那么地磁場(chǎng)和重力場(chǎng)的點(diǎn)積應(yīng)該是不變的(點(diǎn)積不就是兩個(gè)向量的模乘以cos夾角嘛)。利用這點(diǎn)也可以構(gòu)成校準(zhǔn)算法。 當(dāng)然還可以結(jié)合點(diǎn)積不變法和橢球擬合法進(jìn)行更為高級(jí)的校準(zhǔn)。這里放一個(gè)以前弄的基于MCU的直接計(jì)算硬磁軟磁干擾。 利用橢球擬合 和 點(diǎn)積不變法 相結(jié)合的方法。只需要很少采樣點(diǎn)，而且不需要靜止采樣。即可計(jì)算出 軟磁干擾矩陣 和 硬磁bias。MCU計(jì)算硬磁軟磁干擾https://www.zhihu.com/video/1191654170040848384

參考

總結(jié)

以上是生活随笔為你收集整理的椭球拟合的电子罗盘磁差补偿_NXP传感器融合笔记09(地磁，干扰及校准，椭球拟合)...的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。