基于级联滤波的行人惯性室内定位方法

张雨帅，郭建新，朱 锐，王祖良，姬 翔 ，王利平

（西京学院 信息工程学院，西安 710123）

0 引言

目前，室内定位导航已经成为各种基于位置的服务和应用的共同研究热点。针对室内环境中的行人导航问题，国内外学者提出了许多解决方案，如基于惯性导航系统（inertial navigation system,INS）、无线保真（wireless fidelity, WiFi）、蓝牙（bluetooth,BT）、超宽带（ultra-wide band,UWB）等技术的导航系统。其中，由于WiFi在全球的广泛部署和使用，研究人员对WiFi指纹定位技术做了大量的研究。WiFi指纹定位技术使用WiFi信号强度进行建模和测量。但是WiFi指纹定位技术也存在着许多问题，WiFi指纹定位技术需要多种算法作为支撑，计算量大和算法复杂度相对较高。此外，由于该方法需要大量的先验信息作为数据支撑，前期工作具有较高的成本。同样，基于蓝牙、超宽带的导航方法也可以提供高效的室内导航服务。然而，这些方法需要前期建立非常可靠的基础设施以确保导航系统的性能，这无疑会增加前期工作的成本。

惯性导航系统是一种自主导航技术，它不需要提前在环境中预安装任何设备，可以有效地避免上述技术难以克服的问题。并且随着微机电系统（microelectromechanical systems, MEMS）的发展，惯性传感器作为一种更廉价、更实用的定位解决方案引起了人们的广泛关注。考虑到人体行走步态的独特性，将MEMS传感器安装在用户的脚上，可以有效地限制惯性导航的导航误差。在足部安装模式下，当周期性地检测到脚的相位，则会触发零速更新（zero velocity update, ZUPT），使用卡尔曼滤波（Kalman filter，KF）算法估计和校正惯性导航的解算误差及惯性传感器的固有偏差。文献[12]在零速更新的基础上，提出了启发式航向修正和零角速度更新，可以在零速更新阶段修正惯性导航的航向误差。尽管基于卡尔曼滤波零速修正的惯性导航系统有时可以产生相对较好的导航结果，但存在较大的航向误差，而且MEMS惯性传感器也存在相对较大的不确定性和随机性偏差。惯性传感器的导航的偏差还会随着时间的推移而不断增长。为了解决行人惯性导航系统的缺点，研究人员将其与额外的传感器或辅助信息融合，从而提高导航定位精度。多传感器融合定位方法都能有效地为用户提供高精度的室内定位导航。然而，这些方法的基础设施成本很高，需要大量的劳动力来建立一个完整的室内定位导航系统。文献[15]提出一种基于粒子滤波地图与惯性导航融合的室内定位系统，通过依靠地图信息来修正导航误差。在没有额外的辅助定位仪器和设施情况下，室内平面地图信息是一种经济简洁补偿定位误差的方法。

鉴于以上研究内容，本文提出一种级联结构的室内定位导航系统，将基于扩展卡尔曼滤波的启发式航向修正，零速更新，零角速度更新与室内平面地图信息相结合，利用行人运动特征及室内建筑结构信息，将扩展卡尔曼滤波和非递归贝叶斯滤波级联在一起，通过在室内行人导航系统中使用级联结构算法，从而减小位置更新过程中的误差，在一定程度上提高导航计算精度。

1 扩展卡尔曼-非递归贝叶斯级联滤波算法

本研究使用惯性传感器和室内地图信息，充分利用扩展卡尔曼滤波的优点，同时使用启发式航向修正、零速更新及零角速度更新三种算法并提出了一个由双层架构组成的级联结构的扩展卡尔曼滤波和非递归贝叶斯滤波算法，其结构如图1所示。图1中：PDR（pedestrian dead reckoning）为行人航迹推算算法。

图1 级联结构的行人惯性定位导航系统

下级滤波器使用基于扩展卡尔曼滤波的启发式航向修正、零速更新、零角速度更新作为初始惯性导航解算误差修正方案；因此，可以有更好的惯性导航解算结果提供给上级滤波器，同时下级滤波器也可作为单独的零速修正惯性导航系统。上级滤波器应用行人航位推算方法来计算行人的位置。利用下级滤波器导航结果计算出步长和航向的变化，用以更新上级滤波器的非线性状态。将先验地图信息作为一个独立的度量来修正低级滤波器的导航结果，两级滤波器之间的关系如图1所示。在下级扩展卡尔曼滤波中，选择行人的姿态信息、角速度、位置、速度以及加速度信息作为状态向量；对于非递归贝叶斯滤波器，状态向量中只有二维位置信息，从而减小计算负担。

2 扩展卡尔曼滤波的零速修正算法

在下级滤波器中，惯性导航系统解算算法的连续模型如下：

1）采用卡尔曼滤波方法估计偏差项对原始加速度和陀螺传感器数据进行偏置补偿，即

2）利用偏置补偿陀螺仪数据进行方向更新，即

4）将重力加速度移除后的加速度值进行积分，获得导航坐标系下的速度，再对速度积分获得导航坐标系的位置为

式中：=[,,]为时间时扩展卡尔曼滤波校正之前的三轴速度；=[,,r]为时间时扩展卡尔曼滤波校正之前的三轴位移。

行人惯性导航系统输入数据是以设备帧表示时间序列的加速度和角速度。利用加速度的二重积分计算行人二维地图上的位移。然而，由于传感器干扰引起的误差累积，位移的偏差随时间增加而变大。使用基于扩展卡尔曼滤波的零速修正算法，可以修正导航的累计误差，而在零速修正算法中，关键的步骤是对零速状态进行检测。文献[18]提出了一种最优零速检测器，该算法的性能优于基于加速度幅值零速检测算法和加速度方差零速检测算法，因此采用最优零速检测器作为下级滤波器的零速检测算法。

最优零速检测器的模型为

当测量序列的值小于阈值时，行人处于零速状态，反之则为运动状态。当行人处于静止状态时，依据以下 6个步骤使用扩展卡尔曼滤波算法对行人的惯性导航解算结果进行修正：

1）时间处的15个误差状态向量为

2）线性化状态转换模型为

式中：为单位矩阵；为零矩阵；为斜对称矩阵。

的计算方法为

其计算方法为

3）测量模型为

4）时刻的误差状态是在获取时刻的测量结果后得到的。在获取时刻的测量结果后，卡尔曼滤波更新方程为

式中：K为卡尔曼滤波增益；m为实际的测量误差；δx为预测的错误状态。

在本文的扩展卡尔曼滤波算法中对启发式航向修正（HDR）、零角速率更新（ZARU）、零速更新(ZUPT)三种算法进行了融合应用。实际误差测量m为扩展卡尔曼滤波提供信息，实际误差测量m计算为

测量矩阵的为

式中，=[0 0 1]为航向误差的观测矩阵。

5）卡尔曼滤波增益用通常的公式计算为

式中：P为估计误差协方差矩阵，该矩阵是在时刻的基础上，根据时刻−1收到的测量结果计算得到的。在时刻使用约瑟夫公式中的卡尔曼增益计算出协方差矩阵P，即

6）使用无偏误差项滤波后的状态向量δx来优化当前的速度和位置，即

3 非递归贝叶斯的地图匹配算法

通过下级扩展卡尔曼滤波修正后的导航结果计算步长和航向，结合其步长和航向信息，实现了PDR作为本文上级非递归贝叶斯滤波器使用的状态模型。

当行人静止时，每两个坐标之间的距离被定义为步长，即

式中：x、y为根据下级扩展卡尔曼滤波估计的位置。每一步的航向变化是指当行人静止时，扩展卡尔曼滤波提供的每两个航向之间的差值，即

因此，非递归贝叶斯滤波的测量模型可以表示为

式中：v、v为独立于步长和航向变化的噪声。

与基于粒子滤波的地图匹配导航研究不同，非递归贝叶斯滤波地图匹配算法利用室内平面图建立一个保持静态的栅格化概率分布函数(|)，为地图先验概率。如图2所示，密度水平与行人在相应位置的行走可能性成正比，当墙边界开始时，蓝色部分的分数降至零，于是将行人行走路径约束至室内走廊中，因此非递归贝叶斯滤波不需要大量的蒙特卡罗模拟，也不需要对每个粒子进行明确的穿墙检查。

图2 静态的平面地图概率分布函数

由于步长和航向都会间接影响行人的位置似然，通过静态平面图概率密度函数和扩展运动模型来解决似然项的不确定性。扩展运动模型(|,… ,)预测行人在直线段运动时在直线上的位置，以及在靠近走廊拐角时在曲线上的位置。传感器似然和先验概率密度函数的均值位于一个行人环绕概率密度函数(|,)，该概率密度函数由步长表示位置密度。先验、似然、基于步长的似然和静态平面图组合形成后验概率密度函数。因此，非递归贝叶斯滤波器的最终滤波方程为

式中：(|)为传感器似然概率密度函数；(|)为先验概率密度函数。

4 实验结果与分析

为对基于级联滤波的行人惯性室内定位方法的性能进行评估，在实际的室内进行了实验测试，使用克斯-艾奥科技有限公司（x-io Technologies Limited）的NGIMU作为惯性导航传感器设备，其参数如表1所示。本系统使用了带有外壳的NGIMU，尺寸为56 mm×39 mm×18 mm，重量为46 g。如图3所示，用一条拉伸带将NGIMU固定在脚上。

表1 NGIMU 传感器参数

图3 安装在足部的NGIMU

在实验楼内开展行人室内导航实验（楼层平面图如图4所示），走廊呈回字形，南侧走廊长约80.8 m，南北向走廊长约27.17 m，走廊宽度约2 m。实验从地图的西南角出发行走一圈回到起点，行走规划的轨迹如图4所示，行走一圈的距离大约为215.94 m。

图4 室内平面图

使用采集的数据开展扩展卡尔曼滤波零速更新惯性行人导航算法以及基于级联结构的扩展卡尔曼滤波、非递归贝叶斯滤波导航算法实验。215.94 m测试实验零速修正校正方案下的二维平面位置估计如图5所示。

图5 215.94 m测试实验二维平面的位置估计

图6至图9分别为 52.17、215.94、431.88、647.82 m测试实验的级联惯性导航算法与零速修正惯性导航算法导航估计轨迹对比。因为采集数据时，行人行走的状态以及传感器固有的误差，相同的运动轨迹导航解算出的轨迹会略微有所不同，但这并不影响惯性导航系统的整体性能。

图6 52.17 m测试实验二维平面的位置估计

图7 215.94 m测试实验导航结果对比

图8 431.88 m测试实验导航结果对比

图9 647.82 m测试实验导航结果对比

为验证本系统的有效性和稳定性，本文进行了多组不同距离室内实验，测试人员的行走速度在0.93~1.2 m/s间，属于慢速行走。本文算法与零速更新算法和零速修正算法在不同实验条件下解算的距离如表2所示，并以解算距离与实际距离之间差值作为导航算法的评价标准，实验结果如图10所示。

表2 不同算法解算距离结果 单位：m

图10 实验导航距离误差对比

从图6至图9可以看出，级联结构算法估计轨迹接近实际测试轨迹，与零速修正算法对比可以进一步消除惯性导航的航向漂移。图10为零速更新算法、零速修正算法、级联结构算法三种估计行走的距离误差，仅使用零速更新算法的惯性导航系统不同距离实验中平均误差为 30.35 m,基于零速修正的导航系统不同距离实验的平均距离误差为12.40 m，而级联结构的惯性导航系统不同距离实验的平均距离误差为 0.45 m。随着距离的增加，零速修正算法的距离误差最大，其次是零速修正算法，这是因为随着运动距离和时间的增加，惯性导航的误差累计会越来越难以克服，而本文提出的级联结构惯性导航算法可以有效地将距离误差控制在1 m以内。综上所述，本文提出的系统可以很好地校正航向漂移的问题，使行人行走的轨迹更加接近实际规划轨迹，并且本系统可以进一步纠正距离误差。

4 结束语

本文提出了一种完全非基础设施的、低成本的室内导航系统，它比现有的方法更便宜、更快。该算法只使用室内地图信息和NGIMU传感器。由于该方法仅使用室内地图信息和惯性导航传感器，因此不需要预先测量、预先安装或额外的传感器。该系统采用两层结构算法，融合室内地图信息和MEMS传感器，来实现室内导航。实验结果表明：所提出的方法能够达到所需的导航计算精度；地图匹配的非递归贝叶斯滤波方法可以大幅度提高零速修正惯性导航的精度。