地磁，干擾及校准，橢球擬合

參考 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