基于加强抗差自适应卡尔曼滤波的行人室内定位系统*

张 海， 樊启高， 庄祥鹏， 贾 捷， 张鹏松

(江南大学 物联网工程学院，江苏 无锡 214122)

0 引 言

室内定位是指在室内环境中实现位置定位，在应急救援、物流和综合性医院等行业均展现出巨大的前景[1～4]。由于室内定位的方法容易受到外部环境干扰。微机电惯性测量单元(MEMS-IMU)具有结构简单、体积小点，同时惯性测量完全自主，全天候，不受外界环境干扰，无信号丢失，适合于室内定位。

惯性导航定位技术以牛顿力学为基础，通过测量载体在惯性参考系的加速度，对时间进行积分，长时间定位会存在累积误差。文献[5]利用Bagging模型深度学习惯导误差之间的内在关系，对惯性导航的误差进行抑制。文献[6]提出惯性导航系统/超短基线/多普勒计程仪(INS/USBL/DVL)组合导航联邦滤波方法，有效抑制惯导的累积误差。针对特定的行人室内定位，步态检测模式被提出[7]。

在滤波算法方面，由于不能准确估计系统噪声和量测噪声的统计特性，传统卡尔曼(Kalman)滤波效果不佳[8]。文献[9]引入模糊自适应的卡尔曼滤波,可以在线自适应调整量测噪声方阵，提高了定位精度，但动态变差；文献[10]根据极大似然准则构造系统噪声的估计模型,引入滚动时域策略对模型进行优化,利用二次规划方法求取噪声统计的估计值,但计算量偏大。此外，Sage-Husa自适应滤波也适用于定位系统，通过噪声估计器实时估计和修正噪声的统计特性，从而降低误差[11～13]。Sage-Husa算法难以同时精确给出系统噪声和量测噪声的统计特性。

简化Sage-Husa自适应算法是在某一噪声变化不大情况下，在线估计和修正另一噪声的统计统计特性，实用性较高[18]。

1 系统模型

1.1 行人定位模型

行人定位模型包含数据采集到位姿输出的全过程，如图1所示。通过MEMS-IMU获取加速度、角速度以及磁场强度，并对其进行预滤波处理。将加速度计和磁强计在稳态下的姿态角，以及陀螺仪在动态下的姿态角优势互补，进行滤波融合，获得较为精确的空间三维姿态角。将加速度进行二次积分分别获取行人的速度以及位置。采用零速阈值判断对行人移动过程中的零速区间进行判断，并进行零速更新，提高速度的精确性。数据滤波部分是将惯性导航算法得到的角速度误差、加速度误差、速度误差以及位置误差作为定位系统的状态矢量，以更新后的速度以及位置为定位系统的量测矢量。通过滤波算法，对系统内部误差进行估计以及修正，提高定位系统的精度。

图1 行人定位原理

1.2 小波预处理

受制造工艺和使用环境的影响，MEMS误差成为制约惯性导航系统精度的主要原因。目前主要的去噪方法是小波去噪，首先将信号S分解为低频噪声A与高频噪声D，n次分解后为

(1)

根据信号和噪声在各尺度上的不同特点，选择合适的阈值准则处理小波系数，最后重构信号，实现了信噪分离。

1.3 融合滤波

姿态矩阵计算直接影响姿态、速度以及位置的精确性。陀螺仪存在着静态漂移现象，在计算姿态时会产生累积误差；加速度计和磁强计的动态响应较差，但没有累积误差。因此在静态和动态条件下分别采用加速度计，磁强计输出的姿态信息和陀螺仪输出的姿态信息，然后对两者输出进行优势互补，提高姿态矩阵的精确度。融合滤波算法如图2所示，将加速度计与磁强计计算得到的姿态角与最终姿态角的误差进行PI控制，再采用互补滤波算法对姿态信息进行融合，提高姿态的精度。

图2 融合滤波

(2)

1.4 零速更新算法

行人在移动时左右脚交替移动，每步移动可近似为一个加速和减速的过程。同一只脚随着脚后跟离地而加速，脚后跟着地而减速。根据加速度和角速度的变化，形成“加速—减速—零速—加速”的循环模式。

行走周期内存在脚完全接触地的情况，即零速区间。通过加速度和角速度变化来分析与识别行人的零速区间，并进行零速更新。本文在此基础上，采用多条件阈值判别算法对零速区间进行判断

(3)

(4)

2 加强抗差自适应卡尔曼滤波

2.1 自适应卡尔曼滤波

针对行人在二维坐标系中的定位要求，本文以行人的角速度、加速度、速度以及位置的误差作为该系统的状态量；以速度以及位置的计算值为定位量测量。卡尔曼滤波利用信号与噪声的状态模型，采用前一时刻的估计值和当前时刻的观测值来更新对当前状态变量的估计

x(k+1)=Φ(k+1,k)x(k)+w(k)

(5)

Z(k)=H(k)x(k)+v(k)

(6)

式中x(k)为k时刻系统状态量；Φ(k+1,k)为k时刻到k+1时刻的状态转移阵；w(k)为系统噪声矢量；Z(k)为k时刻测量矢量；H(k)为k时刻测量方程的系数矩阵；v(k)为测量噪声矢量。 卡尔曼滤波主要由预测过程和更新过程组成。

1)预测过程

(7)

2)更新过程

(8)

(9)

(10)

标准卡尔曼滤波在系统模型已知，过程噪声和量测噪声已知时才能实现最优估计滤波，而实际应用中，噪声往往不是固定的。假设行人惯性定位系统中系统噪声具有一定的稳定性，只需对量测噪声进行估计。简化Sage-Husa自适应滤波算法通过量测噪声估计器，可以实时修正量测噪声的统计特性

(11)

式中dk=(1-b)/(1-bk+1)，b为遗忘因子。

2.2 抗差估计

简化Sage-Husa自适应滤波一定程度上提高了系统的鲁棒性，但不能剔除量测值中的野值。本文将粗差归入随机模型，通过调节观测值方差的大小来抑制粗差的影响。

定义k时刻的新息矩阵ek和对应的协方差矩阵Ck为

(12)

当系统没有故障时，新息是零均值白噪声序列。当新息实际统计特性与理论不相符时，则判断过程出现异常。在故障检测方面，新息χ2检测算法是一种有效的方法。

假设检验统计量

(13)

新息ek为均值为零的正态分布，所以，Tk服从自由度为m(m为观测值的维数)的χ2分布。把故障检测条件转变为假设检验条件，根据χ2分布的定义

{H0:Tk～χ2(m,0),H1:Tk～χ2(m,λ)

(14)

若Tk≤TD，则判断观测量无异常，否则观测量存在异常值，则需要引入加权因子对新息协方差放大，降低野值对滤波的影响。利用Huber函数构造加权因子

(15)

令α=diag[α1,α2,…,αn]，i=1,2…n,观测值的等价协方差更新为Rk=Rk/α。

2.3 强跟踪滤波

由于系统模型的简化、实际系统的参数发生变动等，系统模型存在大量的不确定性。因此，在Suge-Husa自适应滤波算法的基础上，引入强跟踪滤波算法。强跟踪滤波算法利用衰减记忆滤波思想，将渐消因子λk引入状态预测误差协方差矩阵Pk,k-1，实时调整滤波增益矩阵，强迫滤波残差序列保持相互正交，使得强跟踪滤波器保持对系统实际状态的跟踪。

引入渐消因子λk

(16)

式中λk≥1为自适应渐消因子，通过下式进行确定

(17)

3 实验验证与结果分析

3.1 实验环境

实验采用荷兰Xsens公司的Mti 系列MEMS IMU，主要由3个相互正交的加速度计、3个陀螺仪及1只三轴磁传感器组成。Xsens惯导模块安装在鞋背上。

3.2 实验结果与分析

3.2.1 信号预处理分析

为重构出测量白噪声和有色白噪声，采用小波分解法。以加速度X轴输出值为例，采用“db6”小波基函数，“rigrsure”的阈值准则和软阈值方法，获得原始信号和有色噪声的对比，如图3所示。

图3 小波分解对比

3.2.2 姿态分析

为验证姿态融合滤波算法对姿态角精度的提高，本文利用上述实验设置获取姿态角的对比，如图4所示。

图4 姿态对比

由图4可见，初始姿态值中存在着测量噪声以及累积误差。经过融合滤波，测量噪声被抑制，输出值的毛刺大大削弱，姿态值的精确度得到提高。

3.2.3 零速修正分析

如图5(a)所示，根据加速度计、磁强计和陀螺仪的输出，利用零速更新算法得到零速区间。根据零速区间对行人速度进行修正，如图5(b)所示。零速校正作为一种误差补偿算法，有效降低了速度的累积误差。

图5 零速区间与速度修正

3.2.4 轨迹对比分析

为验证加强型抗差自适应滤波算法的性能，本文在不同定位滤波算法下进行了10组行人定位实验，并将实验数据进行平均处理，获得如图6所示的轨迹对比，位置误差对比如图7。

图6 轨迹对比

图7 位置误差对比

相比于传统Kalman滤波，AKF和SAKF算法下的运动轨迹更接近实际运动轨迹。自适应Kalman滤波在利用测量数据进行滤波的，同时对噪声统计特性进行估计和修正。

与SAKF相比，AKF下的轨迹含有不少尖峰，相对波动较大。AKF不能剔除野值的干扰，SAKF利用新息检测野值，构造等价协方差矩阵，可以有效的抑制野值的干扰。同时引入强跟踪滤波算法，进一步抑制发散。

4 结 论

将提出的SAKF算法与KF和AKF算法进行比较。实验结果表明：SAKF滤波算法适用于行人惯性定位系统，有效提高了行人定位精度，有较强的实用性。