基于RLM 的MEMS 三轴磁力计标定方法∗

陈洋豪，徐 祥，李 凤，邵 雷∗

(1.苏州大学电子信息学院，江苏 苏州 215006；2.南京理工大学自动化学院，江苏 南京 210094)

当前，随着微机电系统技术的不断发展，MEMS磁力计已经广泛应用于惯性导航与组合导航中的各个领域[1－4]。在惯性导航与组合导航系统中，磁力计可以提供重要的航向信息。由于MEMS 三轴磁力计存在输出精度不高的问题，使得三轴磁力计测量所得数据与真实的地磁矢量之间偏差较大，进而提供错误的航向信息。所以为了提高三轴磁力计的测量精度，在使用前对其进行标定就显得尤为重要。

为了提高磁力计的测量精度，众多学者对此进行了广泛的研究并提出相应的标定方法。李智、龙礼、Xia、安亮亮等[5－8]提出了基于椭球参数和磁力计误差参数输出模型的函数方程，然后通过拟合椭球参数来计算磁力计误差参数，从而达到标定磁力计的目的。Yu 等[9]则根据地球上某个地区当地重力矢量与地磁矢量之间夹角固定不变的原理，再利用标定好的加速度计来标定磁力计。在椭球模型的基础之上，一些学者分别使用了卡尔曼滤波、牛顿法、列文伯格法、自适应迭代最小二乘法来估计误差参数[10－13]。此外，武元新、Kokm 等[14－15]则提出了一种基于极大似然估计的方法来实现对三轴磁力计误差的校正。

上述这些方法仅仅只考虑到由于制造加工工艺对磁力计所带来的误差影响，并没有考虑到磁力计容易受到外部环境的磁干扰这一情况。因此，本文提出了一种基于鲁棒列文伯格－马夸尔特算法对磁力计进行标定。在建立磁力计误差模型方程的基础上，通过在迭代过程中依据拉依达准则对野值数据进行识别并剔除[16]，然后将剩余数据赋予对应的权重，从而削弱野值对标定结果的影响，提高了对误差参数估计的准确性。

1 MEMS 三轴磁力计误差分析与建模

由MEMS 三轴磁力计测量机理可知，含有磁畸变以及磁力计自身误差的输出可建模为[13－14]:

式中:mm表示三轴磁力计在载体系下的输出；S表示标度因数；Cno表示非正交性误差；Csi表示软磁误差矩阵。表示由当地导航坐标系(n系)转向载体坐标系(b系)的方向余弦矩阵。mn表示地磁矢量在导航系下的投影矢量。bh表示硬磁误差矩阵。bm表示零偏误差矢量。ε表示随机噪声。在本文中，定义的导航坐标系为东－北－天坐标系，载体坐标系为右－前－上坐标系。

方便起见，可将式(1)简化为等效误差模型:

式中:mb＝mn，C＝SCnoCsi表示等效软磁误差矩阵，b＝SCnobh＋bm表示等效硬磁误差矩阵。

由于随机噪声可以通过采集的数据平均化来减弱影响[17]。则可以对式(2)进行简化变形后可得三轴磁力计误差标定模型:

2 基于RLM 的三轴磁力计标定方法

为了求解式(3)中磁力计的最优误差参数，可以将其转变为求解非线性最小二乘最优化的过程。本文的非线性最小二乘问题可以表示为式(4):

式中:根据式(3)所定义的标定误差参数为:

Loss(Xi)为代价函数，表示标定后磁场强度与当地地磁强度的残差，通过本文所提RLM 法最小化代价函数，求解误差参数，然后代入式(3)求得标定后的磁场数据，尽可能接近于真实值。代价函数具体形式为:

式中:wi表示第i个数据的权重；‖mlocal‖表示地球上某一地区的地磁强度大小；表示第i个标定后的磁力计数据。

本文所提算法的迭代通式与LM 法[12]一样，具体形式为:

式中:Xnew为经过一次迭代后的新误差参数；J(N×12)为雅克比矩阵，其中N是采样数据点个数，12 是误差参数个数；μ为阻尼因子，当μ过大时，算法接近为梯度下降法，此时算法收敛较慢，当μ过小时，算法则接近为高斯牛顿法，此时算法收敛较快。即本文所提算法继承了传统LM 法既有高斯牛顿法的收敛速度快又有梯度下降法逼近最优解的特点。

值得注意的是式(7)中阻尼因子μ会根据优化随时进行调整，控制着迭代的下降方向与步长。本文调整更新μ的步骤如下:

式中:g(X)是非线性最小二乘f(X)的近似线性函数；如果ρ>0 则代表近似模型较好，然后根据式(9)调整μ；如果ρ<0，则根据式(10)调整μ。

考虑到算法应用于工程实际中，可能需要面对不同的标定环境，一旦三轴磁力计测量数据中存在部分野值时，传统的LS 椭球拟合法、LM 法所求得的误差参数势必受到影响，将偏离正常值。为了解决这个问题，本文便在传统LM 算法的基础上又进行了如下改进:根据式(7)迭代所得新的误差参数对磁力计数据进行标定，将标定后的磁力计数据模值与当地地磁矢量模值做残差为:

然后便可以计算各数据权重为:

式中:μerror、σerror分别为式(11)中N个残差的均值与标准差。

为了体现本文的鲁棒性，如果数据中不存在野值，则根据式(12)在第一次迭代结束后各数据权重都将接近于1 附近。如果存在野值，第一次迭代结束后部分野值数据的权重虽然会显著变小但是依旧会影响标定结果，所得到的只是粗精度的标定参数。为了进一步解决这个问题，本文又接着对这N个残差作拉依达准则检测，拉依达准则又称为3σ准则，残差落在区间[－3σ，3σ]的概率≥99.7%，落在该区间外的概率为<0.3%。若满足式(13)所提供的条件，则该数据即可被识别为野值，并将其从原有的N个数据中剔除。

将剔除完后剩余数据的个数记为Nnew，并更新这Nnew个数据的均值与残差，并重新计算剩余各个数据的权重。最后即可根据所得权重去更新式(6)所示的代价函数以及式(3)所示的标定后磁力计数据直至算法满足终止条件。本文算法迭代终止条件有3 个，满足其中任意一个则迭代结束。分别为:

①设置最大迭代次数M，本文设置为50。

②当满足‖JTLoss(X)‖<ε1可认为算法已经收敛。ε1为设定的常量，本文取0。

③当满足‖Xnew－X‖<ε2(‖X‖＋ε2)即误差参数X改变量过小，可认为算法已经收敛。ε2为设定的常量，本文取10－6。

3 仿真分析和实验验证

3.1 仿真分析

为了对本文所提算法的有效性进行验证，首先进行了仿真测试。选择苏州的地磁场信息作为仿真环境，苏州地区磁场强度‖mlocal‖＝491.682 mG。仿真设置均值为0，标准差为0.03 的噪声，本文将对磁力计数据进行归一化处理，然后根据式(2)所提供的MEMS 三轴磁力计误差模型，通过仿真产生了空间多位置7 000 个数据采集点，其中野值数据共91 个且分布于各个位置并设置各个数据初始权重均为1 以及式(5)中的误差参数初始值为:

图1 所示即为含野值的三轴磁力计数据归一化输出。从图中可以发现，野值数据的分布偏离椭球球面。在经过一次迭代后，各数据权重分布如图2所示，本文算法识别到部分野值数据后其权重接近于0。经过本文方法标定后，严重偏离正常范围的野值将会被剔除掉，其结果如图3 与图4 所示，分别为经过本文方法标定后所得的磁力计输出与最终各数据的权重。

图1 仿真产生含野值MEMS 三轴磁力计输出

图2 仿真磁干扰环境第一次迭代后各数据权重

图3 RLM 标定后MEMS 三轴磁力计输出

图4 仿真磁干扰环境最后一次迭代后各数据权重

仿真设定的误差参数真值以及传统方法与本文方法标定所得误差参数结果如表1 所示。本文所提的RLM 法与传统LM 法迭代所得误差参数过程图分别如图5～图8 所示。

表1 各方法标定仿真数据后所得误差参数

图5 误差参数R 估计值迭代过程图(R11，R12，R13)

图6 误差参数R 估计值迭代过程图(R21，R22，R23)

图7 误差参数R 估计值迭代过程图(R31，R32，R33)

图8 误差参数b 估计值迭代过程图

从表1 以及图5～图8 中显然可以发现，LS 椭球拟合法以及LM 法因为受野值影响，标定得到的R、b与真值相比相差较大。而本文方法则不同，其误差参数与真值较为接近。图9 中，将本文所提方法与LM 法和LS 椭球拟合法标定后的归一化磁矢量模值进行了对比。经过标定后，本文所提方法相比于其他两种方法磁矢量归一化模值更趋近于1。并且在表2 中，相较于传统方法，本文所提方法模值标准差分别减小了近80%与74%。

表2 各方法标定仿真数据后指标对比

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

3.2 实验验证

为了测试本文算法在实际环境中的运行效果，设计的嵌入式硬件系统平台采用意法半导体公司的STM32F405RGT6 作为主控，该芯片为32 位的MCU，且工作主频为168 MHz。九轴MEMS 惯性传感器采用ICM20948，该IMU 包含三轴加速度计、三轴陀螺仪以及三轴磁力计。主控芯片通过SPI 协议采集ICM20948 的三轴磁力计数据并设置其采样频率为100 Hz。然后将该硬件平台固定于3D 无磁框架上，通过USB 虚拟串口将采集到的数据发送给上位机并存储。

为了体现出多环境下本文方法的有效性，本文的模拟实验分为两部分组成。第一次实验为磁干扰环境下，实验步骤为:将系统平台在室内进行空间多方位旋转，期间以一铁锤以匀速状态逐步靠近ICM20948，然后再以匀速状态逐步远离ICM20948，重复以上操作2 次。为了验证在第一次实验下通过本文方法标定所得的误差参数是否相对准确。然后又进行了第二次无磁干扰环境下的实验。不同于第一次实验，第二次实验则将系统平台在室外空旷环境下进行空间多方位旋转。图10 所示为上述第一次实验环境下三轴磁力计原始数据归一化之后在椭球表面的分布情况。经过本文方法剔除野值数据并标定后，剩余三轴磁力计数据归一化输出分布如图11 所示，相比于标定前，数据都稳定分布在半径为1 的单位球体上。

图10 第一次实验环境下ICM20948 三轴磁力计原始输出

图11 第一次实验环境下RLM 标定ICM20948三轴磁力计输出

图12 与图13 分别为第一次实验环境下初次迭代后以及最终迭代后的各数据权重，图中表明各权重分布现象皆与理论和仿真分析一致。

图12 第一次实验环境下第一次迭代后各数据权重

图13 第一次实验环境下最后一次迭代后各数据权重

经过第一次实验与第二次实验下标定所得到的误差参数分别如表3 与表4 所示。从表4 中可以看出，无干扰环境下三种方法标定的误差参数均很接近。但是在表3 中，由于出现磁干扰，LS 椭球拟合法和LM 法由于受影响较大，所得参数与表4 相比均出现较大偏差，但是本文方法则受此影响较小，所得参数与表4 中的参数均非常接近。

表3 各方法标定第一次实验数据后所得误差参数

表4 各方法标定第二次实验数据后所得误差参数

此外，在图14 中，将本文所提方法与LM 法和LS 椭球拟合法标定后的归一化磁矢量模值进行了对比。经过标定后，磁干扰环境下本文所提方法相比于其他两种方法磁矢量模值更具有稳定性，其余两种方法因为未考虑野值影响，模值在1 附近波动较大。其均值与标准差如表5 所示，本文方法标定后的模值标准差比LS 椭球拟合法减小了近90%。而在图15 以及表6 中，因为磁力计工作在空旷环境下，所以各方法标定后的模值、均值以及标准差都极为接近。

表5 各方法标定第一次实验数据后指标对比

表6 各方法标定第二次实验数据后指标对比

图14 第一次实验环境下ICM20948 三轴磁力计模值标定前后对比

图15 第二次实验环境下ICM20948 三轴磁力计模值标定前后对比

4 结论

本文提出了一种基于鲁棒列文伯格－马夸尔特的MEMS 三轴磁力计标定方法。首先，分析了MEMS三轴磁力计的各种误差并建立标定输出模型方程；接着，提出了鲁棒列文伯格－马夸尔特标定算法对野值数据先削弱其影响，然后再对误差参数进行估计；最后，设计了仿真和系统实验来验证本文所提方法的可行性。实验结果表明，MEMS 三轴磁力计在处于磁干扰环境下，本文所提出的标定方法相比于传统方法模值标准差减小了近90%，能有效提高三轴磁力计标定结果的准确性。此外，若无磁干扰影响，本文方法相比于传统方法，仍然可以标定得到近似的误差参数。综上，可以体现出本文方法的多环境下可适用性强、标定结果精准度高、稳定性好等优点。可为后续嵌入式惯性导航系统以及组合导航系统提供更为有效、正确的磁场信息，具备较高的工程意义。