基于IMU误差补偿的磁力计增强型组合定位方法

上官伟，陈晶晶，谢朝曦，姜 维

(1.北京交通大学 电子信息工程学院,北京 100044；2.北京交通大学 轨道交通控制与安全国家重点实验室，北京 100044；3.北京市电磁兼容与卫星导航工程技术研究中心,北京 100044；4.杭州海康威视数字技术股份有限公司，浙江 杭州 310051)

目前全球导航卫星系统(Global Navigation Satellite System,GNSS)特别是北斗导航卫星系统(BeiDou Navigation Satellite System，BDS)已经成为全天候、高精度、高连续性的主流定位方法，广泛应用于车载导航、列车定位中。但由于卫星覆盖率有限，以及信号遮挡、多径效应等情况，易造成信号失锁、定位信息丢失等。文献[1]从技术研究进展和发展方向上分析了当前的四大卫星导航技术，指出当前导航系统易受卫星信号环境的影响，在复杂情况下难以满足高实时性、高连续性的定位需求。

为了保证卫星信号失效时的正常定位需求，常常引入惯性导航系统 (Inertial Navigation System，INS) 与卫星导航系统进行融合滤波定位，二者优势互补，既弥补了惯导系统的误差累加性，又解决了卫星信号定位的不连续性，使组合系统定位结果更可靠。文献[2]研究了列车定位导航技术中基于北斗和捷联惯导系统紧组合导航(BDS/SINS)的列车定位方法，将二者的定位信息差值作为反馈修正值反馈给惯导系统进行误差补偿校正，修正定位误差。但是由于惯导系统的定位精度依赖于微机械系统(Micro Electro Mechanical Systems,MEMS)，其测量精度易受外界环境因素的影响，在较长时间卫星失锁环境下，位置、速度的测量误差会随时间累积[3]，因此，对MEMS的误差补偿是提高组合系统定位精度的有效手段。

惯性测量单元(MEMS-IMU)的误差通常从两方面进行分析：系统误差和随机误差。MEMS-IMU的加速度计和陀螺仪的系统误差中70%～80%[4]是惯性器件误差，来源于器件本身的机械加工过程，如零偏误差、刻度因子误差、非正交误差，以及器件在使用过程中随环境变化产生的温度漂移等；其余误差来源于各惯性传感器组合安装过程中的安装误差和杆臂效应。MEMS-IMU的随机误差则受到多种误差源影响，如AD/DA量化影响，以及电气噪声、机械结构、载体震动等环境因素影响[5]等。

在惯性器件误差补偿方面，Park等[6]利用GM模型和AR模型，对MEMS系统误差建立了简化的误差模型对其性能进行评估；Artese 等[7]总结了MEMS惯性器件的零偏误差、安装误差和标度因子等主要误差项，提出了用简易低精度转台校准陀螺仪的方法。这些简化的系统误差模型极大地改善了惯导产品的性能，但是在陀螺仪的标定方案中未考虑地球自转对旋转轴的影响，文献[8]采用双向旋转法和十二位置法对惯性器件系统误差进行了标定补偿。

在随机误差补偿方面，由于MEMS惯性器件的随机误差受环境因素影响无法事先校正，因此目前主要采用的补偿方法是功率谱密度分析法、自相关函数分析法和 Allan方差法[9-10]等。其中Allan方差法是辨识惯性器件随机误差的标准方法[11]，最初应用于光纤陀螺器件，后来不少学者证明Allan方差法同样适用于MEMS器件的误差辨识[12-13]。Allan方差法通过分析陀螺仪或加速度计采集的静态数据，对量化噪声、零偏不稳定性、角速率随机游走、角度随机游走和斜坡误差进行辨识，并确定相应的误差参数。本文在Allan方差辨识随机误差的基础上，将随机误差方差归一化处理，应用到组合定位系统的状态方程中。

上述研究对惯性器件的系统误差和随机误差进行了校正，然而捷联惯导系统的递推过程较高地依赖载体姿态信息[14]，而纯惯导系统定位过程中存在着姿态发散的问题，因此在长隧道或者卫星受遮挡严重的特定环境中，还需要对惯导系统姿态发散进行弥补与校正。磁力计在地球南北极的天然磁场环境中能有效地感应地磁场强度，利用其磁场感应数据计算出载体的姿态[15]，引入磁力计提供额外的姿态观测量可以较好地解决INS姿态发散问题。

根据上述研究现状，本文针对惯性测量单元的误差以及在卫星失效时组合系统的定位连续性问题，提出一种基于IMU误差补偿的磁力计增强型组合定位方法，补偿校正惯性器件的系统误差和随机误差，提高GNSS/INS组合系统的定位精度；引入磁力计形成增强型组合定位系统，进一步保证组合定位系统的复杂环境可用性。

1 系统概述

传统的GNSS/INS组合模型采用卡尔曼滤波算法，其状态空间可以写为

(1)

其状态方程可以写为

(2)

组合系统的量测矢量为INS和GNSS的速度误差和位置误差，其量测方程可以写为

(3)

GNSS/INS组合系统存在两个主要问题：①在卫星失效时，组合系统定位准确性较高地依赖于惯导器件的测量精度；②长时间的惯导递推受姿态发散的影响，其定位误差会随时间推移迅速增大，造成定位结果可用性较差。

针对以上问题，本文首先在MEMS-IMU系统误差标定补偿的基础上，在系统的状态方程中增加惯性器件的随机误差状态向量，对惯性器件的随机误差进行在线动态补偿；其次，通过磁力计的姿态输出值增加额外的姿态信息观测量，保障惯导系统的持续定位性能。GNSS/INS/磁力计增强型组合系统校正过程见图1。图1中，Φ、A、P、V分别为载体的姿态角、加速度、位置、速度。

图1 GNSS/INS/磁力计增强型组合系统校正过程

2 误差补偿及校正方法

针对惯性单元(IMU)的系统误差，采用相应的标定方法进行建模补偿；针对IMU的随机误差，建立有色噪声微分方程作为组合系统新的状态向量。

2.1 IMU系统误差标定补偿

IMU系统误差补偿选取误差项中影响较大的3个误差项，零偏误差、刻度因数误差、安装误差，进行建模补偿。陀螺仪的误差补偿利用转台提供精确的角速率输入，采用正反双向旋转法标定；加速度计的误差补偿利用转台提供精确的加速度输入，采用静态十二位置法进行标定补偿[8]。

(1)陀螺仪误差标定补偿步骤：

Step1对陀螺仪的误差项建立误差补偿模型。

Step2通过提取陀螺仪的整数周输出值消除地球自转在非旋转轴的误差。

Step3通过多组不同速率ω1、ω2、ω3、…、ωn采样，利用最小二乘法对刻度因数误差矩阵ΔSw和安装误差矩阵Mw进行估计。

Step4采用双向位置旋转法，得到相同大小、相反方向的±ω角速率对应的整周输出值，消除零偏误差bω中地球自转在旋转轴的影响。

(2)加速度计误差标定补偿步骤：

Step1对陀螺仪的误差项建立误差补偿模型。

Step2设计十二位置编排方案，利用转台分别对每一种方案在加速度计的3个轴施加重力加速度。

Step3采集12组加速度计量测值M12×4和加速度计真值T12×3，利用最小二乘法估计加速度计刻度因数误差矩阵ΔSa、安装误差矩阵Ma和零偏误差矩阵ba。

2.2 IMU随机误差在线动态补偿

针对IMU使用过程存在的随机误差，首先利用Allan方差辨识法进行参数辨识，再根据有色噪声的功率谱密度函数建立噪声微分方程，归一化处理之后作为组合系统的估计参数之一，建立卡尔曼滤波状态方程构成在线补偿模型。

2.2.1 有色噪声建模

首先使用Allan方差法对惯性器件的随机误差进行辨识，随机误差参数[16]见表1。

表1 随机误差参数

表1中τ为随机误差的采样时间。本文将随机噪声分解为白噪声和有色噪声，表1中量化噪声和角度随机游走可以当作白噪声，其余3个当作有色噪声。有色噪声功率谱密度函数见表2。

表2 有色噪声功率谱密度函数

有色噪声的功率谱密度函数P(ω)与其传递函数G(jω)之间满足[17]

P(ω)=|G(jω)|2

(4)

同时，传递函数G(jω)与有色噪声的傅里叶变换X(jω)之间满足

X(jω)=G(jω)

(5)

通过傅里叶逆变换可得到以下微分方程：

(1)零偏不稳定性微分方程

零偏不稳定性误差可以看作一阶高斯-马尔可夫过程[18]，其传递函数近似为

(6)

进行傅里叶逆变换得到零偏不稳定性的随机微分方程为

(7)

(2)随机游走误差微分方程

随机游走误差的传递函数为

(8)

进行傅里叶逆变换得到随机游走误差的随机微分方程为

(9)

式中：u2(t)为一个高斯白噪声。

(3)斜坡误差微分方程

斜坡误差微分方程可以看作二阶高斯-马尔可夫过程，其传递函数可近似为

(10)

进行傅里叶逆变换得到斜坡误差的随机微分方程为

(11)

2.2.2 有色噪声归一化

根据文献[19]提出的等价性原理，上述3项有色噪声的微分方程经过等价转换，可以用一个随机微分方程来描述。记D为微分算子，则上述3个有色噪声微分方程可写为

(D+β)dB(t)=βBu1(t)

(12)

DdK(t)=Ku2(t)

(13)

(14)

将3个误差项合并，记作z=dB+dK+dR，则上述3个微分方程相加可得

(15)

归一化之后的随机误差微分方程为

z(4)(t)+a1z(3)(t)+a2z(2)(t)+a3z(1)(t)+a4z(0)(t)=

b0w(3)(t)+b1w(2)(t)+b2w(1)(t)+b3w(0)(t)

(16)

(17)

2.2.3有色噪声在线随机补偿模型

将归一化的随机误差项添加到组合系统状态方程(2)中，新的状态方程改写为

(18)

(19)

将式(16)写成：

(20)

zi(t)=Hiyi(t)

(21)

(22)

将uc(t)用状态噪声向量和转移矩阵来表示：

(23)

(24)

(25)

(26)

Fyy=diag(F1…F6)

3GNSS/INS/磁力计增强型组合定位系统

通过对GNSS/INS组合系统的惯性器件系统误差和随机误差进行建模补偿，可以提升惯导的定位精度。但是在隧道、半边天等卫星失效的环境下，GNSS/INS组合系统的状态观测方程中缺少由卫星提供的速度、位置信息，只能依靠INS进行姿态、速度、位置解算，而单纯的惯导递推最大的问题便是姿态发散过快，因此缺少卫导系统的数据校正将会产生较大的定位误差。

本文在GNSS/INS组合系统中增加可以感应地磁强度来计算载体姿态角的磁力计，在系统观测量中添加磁力计的姿态值，修正惯导系统的姿态角，进行有效的姿态保持。

3.1 增强型组合系统概述

在卫星信号有效区域，GNSS/INS/磁力计增强型组合系统以GNSS/INS组合的观测误差及磁力计/INS组合的姿态误差作为卡尔曼滤波观测量，共同组成组合导航输出信息；在卫星信号较弱或失效区域，经误差补偿后的惯导系统根据磁力计提供的高精度姿态信息修正载体姿态误差，在此基础上进行惯导递推，解算数据作为INS/磁力计组合系统的输出观测量。校正后的GNSS/INS/磁力计增强型组合导航系统的系统结构见图2。

图2 GNSS/INS/磁力计增强型组合导航系统结构

GNSS/INS/磁力计组合相对于传统的组合导航系统在传感器上增加了磁传感器，其作用有2个：①在卫星失效时的纯惯导模式下，增强型组合系统能在较长时间内保证姿态不发散，从而维持惯导系统的精度，弥补姿态发散较快的缺陷，为纯惯导模式提供高精度的初始预备值；②在GNSS/INS/磁力计组合模式下，磁力计提供更精确姿态信息的反馈量对惯导系统的航向角进行校正，使得组合导航定位结果更加准确。

3.2 GNSS/INS/磁力计组合定位算法

本部分将分别介绍增强型组合系统2种工作模式的组合算法：INS/磁力计组合和GNSS/INS/磁力计增强组合。

3.2.1 INS/磁力计组合

(27)

(28)

INS/磁力计组合系统的状态观测量与纯惯导模式类似，加速度计测量得到载体的惯性加速度，转换为导航坐标系下的加速度之后通过积分运算更新速度和位置。

速度V的微分方程[21]为

(29)

位置p的微分方程为

(30)

3.2.2 GNSS/INS/磁力计组合

GNSS/INS/磁力计组合定位算法进行了两部分改进：状态方程新增了随机误差的误差状态方程；量测方程新增了磁力计的姿态观测量。

由随机误差建模可以得到随机误差状态向量，则GNSS/INS/磁力计组合系统状态向量为

(31)

考虑随机误差之后的增强系统状态方程为

(32)

Zφ=φINS-φM

(33)

结合导航误差观测方程，增强系统新的量测值Zk可以写为

(34)

相应的量测矩阵Hk表达式为

(35)

4 实验结果

本实验采用SBG公司Ellipse-N提供的参考系统，位置精度为2 m，航姿精度为0.1°，惯性测量单元和GNSS采样频率为50 Hz。分别用传统GNSS/INS组合定位算法和GNSS/INS/磁力计增强型组合定位算法对采集的数据进行处理，在3种实验环境下分别设置卫星条件良好和卫星信号中断的对比实验。

4.1 开阔场景

本部分实验选取环形操场为实验场地，验证开阔场景下本文提出的增强型算法分别在卫星条件良好和卫星条件中断情况下的算法性能。

4.1.1 卫星条件良好

在无遮挡环境下，卫星接收的速度、位置信息有效，组合系统工作模式为GNSS/INS/磁力计组合,在量测方程中增加姿态观测量。卫星观测良好条件下GNSS/INS/磁力计组合与传统GNSS/INS组合的姿态误差对比见图3。

图3 卫星观测良好条件-姿态误差对比(开阔场景)

由图3可见：传统算法的航向角误差较大，说明该算法无法有效估计航向角，随时间递增航向角最大误差达到-9.295°；俯仰角和翻滚角相对于航向角误差较小，最大误差分别达到0.971°和1.033°。增强型算法的航向角误差绝对值基本稳定在1°左右，这对于组合系统后续的轨迹推算起到关键性作用；俯仰角和翻滚角度误差绝对值保持在0.5°之内。

在姿态误差得到有效改善之后，增强型算法将对载体的位置和运动轨迹进行相应的校正。两种算法在卫星观测良好条件下北向、东向的位置误差对比见图4。

图4 卫星观测良好条件-位置误差对比(开阔场景)

由图4可见，传统算法的位置误差存在较严重的不稳定性，北向位置误差最大达到2.457 m，东向位置误差最大达到3.529 m；而增强型算法的位置误差曲线相较而言趋势更加平稳，北向位置误差不超过0.5 m，精度增加了76%，东向位置误差保持在1 m以内，精度增加了72%。由此可以得出，增强型算法的位置误差整体上较传统算法得到了较大的改善，定位精度保持在1 m级。传统算法和增强型算法的位置误差均值、方差分析见表3。

表3 卫星观测良好条件-位置误差分析(开阔场景) m

位置误差的校正最终体现在载体的运动航迹上，GNSS/INS/磁力计组合、GNSS/INS组合、参考系统的整体轨迹对比见图5。

图5 卫星观测良好条件-航迹对比(开阔场景)

图5中局部轨迹放大后的误差对比见图6。综上，GNSS/INS/磁力计增强型算法在卫星条件良好时，能有效改善姿态误差，提高组合定位的精度。

图6 卫星观测良好条件-航迹误差局部放大对比(开阔场景)

4.1.2 卫星条件中断

在有遮挡环境下，卫星信号接收机将无法提供正确的速度、位置信息，组合系统工作模式为INS/磁力计组合。选区范围为13 000～13 500之间的500个采样点设置人为卫星信号中断，中断时间为10 s。在卫星观测中断条件下计算增强型组合系统与参考系统的姿态误差和位置误差，设置GNSS/INS组合系统与参考系统的姿态误差和位置误差的对比项。

卫星观测中断条件下增强型组合系统和GNSS/INS组合系统两种算法的姿态误差对比见图7。在卫星中断环境下传统算法的航向角在第13 500采样点处累计误差达到-8.216°，对应的无中断环境下传统算法航向角误差为-7.579°；而增强型算法在两种环境下的航向角误差均在1°以内，俯仰角、翻滚角误差保持在±0.5°内。这说明，在没有卫星信号情况下增强型组合算法对惯导姿态进行了校正，其航向角、俯仰角、翻滚角均得到了相应的改善。

图7 卫星观测中断条件-姿态误差对比(开阔场景)

卫星观测中断条件下两种组合系统的位置误差对比见图8，在13 000～13 500采样点之间，两种算法均出现了较大的位置误差。在13 500采样点处，传统算法的北向位置误差达到最大值1.757 m，增强型算法的北向位置误差绝对值为0.425 m，位置精度增加了75%；传统算法的东向位置误差达到最大值5.249 m，增强型算法的东向位置误差绝对值为2.925 m，位置精度增加了45%。

图8 卫星观测中断条件-位置误差对比(开阔场景)

传统算法和增强型算法在卫星观测中断条件下的位置误差均值、方差分析见表4。增强型组合算法、GNSS/INS组合、参考系统的整体航迹对比见图9。

表4 卫星观测中断条件-位置误差分析(开阔场景) m

图9 卫星观测中断条件-航迹对比(开阔场景)

由图9可以看出，在卫星信号中断期间，传统算法轨迹出现了明显偏移，在信号恢复之后逐渐收敛至正确的载体航迹；增强型算法也出现了较明显的位置偏移，但是相对传统算法而言，在卫星信号恢复之后其收敛速度较快，且定位精度明显提升。

4.2 道路遮挡场景

以上实验验证了开阔环境下增强型组合算法在卫星条件良好和卫星信号中断时能有效、及时地修正载体运动轨迹，保证定位精度。在上述实验基础上，选取道路遮挡场景作为试验场地，验证在建筑物、树木等构成的复杂环境中算法的可用性。

道路场景在卫星观测良好条件下的航迹对比见图10。增强系统的定位结果与参考系统的定位结果保持高度一致，其位置误差均值、方差分析见表5。

图10 卫星观测良好条件-航迹对比(道路场景)

表5 卫星观测良好条件-位置误差分析(道路场景) m

在采集的实验数据中人为设置GNSS信号中断，选取范围为2 000～2 200之间的200个采样点，中断时间为4 s。道路场景在卫星信号中断4 s条件下的航迹对比见图11。增强系统能够有效消除惯导递推的误差累积，减少组合定位的位置漂移，提高组合系统在卫星失效时的可用性和定位精度，其位置误差均值、方差分析见表6。

图11 卫星观测中断条件-航迹对比(道路场景)

表6 卫星观测中断条件-位置误差分析(道路场景) m

4.3 铁路场景

以上结果验证了低速道路实验环境中算法的可用性，为了进一步验证论文所提出方法在列车定位中的应用效果，利用朔黄铁路的部分数据进行实验，并对卫星信号无中断、有中断环境下的定位结果进行对比分析。实验线路总长度为11 km左右，采集时间为500 s，频率为50 Hz。

4.3.1 卫星条件良好

在无遮挡环境下，传统的GNSS/INS组合定位方法和GNSS/INS/磁力计增强型组合定位方法的位置误差对比见图12，均值、方差分析见表7。

图12 卫星观测良好条件-位置误差对比(铁路场景)

表7 卫星观测良好条件-位置误差分析(铁路场景) m

在卫星观测良好条件下，增强型算法的北向位置误差和东向位置误差均为米级，与传统算法相比具有更高的鲁棒性。

4.3.2 卫星条件中断

在列车实际运行线路上存在较多的隧道场景和山体环境，为了模拟卫星信号不可用条件下组合算法的性能，本实验在定位数据中分别设置10 s和50 s的中断时间，对比分析传统算法和增强型算法的定位结果。

传统算法和增强型算法在卫星信号10 s中断时间下的航迹对比见图13，位置误差的极差、方差分析见表8。传统算法的北向位置明显偏离实际位置，最大偏移量为-22.79 m，极差为27.29 m；增强型算法在相同情况下的北向位置最大偏移量为2.355 m，极差为2.945 m，位置精度增加90%；两者东向位置误差的极差分别为26.36 m和8.934 m，增强型算法东向位置精度增加70%。

图13 卫星观测中断10 s-航迹对比(铁路场景)

表8 卫星观测中断10 s-位置误差分析(铁路场景) m

传统算法和增强型算法在卫星信号50 s中断时间下的航迹对比见图14，位置误差的极差、方差分析见表9。

图14 卫星观测中断50 s-航迹对比(铁路场景)

表9 卫星观测中断50 s-位置误差分析(铁路场景) m

在卫星信号中断50 s的情况下，列车平均速度为25 m/s，列车的行驶距离为1 km，此时传统算法的定位误差已经达到了百米级别，远远超过了正常定位需求的范围；增强型算法也出现了一定的位置累积，但相对于传统算法来说具有较明显的优势，其总体定位误差减小1/2左右，并且能在卫星信号可用之后以较快的速度修正组合定位的误差，使列车定位结果收敛到正确轨迹。

5 结论

(1)在卫星失效区域，可通过标定补偿的方式来消除GNSS/INS组合定位中IMU的系统误差，提高组合导航系统的性能。

(2)对于定位过程中IMU的随机误差补偿，本文采用的方法是对随机误差项的有色噪声建立微分方程，归一化处理后作为组合系统的状态观测量进行在线补偿。

(3)引入磁力计形成GNSS/INS/磁力计增强型组合定位系统，实现了组合定位系统在卫星观测中断条件下的定位可用性，提高了组合定位系统的导航精度。

(4)采用SBG公司Ellipse-N惯性测量单元采集实验数据，设置3种实验环境，分别在卫星信号中断和卫星信号良好2种条件下对本文提出的定位方法进行了验证，结果表明，本实验提出的惯性器件误差的标定补偿和在线补偿，以及磁力计的姿态校正，对系统的定位精度和有效性均有一定的改善作用。

本文的局限性在于只考虑了有限个误差项，未考虑MEMS惯性器件的温度漂移对惯性器件定位精度的影响。