一种基于自适应无迹卡尔曼滤波的室内定位方法与流程

本发明属于室内定位算法领域，特别涉及一种基于自适应无迹卡尔曼滤波算法的室内定位方法。

背景技术：

室内定位技术在当今生活中不可或缺，常用的室外定位导航的全球定位系统在室内复杂环境中，信号弱且不能保证信息有效性，为了满足各种室内定位的需求，研究人员已经研究开发了很多适合室内环境的定位系统，从单一数据源定位到多种方式相互辅助定位，部分技术可取得较为满意的定位效果。

微机电系统的快速发展带来了惯导系统的平民化，以此为基础开发的航迹推算系统可以估计实时轨迹，但存在累积误差，长时间的轨迹导航效果不佳；另一方面，wifi(wirelessfidelity，无线保真技术)设备的普及使得通过无线电射频信号定位成为现实，通过信号衰减模型和指纹匹配都可以完成定位，但在复杂环境中受多径效应、人身遮挡等问题影响，加上信号有跳变的情况出现，平均误差较大。在低成本和易实现的情况下，科研人员对融合定位技术中融合惯导系统和无线的技术愈发感兴趣，开发了很多两种技术结合的室内定位方法，意在结合这两者所各自具有的优势，常用的主要是各种滤波算法。

上个世纪六十年代开始，卡尔曼滤波理论就被使用来对根据系统的量测值去消除系统的随机干扰。卡尔曼滤波通过实时更新均值和协方差实现滤波过程，是一种适用于线性系统的估计方法，而在现实问题上，需要我们进行估计的状态所组成的系统常常是非线性的，因此在随后的这几十年间发展了一系列基于卡尔曼滤波的非线性估计算法，如拓展卡尔曼滤波，无迹卡尔曼滤波，粒子滤波等。拓展卡尔曼滤波通过将非线性系统线性化，而后使用卡尔曼滤波的方式进行估计，应对非线性较低的系统有良好的性能，而在非线性强的场合难以给出可靠的估计；无迹卡尔曼滤波通过无迹变换而后实现滤波，能够提供更高阶的估计精度，同时计算量更小，应对较高非线性的系统也能够给出较好的估计结果；粒子滤波是一个完全的非线性估计器，对于非线性系统具有更好的性能表现，但是计算量将大幅增加，使用于特定高精度场合。卡尔曼滤波及其拓展形式都是建立在已知模型的情况下，对于稳定的系统可以实现很好的估计，但是对于模型不确定的情况就会出现滤波效果变差，估计结果偏差变大，从而导致在多种运动结合的情况下定位结果变差差的问题。

技术实现要素：

本发明的目的，在于提供一种基于自适应无迹卡尔曼滤波的室内定位方法，采用无迹卡尔曼滤波滤波算法融合wifi指纹定位和航迹推算的室内定位方法，并结合观测值和后验估计结果偏差设计自适应调整的过程噪声，实现在未知模型和行人状态多变情况下滤波的优良性能，保证长期稳定的定位精度。

为了达成上述目的，本发明的解决方案是：

一种基于自适应无迹卡尔曼滤波的室内定位方法，包括如下步骤：

步骤1，根据室内环境收集采样点处接收到的路由器信号并组合成为指纹存入数据库，接着实时通过行人手中的手机采集路由器信号序列，将序列和数据库中的指纹进行匹配，估计得到近似位置；

步骤2，通过传感器获取加速度、角加速度和磁场强度数据，通过航迹推算算法中步长检测算法和航向推算算法分别求得行进中步长和方向；

步骤3，利用步骤1、2中的估计位置信息、步长和方向信息构建合适无迹卡尔曼滤波算法的系统方程和测量方程模型，并根据观测值和后验估计值的偏差设计自适应噪声缩放因子，修正当前时刻的过程噪声的方差，最后通过迭代更新估计位置。

上述步骤1中，路由器信号指纹形式是一个向量，按照路由器的mac地址排序接收到的信号强度序列，表达式如下：

指纹序列＝[rssi1,rssi2,rssi3,…,rssin]

其中，rssi表示信号强度，n表示定位所在地点收集到的路由器信号个数，每一个指纹序列代表一个采样点的特征。

上述步骤1中，采用k近邻匹配方法进行匹配，首先计算扫描到的指纹同数据库中每一个采样点指纹的匹配度l：

其中，下标i代表获得的指纹同指纹库中第i个采样点进行比较，j代表指纹中第j个信号强度，n代表指纹序列的长度；rol代表通过手机实时采集到的指纹序列，roff代表指纹库中每一个采样点的指纹序列；

将得到的所有匹配度进行排序，选出最小的k个，根据选出的k个采样点已知的位置，求均值得到的结果作为无线指纹的定位位置。

上述步骤2中，步长检测算法的计算表达式如下：

其中，f表示步行的频率，s表示步长。

上述步骤2中，航向推算算法以四元数形式进行计算，通过四元数与方向余弦矩阵的对应关系，求解得到欧拉角，其中航向角为四元数[q1234]通过一阶荣格库塔求解以下微分方程得到：

其中，t为每一步的时间长度，也即一个时间周期，[gxgygz]为通过加速度和磁场强度修正过的三轴角加速度值。

上述步骤3中，系统方程模型建立如下：

其中，xk和yk为二维坐标位置第k步后的位置，sk为第k步行走的步长，ψk为第k步后的人体朝向角度，为航向角的变化量，wk-1为3维的系统过程噪声；

测量方程模型建立如下：

其中vk为5维的系统观测噪声；

系统方程和测量方程的离散形式如下：

xk＝f(xk-1,uk-1)+wk-1

zk＝h(xk)+vk

其中，f和h为非线性向量函数，xk-1为第k-1步的状态量；uk-1为第k-1步的外部输入量，一般情况下可以不作考虑；wk-1和vk是方差分别为qk和rk的零均值白噪声序列；

通过无迹变换，求得2n+1个sigma点，通过预测得到先验估计及其协方差接着通过测量更新得到测量预测及其协方差pz和相关协方差pxz；最后得到迭代更新的卡尔曼增益kk，后验估计及其协方差pk：

其中，为pz的逆矩阵，zk为实际的测量数据向量。

上述步骤3中，自适应噪声缩放因子λk通过观测值zk和后验估计值的偏差ek进行设计，对先验估计协方差进行修正，原形式如下：

其中为未加入噪声的先验估计协方差，qk为系统过程噪声协方差矩阵；

加入修正因子λk后：

ek通过观测值zk和后验估计值求得：

其中i为单位矩阵，h为非线性向量函数h无迹变换后的函数，vk为观测噪声；

噪声缩放因子λk通过偏差ek求解：

λk＝ak/bk

tk＝i-hkk

其中，μ为计算步长，根据时延和计算能力进行调节；

因此每一时刻的过程噪声方差：

qk+1＝λkqk

其中，tr为求矩阵的迹，上标t为矩阵的转置，r为系统观测噪声协方差矩阵。

采用上述方案后，与现有技术相比，本发明的有益效果是：该自适应卡尔曼滤波算法可以应对室内的复杂环境和行人定位时所表现出的不同状态，通过自适应调整模型中过程噪声的参数，确保模型快速匹配当前状态，保证滤波效果持续处于高效状态。此外无迹卡尔曼滤波算法适用于融合定位的非线性模型，能充分利用定位模型中两种数据在定位上各自的优点，并且相较于拓展卡尔曼滤波等其他适合非线性模型的算法具有更高效的滤波效果。

附图说明

图1是本发明的流程图；

图2是本发明的航迹推算具体实现示意图；

图3是本发明的过零点步频检测示意图；

图4是本发明的指纹定位现场示意图。

具体实施方式

以下将结合附图，对本发明的技术方案及有益效果进行详细说明。

如图1所示，基于自适应无迹卡尔曼滤波算法的室内定位包括三个部分，分别是航迹推算、指纹定位和自适应无迹卡尔曼。航迹推算通过过零检测得到步频，用于步长计算；通过航向检测算法整合传感器数据得到方向角。指纹定位通过离线指纹采集和在线指纹匹配两个步骤实现。自适应无迹卡尔曼通过整合航迹推算和指纹定位的结果建立模型，并对模型中噪声参数进行修正来实现定位。

●航迹推算

航迹推算部分分为三个部分，如图2所示分别是过零点步频检测，步频步长模型获取和航向检测。

■过零点步频检测

根据人体运动的模型，从一侧足跟着地始到同侧足跟再次着地止，这段时间称为一个步态周期。通过对该周期内加速度值进行采样提取可以得到该周期内的一个变化趋势，可以用于步数的检测。由于前进过程中三个方向的加速度都是存在的，因此在对加速度值进行采样时，首先取三轴加速度值的平方和后开根号，即取模值

接着消除重力g影响，得到初步采样结果ai：

由于传感器测量存在噪声，初步采样出来的加速度值抖动很厉害，影响过零点检测的精度，因此需要通过滤波器进行再次优化，得到滤波后的加速度值af1iter：

afliter＝fir64_filter(a)

这里采用一个n＝63的fir滤波器，可以有效抑制高频噪声，整个步态周期的加速度值变化将变得平滑，大大提高过零点检测的精度。

在过零点检测中，如图3所示为滤波后的加速度值变化情况，一个步态周期经过两次零点，其中点1到点3就是一个步态周期。接着根据该步态周期的时间t可以得到步态周期的频率f，即步频f。

■步频步长模型获取

根据人体机械学原理，步频和步长是正比关系，，这里采用了线性的步频步长关系，通过多次实验分析获得的步长和步频关系式如下：

步长

■航向检测

航向推算算法使用以陀螺仪数据为主，磁力计和加速度计数据为辅的方式，以四元数形式进行计算。四元数与方向余弦矩阵存在一一对应的关系，因此可以用来求解欧拉角。

方向余弦矩阵：

四元数矩阵：

通过上式求解得到欧拉角，其中航向角为

其中四元数[q1q2q3q4]通过一阶荣格库塔求解以下微分方程得到：

其中[gxgygz]为通过加速度和磁场强度修正过的三轴角加速度值。

●指纹定位

指纹定位部分分为两个部分，分别是离线指纹采集和在线指纹匹配。

■离线指纹采集

在进行定位之前，本算法需要收集目标场所特定点位的信号特征，这里称为指纹采集阶段。如图4所示为指纹定位场所示意图，十字星位置为可接入点(ap,accesspoint，无间断发出无线信号；圆点位置为采样点，这里就是需要收集信号特征的位置，采样点间隔一般选择1-2米。这里总共6个ap点，那么收集到的信号序列如下：

指纹序列=[rssi1,rssi2,rssi3,rssi4,rssi5,rssi6]

其中rssi为信号强度值。该序列是一个采样点的指纹，将所有采样点的指纹保存到数据库，则完成离线采集过程。

■在线指纹匹配

在线指纹匹配阶段，用户使用信号接收器扫描ap信号，得到指纹序列rol，将该指纹序列同数据库中的指纹序列roff进行匹配，这里采用欧氏距离的计算方式得到匹配度l：

接着对所有的匹配度进行排序，选择最小的k个，这里k一般取3或4。根据选择出来的k个采样点，得到数据库中这些点的位置，进行平均以后作为指纹定位的结果。

●自适应无迹卡尔曼

自适应无迹卡尔曼部分主要是两个部分，分别是模型搭建和过程噪声修正。

■模型搭建

系统模型和测量模型根据估计位置(x,y)，航向角ψ，步长s等数据进行建立。

系统模型建立如下：

其中为航向角的变化量，wk-1为3维的系统过程噪声。

测量方程模型建立如下：

其中vk为5维的系统观测噪声。

系统方程和测量方程的离散形式如下：

xk=f(xk-1,uk-1)+wk-1

zk＝h(xk)+vk

其中f和h为非线性向量函数。wk-1和vk是方差分别为qk和rk的零均值白噪声序列。

无迹卡尔曼滤波算法通过对非线性模型进行无迹变换后进行滤波过程。首先选择2n+1个sigma点，进行预测过程，计算得到先验估计及其协方差而后通过量测更新，计算得到测量预测及其协方差pz和相关协方差pxz。根据这些结果求解卡尔曼增益kk，后验估计及其协方差pk。

循环进行sigma点采样计算，预测过程和测量更新实现滤波。

■过程噪声修正

该过程对先验估计协方差进行修正，由于实际滤波过程中模型搭建的不如意和协方差参数的选择不当将导致收敛过程不稳定，出现滤波效果变差的情况。因此根据观测值zk和后验估计值设计了自适应噪声缩放因子λk来调整先验估计协方差原形式如下：

其中为未加入噪声的先验估计协方差。

加入修正因子λk后：

噪声缩放因子λk通过偏差ek求解：

λk＝ak/bk

tk＝i-hkk

ek为观测值zk和后验估计值的偏差：

其中i为单位矩阵，vk为观测噪声。

上一步模型搭建中，通过预测模型得到先验估计和协方差时，加入该步骤，对协方差进行修正，实现自适应的匹配协方差参数，以达到模型更优状态。

综合上述，本发明一种基于自适应无迹卡尔曼滤波的室内定位方法，首先，通过手机收集路由器信号并组合成为指纹，采用指纹匹配算法对比离线指纹库得到估计位置；然后，通过手机传感器获取加速度、角加速度和磁场强度数据，通过航迹推算算法计算脚步长度和前进方向；最后，利用前两个步骤中的结果构建无迹卡尔曼滤波模型，在过程噪声变化时，对过程噪声进行自适应更新，最后通过该优化算法获得实时定位位置。本发明融合wifi信号指纹和传感器数据，可实现低成本和高精度的定位，并且可以应对模型搭建不准确而导致的系统噪声变化和估计结果变差的情况，保持特定情况下定位效果的稳定性。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。