基于磁干扰补偿的智能手机车载AHRS 算法∗

刘少兵，纪新春，魏东岩，李 雯，刘宇欣，巩应奎，袁 洪

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

车辆导航需求日益迫切，随着智能手机应用的普及，智能手机导航已经逐渐取代专用导航终端，成为大众车辆导航的主流方式之一[1]。目前绝大多数智能手机中均内置了全球导航卫星系统(Global Navigation Satellite System，GNSS)模块、微机电惯性测量单元(MEMS-based Inertial Measurement Unit，MIMU)、磁力计等传感器，具有强大的感知能力、运算能力与通信能力[2]。车辆导航通常选取航位推算系统(Dead Reckoning System，DR)与GNSS 进行组合定位[3]。在开阔场景下，GNSS 可用时，可由GNSS 提供准确的定位信息，对DR 累积误差进行校正。而在如城市峡谷等复杂场景中，GNSS 失锁后DR 累计误差无法得到及时校正。智能手机内置的MIMU 成本较低，存在各种器件误差，定位结果会迅速发散，其中航向发散是其主要因素之一。而磁力计通常作为地磁场敏感器件，测量磁场的大小和方向，将磁力计与MIMU 测量信息融合，进行航姿参考系统(Attitude and Heading Reference System，AHRS)解算，得到车辆实时航向[4－5]，对DR 累积误差进行校正，完成实时、连续的车辆定位。

为了提高AHRS 估算精度，需要对传感器进行误差标定和干扰补偿。目前关于MIMU 的误差标定和补偿研究已较为成熟，考虑MIMU 误差的影响，对其零偏误差进行了标定和补偿[4，6]，有效提高了AHRS 算法的估算精度，除MIMU 误差外，磁力计误差也是影响系统精度的主要因素之一，但与MIMU误差大多源自器件本身不同，磁力计还受车辆载体磁干扰影响[7－8]，从而引起较大的测量误差，进而导致航向的偏差。对于磁力计标定的研究，目前常用的方法有椭球拟合法、多传感器辅助法等，文献[9－12]中提出利用椭球拟合法对三轴磁强计各项误差进行标定并补偿，提高了磁强计的测量精度。文献[13]利用加速度计辅助磁力计标定，利用重力矢量和地磁矢量的点积不变性求解磁力计误差。文献[14]中利用加速度、陀螺仪以及磁力计解算的姿态角，构建误差模型，完成了9 轴传感器各项误差的标定。但椭球拟合法和多传感器辅助法需要采集磁强计和辅助传感器多个姿态下的数据，在无环境磁场干扰下才能保证较好的标定效果，否则无法确保标定结果的准确性，且都属于离线标定方法，不适用于车辆导航中需要实时去除车辆载体干扰的场景。当磁场测量设备固联在载体内部时，为了估算载体磁干扰，需要将载体连同设备全方位地旋转或翻转[15－16]，这在车辆导航场景下也难以实现。

针对上述问题，提出了一种在线磁干扰补偿的车载AHRS 算法。在开阔环境下利用GNSS 获取航向信息作为辅助，在线估计车辆载体磁干扰，将补偿后的磁力计与MIMU 数据融合进行AHRS 解算。并提出一种基于方差和多传感器联合的车载准静态磁场检测方法，在环境磁场干扰过大时自适应调整车载AHRS 算法中磁力计测量噪声参数权值。选取城市道路场景进行跑车实验，结果表明，经过所提出方法对车辆载体引起的磁力计测量干扰在线补偿及准静态磁场检测后，AHRS 解算的航向精度显著提高，满足智能手机车辆导航应用需求。

1 磁力计误差模型

磁力计误差主要源于器件自身误差和环境干扰引起的误差，其中器件自身误差主要包含零偏误差、比例因数误差以及三轴非正交误差，一般由器件制造以及安装所引起。环境干扰主要包含硬磁干扰和软磁干扰，在车辆导航场景下，硬磁误差一般是由车辆载体的硬磁材料磁化后产生的外加磁场引起，可将其整合为零偏误差。软磁误差一般是由车辆载体的软磁材料受外部磁场磁化后产生的感应磁场引起，可将其整合为比例因数误差和非正交误差。综合分析各类型误差的机理及其对磁力计测量结果的影响[8－12]，一般可建立磁力计测量误差模型如下:

式中:m为磁力计的三轴测量值，mb为b系的磁力计测量的三轴地磁场强度。A为比例因数误差和非正交误差矩阵，由磁力计自身灵敏度、非正交误差以及环境中的软磁干扰引起。bm为磁力计的零偏误差，由磁力计自身零偏误差和环境中的硬磁干扰引起，nm为测量噪声，其方差为。

若要对式(1)中完整的误差参数进行估计，需要通过椭球拟合等方法，利用转台等设备辅助来翻转载体以采集标定数据。而在智能手机车辆导航过程中，手机需要与车辆载体固联，不可能将手机连同车辆载体进行翻转，故难以激励所有误差项。对于智能手机车辆导航而言，其误差主要由器件自身零偏和车辆载体等硬磁干扰引起，而比例因数和非正交误差影响相对较小[17]，仅考虑自身零偏和车辆载体磁干扰的影响，式(1)可写成:

式中:bm为磁力计自身零偏和车辆载体磁干扰，也是车辆导航过程中影响航向精度的主要误差源，本文主要对该部分干扰和误差进行补偿。

2 基于磁干扰补偿的智能手机车载AHRS 算法框架

本文提出的基于磁干扰补偿的智能手机车载AHRS 具体包含基于GNSS 辅助的车辆载体磁干扰在线估计算法和基于准静态磁场检测的车载AHRS算法两部分。算法框图如图1 所示。

图1 算法框图

其中基于GNSS 辅助的车辆载体磁干扰在线估计算法部分，利用GNSS 信息作为量测信息，构建扩展卡尔曼滤波器(Extended Kalman Filter，EKF)对车辆载体磁干扰进行在线估计并补偿，图中分别表示车辆载体引起的磁力计测量干扰。基于准静态磁场检测的车载AHRS 算法部分，采用三轴加速度计和载体磁干扰在线补偿后的三轴磁力计辅助三轴陀螺仪的形式构建扩展卡尔曼滤波器，融合三种传感器数据，实时解算车辆的姿态角信息，图中θ、γ、φ分别表示俯仰角、横滚角和航向角。利用基于方差和多传感器联合的车载准静态磁场检测方法，在非准静态磁场检测区间下自适应调整磁力计的量测噪声协方差，提高算法在磁场干扰复杂环境下的航向解算精度。

3 基于准静态磁场检测的车载AHRS算法

3.1 车载AHRS 算法EKF 初始化

磁力计的输出模型如式(2)所示，对于MIMU输出模型，同样仅考虑零偏误差，忽略比例因子和非正交误差的影响，即:

式中:a、ωg分别为加速度计和陀螺仪的三轴测量值，ab、ωb分别为b系下加速度计和陀螺仪的三轴理想输入值，ba、bg分别为加速度计和陀螺仪的三轴零偏，na、ng分别为测量噪声，其方差分别为、。

车辆导航中，导航坐标系(n系)为东北天坐标系，载体坐标系(b系)为右前上坐标系。利用加速度计与磁力计测量信息可完成载体姿态角估计，其具体公式为:

式中:θ、γ、φ为俯仰角、横滚角和航向角，ax、ay、az分别为加速度计的三轴测量值，mx、my、mz分别为磁力计的三轴测量值，g为当地重力加速度，α为当地的磁偏角。

由姿态矩阵可计算初始的四元数为:

式中:q0，q1，q2，q3可由初始姿态矩阵求得。

四元数微分方程为:

式中:⊗为四元数乘法符号，令ωg＝[ωxωy ωz]T，则:

姿态四元数更新公式为:

式中:T为采样时间间隔。

3.2 状态预测过程

将姿态四元数作为AHRS 算法中EKF 滤波器的状态矢量:

3.3 量测更新过程

将ab、mb与加速度计、磁力计测量值的差值作为观测量，即

3.4 准静态磁场检测

在车辆导航应用场景，通常车辆载体引起的磁场干扰是磁力计测量干扰的主要来源，磁干扰在线补偿的主要目的也是消除这部分干扰误差。但在实际情况下，如会车时或路况复杂时难以避免会存在较大干扰源。这部分干扰主要以硬磁干扰为主，造成相对于车辆载体更复杂的磁干扰，进而会严重影响载体磁干扰在线估计精度及航向解算精度，因此需要对磁场环境进行实时的准静态磁场检测。

通常仅将没有环境磁场干扰时的地磁场认为是准静态磁场。本文针对车载导航特殊应用场景，认为准静态磁场由地磁场和车辆载体干扰磁场共同构成。由于车辆载体会引起较大的硬磁干扰，使得磁力计三轴测量值的合磁场强度一般要大于当地地磁场强度。此外，当车辆航向发生改变时，三轴磁力计测量值的合磁场并非一个固定值。此时将当地地磁场强度作为阈值判断是否处于准静态磁场环境，无法满足车辆导航应用需求。本文提出一种准静态磁场检测方法，将滑动窗口内三轴磁力计测量值方差大小作为基础条件，多传感器测量信息联合，检测是否处于准静态磁场环境:

式中:vark:k＋L(mx)、vark:k＋L(my)、vark:k＋L(my)为第k个长度为L的窗口内三轴磁力计测量磁场的方差，varinitial为静态时刻下由磁力计测量数据统计的方差经验值，Δφm，k为第k窗口内由磁力计求得的航向角变化量，Δφg，k为第k窗口内由陀螺仪求得的航向角变化量，Δφinitial为静态时刻下由磁力计和陀螺仪统计的航向角差值的统计经验值。当满足上式时，视该窗口内测量磁场为准静态磁场区间，此时自适应调整EKF算法中磁力计观测噪声的协方差权值。当判断该窗口属于准静态磁场区间时，减小协方差权值；当判断该窗口为非准静态磁场区间时，意味着除车辆载体干扰外，环境中还存在较大的磁干扰，此时增大协方差权值。自适应方差公式如下:

4 基于GNSS 辅助的车辆载体磁干扰在线估计算法

上述车载AHRS 算法中，车辆载体引起的磁力计测量干扰会严重影响航向估算精度。基于式(2)的磁力计输出模型，提出一种基于GNSS 辅助的车辆载体磁干扰在线估计算法，在GNSS 可用时，利用GNSS 航向信息作为观测量，构建扩展卡尔曼滤波器，对车辆载体引起的磁力计测量干扰进行在线估计并补偿。

4.1 状态预测过程

构建扩展卡尔曼滤波器对磁力计三轴干扰进行在线估计，选取磁力计自身零偏和载体磁干扰共同引起三轴磁力计的零偏作为状态矢量:

系统状态预测方程为:

4.2 量测更新过程

以GNSS 航向和磁力计解算航向的差值作为系统观测量，系统状态观测方程为:

式中:Z＝[Δφ]，Δφ＝φgnss－φm，φgnss为GNSS 提供的航向，φm为式(5)加速度计和磁力计测量信息解算的航向，V为GNSS 航向量测噪声，协方差矩阵R＝系统观测矩阵H＝[H1H2H3]。

引入三轴磁力计零偏，由式(4)可得航向角计算公式为:

可得:

利用上述算法在线估算的磁力计的零偏，对磁力计进行实时补偿，补偿后磁力计输出为:

5 实验与分析

5.1 实验场景

实验采用华为Mate20 内置的MIMU 及磁力计获取实验数据，MIMU 型号为意法半导体公司生产的iNemo LSM6DSM，磁力计型号为AKM 公司生产的AKM09918。传感器参数指标如表1 所示。

表1 MIMU 和磁力计参数指标

智能手机放置位置及实验车辆如图2 所示，进行跑车实验，实验区域为中科院北京新技术基地周边区域。具体车辆行驶路线如图3 所示，在图中对车库区域进行了标识。

图2 实验设备图

图3 实验路线图

5.2 结果分析

采用本文提出的算法对车辆载体引起的磁力计测量干扰进行在线估计和补偿，根据GNSS 可用星数和位置精度因子(Position Dilution of Precision，Pdop)来判断信号质量的优劣。当GNSS 信号较好时，进行载体磁干扰在线估计。补偿前磁力计水平轴输出结果如图4 中实线和点线所示，其中100 s～400 s 和400 s～700 s 车辆行驶方向完全相反，由于车辆载体干扰的存在，磁力计的水平轴测量数据产生了严重的偏移。经在线标定补偿后，磁力计输出结果如图4 中点划线和虚线所示。可以看出，车辆载体干扰引起的磁力计测量干扰得到了有效修正。需要说明的是，由于车辆行驶过程中，俯仰角和横滚角的变化幅度较小，难以激励出天向磁力计的误差，其误差估算结果收敛性较差，故未对磁力计Z轴测量干扰进行补偿。

图4 补偿前后磁力计水平轴数据

首先采用本文提出的方法进行准静态磁场检测，当处于非准静态磁场环境中时，自适应调整车载AHRS 算法解算过程中的磁力计观测噪声参数。为了达到较好的标定和航向解算效果，在室外开阔区域，参数阈值设置较为严格，其中varinitial为0.8，Δφinitial为0.5。而在车库等干扰较大的区域，参数阈值设置较为宽松，其中varinitial为8，Δφinitial为2。得到准静态磁场检测结果如图5 所示，当检测器(图5中实线所示)值为0 时，认为该点磁场为准静态磁场，否则为非准静态磁场。从图中约800 s 后的车库区域检测结果可以看出，车库停放车辆较多，硬磁干扰较大，但仍有部分区间处于准静态磁场区间。

图5 准静态磁场检测结果

Mata20 内置MIMU 采集的数据如图6 所示，利用上文所述的车载AHRS 算法进行航向解算。设置3.4 节中参数k1＝200，k2＝150。

图6 实验中Mata20 内置MIMU 数据

AHRS 算法解算结果如图7 所示，其中实线为参考航向。车辆载体磁干扰补偿前航向解算结果如图7 中点划线所示，由于车辆载体存在严重的磁干扰，解算的航向严重失准。补偿车辆载体的磁干扰后，航向解算结果如图7中点线所示，由图8 所示的误差图(图8 中点线部分)可以看出，在环境干扰较小时，航向精度在10°以内。但在车库环境中，航向精度在20°左右。结合准静态磁场检测并补偿车辆载体的磁干扰后，航向解算结果如图7 中虚线所示，其基本与真值重合，由图8 中的误差图(图8 中实线部分)可以看出，经过准静态磁场检测后，非准静态磁场区间中的误差大幅度减小，总体航向误差约在5°以内。

图7 补偿前后航向与参考航向

图8 补偿前后航向误差

为验证本文所提出算法的实际定位效果，将车辆从室外开阔场景行驶到室内地下车库中，利用普通车载AHRS＋DR＋GNSS 进行组合定位解算，其结果如图9 所示。由于室内地下车库(图9 左下角实线部分所示)无GNSS 信号，且车辆载体误差未消除，车载AHRS 解算的航向严重失准，无法利用GNSS 信息或车载AHRS 航向校正DR 误差，导致定位结果出现明显偏移。根据本文提出的算法，利用在室外开阔场景标定的车辆载体干扰误差，对车辆载体磁干扰进行补偿。定位结果如图10 所示，可以看出，本文提出的磁干扰补偿的车载AHRS 算法能够提供准确的航向，修正DR 系统的累积误差，保证良好的定位结果。

图9 误差补偿前定位结果

图10 误差补偿后定位结果

6 结论

提出了一种基于磁干扰补偿的智能手机车载AHRS 算法，利用手机内置GNSS 传感器提供的测量信息，对车辆载体磁干扰在线估计。结合准静态磁场检测，进行车辆AHRS 航向解算。跑车结果表明，该方法能够准确估计车辆载体磁干扰，大幅度提高AHRS 航向精度，有效抑制GNSS 失锁时DR 系统定位发散速度快的问题，提高定位精度，满足车辆导航应用需求。