自适应抗差最小二乘估计的捷联磁力计标定

张 攀，刘锡祥，黄 荣，黄永江

(东南大学 仪器科学与工程学院/微惯性仪表与先进导航技术教育部重点实验室，南京 210096)

0 引言

地磁场具有无源无辐射、不受载体运动影响且无误差积累等优势，可以提供全天候、全地域的导航信息；根据地磁场信息进行航向修正或者地磁匹配是组合导航定位的关键步骤之一。行人导航定位设备采用的捷联式三轴磁力计能实时测量其周围磁场信息，由地磁场计算得到的航向在无源行人导航定位系统的初始对准和导航解算误差抑制方面有着十分重要的作用；然而地磁匹配用的地磁场和其他干扰磁场相互耦合，这严重影响磁力计的测量精度和地磁匹配应用效果：因此磁力计的标定和磁补偿技术是地磁应用的前提和关键技术，也是制约行人导航定位精度的关键因素之一[1]。

关于磁力计的标定和磁补偿技术一直以来推陈出新，目前广泛使用的是文献[2]提出的经典Two-Step法，该算法利用最小二乘估计(least squares，LS)对磁力计误差模型中的刻度因子和硬磁误差进行估计。文献[3]考虑了安装误差和非正交误差，扩展了数学模型，并通过中间变量代数求解出各项误差参数。在此基础上，为获得更精确的解，文献[4]利用非线性参数估计如牛顿迭代法等进行求解。文献[5]针对磁力计的观测方程两边同时含有噪声的情况，提出了基于总体最小二乘算法的标定方法。基于椭圆拟合的思想，文献[6-7]分别利用易于实现的递推最小二乘法和截断总体最小二乘法估计椭圆参数对磁场进行补偿，均取得不错的效果。

固联在载体上的捷联磁力计对周围磁场十分敏感，数据采集中不可避免会出现误差，如果不对其进行处理，将会严重影响误差参数的估计结果。本文基于Two-Step法，将自适应抗差最小二乘估计(adaptive robust least squares，ARLS)用于磁力计误差模型的参数估计中，采用IGG权函数给出自适应调整权函数的阈值，其中IGG方案由中国科学院大地测量与地球物理研究所(Institute of Geodesy & Geophysics，Chinese Academy of Sciences，IGG)提出，结合行人导航定位的应用场景，对初始航向和航向误差抑制进行研究。

1 磁力计误差数学模型

由于软磁材料的存在，在外部磁场激励下其会产生随着外部磁场大小和方向变化的磁场并叠加在磁力计输出上。通过选择标定现场区域可以避免较复杂的磁场环境。而相对于地磁场和硬磁场来说，磁力计及外围电路的磁场可以忽略。因此本文只考虑地磁场和硬磁为外部激励磁场，并假设软磁误差与外部磁场为线性且无磁滞。

结合上述5种误差，捷联三轴磁力计的误差模型如图1所示，其输出模型为

(1)

图1 捷联磁力计的误差模型

在实际标定和应用中，测量噪声较小，暂不考虑。从输出模型可以发现，非正交误差、刻度因子误差与软磁误差在数学上是无法分离的(当然，在特定环境可以将3者区分，就标定而言3者耦合不影响标定结果)，则记K-1=CmCsfCsi。将K-1代入原输出模型并展开可以得到

(2)

(3)

2 Two-Step方法

在航向未知且无其他设备辅助情形下，Two-Step方法通过将磁力计输出模型(1)进行转化，通过最小二乘法解算未知量，并通过代数分解求出了磁力计的误差参数。

基于磁场标定现场的地磁场模值R为一常量的原则，有如下方程

(4)

(5)

A×X=L

(6)

式中：A∈Rm×n(m为采样点个数，n为未知数个数，3维模式下n为9，2维模式下n为6；X∈Rn×1；L∈Rm×1；

一般有m>n，Two-Step法标定的第一步就利用如下最小二乘法解上述超定方程

(7)

第二步就是通过最小二乘估计得到的解，代数分解出磁力计输出模型中的误差参数。

在实际标定过程中，A和L都含有观测量的分量，因此都不可避免地引入一定误差影响到最小二乘估计，此时最小二乘估计有偏，并不是最优的；传感器一旦出现较大粗差就会严重影响到参数估计结果，致使标定失败，进而影响到组合导航精度。

3 自适应抗差估计方法

从构造一类自适应抗差最小二乘求解方程式出发，自适应抗差最小二乘估计使得该估计过程能自动地在广泛的误差分布类之间选择最适估计[8]。在磁力计标定过程中，相比最小二乘估计，自适应抗差估计通过残差调整权重，能够在粗差出现时获得更加精确的误差参数估计结果。

根据磁力计的参数估计模型和抗差估计原理[8]，参数平差模型为

V=AX-L权阵P

(8)

vi=aiX-Li权Pi

(9)

式中：ai为A阵之第i行元素组成的行向量；vi和Li分别为残差V和L的分量；等价权阵P是残差V和Pi的函数[9]。根据IGG方案[8-9]取等价权函数为

(10)

式中：v′i为第i组测量值的标准化残差；根据经验K0选择1.0～1.5较好，K1在3.0左右为宜[5]。则误差参数的抗差解为

(11)

自适应抗差最小二乘估计的迭代形式为

(12)

4 实验与结果分析

4.1 2维平面实验

为了验证基于ARLS的磁力计标定的有效性，先进行2维平面上的标定，以满足无法充分旋转获得3维数据的情况。实验时，在数据中随机加入幅值为地磁场10 %的粗差，以验证本文提出的方法对粗差的抑制效果。将三轴磁力计置于水平面，绕天向轴缓慢旋转若干周，按照ARLS和Two-Step方法分别进行标定，结果如图2～图3所示。

图2 Two-Step与ARLS标定前后水平磁场

图3 Two-Step与ARLS标定前后磁场幅值

图2、图3的结果显示：标定前磁力计测量值波动较大，误差干扰严重，无法测量地磁场强度；而标定和磁补偿后，地磁场水平分量不再是椭圆，而是圆心在原点的圆，且磁场幅值稳定，表明LS和ARLS在无法做3维全姿态运动时均可以对磁力计进行有效标定。在图3的采样区间700到900段加入粗差后，LS方法受粗差影响导致参数估计偏离真值严重，在粗差出现处产生明显的突变。而ARLS能够将粗差的影响限制在函数模型的残差中，很好地抑制粗差，减小粗差对标定效果的影响。

4.2 3维标定实验

为了检验3维旋转情况下的标定效果，在某室内环境下先将三轴磁力计绕天向轴旋转若干周，再将三轴磁力计绕东向轴旋转若干周。2种方法标定前后的磁场分布及幅值如图4、图5所示。

图4 磁力计3维标定前后磁场分布

图5 标定前后3维地磁场幅值

在图5所示3维实验的采样区间内出现了磁场幅值约20 %的粗差，图中可见：ARLS能动态地调整抗差因子，减小含有粗差的测量信息在估计中的权重，降低粗差对参数估计的影响，有效提高磁力计标定的精度；而LS不具有抗差能力，无法应对现场出现不良数据的情况，使得其标定结果不可信。为了判断标定质量和标定方法的效果，可通过均方根误差RMSE来比较。

(16)

表1 标定实验残差均方根统计

从表1中可知，ARLS算法的残差均方根误差小于Two-Step，参数估计效果更佳，具有更高的精度。

4.3 行人导航定位系统实验

行人导航定位系统结构如图6所示。

图6 行人导航定位系统算法结构

微机电系统惯性测量单元(microelectromechanical systems-inertial measurement unit，MEMS-IMU)与磁力计置于人体足部[10]。由于MEMS-IMU精度较低，初始对准过程无法获得准确的航向信息；在由加速度计敏感的重力投影值求得水平姿态角后，行人导航定位系统初始对准中航向信息可以由磁力计测得的地磁场获取：因此在初始对准之前的磁力计标定和磁补偿对于初始对准和后续导航解算十分重要。

在行人行走过程中，惯性测量单元置于人体足部，利用陀螺仪和加速度计可以检测出人行走时脚部与地面完全接触(即零速区间)的瞬间，此时速度为零成为Kalman滤波器的观测量。在仅有零速这一个观测量时，速度、位置等误差发散能够被抑制[10-11]；但是由于航向误差无法估计，因此需要引入磁力计来修正航向角误差[12]。

将MEMS-IMU置于足部，在行走前按照上述2种标定方法对磁力计进行标定，然后在东南大学四牌楼校区做2组行走实验：实验一为在某南北向路上进行50 m直线折返行走，行人导航定位结果如图7所示；实验二为在四牌楼田径场400 m跑道进行单圈行走，定位和航向角结果如图8所示。

图7 南北路50 m直线折返行走实验

图8 田径场400 m跑道单圈行走实验

在初始对准前，对固联在足部的捷联磁力计按照上述方法进行标定，利用地磁来获得初始航向。在上述试验中，由法国SBG系统给出的INS/GNSS组合导航系统的位置和航向角作为实验的参照标准值。在行走过程中，在零速时刻对航向进行补偿使得航向漂移得以抑制。通过实验结果对比，ARLS方法标定后，初始航向偏差由原来的6.14°减小到3.78°，标定后ARLS方法的初始航向更加准确。行人导航定位系统定位结果的终点误差由原来的8.2减小到0.7 m，终点误差减小了15 %；随着时间推移，磁力计的辅助能够有效抑制误差发散，航向角得到较好的校正。

5 结束语

磁力计的标定是地磁匹配和组合导航的前提和关键。本文将自适应抗差最小二乘估计应用到捷联三轴磁力计的在线标定上，考虑到现场测量值中可能出现粗差的情况，对原有的最小二乘估计做出了改进，当载体无法做全姿态运动时，该算法也可以进行较好的标定，增强算法的适应性和抗干扰能力。实验表明，无需借助转台等大型设备，只需要简单的旋转，基于ARLS的标定方法就能完成捷联三轴磁力计的标定，并且能有效地校正初始航向角，提高行人定位系统的导航定位精度。

[1] 陈卓，田风勋，孙建军.地磁导航关键技术研究进展综述[J].测绘与空间地理信息，2016，39(1)：16-18.

[2] GEBRE-EGZIABHER D, ELKAIM G H, POWELL J D, et al. A non-linear, two-step estimation algorithm for calibrating solid-state strapdown magnetometers[EB/OL].[2017-06-28].https://users.soe.ucsc.edu/～elkaim/Documents/TwoVec.pdf.

[3] FOSTER C C, ELKAIM G H. Extension of a two-step calibration methodology to include nonorthogonal sensor axes[J]. Aerospace & Electronic Systems IEEE Transactions on, 2008, 44(3):1070-1078.

[4] DORVEAUX E, VISSIERE D, MARTIN A P, et al. Iterative calibration method for inertial and magnetic sensors[EB/OL].[2017-06-28].https://hal-mines-paristech.archives-ouvertes.fr/hal-00473610/document.

[5] 吴志添，武元新，胡小平，等．基于总体最小二乘的捷联三轴磁力仪标定与地磁场测量误差补偿[J]．兵工学报，2012，33(10)：1202-1209.

[6] 盛蔚，蒋勇. 三轴磁传感器系统的在线标定[J]. 弹箭与制导学报,2015,35(3):179-182.

[7] 张滢，杨任农，李明阳，等. 基于截断总体最小二乘算法的车载三轴磁力仪标定[J]. 兵工学报,2015,36(3):427-432.

[8] 杨元喜. 自适应抗差最小二乘估计[J]. 测绘学报,1996,25(3):206-211.

[9] 任夏，李铸洋，丁阳. 抗差自适应滤波算法在实时定轨中的应用[J]. 导航定位学报,2016,4(2):26-28+35.

[10] FISCHER C, SUKUMAR P T, HAZAS M. Tutorial: implementation of a pedestrian tracker using foot-mounted inertial sensors[EB/OL].[2017-06-28].http://www.comp.lancs.ac.uk/～hazas/Fischer13_ImplementingaPedestrianTracker.pdf.

[11] 马明，宋千，李杨寰，等.基于地磁辅助的室内行人定位航向校正方法[J].电子与信息学报，2017，39(3)：647-653.

[12] 钱伟行，朱欣华，苏岩.基于足部微惯性/地磁测量组件的个人导航方法[J].中国惯性技术学报，2012，20(5)：567-572.