六轴IMU补偿的磁力计动态稳定校准

李文宽，蔡浩原，赵晟霖，刘春秀

(1.中国科学院空天信息研究院，北京 100190；2.中国科学院大学，北京 100049)

0 引言

在导航定位中，确定机器人、飞行器等载体的姿态是十分重要的，姿态可以通俗地使用欧拉角(俯仰角、横滚角和姿态角)来表示，目前通常使用六轴IMU来计算欧拉角，其中的陀螺仪感知三轴角速度值，由于重力加速度的存在，加速度计可以准确的感知横滚角和俯仰角，但六轴IMU只能获得航向角的相对变化值，并且由于陀螺仪存在不同程度的漂移现象，所以得到的航向角也会漂移，存在不稳定的现象。因此往往需要添加磁力计，组成九轴IMU来计算航向角。由于磁力计极易受到周围环境磁场的干扰，所以在使用过程中，往往需要先对磁力计进行校准，得到软铁干扰和硬铁干扰参数，磁力计数据在经过这些参数的处理后，才能准确的指示航向[1-3]。

磁力计校准方法可以根据是否需要外部设备分为两种类型。一种是基于磁场数据的约束，对磁场建模并仅使用磁力计数据来计算校准参数[4-5]。最常用的方法是椭圆拟合方法[6]和最大和最小值方法。朱建良等[7]首先线性化椭球表面方程，然后分别用最小二乘法和总体最小二乘法拟合得到椭球的参数；最大和最小值的方法是在3个轴上收集磁力计的最大和最小测量值，然后计算椭球面参数，其原理类似于椭球拟合法。虽然这种方法可以取得很好的效果，但对磁力计数据要求很高，因此往往需要用户进行特定的操作(如绕“8”)来收集数据，这对用户的使用来说非常不友好，而且在某些情况下(如机器人、无人机)很难实现。不仅如此，由于这种方法不能实时运行，所以需要在磁力计使用环境改变后再次校准。

另一种方法是使用外部设备来进行校准，最常用的是惯性测量单元(IMU)。M.Kok等[8]将磁力计和六轴IMU的数据建模为最大似然问题并进行求解，该方法使用来自2个不同的传感器单元实现了不错的校准效果，并且避免了某些操作的执行，但却无法实时运行。K.Han等[9]使用EKF(扩展卡尔曼滤波器)将磁力计和陀螺仪的数据融合实现了实时校准，但由于陀螺仪的漂移，得到的磁力计数据也不是很稳定。M.Zhu等[10]通过求解均匀最小二乘问题，在陀螺仪的帮助下提出了一种有效的磁力计校准算法。该算法能够在一个步骤中完成磁力计与陀螺仪固有误差和磁力计本身干扰误差的计算。

本文先通过互补滤波使用加速度修正陀螺仪数据，然后使用修正后的陀螺仪数据对磁力计进行旋转，以更准确地预测磁力计数据，完成扩展卡尔曼滤波中的状态预测过程，接着使用扩展卡尔曼滤波融合预测值和磁力计测量值，实现磁力计的动态校准。实现了一种稳定、高精度的实时磁力计校准。

1 磁力计量测模型

磁力计测量的磁场是使用环境中的磁场，包含了可以用于计算航向的地磁场和传感器附近的一些铁磁材料产生的磁场，所以需要对磁力计的测量值进行校准，保留地磁场信息，并以此来计算航向。

如果没有铁磁性材料的干扰，磁力计在t时刻的量测值hm,t与当地的地磁场mn的关系为[6，11-12]：

(1)

由此可知，理想情况下，磁力计的量测值应该位于一个球体表面，该球体的球心是原点，半径是当地地磁场强度。

铁磁材料对磁力计测量值产生的影响可以分为硬铁干扰和软铁干扰，硬铁干扰是铁磁材料的永久性磁化引起的，通常它会在磁力计的量测值上产生一个3×1的零漂ohi。软铁干扰是由于铁磁材料受到外部磁场的磁化而引起的，因此取决于材料相对于局部磁场的方向，通常用一个3×3的矩阵Csi来表示。结合公式(1)，可以推导出一般情况下的磁力计量测公式为：

(2)

同时，由于加工工艺等的影响，磁力计传感器的轴和IMU的轴不能保证完全重合，因此IMU的载体坐标系b与磁力计的载体坐标系bm之间仍然有一个旋转误差Rbmb，因此公式(2)可以写成：

(3)

另外，还有一些磁力计传感器本身的固有误差，这些误差也是在传感器的加工过程中确定的，每个传感器的误差值也不尽相同，这些固有误差分别为：

(1)磁力计传感器三轴没有严格正交引起的非正交(non-orthogonality)误差，使用3×3的矩阵Cno来表示；

(2)零漂(zero bias)的存在会在3个测量值上产生一个固定值，使用3×1的矢量ozb来表示；

(3)磁力计传感器3个轴的灵敏度不同，会对测量值产生缩放(scale)的影响，使用3×3的矩阵Csc来表示。

结合公式(3)，可以得到完整的磁力计量测模型：

(4)

对其进行化简并添加量测噪声，得到磁力计读数的表达式为：

(5)

D=CscCnoCsiRbmb

(6)

o=CscCnoohi+ozb

(7)

2 IMU数据处理

惯性测量单元(IMU)是一种可以测量物体3个轴的角速度和加速度的传感器，通常用于感知载体的姿态以及惯性导航。 加速度计和陀螺仪都可以计算姿态，但是各有优点和缺点：加速度计长期使用时计算出的姿态可信度比较高，没有累计误差，但是它对振动等干扰十分敏感，并且不能感知航向角的变化； 陀螺仪对振动不敏感也可以感知航向角的变化，但是长期使用陀螺仪会产生很严重的漂移。鉴于加速度计和陀螺仪各自的特性，使用互补滤波对二者数据进行融合[13-15]。

(8)

之后使用归一化后的加速度计的实际量测值a，和理论量测值v进行向量叉乘，得到陀螺仪的补偿校准值e。

e=v×a

(9)

然后使用PI控制器进行滤波，对陀螺仪消除漂移误差。只要存在误差，控制器便会持续作用，直至误差为0。控制的效果取决于P和I的参数，分别对应比例控制和积分控制的参数。

(10)

3 磁力计校准

3.1 状态转移方程的建模

由于磁力计和陀螺仪测量的是同一载体的姿态信息，所以二者感知到的姿态变化理论上应该是相同的，可以利用这一关系建立角速度和磁力计量测值之间的关系。

根据旋转矩阵的相关知识，其关于时间的导数可以写为：

(11)

该式被称为泊松公式(Possion’s equation)，其中∧为反对称矩阵算子：

(12)

将式(1)和式(12)相结合，就可以得到陀螺仪数据与磁力计数据二者之间的联系：

(13)

(14)

之后选取磁场数据hm,t、畸变矩阵D和偏移矢量o中的各个元素作为t时刻的状态量，记为Xt。

(15)

所以扩展卡尔曼滤波中的状态转移过程为：

(16)

(17)

(18)

将其写为统一的矩阵形式为：

Xt+1=FtXt+ωt

(19)

(20)

式中：ωt为状态转移过程中的噪声，它的方差矩阵记为Qt：

(21)

至此，推导完成了扩展卡尔曼滤波中的状态转移过程。

3.2 量测方程的建模

式(5)中，已经给出了磁力计的量测模型，量测值为hm,t，但是由于状态量设为了Xt，其中不仅包含了量测值hm,t，还包含了畸变矩阵D和偏移矢量o，所以需要求解其雅可比矩阵，对其进行线性化。解得量测方程为：

Zt=HtXt+vt

(22)

(23)

3.3 扩展卡尔曼滤波

根据以上内容，结合扩展卡尔曼滤波算法，推导出适用于磁场校准的EKF公式为：

预测阶段：

Xt|t-1=FXt-1

(24)

(25)

更新阶段：

(26)

Xt=Xt|t-1+Kt(Zt-HXt|t-1)

(27)

Pt=(I-KtHt)Pt|t-1

(28)

式中：Xt|t-1为状态量从t-1时刻到t时刻的一步预测值；Xt和Pt为通过EKF算法求得的t时刻的状态量估计值和状态的方差估计值；Kt为滤波增益。

4 实验

4.1 算法介绍

为了对比本文提出算法在磁力计动态校准时的稳定性，分别实现了校准算法中较为经典的基于最小二乘的椭球拟合算法和目前最为新颖的仅使用陀螺仪补偿的校准算法，并将本文算法与这两种算法进行对比试验。

4.1.1 基于最小二乘的椭球拟合算法

椭球拟合算法是一种仅使用磁力计就可以进行校准的方法，不需要其他外部设备。通过充分旋转传感器来获取广泛分布在椭球表面的磁力计数据，通过将椭球方程线性化后，使用最小二乘法求解椭球参数，从而获得当地磁场的校准参数，并对磁力计数据进行校准。但是这种方法需要用户旋转传感器来采集数据，使用并不是很方便，并且当磁场环境改变时，需要重新采集数据进行校准，不能实时动态校准。并且由于仅使用了磁力计数据，磁力计的噪声也难以抑制，影响磁力计数据的质量。

4.1.2 仅使用陀螺仪补偿的校准算法

仅使用陀螺仪补偿的校准算法是通过陀螺仪数据对磁力计数据进行旋转预测，然后使用扩展卡尔曼滤波对预测值和量测值进行融合。这种方法虽然实现了实时的动态校准，且对数据分布的要求比较低，但是由于陀螺仪数据没有预先进行处理，存在较为严重的漂移现象，所以磁力计数据并不稳定，同样存在漂移现象。

4.2 实验对比

本文中使用九轴IMU模块LMPS-B2(如图1所示)来采集磁力计、陀螺仪和加速度计的原始数据，模块的采样频率是100 Hz，并通过蓝牙将数据实时传输到电脑客户端。

图1 九轴IMU模块LMPS-B2

为保证实验效果，数据采集时要求传感器一直处于同一磁场环境下。

先充分旋转传感器，使得数据的分布满足椭球拟合算法的需求，之后将传感器静止10 min，采集静止数据。对于椭球拟合算法，由于其不能实时校准数据，所以将旋转时的数据输入到算法中计算校准参数，之后用这些校准参数对静止时数据进行校准。而仅使用陀螺仪补偿的校准算法和本文中的校准算法可以实时输出校准后的数据。

图2是3种算法的校准效果，图中实点数据点是原始数据，叉点数据点是算法校准后的数据(每隔10个数据画一个点)，可以发现由于软铁干扰和硬铁干扰的存在，原始数据所在的椭球并不在原点位置处，这也是磁力计数据校准的意义所在。经过3种算法校准后，磁力计的数据可以很好的恢复到理想球体附近，说明3种算法都达到了不错的校准效果。

(a)椭球拟合算法 (b)仅使用陀螺仪补偿的算法 (c)本文算法图2 3种算法的校准效果

接下来比较各算法在磁力计校准的稳定性上的差异。

首先来对比基于最小二乘的椭球拟合算法和本文算法在磁场校准时的稳定性，图3中画出了10 min的静止时间内，磁力计传感器x轴数据的情况，图中直线是椭球拟合算法校准后的磁力计数据，点是本文算法校准后的数据。可以发现，由于磁力计本身量测时存在噪声，所以数据的波动是很大的，而经过椭球拟合算法校准后的数据，虽然能够剔除软铁干扰和硬铁干扰，但是不能消除这一部分噪声，导致校准后的数据仍存在很大的波动，大概在2 μT左右，而本文算法融合了加速度计和陀螺仪的数据来对磁力计数据进行校准，可以很好的降低磁力计数据的噪声波动，仅为0.5 μT左右，提高了数据的可信度和稳定性。

图3 x轴的磁力计数据

其他轴上的效果也是如此，图4是磁力计3个轴上静止数据的箱型图，图5是xz平面上的数据以及置信度为95%的置信椭圆，可以发现每个轴上，本文算法校准后的噪声波动都小于椭球拟合算法。

(a)磁力计x轴数据 (b)磁力计y轴数据 (c)磁力计z轴数据图4 各轴数据的箱型图

图5 x轴和z轴的磁力计数据和置信椭圆

同时，还采集了多组数据进行实验，并计算了静止时三轴数据上的协方差矩阵，如表1所示，从表中可以发现本文算法校准后数据的方差普遍小于椭球拟合算法。

接下来比较仅使用陀螺仪补偿的磁场校准算法和本文算法，由于二者都可以实时的进行磁场校准，所以直接比较2个算法输出的参数即可。为了更加直观的比较两种算法的稳定性，本文计算了从静止开始，每一个数据点到静止起始数据点的距离，并以此来定量的表示磁力计数据的漂移情况，距离越大，漂移越严重，稳定性越差。

由于两种算法都用到了陀螺仪的数据，而陀螺仪本身就已经存在了一定的漂移现象，为了定量的分析不同陀螺仪漂移下，两种算法的稳定性。首先计算了陀螺仪三轴的漂移值，并在陀螺仪数据中将其剔除，然后人工添加不同级别的漂移，并比较各个情况下两种算法的漂移距离。

如图6所示，分别在陀螺仪数据上添加了0.1～0.5(°)/s的不同级别的零漂，然后将其输入两种算法中进行计算，图中2条直线分别是本文算法计算得到的磁力计漂移距离和陀螺仪补偿算法的漂移距离，可以明显的发现，本文算法在减少磁力计数据漂移、提高校准稳定性上有着良好的效果。

给陀螺仪3个测量轴添加了0.3(°)/s的零漂，并验证本文算法对漂移的抑制效果。由于加速度计具有长期稳定性，加入了加速度计的辅助后，本文算法的校准数据在10 min内仅漂移了1.67 μT，优于陀螺仪补偿算法的19.56 μT。

5 结束语

本文先通过互补滤波使用加速度修正陀螺仪数据，然后使用修正后的陀螺仪数据对磁力计进行旋转，以更准确地预测磁力计数据，完成扩展卡尔曼滤波中的状态预测过程，接着使用扩展卡尔曼滤波融合预测值和磁力计量测值，实现磁力计的动态校准。实验表明，相比于传统的椭球拟合算法，本文算法可以降低磁力计数据的噪声波动，二者的噪声波动分别为2 μT和0.5 μT；相较于最新的陀螺仪补偿算法，由于加速度计具有长期稳定性，可以抑制磁力计数据的漂移现象，磁力计数据的漂移距离由19.56 μT降低到了1.67 μT。实现了一种稳定、高精度的实时磁力计校准。

本文仅对磁力计数据进行了校准，并未继续求解航向等姿态信息，后续可以将校准后的磁力计数据用于姿态结算等多种需要磁场信息的算法当中。