CN105588566A - 一种基于蓝牙与mems融合的室内定位系统及方法 - Google Patents

Abstract

本发明公开了一种基于蓝牙与MEMS融合的室内定位系统及方法，设计了蓝牙/MEMS融合定位的粒子滤波器，充分利用两个子系统的可用信息，其中蓝牙系统是依据离线建库后在线匹配获取用户信息，MEMS组合系统是通过航向角解算和动态步长估计来获得用户的位置预测信息。采用扩展卡尔曼近似粒子滤波器作为融合导航滤波器，融合两个子系统的用户位置信息数据，通过合理选取状态变量和观测变量，建立状态方程和观测方程，最终得到最优的用户位置信息，实现室内准确定位。本发明采用蓝牙/MEMS室内融合定位能够解决传统室内定位系统中存在的定位精度低、设备成本高、覆盖范围小、定位连续性差等缺点。

Description

一种基于蓝牙与MEMS融合的室内定位系统及方法

技术领域

本发明属于定位技术领域，具体涉及一种基于蓝牙与MEMS融合的室内定位系统及方法。

背景技术

随着人类社会的发展，人们对自身位置的需求越来越多，社会也发展了许多定位方法和系统，例如：全球卫星定位系统能够为人类提供较为准确的室外定位，但是由于卫星信号受到遮挡且存在严重的多径效应，使得在室内卫星定位精度严重下降，甚至出现无法正常定位的情况。但是随着智能终端的广泛使用，移动通信快速发展，室内准确定位逐渐受到重视。尤其是作为定位技术的最后一米，室内定位技术有着广泛的应用前景。比如在一些大型商场、火灾现场，都需要借助室内定位技术快速找到出口，同时商家也可以借助于室内定位向用户介绍商品信息；在地下停车场，可以借助室内定位技术快速找到停车位；在医院，可以利用室内定位技术对需要看护的病人进行定位；还有在一些易发生矿难的矿井下更需要地下室内的准确定位。

目前针对室内定位主要有以下几种定位方法：

(1)WiFi技术

通过无线接入点(包括无线路由器)组成的无线局域网络(WLAN)，可以实现复杂环境中的定位、监测和追踪任务。它以网络节点(无线接入点)的位置信息为基础和前提，采用经验测试和信号传播模型相结合的方式，对已接入的移动设备进行位置定位，最高精确度大约在1米至20米之间。如果定位测算仅基于当前连接的WiFi接入点，而不是参照周边WiFi的信号强度合成图，则WiFi定位就很容易存在误差(例如：定位楼层错误)。另外，WiFi接入点通常都只能覆盖半径90米左右的区域，而且很容易受到其他信号的干扰，从而影响其精度，定位器的能耗也较高。

(2)RFID技术

RFID定位技术利用射频方式进行非接触式双向通信交换数据，实现移动设备识别和定位的目的。它可以在几毫秒内得到厘米级定位精度的信息，且传输范围大、成本较低；不过RFID不便于整合到移动设备之中而且作用距离短(一般最长为几十米)，使得RFID定位技术的适用范围受到局限。

(3)MEMS传感器自主定位技术

随着微机电系统(Micro-Electro-MechanicalSystems，MEMS)技术的快速发展，各种传感器，如：陀螺仪、加速度计、磁力计的应用越来越广泛，多被应用与移动终端里面。目前基于MEMS传感器的导航系统主要有两种：传统的惯性积分导航和行人航迹推算导航。传统的惯性积分导航，根据牛顿运动定律，利用测得三轴加速度计和三轴陀螺仪的数据积分计算出三维姿态、速度和位置。理论上这种方式定位精度会很高，但是实际应用中，由于惯性传感器自身的漂移、测量噪声、行人的抖动等因素影响，多次积分后会产生累积误差，几十秒之内误差可达几十米，甚至几百米。基于行人航迹推算(PedestrianDeadReckoning，PDR)方法利用行人的位移和航向进行位置解算，相比于传统的惯性积分导航更准确且更适用于行人导航，但是其定位依然受步长、航向、步态检测精度影响，随着时间的增加，依然存在累积误差。

(4)蓝牙技术

蓝牙通讯是一种短距离低功耗的无线传输技术，在室内安装适当的蓝牙局域网接入点后，将网络配置成基于多用户的基础网络连接模式，并保证蓝牙局域网接入点始终是这个微网络的主设备。这样通过检测信号强度就可以获得用户的位置信息。对于持有集成了蓝牙功能移动终端设备，只要设备的蓝牙功能开启，蓝牙室内定位系统就能够对其进行位置判断。但是蓝牙系统的缺点是覆盖范围小，定位距离短。

针对现有室内定位技术存在的缺陷和不足，有必要设计一种基于蓝牙与MEMS融合的室内定位系统及方法。

发明内容

本发明公开了一种基于蓝牙与MEMS融合的室内定位系统及方法，以实现室内大面积环境下的连续定位，且室内定位精度高。

本发明所述的基于蓝牙与MEMS融合的室内定位系统，1、一种基于蓝牙与MEMS融合的室内定位系统，其特征在于：包括蓝牙指纹定位模块(1)、MEMS定位模块(2)、扩展卡尔曼近似粒子滤波器模块(3)和定位结果显示模块(4)，蓝牙指纹定位模块、MEMS定位模块分别与扩展卡尔曼近似粒子滤波器模块连接，扩展卡尔曼近似粒子滤波器模块与定位结果显示模块连接；

所述蓝牙指纹定位模块，首先在离线阶段完成蓝牙指纹数据库的生成，即在室内实验区域布置多个锚点，接收室内环境中用户蓝牙模块所发来的RSS数据并储存，随后发送到服务器端，通过服务器端的预处理算法，形成室内实验区域的蓝牙指纹数据库；其次是在线定位阶段，在线定位阶段主要包括：待测点信号的实时采样、预处理采样数据，将处理后数据通过加权K近邻定位算法得出对采样数据的位置预测信息；

所述MEMS定位模块，用于行人的动态步长估计和航向角解算，当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据通过卡尔曼滤波算法解算出行人航向角信息，其中，所述九轴传感器数据包括三轴加速度计所检测数据、三轴磁力计所检测数据和三轴陀螺仪所检测数据；

所述扩展卡尔曼近似粒子滤波器模块，采用扩展卡尔曼近似粒子滤波算法融合所述位置预测信息和所述步长信息和航向角信息，得到最优的用户位置信息；

所述定位结果显示模块，包括智能手机客服端和web端，所述智能手机客服端和web端显示所述扩展卡尔曼近似粒子滤波器模块输出的最优的用户位置信息。

本发明所述的一种基于蓝牙与MEMS融合的室内定位方法，采用本发明所述的基于蓝牙与MEMS融合的室内定位系统，包括以下步骤：

步骤1、基于蓝牙系统的位置预测：首先在离线阶段完成蓝牙指纹数据库的生成，即在室内实验区域布置多个锚点，接收室内环境中用户蓝牙模块发来的RSS数据并储存，随后发送到服务器端，通过服务器端的预处理算法，形成室内实验区域的蓝牙指纹数据库；其次是在线定位阶段，在线定位阶段主要包括：待测点信号的实时采样、预处理采样数据，将处理后数据通过加权K近邻定位算法得出对采样数据的位置预测信息；

步骤2、行人动态步长估计和航向角解算：当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据通过卡尔曼滤波算法解算出行人航向角信息；

步骤3、设计蓝牙与MEMS融合定位的扩展卡尔曼近似粒子滤波器，选择状态变量和观测变量，建立状态方程和观测方程；

步骤4、将扩展卡尔曼滤波算法引入蓝牙与MEMS融合算法中，形成扩展卡尔曼近似粒子滤波算法，利用扩展卡尔曼滤波更新方法得到当前时刻的最新粒子集，在产生粒子中加入观测信息，将粒子滤波的观测值与预测值对比后用条件概率计算粒子权值，完成观测更新，经过重采样过程后，复制高权值粒子，舍弃低权值粒子，最后将所有粒子和其对应的权值进行累加，得到最优的用户位置信息；

步骤5、智能手机客服端和web端实时动态显示所述最优的用户位置信息。

进一步，所述状态变量、状态方程和观测变量、观测方程如下：

对于状态方程，选择状态向量为东位置、北位置、速度和航向作为状态变量，x＝[whuz]^T，选用蓝牙指纹匹配输出的位置预测信息、MEMS输出的速度和航向角信息作为观测变量，y＝[w^blueth^bluetu^memsz^mems]^T；

状态方程如下：

$x_t=f(x_{t-1},W_{t-1})=\left[\begin{array}{cccc}1& 0& \sin \left(z_{t-1}\right)& 0\\ 0& 1& \cos \left(z_{t-1}\right)& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\×\left[\begin{array}{c}{w}_{t-1}\\ h_{t-1}\\ u_{t-1}\\ z_{t-1}\end{array}\right]+W_{t-1};$

式中x_t＝[w_th_tu_tz_t]^T，w_t表示t时刻待估计的东位置，h_t表示t时刻待估计的北位置，u_t表示t时刻待估计的速度，z_t表示t时刻待估计的航向，w_t-1表示t-1时刻估计出的最优北位置，h_t-1表示t-1时刻估计出的最优东位置，u_t-1表示t-1时刻估计出的最优速度，z_t-1表示t-1时刻估计出的最优航向角，W_t-1是均值为0的高斯白噪声序列，且i,j＝1,2,...,m，m为需要估计的变量个数，Q(Q≥0)为m维过程噪声协方差矩阵：

观测方程如下：

$y_t=h(x_t,V_t)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\×\left[\begin{array}{c}{w}_t\\ h_t\\ u_t\\ z_t\end{array}\right]+V_t;$

式中 $y_t={\left[\begin{array}{cccc}{w}_t^{bluet}& h_t^{bluet}& u_t^{mems}& z_t^{mems}\end{array}\right]}^T,$ 表示t时刻蓝牙指纹匹配后定位输出的东位置，表示t时刻蓝牙指纹匹配后定位输出的北位置，表示t时刻MEMS输出的速度，表示t时刻MEMS输出的航向；w_t表示t时刻待估计的东位置，h_t表示t时刻待估计的北位置，u_t表示t时刻待估计的速度，z_t表示t时刻待估计的航向，V_t是均值为0的高斯白噪声序列，且E[V(i)V^T(j)]＝R(i,j)δ_ij,i,j＝1,2,...,n，n为观测变量的个数，R(R>0)为n维观测噪声协方差矩阵。

进一步，利用扩展卡尔曼滤波更新方法得到当前时刻的最新粒子集，在产生粒子中加入观测信息，具体如下：

假设初始状态的概率密度函数p(x₀)已知，基于p(x₀)随机产生N个初始粒子，记为由用户选择参数N作为在计算量和估计精度之间的权衡；在第t-1时刻对用扩展卡尔曼滤波算法对第t时刻每个粒子进行状态更新，主要分为两步：

a)时间更新：

$\begin{array}{c}{\hat{x}}_{t|t-1,i}^{-}=f\left(x_{t-1,i}^{+}\right)\\ P_{t|t-1,i}=F_{t,i}\left(t\right)P_{t-1,i}{F}_{t,i}^T\left(t\right)+Q_t\end{array};$

其中，f(·)为状态方程，式中为第i个粒子在第t-1时刻的最优状态值，为第i个粒子在第t时刻的状态一步预测值，P_t-1,i为第i个粒子在第t-1时刻的误差协方差矩阵，P_t|t-1,i为第i个粒子在第t时刻的误差协方差矩阵一步预测值，Q_t为第t时刻的过程噪声矩阵；

b)测量更新：

$K_{t,i}=P_{t|t-1,i}{H}_{t,i}^T{\[H_{t,i}{P}_{t|t-1,i}{H}_{t,i}^T+R_t\]}^{-1}$

${\hat{x}}_{t,i}^{+}={\hat{x}}_{t|t-1,i}^{-}+K_{t,i}\[y_t-h\left({\hat{x}}_{t|t-1,i}^{-}\right)\];$

P_t,i＝P_t|t-1,i-K_t,iH_t,iP_t|t-1,i

其中，h(·)为观测方程，式中K_t,i为第i个粒子在第t时刻的增益矩阵，y_t为第t时刻观测向量，P_t,i为第i个粒子在第t时刻的误差协方差矩阵，为第i个粒子在第t时刻的最优状态值，R_t为第t时刻的观测噪声矩阵。

进一步，所述估计行人步长的方法：

通过分析不同人的行走行为，当行走时，只有当脚落地时，膝盖才会弯曲；因而，当脚落地时，将腿视为固定长度的杠杆，由实验可得出计算步长的经验公式；

$step\_length=C\sqrt[4]{A_{max}-A_{min}};$

$C=\frac{d_{real}}{d_{estimated}};$

$A=\sqrt{a_x^2+a_y^2+a_z^2};$

式中A是步长估算参数，A_max、A_min分别为每一步加速度模值的最大值和最小值，其中a_x、a_y、a_z分别为加速度计的三轴x轴，y轴，z轴输出值，由步态判别条件可直接得到，即是比较最大值阈值和最小阈值C为校准系数，由参考轨迹距离的真实值d_real与估计值d_estimated的比率求得，得出步长后再根据记录时间间隔计算出速度；设行人速度用表示，时间为T，则速度计算公式为：

$u_t^{mems}=\frac{step\_length}{T}.$

本发明具有以下优点：利用蓝牙与MEMS的扩展卡尔曼近似粒子滤波器算法，克服MEMS器件长时间定位所带来的累积误差和蓝牙信号强度的不稳定性，充分利用了观测信息，提高了定位精度，实现了准确定位；同时，利用动态步长估计算法能够真实的反映行人步长信息。该系统还添加了定位结果显示模块，能够为不同用户提供定位需求。

附图说明

图1为本发明的原理框图；

图2为本发明所述的蓝牙与MEMS融合的室内定位方法整体算法框图；

图3为本发明所述的蓝牙与MEMS融合的室内定位系统图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

如图1所示，本发明提出的基于蓝牙与MEMS融合的室内定位系统包括蓝牙指纹定位模块1、MEMS定位模块2、扩展卡尔曼近似粒子滤波器模块3和定位结果显示模块4；蓝牙指纹定位模块、MEMS定位模块分别与扩展卡尔曼近似粒子滤波器模块连接，扩展卡尔曼近似粒子滤波器模块与定位结果显示模块连接。

所述蓝牙指纹定位模块，首先在离线阶段完成蓝牙指纹数据库的生成，即在室内实验区域布置多个锚点，接收室内环境中用户蓝牙模块所发来的RSS数据并储存，随后发送到服务器端，通过服务器端的预处理算法，形成室内实验区域的蓝牙指纹数据库；其次是在线定位阶段，在线定位阶段主要包括：待测点信号的实时采样、预处理采样数据，将处理后数据通过加权K近邻(WeightedK-NearestNeighborhood，WKNN)定位算法得出对采样数据的位置预测信息。

所述MEMS定位模块，用于行人的动态步长估计和航向角解算，当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据通过卡尔曼滤波算法解算出行人航向角信息，其中，所述九轴传感器数据包括三轴加速度计所检测数据、三轴磁力计所检测数据和三轴陀螺仪所检测数据；

所述扩展卡尔曼近似粒子滤波器模块，采用扩展卡尔曼近似粒子滤波算法融合所述位置预测信息和所述步长信息和航向角信息，得到最优的用户位置信息。具体为通过建立观测方程和状态方程，根据状态转移方程能对每一个粒子得到一个预测粒子，此时预测信息融合在粒子的分布中，然后利用扩展卡尔曼滤波更新方法将观测信息融合在每一个粒子的权重中，最后将观测值与预测值对比后用条件概率计算权值，通过重采样环节后，将不同权值的粒子进行累加，得到更新后的用户位置信息，即最优的用户位置信息。

所述定位结果显示模块，包括智能手机客服端和web端，所述智能手机客服端和web端显示所述扩展卡尔曼近似粒子滤波器模块输出的最优的用户位置信息。

如图2所示，本发明所述的一种基于蓝牙与MEMS融合的室内定位方法，采用本发明所述的基于蓝牙与MEMS融合的室内定位系统，包括以下步骤：

步骤1、基于蓝牙系统的位置预测：首先在离线阶段完成蓝牙指纹数据库的生成，即在室内实验区域布置多个锚点，接收室内环境中用户蓝牙模块发来的RSS数据并储存，随后发送到服务器端，通过服务器端的预处理算法，形成室内实验区域的蓝牙指纹数据库；其次是在线定位阶段，在线定位阶段主要包括：待测点信号的实时采样、预处理采样数据，将处理后数据通过加权K近邻定位算法得出对采样数据的位置预测信息。

步骤2、行人动态步长估计和航向角解算：当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据通过卡尔曼滤波算法解算出行人航向角信息。

步骤3、设计蓝牙与MEMS融合定位的扩展卡尔曼近似粒子滤波器，选择状态变量和观测变量，建立状态方程和观测方程。

步骤4、将扩展卡尔曼滤波算法引入蓝牙与MEMS融合算法中，形成扩展卡尔曼近似粒子滤波算法，利用扩展卡尔曼滤波更新方法得到当前时刻的最新粒子集，在产生粒子中加入观测信息，将粒子滤波的观测值与预测值对比后用条件概率计算粒子权值，完成观测更新，经过重采样过程后，复制高权值粒子，舍弃低权值粒子，最后将所有粒子和其对应的权值进行累加，得到最优的用户位置信息。

步骤5、智能手机客服端和web端实时动态显示所述最优的用户位置信息。

1)蓝牙指纹定位

蓝牙指纹定位包括离线建库和在线定位两个部分，先离线建库后在在线匹配实现定位。

离线阶段也称为采集阶段，该采集阶段事先在实验区域布置好多个锚点实现定位服务信号的覆盖，以及一个蓝牙模块，区域内的各个锚点接收到蓝牙模块的RSS数据并实时发送到服务器端储存，从而建立蓝牙模块在每个位置的RSS信号指纹库，本文以低功耗蓝牙4.0作为发射信号设备，以低功耗蓝牙的RSS信号值作为位置指纹定位参考值。用{Ψ_i,j(τ),τ＝1,…q,q>1}表示为在第j个参考点RP_j处，接收来自第i个无线接入点的q个信号强度时间序列值。完成所有RP的时间序列采集后，构建指纹数据库，用Ψ表示：

其中，i＝1,2,…,L，j＝1,2,…,N；L表示定位区域内的可接收信号的锚点总数；N表示所记录的参考点总数；q表示采集RSS测量值的总次数。

在线定位阶段：待测点信号的实时采样、预处理采样数据、精定位，通过接收到的RSS数据与服务器端数据库相匹配寻找最精确的定位位置。

a)预处理采样数据：对待测点信号进行预处理，是将采样的RSS数据通过均值滤波，均值滤波的原理如下式所示：

$\stackrel{\‾}{{\mathrm{RSS}}^i}=\frac{1}{N}\underset{n=1}{\overset{N}{\Σ}}{\mathrm{RSS}}_n^i;$

其中，为第i个锚点接收到的RSS值，n为在某测试点采样次数，为处理以后的结果。

b)精定位：将接收到的RSS数据与服务器端数据库相匹配寻找最精确的定位位置。此处采用加权K近邻算法，具体如下：

$(w_t^{bluet},h_t^{bluet})=\underset{i=1}{\overset{K}{\Σ}}\frac{\frac{1}{d_{i,j}+\mathrm{\ϵ}}}{\underset{j=1}{\overset{K}{\Σ}}\frac{1}{d_{i.j}+\mathrm{\ϵ}}}\×(x_i,y_i);$

式中ε为一个极小量，为测试点到第i参考点，第j个无线接入点的欧氏距离d_i,j的倒数，测试点到K个参考点欧氏距离倒数之和，(x_i,y_i)为定位匹配后的参考点坐标值。

2)MEMS定位数据处理

主要包括两部分，行人动态步长估计和航向角解算。当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据(即三轴加速度计所检测的数据、三轴磁力计所检测的数据和三轴陀螺仪所检测的数据)，通过卡尔曼滤波算法解算出行人航向角信息。

a)步长估计和速度计算如下：

通过分析不同人的行走行为，当行走时，只有当脚落地时，膝盖才会弯曲。因而，当脚落地时，腿可以被视为固定长度的杠杆。由实验可得出计算步长的经验公式：

$step\_length=C\sqrt[4]{A_{max}-A_{min}};$

$C=\frac{d_{real}}{d_{estimated}};$

$A=\sqrt{a_x^2+a_y^2+a_z^2};$

式中A是步长估算参数(不同的人有不同的参数)，A_max、A_min分别为每一步加速度模值的最大值和最小值，其中a_x、a_y、a_z分别为三轴加速度计的x轴，y轴，z轴的输出值，由步态判别条件可直接得到，即是比较最大值阈值和最小阈值C为校准系数，由参考轨迹距离的真实值d_real与估计值d_estimated的比率求得，得出步长后再根据记录时间间隔计算出速度；设速度用表示，时间为T，则速度计算公式为：

$u_t^{mems}=\frac{step\_length}{T}.$

b)航向角解算如下：

采用了基于四元素的扩展卡尔曼滤波算法，选取三轴加速度计、三轴磁力计在载体坐标系的输出值作为观测变量，将四元素作为状态变量。为了克服载体运动产生的线性加速度对加速度计输出数据的影响和当载体运动在如铁制品等磁场干扰环境中磁力计输出受影响，算法中提出将观测变量的变化通过误差检测，根据检测结果动态调整观测噪声协方差矩阵R的值。

i.状态方程的建立：

捷联姿态解算理论中，可以知道姿态角与四元素之间存在相互转换关系，所以只要对四素的四个参数做出估计即可解算出对应的姿态矩阵，从而得到载体的实时姿态信息。选取四元数[q₀q₁q₂q₃]的四个参数为状态变量，即：

X＝[q₀q₁q₂q₃]^T；

根据捷联惯性导航系统的四元素微分方程建立，可以将状态方程写为：

$\stackrel{\·}{X}=\frac{1}{2}\left[\begin{array}{cccc}0& -({\mathrm{\ω}}_x-w_x)& -({\mathrm{\ω}}_y-w_y)& -({\mathrm{\ω}}_z-w_z)\\ ({\mathrm{\ω}}_x-w_x)& 0& ({\mathrm{\ω}}_z-w_z)& -({\mathrm{\ω}}_y-w_y)\\ ({\mathrm{\ω}}_y-w_y)& -({\mathrm{\ω}}_z-w_z)& 0& ({\mathrm{\ω}}_x-w_x)\\ ({\mathrm{\ω}}_z-w_z)& ({\mathrm{\ω}}_y-w_y)& -({\mathrm{\ω}}_x-w_x)& 0\end{array}\right]X;$

其中，三轴陀螺仪的输出分别为ω_x、ω_x、ω_z，而w_x，w_y，w_z为三轴陀螺仪输出误差，此处只是考虑了白噪声。

将上面的状态方程线性近似离散化，取一阶展开式，忽略高阶小量可以得到：

X(t)＝Φ(t,t-1)X(t-1)+Γ(t-1)W(t-1)；

其中：

$\mathrm{\Φ}(t,t-1)=\left[\begin{array}{cccc}1& -{\mathrm{\ω}}_x\left(t\right)\frac{\mathrm{\Δ}T}{2}& -{\mathrm{\ω}}_y\left(t\right)\frac{\mathrm{\Δ}T}{2}& -{\mathrm{\ω}}_z\left(t\right)\frac{\mathrm{\Δ}T}{2}\\ {\mathrm{\ω}}_x\left(t\right)\frac{\mathrm{\Δ}T}{2}& 1& {\mathrm{\ω}}_z\left(t\right)\frac{\mathrm{\Δ}T}{2}& -{\mathrm{\ω}}_y\left(t\right)\frac{\mathrm{\Δ}T}{2}\\ {\mathrm{\ω}}_y\left(t\right)\frac{\mathrm{\Δ}T}{2}& -{\mathrm{\ω}}_z\left(t\right)\frac{\mathrm{\Δ}T}{2}& 1& {\mathrm{\ω}}_x\left(t\right)\frac{\mathrm{\Δ}T}{2}\\ {\mathrm{\ω}}_z\left(t\right)\frac{\mathrm{\Δ}T}{2}& {\mathrm{\ω}}_y\left(t\right)\frac{\mathrm{\Δ}T}{2}& -{\mathrm{\ω}}_x\left(t\right)\frac{\mathrm{\Δ}T}{2}& 1\end{array}\right];$

$\mathrm{\Γ}\left(t\right)=\left[\begin{array}{ccc}{q}_1\left(t\right)& q_2\left(t\right)& q_3\left(t\right)\\ -q_0\left(t\right)& q_3\left(t\right)& -q_2\left(t\right)\\ -q_3\left(t\right)& -q_0\left(t\right)& q_1\left(t\right)\\ q_2\left(t\right)& -q_1\left(t\right)& -q_0\left(t\right)\end{array}\right]\mathrm{\Δ}T;$

Φ(t,t-1)为状态方程，将W(t)近似为白噪声序列，其方差阵Q(t)非负定，第t时刻的过程噪声矩阵，其中I为3×3单位矩阵。Γ(t)为噪声驱动阵。此处将过程噪声矢量的初始值设为Q＝diag([111])，ΔT为更新时间。

ii.观测方程的建立：

为了进一步得到精确的姿态角，则需要利用三轴加速度计和三轴磁力计对重力向量和地磁向量的测量值作为观测量，修正由三轴陀螺仪更新的四元素，以得到最优的状态值输出。则观测量选取为：

Z＝[a_xa_ya_zm_xm_ym_z]^T；

其中a_x、a_y、a_z为三轴加速度计在载体坐标系中x轴，y轴，z轴的测量值，通过归一化处理后的数据，m_x、m_y、m_z为三轴磁力计在载体坐标系中x轴，y轴，z轴的测量值，通过归一化处理后的数据。

通过对三轴加速度计和三轴磁力计分析，建立传感器与四元素的对应关系，得到系统的观测方程，及其离散后的观测方程，即：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ m_x\\ m_y\\ m_z\end{array}\right]=h\left(X\right)+V=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ ({q_0}^2+q_3^2-q_1^2-q_2^2)\\ 2(q_1{q}_2+q_0{q}_3)b_y+2(q_1{q}_3-q_0{q}_2)b_z\\ (q_0^2-q_1^2+q_2^2-q_3^2)b_y+2(q_2{q}_3+q_0{q}_1)b_z\\ 2(q_2{q}_3-q_0{q}_1)b_y+(q_0^2-q_1^2-q_2^2+q_3^2)b_z\end{array}\right]+V;$

其中V为量测噪声。h(·)为观测方程，通过对非线性系统进行线性化，整理可以得到：

Z(t)＝H(t)X(t)+V(t)；

其中将观测噪声定义为V(k)，它是零均值的白噪声，其方差阵R(k)为正定，其中I为3×3的单位矩阵。此处的将观测噪声方差阵的初始值设为R＝diag([111111])，b_x、b_y、b_z为三轴磁力计在地理坐标系中的测量值。

iii.卡尔曼滤波算法过程：

通过步骤b得到状态方程和观测方程，根据传统扩展卡尔曼滤波算法可以迭代计算出每个时刻的最优状态变量，即是最优四元素，然后利用四元素与航向角的转换关系，得到每个时刻最优航向角。具体推算过程如下：

①状态变量的预测值：

${\hat{X}}_{t+1|t}=f_t({\hat{X}}_{t|t},t);$

②转移矩阵的线性化：

$f_x\left(t\right)\≈{\frac{\∂f(X_t,t)}{\∂X_t}|}_{X_t={\hat{X}}_{t|t}};$

$h_x\left(t\right)\≈{\frac{\∂h(X_{t+1},t+1)}{\∂X_{t+1}}|}_{X_{t+1}={\hat{X}}_{t+1|t}};$

③误差协方差矩阵预测：

P_t+1|t＝f_t(t)P_t|tf_t ^T(t)+Q_t；

④增益矩阵：

$K_{t+1}=P_{t+1|t}{h}_x^T\left(t+1\right){\[h_x\left(t+1\right)P_{t+1|t}{h}_x^T\left(t+1\right)+R_{t+1}\]}^{-1};$

⑤状态估计校正值：

${\hat{X}}_{t+1|t+1}={\hat{X}}_{t+1|t}+K_{t+1}\[Z_{t+1}-h\left({\hat{X}}_{t+1|t},t+1\right)\];$

⑥误差协方差估计校正：

$P_{t+1|t+1}=P_{t+1|t}-K_{t+1}{h}_x(k+1)f_t\left(k\right)P_{t+1|t}^T;$

⑦航向角求解：

$z_t^{mems}=arctan(-\frac{2(q_1{q}_2+q_0{q}_3)}{q_0^2+q_1^2-q_2^2-q_3^2});$

其中，f_t(·)为状态方程，式中为第t时刻的最优状态值，为第t+1时刻的状态一步预测值，P_t|t为第t时刻的误差协方差矩阵，P_t+1|t为第t+1时刻的误差协方差矩阵一步预测值，Q_t为第t时刻的过程噪声矩阵其中，h_x(·)为观测方程，式中K_t+1为第t+1时刻的增益矩阵，Z_t+1为第t+1时刻观测向量，P_t+1|t+1为第t+1时刻的误差协方差矩阵，为第t+1时刻的最优状态值，R_t+1为第t+1时刻的观测噪声矩阵。

iv.自适应调整R：

为了使修正值更加准确，使用频率抽样对观测噪声矩阵R进行调整，把抽样值的平均值作为最后的R值，最后得到航向角。通过对观测量数据影响因素的分析，改进扩展卡尔曼滤波算法。本文中先对三轴加速度计和三轴磁力计的输出数据进行分析，确定各传感器是否受到上述情况的影响，再根据分析的结果自适应调整观测噪声协方差矩阵。

${\mathrm{\σ}}_a^2=k_a\left(\right|\left|\right|a_{k+1}\left|\right|-\left|\right|g\left|\right|\left|\right);$

${\mathrm{\σ}}_m^2=k_m\left(\right|\left|\right|m_{k+1}\left|\right|-\left|\right|m\left|\right|\left|\right);$

其中k_a、k_m为设定的加速度计和磁力计的误差权重因子，具体取值是通过实验得到，g、m分别为重力加速度和当地磁场强度。可以看到当载体的线性加速度越大，观测噪声协方差越大，当载体受到的磁干扰越大，观测噪声协方差越大。

3)扩展卡尔曼近似粒子滤波器模块

设计蓝牙与MEMS融合定位的粒子滤波器，选择状态变量和观测变量，建立状态方程和观测方程，将扩展卡尔曼滤波算法引入蓝牙与MEMS融合算法中，形成扩展卡尔曼近似粒子滤波算法，利用扩展卡尔曼滤波更新方法得到当前时刻的最新粒子集，可以在产生粒子中加入观测信息，使所得的粒子更加真实有效，从而提高系统定位精度。

a)状态变量、状态方程和观测变量、观测方程设定如下：

对于状态方程，选择状态向量为东位置、北位置、速度和航向作为状态量，x＝[whuz]^T，选用蓝牙指纹匹配输出的位置预测信息、MEMS输出的速度和航向角信息作为观测量，y＝[w^blueth^bluetu^memsz^mems]^T；

状态方程如下：

$x_t=f(x_{t-1},W_{t-1})=\left[\begin{array}{cccc}1& 0& \sin \left(z_{t-1}\right)& 0\\ 0& 1& \cos \left(z_{t-1}\right)& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\×\left[\begin{array}{c}{w}_{t-1}\\ h_{t-1}\\ u_{t-1}\\ z_{t-1}\end{array}\right]+W_{t-1};$

式中x_t＝[w_th_tu_tz_t]^T，w_t表示t时刻待估计的东位置，h_t表示t时刻待估计的北位置，u_t表示t时刻待估计的速度，z_t表示t时刻待估计的航向，w_t-1表示t-1时刻估计出的最优北位置，h_t-1表示t-1时刻估计出的最优东位置，u_t-1表示t-1时刻估计出的最优速度，z_t-1表示t-1时刻估计出的最优航向角，W_t-1是均值为0的高斯白噪声序列，且i,j＝1,2,...,m，m为需要估计的变量个数，Q(Q≥0)为m维过程噪声协方差矩阵。

观测方程如下：

$y_t=h(x_t,V_t)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\×\left[\begin{array}{c}{w}_t\\ h_t\\ u_t\\ z_t\end{array}\right]+V_t;$

式中 $y_t={\left[\begin{array}{cccc}{w}_t^{bluet}& h_t^{bluet}& u_t^{mems}& z_t^{mems}\end{array}\right]}^T,$ 表示t时刻蓝牙指纹匹配后定位输出的东位置，表示t时刻蓝牙指纹匹配后定位输出的北位置，表示t时刻MEMS输出的速度，表示t时刻MEMS输出的航向；w_t表示t时刻待估计的东位置，h_t表示t时刻待估计的北位置，u_t表示t时刻待估计的速度，z_t表示t时刻待估计的航向，V_t是均值为0的高斯白噪声序列，且E[V(i)V^T(j)]＝R(i,j)δ_ij,i,j＝1,2,...,n，n为观测变量个数，R(R>0)为n维观测噪声协方差矩阵。

假设初始状态的概率密度函数p(x₀)已知，基于p(x₀)随机产生N个初始粒子，记为由用户选择参数N作为在计算量和估计精度之间权衡。

对t＝1,2,...,执行如下步骤：

i.利用已知的状态方程和过程噪声的概率密度函数，执行时间更新，获得先验粒子

$x_{t,i}^{-}=f_{t-1}(x_{t-1,i}^{+},w_{t-1}^i),(i=1,...,N)$

其中，每一个噪声向量是基于已知的w_t-1的概率密度函数随机产生的。

ii.更新粒子状态信息：

在第t-1时刻对(i＝1,...,N)用扩展卡尔曼滤波算法对第t时刻每个粒子进行状态更新，主要分为两步：

时间更新：

$\begin{array}{c}{\hat{x}}_{t|t-1,i}^{-}=f\left(x_{t-1,i}^{+}\right)\\ P_{t|t-1,i}=F_{t,i}\left(t\right)P_{t-1,i}{F}_{t,i}^T\left(t\right)+Q_t\end{array};$

其中，f(·)为状态方程，式中为第i个粒子在第t-1时刻的最优状态值，为第i个粒子在第t时刻的状态一步预测值，P_t-1,i为第i个粒子在第t-1时刻的误差协方差矩阵，P_t|t-1,i为第i个粒子在第t时刻的误差协方差矩阵一步预测值，Q_t为第t时刻的过程噪声矩阵。

测量更新：

$K_{t,i}=P_{t|t-1,i}{H}_{t,i}^T{\[H_{t,i}{P}_{t|t-1,i}{H}_{t,i}^T+R_t\]}^{-1}$

${\hat{x}}_{t,i}^{+}={\hat{x}}_{t|t-1,i}^{-}+K_{t,i}\[y_k-h\left({\hat{x}}_{t|t-1,i}^{-}\right)\];$

P_t+1,i＝P_t|t-1,i-K_t,iH_t,iP_t|t-1,i

其中，h(·)为观测方程，式中K_t,i为第i个粒子在第t时刻的增益矩阵，y_t为第t时刻观测向量，P_t,i为第i个粒子在第t时刻的误差协方差矩阵，为第i个粒子在第t时刻的最优状态值，R_t为第t时刻的观测噪声矩阵。

自适应调整R值：

R(R>0)为n维观测噪声协方差矩阵，在扩展卡尔曼滤波近似粒子滤波算法中，观测值为蓝牙系统得到的行人位置信息以及MEMS系统计算得到的步长信息和航向角信息。由于蓝牙无线信号存在波动性，而且传感器数据也存在噪声和磁干扰，那么观测值的可信度会降低，需要适当的调整观测噪声协方差矩阵R值。此处，是将R值设定为观测值数据方差值的M倍，M＝100，可以根据系统中实际数据特征适当取值。

iii.以量测值y_t为条件计算每个粒子的似然概率密度函数q_i。这个可以通过非线性量测方程和量测噪声的概率密度函数估计来获得：

$\begin{array}{c}{q}_i=p(y_t/x_{t,i}^{-})\\ =P\[(y_t-y^{*})/(x_t={\hat{x}}_{t|t-1,i}^{-})\]\\ =P\[v_t=y^{*}-h\left({\hat{x}}_{t|t-1,i}^{-}\right)\]\\ ~\frac{1}{{\left(2\mathrm{\π}\right)}^{m/2}{\left|R\right|}^{1/2}}\exp \frac{-{\[y^{*}-h\left({\hat{x}}_{t|t-1,i}^{-}\right)\]}^T{R}^{-1}\[y^{*}-h\left({\hat{x}}_{t|t-1,i}^{-}\right)\]}{2}\end{array};$

式中，m表示量测方程的维数，y^*表示量测值，R为量测噪声的协方差矩阵，“～”符号表示该概率密度函数并不真正等于右边的表达式，而是与右边的式子成比例。

利用下式将上面获得的似然概率密度归一化如下：

$q_i=\frac{q_i}{{\mathrm{\Σ}}_{j=1}^N{q}_j};$

现在所有的似然概率密度的和等于1。

iv.基于似然概率密度q_i再随机产生一组后验粒子这称做重采样。重采样有多种采样方法，一种最直接的方法如下：

●在[0,1]的均匀分布上产生一个随机数r；

●累加似然函数q_i，直到累加的总和大于r。也就是但是那么新的粒子被设置为等于旧的粒子

v.现在有一组服从概率密度函数p(x_t/y_t)分布的粒子计算这个概率密度函数的均值即为我们的估计值。

4)定位结果显示模块

如图3所示，服务器端获取当前时刻的所有锚点接收到的用户蓝牙的RSS值以及九轴传感器数据，根据MEMS数据计算当前时刻的航向并解算速度信息，在服务器端采用扩展卡尔曼近似粒子滤波算法融合蓝牙解算得到的位置预测信息和MEMS解算信息(步长和航向角)，得到最优的用户位置信息。位置解算完毕后将最优的位置信息传给智能手机客户端进行显示，web端调用位置解算服务器存储的已解算位置数据库进行实时动态显示该最优的位置信息。

最后应该说明的是：以上所述仅为本发明的优选实施例，并不限制本发明，尽管利用实施例对本发明进行了详细的说明，但对于本领域的技术人员来说，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中的部分技术进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于蓝牙与MEMS融合的室内定位系统，其特征在于：包括蓝牙指纹定位模块(1)、MEMS定位模块(2)、扩展卡尔曼近似粒子滤波器模块(3)和定位结果显示模块(4)，蓝牙指纹定位模块、MEMS定位模块分别与扩展卡尔曼近似粒子滤波器模块连接，扩展卡尔曼近似粒子滤波器模块与定位结果显示模块连接；

所述蓝牙指纹定位模块，首先在离线阶段完成蓝牙指纹数据库的生成，即在室内实验区域布置多个锚点，接收室内环境中用户蓝牙模块所发来的RSS数据并储存，随后发送到服务器端，通过服务器端的预处理算法，形成室内实验区域的蓝牙指纹数据库；其次是在线定位阶段，在线定位阶段主要包括：待测点信号的实时采样、预处理采样数据，将处理后数据通过加权K近邻定位算法得出对采样数据的位置预测信息；

所述MEMS定位模块，用于行人的动态步长估计和航向角解算，当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据通过卡尔曼滤波算法解算出行人航向角信息，其中，所述九轴传感器数据包括三轴加速度计所检测数据、三轴磁力计所检测数据和三轴陀螺仪所检测数据；

所述扩展卡尔曼近似粒子滤波器模块，采用扩展卡尔曼近似粒子滤波算法融合所述位置预测信息和所述步长信息和航向角信息，得到最优的用户位置信息；

所述定位结果显示模块，包括智能手机客服端和web端，所述智能手机客服端和web端显示所述扩展卡尔曼近似粒子滤波器模块输出的最优的用户位置信息。

2.一种基于蓝牙与MEMS融合的室内定位方法，其特征在于：采用如权利要求1所述的基于蓝牙与MEMS融合的室内定位系统，包括以下步骤：

步骤1、基于蓝牙系统的位置预测：首先在离线阶段完成蓝牙指纹数据库的生成，即在室内实验区域布置多个锚点，接收室内环境中用户蓝牙模块发来的RSS数据并储存，随后发送到服务器端，通过服务器端的预处理算法，形成室内实验区域的蓝牙指纹数据库；其次是在线定位阶段，在线定位阶段主要包括：待测点信号的实时采样、预处理采样数据，将处理后数据通过加权K近邻定位算法得出对采样数据的位置预测信息；

步骤2、行人动态步长估计和航向角解算：当行人手持MEMS传感器器件时，MEMS定位模块中的三轴加速度计能捕捉到行人的步态信息，再结合经验步长模型来动态估计行人步长信息，同时将MEMS定位模块中的九轴传感器数据通过卡尔曼滤波算法解算出行人航向角信息；

步骤3、设计蓝牙与MEMS融合定位的扩展卡尔曼近似粒子滤波器，选择状态变量和观测变量，建立状态方程和观测方程；

步骤4、将扩展卡尔曼滤波算法引入蓝牙与MEMS融合算法中，形成扩展卡尔曼近似粒子滤波算法，利用扩展卡尔曼滤波更新方法得到当前时刻的最新粒子集，在产生粒子中加入观测信息，将粒子滤波的观测值与预测值对比后用条件概率计算粒子权值，完成观测更新，经过重采样过程后，复制高权值粒子，舍弃低权值粒子，最后将所有粒子和其对应的权值进行累加，得到最优的用户位置信息；

步骤5、智能手机客服端和web端实时动态显示所述最优的用户位置信息。

3.根据权利要求2所述的基于蓝牙与MEMS融合的室内定位方法，其特征在于，所述状态变量、状态方程和观测变量、观测方程如下：

对于状态方程，选择状态向量为东位置、北位置、速度和航向作为状态变量，x＝[whuz]^T，选用蓝牙指纹匹配输出的位置预测信息、MEMS输出的速度和航向角信息作为观测变量，y＝[w^blueth^bluetu^memsz^mems]^T；

状态方程如下：

$x_t=f(x_{t-1},W_{t-1})=\left[\begin{array}{cccc}1& 0& \sin \left(z_{t-1}\right)& 0\\ 0& 1& \cos \left(z_{t-1}\right)& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\×\left[\begin{array}{c}{w}_{t-1}\\ h_{t-1}\\ u_{t-1}\\ z_{t-1}\end{array}\right]+W_{t-1};$

式中x_t＝[w_th_tu_tz_t]^T，w_t表示t时刻待估计的东位置，h_t表示t时刻待估计的北位置，u_t表示t时刻待估计的速度，z_t表示t时刻待估计的航向，w_t-1表示t-1时刻估计出的最优北位置，h_t-1表示t-1时刻估计出的最优东位置，u_t-1表示t-1时刻估计出的最优速度，z_t-1表示t-1时刻估计出的最优航向角，W_t-1是均值为0的高斯白噪声序列，且i,j＝1,2,...,m，m为需要估计的变量个数，Q(Q≥0)为m维过程噪声协方差矩阵：

观测方程如下：

$y_t=h(x_t,V_t)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\×\left[\begin{array}{c}{w}_t\\ h_t\\ u_t\\ z_t\end{array}\right]+V_t;$

式中表示t时刻蓝牙指纹匹配后定位输出的东位置，表示t时刻蓝牙指纹匹配后定位输出的北位置，表示t时刻MEMS输出的速度，表示t时刻MEMS输出的航向；w_t表示t时刻待估计的东位置，h_t表示t时刻待估计的北位置，u_t表示t时刻待估计的速度，z_t表示t时刻待估计的航向，V_t是均值为0的高斯白噪声序列，且E[V(i)V^T(j)]＝R(i,j)δ_ij,i,j＝1,2,...,n，n为观测变量的个数，R(R>0)为n维观测噪声协方差矩阵。

4.根据权利要求2或3中所述的基于蓝牙与MEMS融合的室内定位方法，其特征在于，利用扩展卡尔曼滤波更新方法得到当前时刻的最新粒子集，在产生粒子中加入观测信息，具体如下：

假设初始状态的概率密度函数p(x₀)已知，基于p(x₀)随机产生N个初始粒子，记为由用户选择参数N作为在计算量和估计精度之间的权衡；在第t-1时刻对用扩展卡尔曼滤波算法对第t时刻每个粒子进行状态更新，主要分为两步：

a)时间更新：

$\begin{array}{c}{\hat{x}}_{t|t-1,i}^{-}=f\left(x_{t-1,i}^{+}\right)\\ P_{t|t-1,i}=F_{t,i}\left(t\right)P_{t-1,i}{F}_{t,i}^T\left(t\right)+Q_t\end{array};$

其中，f(·)为状态方程，式中为第i个粒子在第t-1时刻的最优状态值，为第i个粒子在第t时刻的状态一步预测值，P_t-1,i为第i个粒子在第t-1时刻的误差协方差矩阵，P_t|t-1,i为第i个粒子在第t时刻的误差协方差矩阵一步预测值，Q_t为第t时刻的过程噪声矩阵；

b)测量更新：

$K_{t,i}=P_{t|t-1,i}{H}_{t,i}^T{\[H_{t,i}{P}_{t|t-1,i}{H}_{t,i}^T+R_t\]}^{-1}$

${\hat{x}}_{t,i}^{+}={\hat{x}}_{t|t-1,i}^{-}+K_{t,i}\[y_t-h\left({\hat{x}}_{t|t-1,i}^{-}\right)\];$

$P_{t,i}=P_{t|t-1,i}-K_{t,i}{H}_{t,i}{P}_{t|t-1,i}$

其中，h(·)为观测方程，式中K_t,i为第i个粒子在第t时刻的增益矩阵，y_t为第t时刻观测向量，P_t,i为第i个粒子在第t时刻的误差协方差矩阵，为第i个粒子在第t时刻的最优状态值，R_t为第t时刻的观测噪声矩阵。

5.根据权利要求2或3所述的基于蓝牙与MEMS融合的室内定位方法，其特征在于，所述估计行人步长的方法：

通过分析不同人的行走行为，当行走时，只有当脚落地时，膝盖才会弯曲；因而，当脚落地时，将腿视为固定长度的杠杆，由实验可得出计算步长的经验公式；

$step\_length=C\sqrt[4]{A_{max}-A_{min}};$

$C=\frac{d_{real}}{d_{estimated}};$

$A=\sqrt{a_x^2+a_y^2+a_z^2};$

式中A是步长估算参数，A_max、A_min分别为每一步加速度模值的最大值和最小值，其中a_x、a_y、a_z分别为加速度计的三轴x轴，y轴，z轴输出值，由步态判别条件可直接得到，即是比较最大值阈值和最小阈值C为校准系数，由参考轨迹距离的真实值d_real与估计值d_estimated的比率求得，得出步长后再根据记录时间间隔计算出速度；设行人速度用表示，时间为T，则速度计算公式为：

$u_t^{mems}=\frac{step\_length}{T}.$