基于PDR 和蓝牙加权定位的卡尔曼滤波算法

何梦园，刘小瑜，刘正熙，熊运余，袁钰涵

（四川大学计算机学院，成都610065）

0 引言

随着基位置服务（LBS）[1]的兴起，对于室内定位的需求就越来越强烈，例如商场、医院、停车场等。各学者对各种无线信号定位进行了各方面的研究，包括无线局域网（WLAN）和无线传感器网络。例如红外线[2]、超宽带（UWB）[3]、ZigBee[4]、蓝牙[5]、Wi-Fi[6]等室内定位方法。

但是由于室内环境复杂，单一信号定位精度较低，所以就需要将多种定位技术结合使用。文献[7]将GPS、RFID、Wi-Fi 和计步器 4 种定位融合，提高了定位精度与稳定性；文献[8]利用PDR 与UWB 定位技术结合，采用约束滤波器，使得定位精度达到亚米级；文献[9]采用PDR 与Wi-Fi 的UKF 算法，对室内车辆定位取得较好的效果；在以上方法中，往往需要额外的硬件部署，系统实现复杂。而如今的智能手机性能越来越好，其提供了一系列传感器以及可用于接受信号的Wi-Fi 模块、蓝牙模块等，可直接用于室内定位。文献[10]利用手机中的蓝牙、Wi-Fi 和惯性导航结果进行融合滤波，可得到较好的定位结果。基于移动端惯性传感器的PDR 算法具有短时间内定位精度高，但存在累计误差，而蓝牙定位单点定位误差大但没有误差积累，因此将这两种定位手段结合起来，构成PDR-ibeacon组合定位系统，既能达到较好的定位精度又能提高定位结果的可靠性。

本文提出一种基于位置加权的PDR-ibeacon 卡尔曼融合滤波室内定位方法，并使用智能手机实现。

1 蓝牙指纹定位

1.1 蓝牙指纹采集

蓝牙室内定位技术可分为两种：第一种是基于信号强度的传播模型测定法，通过接受信号强度与信号衰减模型来计算待定位点与参考点的距离，但是由于室内环境复杂，存在多径效应，鲁棒性较差；第二种是通过建立蓝牙指纹库来进行匹配，但该方法需要采集指纹数据，数据库维护压力较大。目前，室内蓝牙定位为了较高精度的定位，主要基于位置指纹实现。

本文采用位置指纹的方式实现，但是为了减少施工成本，不用常用的单点指纹库收集再进行处理的方式采集指纹。为减少采集指纹时间，使用时间-位置匹配的方式采集指纹。同时为减少只采集一次的指纹的随机性的影响，设置置信点指纹，即当收到某个蓝牙标签的信号值大于某个值时，我们判断其在蓝牙标签附近，置信点由在蓝牙标签下收到的该蓝牙信号强度决定，经测试在蓝牙标签1 米范围内的1 分钟内收到的60 次蓝牙信号值一般在-60db～-70db，根据其均值确定置信点指纹数据，例如P={( )x,y ,“rssi:-60db,device:1”}代表在蓝牙标签ID 为1 处的置信指纹。

图1 1米范围内蓝牙信号强度分布图

采集指纹时，采集示意图如下图所示，手持手机匀速行走，根据长度S 和时间T 计算出其速度V=S/T ，根据V 和起点估算出每次收到蓝牙信号时所在位置经纬度，形成一条蓝牙指纹l1={（x,y）,{“rssi:signal intensity,device:id”,…,“rssi:-mdb,device:id”}}，指纹库由多条指纹组成L={l1,l2,…,ln},其中n 为采集的指纹个数，即收到的蓝牙信号次数。

图2 指纹采集示意图

本文使用iPhone 智能手机进行数据采集，因为在Android 平台的智能手机与iPhone 智能手机接受ibeacon 蓝牙信号存在差异，在相同时间下，iPhone 在60 秒内收到的蓝牙信号稳定在60 次左右，而Android 平台手机的结果则较为不稳定，实验结果如图2 所示，对13个蓝牙标签分别进行实验，Android 手机60 秒内最多接受到43 次蓝牙信号，将会导致蓝牙标签分布不均。（其中iPhone 手机型号为iPhone 6s，Android 手机型号为一加3T。）

图3 不同平台下的蓝牙接收频率

1.2 蓝牙指纹匹配算法

（1）WKNN 匹配算法

离线采集完指纹后，定位时需要将收到的蓝牙信号与指纹库进行匹配，才能得到定位结果。最常用的匹配算法为加权 K 最近邻法（Weighted K-Nearest Neighbor，WKNN），定位时首先扫描蓝牙信号，得到信号 待 定 位 点 信 号 强 度 向 量 Signal_V，Signal_V=(v1,v2,…,vj,…,vn)，其中 vj(1 ≤j ≤n)代表收到的第j 个蓝牙标签的信号值，n 为收到的蓝牙标签总个数。然后根据收到的蓝牙信号强度与指纹库中的蓝牙信号强度进行匹配，计算其相似度SI，一般使用欧氏距离计算。在实际环境中，由于范围较大，蓝牙指纹点较多，为提高匹配效率，在计算欧氏距离之前，首先检索本条指纹是否含有Signal_V 中的蓝牙标签信息，如果没有，则不计算。按照欧氏距离计算的公式如下：

式中，k 表示第k 个指纹点，lki表示第k 个指纹点中的第i 个指纹。取前SI 值最小的前k 个点对应的坐标{P1，P2，…，Pi，…，Pk}，使用前 k 个 SI 进行加权，加权公式如下：

最终的加权定位结果计算方式如下所示：

（2）SVM（支持向量机）匹配算法

在实验中发现WKNN 会有跳变加大的情况发生，且较为频繁而且定位会有不在道路上的情况发生。所以为减少此种情况的发生，本文使用基于SVM 的匹配算法，其分为两步：①模型训练；②在线分类。我们可以将定位问题转化为分类问题，即将指纹点所在坐标的二维空间作为类别标记，训练时找出区分不同指纹点的超平面，在线定位时根据蓝牙信号值进行分类得到一个作为类别的坐标点。基于指纹点存在线性不可分的情况，所以引入核函数，使之高维可分，即：

式中K( xi,xj)=xTy 为引入的核函数，C 为惩罚因子表示对错误分类的惩罚程度，然后根据Smo 算法求得最优解 a′=(a1′,a2′,a3′,…,an′)T,并计算：

最终，求解出决策函数：

2 基于智能手机的PDR（行人航位推算）

一般情况下认为人在很短的时间内，做直线运动，当得知人的行走方向、距离以及起始点时就可推算出下一时刻的位置，我们假设起点为（x1,y1），距离为S，方向角为θ，那么推算下一个点（x2,y2）的公式为：

原理如图4 所示：

其后的每一步定位信息都可用式（3）推算得到，其中两个关键参数S 和θ 需要从传感器中获取，S 代表步长，θ 代表方向角，其中方向角可由智能手机传感器直接获得一个指北方向角，而步长可由典型的步频-步长模型解算，步频可由手机传感器的计步器接口给出，步长可根据以下公式计算：

图4 PDR原理示意图[11]

其中a 和b 为固定常量，本文使用文献[12]中的值，f 为步频，可由手机中计步器得到步频。

3 多信号位置加权的卡尔曼滤波算法

卡尔曼滤波的主要思想为获取测量值对估计值进行修正，而基于PDR 的融合滤波一般把步长推算作为估计值，测量值往往是蓝牙定位以及其他信号源定位，而在线定位中，无法在每次有步进时都收到蓝牙定位信息，所以在卡尔曼滤波中无法使用关于步长的状态方程，本文使用速度模型，为了较少计算量，本文采用最简单的线性卡尔曼滤波。多信号位置加权的卡尔曼滤波算法流程图如图5 所示，算法流程分为3 步：

Step1：获取多信号加权测量值，根据每一时刻的信号源定位不同进行不同的加权处理。

Step2：根据设计的状态方程的测量方程进行滤波融合。

Step3：为使输出点迹更为平滑，加入平滑处理，得到更为流畅的轨迹。

在Step1 中，在获取所需测量值时，由于有多种定位信号输出，本文提出多信号位置的加权处理，根据实际效果，设定各加权系数如图5 所示，因为在短时间内的PDR 具有更高的精度，所以其权重较蓝牙定位和置信点都大。

图5 多信号位置加权的卡尔曼滤波算法流程图

Step2 中，建立相关卡尔曼滤波方程，k-1 时刻到k时刻的状态方程和测量方程如下：

其中A 为状态转移矩阵，B 和uk分别为系统输入变量，ωk为过程噪声，H 为测量矩阵，vk为测量噪声。卡尔曼滤波分为两部分，预测与更新，预测阶段使用上一时刻的值对当前时刻作出估计，而更新阶段，利用测量值对预测值进行校正，以获得一个更接近真实值的新估计值。

预测：

更新：

其中zk为测量值，K 为卡尔曼增益，每次滤波时随着计算更新，R 为测量噪声的协方差矩阵，xk为最终卡尔曼估计值，Pk为卡尔曼估计误差的协方差矩阵。根据实际情况建模如下：

(xk,yk)为k 时刻的位置坐标，(vxk,vyk)为k 时刻x和y 方向的速度，T 为时间间隔。测量方程为：

(xk,yk)为通过多信号位置加权得到的定位位置。

本文设初始协方差矩阵P=0.1*I ，I 为单位矩阵；由于没有系统输入，所以忽略系统输入。

过程噪声的协方差矩阵：

测量噪声的协方差矩阵：

Step3 中，因为滤波输出的点会有较多锯齿状以及回拉的点，如图6（a）所示，所以在滤波融合后进行平滑处理，平滑算法示意图如图6（b）所示，我们根据前2 个时刻的运动状态来进行平滑，假设在3 个运动时刻里，运动状态不变，那么根据k-2 时刻和k-1 时刻可推算出k 时刻的位置为(x′k,y′

k)，而k 时刻的卡尔曼滤波结果为，将其进行加权得到平滑结果，计算公式如下：

其中w1+w2=1，权重系数应根据实际情况选取，本文将其都设为0.5。

图6 平滑处理示意图

4 实验结果分析

实验测试环境位于成都市高新区理想中心14 楼，平面图如图7 所示，主要在过道进行测试，地图面积约为1500m2，蓝牙标签部署如图上蓝点所示，总计部署13 个，指纹点共计 170 个。

图7 定位区域平面图

4.1 WWKKNNNN与SSVVMM定位结果对比分析

在静止状态下，随机挑选13 个点作为测试点，分别使用基于距离的WKNN 与SVM 进行定位，得出定位结果，使用平均误差、累计误差分布和最大误差分布作为评价指标，其平均误差如表1 所示。

表1 不同匹配算法平均误差

从平均误差可以看出SVM 较WKNN 具有更高的定位精度，累计误差分布结果和点位最大误差统计图8所示，可以看出SVM 收敛较WKNN 快，且最大误差由20 米降低到14 米，较WKNN 具有更稳定的定位效果。

图8 误差累计分布图

4.2 卡尔曼融合滤波定位结果

在测试环境中，滤波效果如图9 所示，红色的线为滤波融合后的效果，可以看出其与真实路线较为符合，拟合较好，减少了PDR 的累计误差以及蓝牙定位的误差影响。

在此行走过程中，根据速度估计其真实位置，各种定位平均误差表2 所示，SVM_PDR_KF 融合滤波方法将行走状态下的平均误差降低到1.8 米，且最大误差仅3.6 米，较SVM 和PDR 分别提高了81.98%和86.11%。

5 结语

在室内环境中，通过采集的蓝牙指纹信息与SVM算法辅助PDR 进行了定位测验。比较了WKNN 与SVM 匹配算法定位效果，结果表明，SVM 具有更高的精度与更好的稳定性。使用卡尔曼滤波进行信号融合，基于多信号的位置加权得到测量值，该方法定位结果优于蓝牙和PDR 单独定位的效果。此方法计算量小，实现简单，可实用于智能手机定位。