基于递推最小二乘法加速度计信息辅助的磁力计标定方法∗

孙逸帆陈洋豪李 凤徐 祥

(苏州大学电子信息学院，江苏 苏州 215006)

随着MEMS 技术的不断发展，MEMS 磁力计有着广泛的应用，通过磁力计可对地磁场进行测量，并计算得到姿态和航向信息。 但三轴磁强计的测量精度会随着制作工艺等问题存在确定性误差，例如标度因子误差，零偏误差，非正交性误差。 此外，外界环境也会对磁力计测量精度带来影响，包括软磁材料引起的软铁干扰和硬磁材料引起的硬铁干扰。 因此，为提高磁力计的测量精度，在使用磁力计前需进行标定[1－2]。

目前，国内外对磁标定问题进行了很多研究。 一种是姿态无关法，即不需要外部姿态信息作为参考，利用归一化后的磁矢量输出的模值为1，可对磁力计误差参数进行标定。 在文献[3－4]中，作者通过将磁矢量的输出投射到一个椭球上，构建了磁力计的误差参数与椭球参数的函数关系式，通过拟合椭球参数完成对磁力计误差参数的估计。 为了进一步提高标定速度与精度，在文献[5－6]中，作者通过模值标定法构建了关于磁力计误差参数的代价函数，并通过递推最小二乘法或列文伯格法对代价函数进行迭代运算进而完成标定。 为克服磁力计随机噪声的影响，在文献[7－8]中，作者对随机噪声进行了考虑，构建了极大似然估计函数最优估计模型，并利用牛顿法对代价函数进行迭代求解。 对最优估计模型进行求解得到磁力计参数精度较高，但由于无外部参考信息，无法完成磁力计坐标系与惯性单元坐标系间未对准误差估计，影响在实际应用中的精度。

另一种磁力计标定方法为姿态相关法，通过利用外部姿态信息，即陀螺输出作为辅助，可对磁力计的误差参数和磁力计与陀螺坐标系之间的未对准误差进行标定[9－10]。 但在利用陀螺信息作为辅助时，首先要保证陀螺的精度。 对于陀螺标定，通常利用转台等设备，但这对MEMS 陀螺标定是昂贵的，且不满足现场标定的实际情况。 因此，先对磁力计单独标定，再使用加速度计信息作为辅助，对磁力计和磁力计与惯性单元间的未对准误差进行标定的两步标定法是近年来研究的主要方向。 在文献[11－13]中，作者首先对加速度计和磁力计的误差参数都采用极大似然估计法进行估计。 在完成传感器的单独标定后，再利用加速度计和磁力计间点积不变的性质，对磁力计和惯性单元间的未对准误差进行估计。 在文献[14]中，作者通过联合标定的方法，将加速度计误差，磁力计误差和磁力计与惯性单元间的未对准误差构建在一个代价函数中，通过对代价函数求解可得到误差参数，但这一求解过程计算量较大，不适用于对计算量有要求的现场标定中。 在文献[15－17]中，作者利用加速度计和磁力计点积不变的性质对磁力计的误差参数和未对准误差构建关系式，通过最小二乘法进行求解。但为标定准确，需采集足够多的数据，计算效率低。

因此，本文提出一种基于递推最小二乘法，利用加速度计信息辅助标定磁力计的方法，对磁力计误差参数和磁力计与惯性单元间的未对准误差进行一步标定。 首先，对加速度计进行标定。 再利用加速度计和磁力计间点积不变的性质，构建误差参数模型。 最后，通过递推最小二乘法，完成对磁力计误差参数的参数识别。

1 相关工作与问题描述

1.1 MEMS 磁力计建模

常用的磁力计误差模型为:

图1 非正交性误差示意图

式中:dm＝－Rmb，nm＝－Rmε。

1.2 MEMS 磁力计标定的椭球拟合法分析

磁力计标定的姿态无关法为椭球拟合标定法，由地磁矢量的模值可得到:

式中:E＝(Rm)TRm，F＝(dm)TRm，G＝(dm)Tdm。 由于存在测量误差，磁力计的输出矢量位于一个椭球面上，而椭球方程的系数是磁力计误差参数的函数。 因此对磁力计误差参数的估计可看做为椭球模型参数的估计。 在对参数Rm求解时，通常通过对E进行Cholesk 分解。 但由于无外部参考信息，因此分解不能得到唯一解。 例如，(Rm)T和Rm为一组解，(Rm)TQT和QRm为一组新的解。 其中Q为正交矩阵，代表标定得到的磁力计坐标系(b∗系)与磁力计真实所在b系间的方向余弦矩阵。 图2 表示磁力计在各个坐标系下三轴输出的示意图，实线代表磁力计在s系的三轴输出，虚线代表磁力计在b∗系的三轴输出，半划线代表磁力计在b系的三轴输出。 如果没有外部参考信息，则无法确定b系与b∗系间的转换关系。

图2 磁力计在s 系，b 系和b∗系坐标系三轴输出示意图

因此，在只针对磁力计传感器单独进行标定时，不唯一解对标定结果没有影响。 而在多传感器的情况下，传感器的误差和传感器间的未对准误差的影响都不可忽略。 为消除这种影响，可通过加速度计的输出作为辅助，完成磁力计与惯性单元间未对准误差的标定。

1.3 MEMS 加速度计极大似然法标定

为实现加速度计辅助磁力计标定，首先应该对MEMS 加速度计进行标定处理。 MEMS 加速度计测量模型为:

对加速度计的标定，通常采用模值法。 但在标定过程中，存在随机噪声的二次项的数学期望不为零这一问题，因此传统方法中标定加速度计过程中忽略随机噪声ε对误差参数的估计属于有偏估计。为消除随机噪声对标定结果的影响，构建极大似然估计模型:

对极大似然函数模型(6)，通常采用牛顿法或梯度下降法，通过迭代计算估计出MEMS 加速度计的误差参数。 为了保证迭代的准确性，需要对迭代的初始值进行估计。 迭代初始值的估计，可在式(5)中忽略噪声ε的情况下，利用最小二乘法估计得到误差参数。

2 加速度计辅助MEMS 磁力计标定方法

2.1 MEMS 加速度计极大似然法标定

因为地球上任一位置的重力矢量与地磁矢量间的夹角为固定值，磁倾角如图3 中虚线所示角，磁倾角γ可表示为:

图3 地球上任一位置矢量示意图

式(10)为对MEMS 磁力计误差参数的线性表达式，针对这个线性回归问题，可以通过最小二乘法或其他方法进行解决，进而完成标定。 为减小计算量，提高效率，本文采用递推最小二乘法对式(10)进行迭代求解。

2.2 递推最小二乘参数识别法

由于标定是在地球上某个位置进行的，因此可认为磁倾角不发生改变，因此标定过程中的重力矢量与地磁矢量点积值为固定值，观测方程可写为:

式中:Kk表示k时刻的增益矩阵；Pk｜k－1表示k时刻一步预测状态误差协方差矩阵；Rk表示k时刻量测噪声协方差矩阵；＾θk｜k表示k时刻误差参数最优估计值；Pk｜k表示k时刻的误差协方差矩阵；I9×9表示大小为9×9 的单位矩阵。 误差协方差矩阵初始值设置如(16)所示，迭代参数的初始值设置为(17)所示:

3 仿真和实验

3.1 仿真部分

为验证本文提出算法，仿真产生多位置MEMS加速度计与MEMS 磁力计数据。 设置数据采样频率为100 Hz。 根据MEMS 磁力计误差模型式(2)，设置磁力计误差参数如(18)，(19)所示。 MEMS 磁力计噪声设定为均值为零，标准差为0.007 的高斯白噪声。 仿真产生标定后的MEMS 加速度计数据，其随机噪声设定为均值为零，标准差为0.002 的高斯白噪声。

仿真产生的50 s 数据通过本文提出的方法进行标定，图4～图6 为估计出的磁力计误差参数迭代过程。 其中，虚线代表设定的磁力计参数，实线代表磁力计估计得到参数。 从图中可看出，在5 s 左右，dm和Rm的估计值已经收敛到设定真值的附近。 观察30 s～50 s 迭代过程中dm和Rm的估计值与设定真值间误差，可看出误差分别为10－3和10－4量级。 因此，可看出本文提出标定方法收敛速度快，收敛值准确。

图4 MEMS 磁力计dm 估计值迭代过程图

图5 MEMS 磁力计Rm 估计值的模值迭代过程图

图6 MEMS 磁力计Rm 估计值的模值迭代过程图

图7 中，将本文提出方法与椭球拟合法标定后的磁矢量模值进行比较。 其中，点划线表示标定前磁矢量模值，实线表示椭球拟合法标定后磁矢量模值，虚线表示本文方法标定后磁矢量模值。 从图中可看出，经过标定后，两种方法的磁矢量模值都在1 附近波动。 经过椭球拟合法标定后的磁矢量模值平均值为0.999 8，标准差为0.004 7。 经过本文标定方法标定后的磁矢量模值平均值为0.998 4，标准差为0.004 8。从模值上看，两种方法标定效果相似。 但本文提出方法是基于线性式(9)，因此，在标定过程中解决了加速度计与磁力计坐标系对准问题，克服了椭球拟合法中分解的不唯一解带来的影响。 根据式(7)，可对加速度计矢量与两种方法标定后的磁矢量间的旋转角进行求解。 图8 中，虚线表示椭球拟合法标定后矢量间旋转角，实线表示本文方法标定后矢量间旋转角。 仿真中设置的磁倾角为47.5°，从图8 中可看出，椭球拟合法标定后矢量间旋转角的曲线波动较大，其均值为47.48°，标准差为2.924。 而经本文方法标定后的矢量间旋转角波动较小，其均值为47.52°，标准差为0.317。 因此，可认为本文在标定过程中稳定完成了对矢量间对准误差的估计。

图7 标定前后磁矢量模值比较图

图8 加速度计矢量与磁力计矢量间旋转角

3.2 实验部分

为采集实际数据对本文算法进行验证，设计了硬件部分。 如图9 所示，其中，STM32F407 作为主控芯片，对ICM20948 九轴MEMS 传感器进行数据采集，通过串口将数据传输到上位机并存储。 设置采样频率为100 Hz，对固定在无磁3D 打印的外壳中的硬件部分进行缓慢旋转，采集100 s 磁力计与加速度计数据并存储。

图9 实验实物示意图

首先，对采集到的ICM20948 加速度计数据进行极大似然标定处理。 其次，利用磁力计和标定后的加速度数据，分别通过椭球拟合法和本文提出方法进行标定处理。 图10 中，对标定前和经过两种方法标定后的磁矢量模值进行了比较。 其中，用o 表示标定前磁矢量模值，用＋表示椭球拟合法标定后磁矢量模值，实线表示本文方法标定后磁矢量模值。从图10 中可看出，本文方法标定后磁矢量模值曲线高于椭球拟合法模值曲线，且经过两种方法标定后的磁矢量模值在1 附近波动。 其中，经过椭球拟合法标定后，磁矢量模值平均值为0.998 7，标准差为0.021。 经过本文方法标定后，磁矢量模值的平均值为1.012 7，标准差为0.021。 因此，可认为完成了磁力计的标定。

图10 ICM20948 磁矢量模值标定前后对比图

在图11 中，可观察到两种标定方法标定后磁力计与惯性单元间的旋转角。 其中，虚线表示椭球拟合法标定后矢量间旋转角，实线表示本文方法标定后矢量间旋转角。 从图中可看出椭球拟合法标定后的矢量旋转角波动较大，而经过本文方法标定后的矢量旋转角波动较小。 在表1 中，可看到经过椭球拟合法标定后磁力计与惯性单元间旋转角均值为29.69°，标准差为29.05。 经过本文方法标定后的磁力计与惯性单元间旋转角均值为47°，标准差为1.682。 而苏州地区的磁倾角为47.5°，考虑到消费级ICM20948 九轴MEMS 传感器精度较低，可认为已完成磁力计与惯性单元间的未对准误差标定。 因此，可认为本文提出方法在完成磁力计标定的同时，完成了加速度计与磁力计间的未对准误差。

图11 ICM20948 数据加速度计矢量与磁力计矢量间旋转角

表1 标定后磁力计与惯性单元间旋转角比较

4 结论

本文提出了一种基于递推最小二乘法利用加速度计信息辅助标定磁力计的方法，在完成磁力计误差参数估计的同时，也完成了加速度计和磁力计间未对准误差的估计。 首先，对加速度计数据完成标定，再利用加计矢量与磁矢量间点积不变的性质，通过递推最小二乘法对磁力计误差参数进行估计，进而完成标定。 通过仿真与实际数据，对本文提出方法进行了验证，结果表明，本文提出方法的标定精度与椭球拟合法相似，标定后的加速度计矢量与磁矢量间旋转角约等于当地磁倾角。 相比椭球拟合法，本文提出的方法克服了由于无外部信息参考而无法确定磁力计误差参数的影响。 并且通过递推最小二乘法对磁力计误差参数估计，计算效率高，可以快速完成标定。