一、地磁传感器介绍
1、 地磁传感器概念:
地磁传感器,又称磁强计、磁力计、电子罗盘,是一种通过测量地磁场的磁感应强度来确定方位的传感器。
在MEMS技术大力发展下,地磁传感器广泛应用于与MEMS 陀螺仪、加速度计进行数据融合,组成AHRS(航姿参考系统 Attitude and heading reference system),从而以较少的成本获得准确的姿态。应用领域有手机、无人机、路基惯性/GPS组合导航、人体动作捕捉等。
2、地磁传感器误差分析:
理论上,地磁传感器的输出在空间上的分布应该是一个球心位于原点的圆球面。由于地磁传感器自身以及所处环境的影响,地磁传感器的实际输出通常为一个偏离原点的椭球面。
地磁传感器的误差主要分为两类: 1)传感器结构、材料和电路引起的误差,如非正交误差、刻度因子误差和零偏误差等; 2)外部干扰磁场的叠加影响,如固定磁场、感应磁场等。
对地磁传感器输出进行建模,并考虑安装误差、非正交误差、刻度因子误差、硬磁误差和软磁误差后,将输出模型进行简化,可以得到输出模型为:1 B ^ b = A M C e b B e e + b b + ω b (1) \hat{B}^{b}=A_MC^b_eB^e_e+b^b+\omega ^b\tag{1} B^b=AMCebBee+bb+ωb(1) A M A_M AM为包含安装误差、非正交误差、刻度因子误差和软磁系数的误差阵; b b b^b bb为包含误差阵 A M A_M AM、硬磁误差和零偏误差的误差矢量, ω b \omega ^b ωb是测量随机误差和载体电路系统产生的杂散磁场造成的噪声和,认为是0均值白噪声。
暂不考虑 ω b \omega ^b ωb,对上式移项取逆后可得地磁传感器输出校正模型: B ^ e e = C b e A M − 1 ( B ^ b − b b ) (2) \hat{B}^{e}_e=C^e_bA_M^{-1}(\hat{B}^{b}-b^b)\tag{2} B^ee=CbeAM−1(B^b−bb)(2) 其中 B ^ e e \hat{B}^{e}_e B^ee为地磁场矢量的估计值, C b e C^e_b Cbe为从传感器坐标系到地理坐标系的方向余弦矩阵,且有 C b e = ( C e b ) − 1 C^e_b=(C^b_e)^{-1} Cbe=(Ceb)−1。
二、地磁传感器标定方法
地磁传感器标定方法主要分为三类,一类为自标定,仅仅利用此传感器自身采集的数据进行分析,得到各项误差系数;第二类为利用陀螺仪/加速度计等其他传感器辅助标定;第三类为利用无磁转台进行基准比较2。本文主要介绍前两类方法。
1.自标定
地磁传感器自标定的方法主要依赖于地磁矢量的模值大小在当地保持基本不变。
在公式(2)中,单独使用地磁传感器无法得到
C
b
e
C^e_b
Cbe,故对公式等号两侧取模可得:
∣
∣
B
^
e
e
∣
∣
=
∣
∣
C
b
e
A
M
−
1
(
B
^
b
−
b
b
)
∣
∣
=
∣
∣
A
M
−
1
(
B
^
b
−
b
b
)
∣
∣
(3)
||\hat{B}^{e}_e||=||C^e_bA_M^{-1}(\hat{B}^{b}-b^b)|| = ||A_M^{-1}(\hat{B}^{b}-b^b)|| \tag{3}
∣∣B^ee∣∣=∣∣CbeAM−1(B^b−bb)∣∣=∣∣AM−1(B^b−bb)∣∣(3) 如此一来,便摆脱了对
C
b
e
C^e_b
Cbe的依赖,在无法获得地磁传感器姿态的情况下,可利用式(3)进行标定补偿。 对矩阵
A
M
−
1
A_M^{-1}
AM−1进行QR分解,可得
A
M
−
1
=
Q
M
R
A
(4)
A_M^{-1}=Q_M R_A\tag{4}
AM−1=QMRA(4) 其中,显然
A
M
−
1
A_M^{-1}
AM−1非奇异,故分解唯一,
Q
M
Q_M
QM为正交矩阵,
R
A
R_A
RA为上三角阵,所有对角线元素为正。 将(4)式带入(3)式,可得
∣
∣
B
^
e
e
∣
∣
=
∣
∣
R
A
(
B
^
b
−
b
b
)
∣
∣
(4)
||\hat{B}^{e}_e||= ||R_A(\hat{B}^{b}-b^b)|| \tag{4}
∣∣B^ee∣∣=∣∣RA(B^b−bb)∣∣(4) 于是,参数空间的维数变为
R
A
R_A
RA的六维加
b
b
b^b
bb的3维共9维。由于QR分解唯一性,保证了参数的唯一性,从而避免了(3)式中由于丢失姿态信息以及求模操作而导致的多值问题。但由于缺失正交阵
Q
M
Q_M
QM,按照(4)进行标定的地磁传感器输出相比理想标定输出相差一个姿态变换,这可以通过引入外部传感器例如陀螺仪/加速度计解决1。
对于式(4),有 R 0 2 = ∣ ∣ R A ( B ^ i b − b b ) ∣ ∣ 2 , i = 1 , 2 , . . . , m (5) R^2_0= ||R_A( \hat{B}^{b} _i-b^b)||^2,i=1,2,...,m\tag{5} R02=∣∣RA(B^ib−bb)∣∣2,i=1,2,...,m(5) 其中, R 0 2 R^2_0 R02为当地地磁场矢量的模值; B ^ i b \hat{B}^{b}_i B^ib为传感器输出矢量; i i i为测量序列次数;m为采样个数。 将上式展开,可得, a 1 x i 2 + a 2 x i y i + a 3 x i z i + a 4 y i 2 + a 5 y i z i + a 6 z i 2 + a 7 x i + a 8 y i + a 9 z i + a 10 = 0 , i = 1 , 2 , . . . , m , (6) {a_1}{x^2_i}+{a_2}{x_i}{y_i}+{a_3}{x_i}{z_i}+{a_4}{y^2_i}+{a_5}{y_i}{z_i}+ {a_6}{z^2_i}+{a_7}{x_i}+{a_8}{y_i}+{a_9}{z_i}+{a_{10}}=0, i=1,2,...,m,\tag{6} a1xi2+a2xiyi+a3xizi+a4yi2+a5yizi+a6zi2+a7xi+a8yi+a9zi+a10=0,i=1,2,...,m,(6) 式中: x i , y i , z i x_i,y_i,z_i xi,yi,zi分别为 B ^ i b \hat{B}^{b}_i B^ib的3个分量, a 1 − a 1 0 a_1-a_10 a1−a10是由参数 R A R_A RA和 b b b^b bb构成的联合变量。 将(6)式写成矩阵形式,有 H X = W , (7) HX=W,\tag{7} HX=W,(7) 式中: X = [ x 1 2 x 1 y 1 x 1 z 1 y 1 2 y 1 z 1 x 1 y 1 z 1 1 x 2 2 x 2 y 2 x 2 z 2 y 2 2 y 2 z 2 x 2 y 2 z 2 1 . . . . . . x m 2 x m y m x m z m y m 2 y m z m x m y m z m 1 ] X=\begin{bmatrix} x_1^2 & x_1y_1 & x_1z_1 &y_1^2 &y_1z_1 &x_1 &y_1 &z_1 &1\\ x_2^2 & x_2y_2 & x_2z_2 &y_2^2 &y_2z_2 &x_2 &y_2 &z_2 &1\\ ... &...\\ x_m^2 & x_my_m & x_mz_m &y_m^2 &y_mz_m &x_m &y_m &z_m &1\\ \end{bmatrix} X= x12x22...xm2x1y1x2y2...xmymx1z1x2z2xmzmy12y22ym2y1z1y2z2ymzmx1x2xmy1y2ymz1z2zm111 X = [ a 1 / a 6 a 2 / a 6 a 3 / a 6 a 4 / a 6 a 5 / a 6 a 7 / a 6 a 8 / a 6 a 9 / a 6 a 10 / a 6 ] T X=\begin{bmatrix} a_1/a_6 &a_2/a_6 &a_3/a_6 &a_4/a_6 &a_5/a_6 &a_7/a_6 &a_8/a_6 &a_9/a_6 &a_{10}/a_6 \end{bmatrix}^T X=[a1/a6a2/a6a3/a6a4/a6a5/a6a7/a6a8/a6a9/a6a10/a6]T W = [ − z 1 2 − z 2 2 . . . − z m 2 ] T W=\begin{bmatrix} -z^2_1 &-z^2_2 &...&-z^2_m \end{bmatrix}^T W=[−z12−z22...−zm2]T H ∈ ℜ m × n , X ∈ ℜ n × 1 , W ∈ ℜ m × 1 H\in \Re^{m\times n},X\in \Re^{n\times 1},W\in \Re^{m\times 1} H∈ℜm×n,X∈ℜn×1,W∈ℜm×1;一般来说, m > n m>n m>n,因此(7)式为超定方程1。 以上理论推导参考文献1。
下面介绍在求得X之后,如何得到
R
A
,
b
b
R_A,b^b
RA,bb。
由向量2范数的定义
∣
∣
X
∣
∣
2
=
X
T
X
||X||^2=X^TX
∣∣X∣∣2=XTX,(5)式可展开为:
R
0
2
=
[
R
A
(
B
^
i
b
−
b
b
)
]
T
[
R
A
(
B
^
i
b
−
b
b
)
]
=
(
B
^
i
b
T
−
b
b
T
)
R
A
T
R
A
(
B
^
i
b
−
b
b
)
,
i
=
1
,
2
,
.
.
.
,
m
.
(9)
R^2_0= [R_A( \hat{B}^{b} _i-b^b)]^T[R_A( \hat{B}^{b} _i-b^b)]=( {\hat{B}^{b}_i}^T -{b^b}^T){R_A}^TR_A( \hat{B}^{b} _i-b^b),i=1,2,...,m.\tag{9}
R02=[RA(B^ib−bb)]T[RA(B^ib−bb)]=(B^ibT−bbT)RATRA(B^ib−bb),i=1,2,...,m.(9)
将上式继续展开,可得
R
0
2
=
B
^
i
b
T
R
A
T
R
A
B
^
i
b
−
B
^
i
b
T
R
A
T
R
A
b
b
−
b
b
T
R
A
T
R
A
B
^
i
b
+
b
b
T
R
A
T
R
A
b
b
,
i
=
1
,
2
,
.
.
.
,
m
.
(10)
R^2_0= {\hat{B}^{b}_i}^T{R_A}^TR_A {\hat{B}^{b}_i}-{\hat{B}^{b}_i}^T{R_A}^TR_Ab^b-{b^b}^T{R_A}^TR_A\hat{B}^{b} _i+ {b^b}^T{R_A}^TR_Ab^b,i=1,2,...,m.\tag{10}
R02=B^ibTRATRAB^ib−B^ibTRATRAbb−bbTRATRAB^ib+bbTRATRAbb,i=1,2,...,m.(10)
由于
B
^
i
b
∈
ℜ
3
×
1
,
R
A
∈
ℜ
3
×
3
,
b
b
∈
ℜ
3
×
1
\hat{B}^{b}_i \in \Re^{3\times 1},R_A \in \Re^{3\times 3},b^b \in \Re^{3\times 1}
B^ib∈ℜ3×1,RA∈ℜ3×3,bb∈ℜ3×1上式中
B
^
i
b
T
R
A
T
R
A
b
b
{\hat{B}^{b}_i}^T{R_A}^TR_Ab^b
B^ibTRATRAbb为标量,对其取转置值不变即
B
^
i
b
T
R
A
T
R
A
b
b
=
(
B
^
i
b
T
R
A
T
R
A
b
b
)
T
=
b
b
T
R
A
T
R
A
B
^
i
b
,
i
=
1
,
2
,
.
.
.
,
m
.
(11)
{\hat{B}^{b}_i}^T{R_A}^TR_Ab^b=({{\hat{B}^{b}_i}^T{R_A}^TR_Ab^b})^T={b^b}^T{R_A}^TR_A\hat{B}^{b} _i,i=1,2,...,m.\tag{11}
B^ibTRATRAbb=(B^ibTRATRAbb)T=bbTRATRAB^ib,i=1,2,...,m.(11)
于是,(10)式可简化为:
R
0
2
=
B
^
i
b
T
R
A
T
R
A
B
^
i
b
−
2
b
b
T
R
A
T
R
A
B
^
i
b
+
b
b
T
R
A
T
R
A
b
b
,
i
=
1
,
2
,
.
.
.
,
m
.
(12)
R^2_0= {\hat{B}^{b}_i}^T{R_A}^TR_A {\hat{B}^{b}_i}-2{b^b}^T{R_A}^TR_A\hat{B}^{b} _i+ {b^b}^T{R_A}^TR_Ab^b,i=1,2,...,m.\tag{12}
R02=B^ibTRATRAB^ib−2bbTRATRAB^ib+bbTRATRAbb,i=1,2,...,m.(12)
其中,第一项为二次型,第二项为一次项,最后一项为常数项。
显然
R
A
T
R
A
{R_A}^TR_A
RATRA为对称阵,令
R
A
T
R
A
=
M
=
[
m
1
m
2
m
3
m
2
m
4
m
5
m
3
m
5
1
]
,
B
^
i
b
=
[
x
y
z
]
T
{R_A}^TR_A=M=\begin{bmatrix} m_1 &m_2 &m_3\\ m_2 &m_4 &m_5\\ m_3 &m_5 &1\\ \end{bmatrix},{\hat{B}^{b}_i}={\begin{bmatrix} x &y &z \end{bmatrix}}^T
RATRA=M=
m1m2m3m2m4m5m3m51
,B^ib=[xyz]T,则
B
^
i
b
T
R
A
T
R
A
=
B
^
i
b
T
M
B
^
i
b
=
[
x
y
z
]
[
m
1
m
2
m
3
m
2
m
4
m
5
m
3
m
5
1
]
[
x
y
z
]
=
m
1
x
2
+
m
4
y
2
+
z
2
+
2
m
2
x
y
+
2
m
3
x
z
+
2
m
5
y
z
.
(13)
{\hat{B}^{b}_i}^T{R_A}^TR_A= {\hat{B}^{b}_i}^TM{\hat{B}^{b}_i}=\begin{bmatrix} x &y &z \end{bmatrix}\begin{bmatrix} m_1 &m_2 &m_3\\ m_2 &m_4 &m_5\\ m_3 &m_5 &1\\ \end{bmatrix}\begin{bmatrix} x\\y\\z \end{bmatrix}\\=m_1x^2+m_4y^2+z^2+2m_2xy+2m_3xz+2m_5yz.\tag{13}
B^ibTRATRA=B^ibTMB^ib=[xyz]
m1m2m3m2m4m5m3m51
xyz
=m1x2+m4y2+z2+2m2xy+2m3xz+2m5yz.(13)
对比上式与(7)式,可得
M
=
[
m
1
m
2
m
3
m
2
m
4
m
5
m
3
m
5
1
]
=
[
X
(
1
)
X
(
2
)
/
2
X
(
3
)
/
2
X
(
2
)
/
2
X
(
4
)
/
2
X
(
5
)
/
2
X
(
3
)
/
2
X
(
5
)
/
2
1
]
(14)
M=\begin{bmatrix} m_1 &m_2 &m_3\\ m_2 &m_4 &m_5\\ m_3 &m_5 &1\\ \end{bmatrix}= \begin{bmatrix} X(1) &X(2)/2 &X(3)/2\\ X(2)/2 &X(4)/2 &X(5)/2\\ X(3)/2 &X(5)/2 &1\\ \end{bmatrix}\tag{14}
M=
m1m2m3m2m4m5m3m51
=
X(1)X(2)/2X(3)/2X(2)/2X(4)/2X(5)/2X(3)/2X(5)/21
(14)
对
M
M
M取乔伊斯基变换可得
R
A
=
[
c
h
o
l
e
s
k
y
(
M
)
]
T
R_A=[cholesky(M)]^T
RA=[cholesky(M)]T
另外,对于一次项,可采用类似比较系数的方法,得
−
2
b
b
T
R
A
T
R
A
=
[
X
(
6
)
X
(
7
)
X
(
8
)
]
(15)
-2{b^b}^T{R_A}^TR_A=\begin{bmatrix} X(6) &X(7) &X(8) \end{bmatrix}\tag{15}
−2bbTRATRA=[X(6)X(7)X(8)](15)
又
R
A
T
R
A
=
M
{R_A}^TR_A=M
RATRA=M,所以有
b
b
T
=
−
1
2
[
X
(
6
)
X
(
7
)
X
(
8
)
]
M
−
1
(16)
{b^b}^T=-\frac{1}{2}\begin{bmatrix} X(6) &X(7) &X(8) \end{bmatrix}M^{-1}\tag{16}
bbT=−21[X(6)X(7)X(8)]M−1(16)
注意:在上述推导中,将Z轴上的分量的大小作为标定后地磁矢量的模值大小,显得不那么严谨。好在磁强计在使用时是用比值进行计算角度,因此问题不大。更为准确的做法我认为是两边同除以模值,让(7)式中a10不必移项,直接变为1即可。标定完毕后,根据位置和时间查询地磁矢量的模值大小(MatLab 中函数igrfmagm和wrldmagm,二者所用模型不同,暂未查询到二者的区别)。
1.1.两步法
第一步: 对于(7)式,假定矩阵H是精确的,W含有噪声,根据Gauss-markov定理,X的最小二乘估计(LS)为 X L S = ( H T H ) − 1 H T W , (17) X_{LS}=(H^TH)^{-1}H^TW,\tag{17} XLS=(HTH)−1HTW,(17) 第二步: 根据算法X中间量的定义,求出 R A R_A RA和 b b b^b bb。
两步法中,H和W均由地磁传感器量测值构造而成,二者都含有噪声,当测量值误差较大值,两步法标定较差。
1.2.基于总体最小二乘(TLS)的算法
基于TLS的方法利用扰动矩阵和扰动向量同时干扰H和W,以便矫正二者内在的扰动,由于考虑到了测量误差,理论上其估计精度比LS更高。 假设H和W含有测量误差,分别记为E和e,(7)式可改写为: ( H + W ) X = W + e (18) (H+W)X=W+e\tag{18} (H+W)X=W+e(18) TLS将其转化为约束最优化问题进行求解1,
最优解可通过对扩展矩阵 G = [ − W H ] G=\begin{bmatrix} -W &H \end{bmatrix} G=[−WH]进行奇异值分解得到1
1.3.递推最小二乘法(RLS)
递推最小二乘法主要将批量处理地磁传感器数据改为递推形式,更适合在线执行3。
文献3所述的参数方程与上面推导的(6)式略有不同,但也只是系数归一化的所参照的参数项不同而已。
对(6)式稍做变换,写成 矩阵形式,有
h
(
k
)
=
[
x
i
2
x
i
y
i
x
i
z
i
y
i
2
y
i
z
i
x
i
y
i
z
i
1
]
h(k)=\begin{bmatrix} x_i^2 & x_iy_i & x_iz_i &y_i^2 &y_iz_i &x_i &y_i &z_i &1 \end{bmatrix}
h(k)=[xi2xiyixiziyi2yizixiyizi1]
z
(
k
)
=
[
−
z
i
2
]
\\z(k)=\begin{bmatrix} -z_i^2 \end{bmatrix}\\
z(k)=[−zi2]
θ
(
k
)
=
[
a
1
/
a
6
a
2
/
a
6
a
3
/
a
6
a
4
/
a
6
a
5
/
a
6
a
7
/
a
6
a
8
/
a
6
a
9
/
a
6
a
10
/
a
6
]
\theta(k)=\begin{bmatrix} a_1/a_6 &a_2/a_6 &a_3/a_6 &a_4/a_6 &a_5/a_6 &a_7/a_6 &a_8/a_6 &a_9/a_6 &a_{10}/a_6 \end{bmatrix}
θ(k)=[a1/a6a2/a6a3/a6a4/a6a5/a6a7/a6a8/a6a9/a6a10/a6]
1.4.非线性寻优法
利用两步法的结果作为初值,然后应用非线性寻优方法例如牛顿法、Levenberg-Marquardt法等进行迭代求解。
1.5.椭球拟合法
此方法是直接拟合公式(6)中所表示的椭球,可以参考各种球面拟合方法。matlab中有相关函数可供调用。
1.6.信号预处理—形态滤波与HHT法
此方法主要是利用形态学滤波去除传感器信号中的脉冲噪声,用希尔伯特-黄变换HHT识别高频噪声,以消除随机噪声对信号的干扰,后续的标定过程还是参考上面的方法4。
2、陀螺仪/加速度计辅助法
可以参考武元新老师发表的这篇文章5,文章还公布了文章的数据,方便复现研究。该文章中还考虑到了磁强计与IMU间微小姿态转换问题。
不过当时复现时候,文章中(20)中Hm调整第一项
S
C
m
×
SCm\times
SCm×的正负号才收敛。自己推一下就看出来了,应该是笔误。
三、总结
参考文献
吴志添, 武元新, 胡小平, 吴美平. 基于总体最小二乘的捷联三轴磁力仪标定与地磁场测量误差补偿[J]. 兵工学报, 2012, 33(10): 1202-1209. WU Zhi-tian, WU Yuan-xin, HU Xiao-ping, WU Mei-ping. Calibration of Strapdown Three-axis Magnetometer and Measurement Error Compensation of GeomagneticField Based on Total Least Squares. Acta Armamentarii, 2012, 33(10): 1202-1209. ↩︎ ↩︎ ↩︎ ↩︎ ↩︎ ↩︎
高可,林新华,储志伟,武靖博.无磁转台的电子罗盘误差分离标定方法[J].传感器与微系统,2017,36(02):21-24. ↩︎
刁云云,高金耀,吴国超,等.基于形态学-HHT算法的船载地磁三分量信号分析与预处理[J].海洋学研究, 2021, 39(3):9.DOI:10.3969/j.issn.1001-909X.2021.03.005. ↩︎
Y. Wu, D. Zou, P. Liu and W. Yu, “Dynamic Magnetometer Calibration and Alignment to Inertial Sensors by Kalman Filtering,” in IEEE Transactions on Control Systems Technology, vol. 26, no. 2, pp. 716-723, March 2018, doi: 10.1109/TCST.2017.2670527. ↩︎