PDR+BLE融合的室内实时定位方法

徐 浩,吴 雪,李春光,秘金钟,陈 冲,李得海

(1.济南市房产测绘研究院,山东 济南 250001; 2.山东皓宇资讯有限公司,山东 济南 250101;3.中国测绘科学研究院,北京 100830; 4.中国电子科技集团第五十四研究所,河北 石家庄 050081)

近年来,随着智能手机的广泛普及和无线电通信技术的快速发展,基于位置的服务(location based service,LBS)日益成为大众用户的迫切需求[1]。GNSS卫星由于受到信号遮挡,难以获得可用的室内定位结果[2],于是逐渐出现了许多室内定位技术。大致可分为:基于射频信号的定位技术、基于传感器的定位技术和多源融合定位技术[3]。基于射频信号定位技术有蓝牙、Wi-Fi、UWB、射频识别和伪卫星等。基于传感器的定位技术有行人航迹推算(pedestrian dead reckoning, PDR)、地磁定位、视觉定位和音频定位等。而多源融合定位技术就是将两种或两种以上不同的定位手段进行融合的定位技术,目前常见的融合方法主要有卡尔曼滤波(Kalman filter, KF)[4]、扩展KF (EKF)、UKF和粒子滤波等。当下,开发出一种稳定、高精度的新型室内定位技术已成为众多学者与企业的共识。

针对PDR和低功耗蓝牙(bluetooth low energy, BLE)定位各自存在的问题及融合过程中的难题,已有众多专家学者提出了尝试性的解决方案。文献[5]融合加速度计、磁力计和陀螺仪,利用载体坐标系转换到导航坐标系下的水平角速度分量建立一个优化问题,求解航向角,但有时坐标系转换不准确。文献[6]提出了将惯性定位与磁场进行双重滤波融合的策略,试验结果精度大为提高,但是硬件成本和计算复杂度太高。文献[7]使用粒子过滤器将Wi-Fi指纹、PDR和地图匹配相融合,提高了定位的准确性和稳健性。文献[8]将PDR和BLE定位结果进行粒子滤波融合,并使用地图匹配进行辅助定位,提高了算法解算性能,使定位结果更具稳健性。

以上解决方案虽然实现了基本的融合定位,但对于融合过程中射频信号定位方式粗差大的问题未能得到有效的解决。因此本文针对PDR+BLE融合定位过程中PDR累积误差难以消除,以及BLE定位粗差大的问题,提出BLE信号连续最大值临近校正PDR算法和基于EKF的自适应抗差PDR+BLE融合定位算法,以期达到限制PDR累积误差和BLE定位粗差的效果,进而提高融合定位精度和稳健性。

1 PDR与BLE定位方法

1.1 PDR定位

PDR算法就是行人在上一步的已知位置出发,根据当前的航向、步长推算出行人的当前位置,然后再从当前时刻的位置出发推算出行人下一步的位置[9]。假设行人从当前位置(E0,N0)沿着航向角φ0行走了步长S0,到达的位置为(E1,N1),则这种位置推算可表示为

(1)

由式(1)可以看出,PDR算法研究的关键点在于确定行人步长、航向角及步频。因此PDR研究的方向可分为以下3个步骤:步态探测、步长估计及航向角估计。

(1)步态探测算法有波峰检测法、零点交叉法、自相关分析法、有限状态机法等[10],其中有限状态机算法实现简单,抗误差干扰能力强,为当前进行步态检测相对最合适的方法。

(2)步长估计算法是PDR算法中的核心步骤。目前常用的步长估计算法有常数步长估计模型、线性步长估计模型、非线性步长估计模型、人工智能步长估计模型[11]。本文采用一种非线性步长估算模型,非线性步长模型基于一个步态周期中加速度的最大值与最小值,步长模型公式为

(2)

式中,K1为待定参数;Amax、Amin分别为步态周期中加速度的最大值与最小值。

(3)除了步态检测和步长估计外,航向估计是PDR的另一个重要组成部分。电子罗盘[12]或陀螺仪通常作为估计航向角的手段,由于智能终端固有的传感器噪声,电子罗盘获得的航向精度不高,但不会长时间漂移。相比之下,利用陀螺仪估计航向,在短时间内精度较高,但会出现漂移问题。为了解决这种漂移问题,本文使用Mahony互补滤波器[13]进行航向角的估计,其基本原理就是用加速度和磁力计磁场对陀螺仪累积误差进行补偿。假设陀螺仪校正误差为e=[exeyez]T,则补偿公式可表示为

e=ea+em

(3)

式中,ea、em分别为加速度计和磁强计读数的误差修正项,可分别表示为ea=[eaxeayeaz]T、em=[emxemyemz]T,计算公式为

(4)

(5)

式中,Kp、Ki分别为比例控制参数和积分控制参数。将修正后的陀螺仪数据代入龙格-库塔四元数微分方程中对四元数进行更新。得到更新后的四元数后,即可求得姿态角,继而获取航向角。

1.2 BLE定位

BLE定位使用的是一种指纹定位算法,通过BLE信号强度的特征来实现定位,只需通过每个接入点(access point,AP)信号强度与位置之间的相对关系即可实现定位。BLE指纹定位包括离线建库阶段和在线定位阶段。离线建库阶段,就是通过离线采集数据建立位置指纹数据库。本文使用静态单点采集方法,在按照指纹点布设规律设置好参考点(reference point,RP)后,在每个RP上采集一定时间的指纹信息,将所有RP都采集完成指纹信息后完成指纹库的构建。假设在室内场景中,有n个参考点(RPs)和m个接入点(APs),最终构建的指纹库可表示为

(6)

在线定位阶段,即在实时定位过程中,在待测点位采集各个AP的RSSI值和MAC地址,再通过匹配算法将实时指纹与指纹库数据进行匹配,找到与待测点位最为相似的参考点[14],最终以参考点位置坐标估算出待测点位的位置。实时定位时,智能终端采集到的实时指纹可表示为

实时指纹=[〈rssi1,rssi2,…,rssim〉]

(7)

匹配算法采用了WKNN[15]算法。计算出待定点与参考点的RSSI欧式距离,取欧式距离最小的前k个参考点位置加权平均即可得到待定点位置坐标。

除了基于指纹库信号匹配的BLE指纹定位之外,还有一种BLE测距算法,随着距离的增大,RSSI会随之衰减,根据此对应关系,实现基于RSSI的测距。对数距离路径损耗模型[16]为最常用的BLE测距模型,具体表达式为

RSSI(d)=A-10nlgd

(8)

式中,RSSI(d)为BLE节点与信号采集设备距离为d时的RSSI值;A代表参考距离取1 m时的RSSI值;n为信号衰减因子。本文选取不同间隔距离作为BLE信标与智能终端的距离对模型参数进行拟合,拟合后的对数路径损耗模型公式为

(9)

2 PDR+BLE室内实时融合定位算法

2.1 BLE信号连续最大值临近校正PDR的融合定位

BLE临近校正PDR是BLE与PDR融合的一种定位算法。PDR过程中,所持智能终端检测到临近BLE节点的RSSI满足临近条件时,将BLE节点位置坐标赋予当前PDR坐标,继续进行PDR行走,起到消除PDR累积误差的作用。传统BLE临近校正PDR算法流程如图1所示。

图1 传统BLE临近校正PDR算法流程

传统BLE临近校正PDR算法的阈值判别方法,仅仅设置一次RSSI阈值判断,临近判别极不准确,本文提出了BLE信号连续最大值临近校正PDR算法。BLE信号连续最大值临近校正法的具体实施步骤如下。

(1)将待临近BLE的MAC地址和临近坐标事先存到设备,为进行PDR行走过程的临近做好准备。

(2)在实验场环境下进行数据采集,将终端设备放在离某一BLE临近点1 m距离处采集1 min的RSSI,计算出RSSI的平均值AVG及标准差STD。

(3)将1 m处RSSI的 AVG和STD设为临近判别RSSI的阈值;将1 m设置为临近判别距离阈值。

(4)进行PDR行走定位。行走过程中随时扫描存入设备中的待临近BLE的MAC地址。

(5)若扫描到已存入手机中的待临近BLE的MAC地址,则继续获取待临近BLE的信号强度RSSI;反之,则继续进行PDR定位。

(6)将扫描到的所有待临近点BLE的RSSI进行信号强度阈值判别。若RSSI>AVG且AVG-RSSI

(7)对所有满足信号强度阈值对比的待临近点RSSI进行排序,并将RSSI最大的BLE 作为临近BLE。

(8)对该临近BLE连续获取3次满足信号阈值判别条件的RSSI最大值。

(9)根据前文选择的三次多项式测距模型反算距离。若3次最大RSSI值反算出的距离均满足距离小于1 m的临近判别距离阈值,则将该临近BLE坐标作为当前PDR定位坐标,校正PDR定位;否则,不进行临近校正,继续直接使用PDR定位。

BLE信号连续最大值临近校正PDR的算法流程如图2所示。

图2 BLE信号连续最大值临近校正PDR算法流程

2.2 基于EKF的自适应抗差PDR+BLE融合定位

为使两种定位方式实现优势互补,可将两种定位方法基于KF组合成一个融合系统。由于PDR定位方法是一种非线性模型,因此用EKF代替KF方法进行融合。融合定位模型主要包括两个模型:一是状态转移模型,二是观测模型。

状态转移模型是由PDR的位置坐标、步长和航向角组成的状态向量。状态向量可表示为

X=[NESψ]

(10)

状态转移模型在k时刻时可表示为

(11)

(12)

观测模型是由BLE定位的位置坐标组成的观测向量。观测向量及模型表示为

Zk=[N′E′]

(13)

其中

观测矩阵Hk可表示为

(14)

通过PDR和BLE定位获取状态向量和观测向量时,利用EKF估计随时间更新状态参数和观测参数,在EKF中,先验估计的过程可表示为

(15)

增益矩阵表示为

(16)

然后根据观测值更新状态向量和协方差矩阵,更新过程可表示为

(17)

PDR+BLE滤波融合定位,可以通过EKF模型有效抑制PDR定位的漂移误差,提高整体融合定位算法的精度和稳定性,但对抑制BLE定位粗差的效果较差,因此需要对BLE定位进行抗差处理。为实现在EKF融合定位过程中对BLE定位的抗差处理,本文引入向量Dk,Dk可表示为

Dk=Zk-Hk·Xk

(18)

为判别在融合过程中是否需要对BLE定位进行抗差处理,可通过计算PDR定位与BLE定位坐标结果的距离差进行判断。若距离相差过大,说明此时BLE定位的结果出现了较大偏差,需要进行抗差处理。由于PDR定位与BLE定位的结果分别属于两种不同的分布,因此不可使用欧式距离,需使用Mahalanobis距离,即马氏距离[17],PDR定位结果与BLE定位结果马氏距离的协方差矩阵可表示为

(19)

那么,PDR定位结果与BLE定位结果之间的马氏距离MDk可表示为

(Zk-Hk·Xk)

(20)

由上文可知观测误差协方差矩阵Rk表示为

(21)

式中,σN为BLE定位结果在北方向的误差方差值;σE为BLE定位结果在东方向的误差方差值。

假设PDR定位误差平均值为Pavg,最大误差值为Pmax,若Pavg

(22)

若MDk

(23)

本文判断是否需要抗差的依据为:Rk越大,代表权重越小,反之代表权重越大。若计算出的马氏距离小于Pavg,说明BLE定位结果接近PDR定位结果,只需将BLE定位结果的权重赋值小一些便可,从而实现对BLE定位的抗差更新。

3 试验与分析

试验场地设置在中国电子科技集团第五十四研究所卫星导航系统与装备技术国家重点实验室三层C7试验场二楼,如图3所示。二层试验场长27.49 m,宽24.92 m,由一个矩形走廊和几个房间组成。华为P40智能手机被选为测试设备。二层测试场地安装了18个BLE信标。此外,在二层试验场按照地板砖的位置,共选定154个指纹RPs。五角星为行走路线起点,黑实线为参考路径路线。指纹信息在行走前采集完成,每个RP采集指纹信息时长1 min,在154个RP上收集了9240个实时BLE指纹数据,完成BLE指纹数据库的构建。行人从起点出发,行走一圈匀速到达终点。

图3 试验场地及行进路线

3.1 BLE临近校正PDR融合定位试验

图4为BLE信号连续最大值临近校正PDR与传统BLE临近校正PDR定位对比效果。将行走轨迹中途经已知点的定位点与已知点坐标进行对比,定位结果误差统计信息见表1。

表1 两种临近方法定位误差对比m

图4 BLE临近校正PDR算法对比

由图4可知,传统BLE临近校正PDR算法仅检测并临近上到临近点1和6,而BLE信号连续最大值临近校正PDR算法则将临近点全部检测到并临近。在定位精度上,平均定位精度提高了0.295 m,约19%,定位精度和稳定性有所提升。

3.2 基于EKF的自适应抗差PDR+BLE融合定位试验

基于EKF常规PDR+BLE融合定位算法与改进的基于EKF的自适应抗差PDR+BLE融合定位算法定位效果对比如图5所示。将行走轨迹中途经已知点的定位点与已知点坐标进行对比,定位结果误差统计信息见表2。

表2 两种融合定位算法定位误差对比m

图5 基于EKF的PDR+BLE融合定位算法对比

基于EKF的自适应PDR+BLE融合定位平均定位精度为1.219 m,定位精度提高了21%,与基于EKF的常规PDR+BLE融合定位相比,平均定位精度提高了0.334 m。在定位精度和稳定性上都显著优于常规EKF融合效果。

3.3 PDR+BLE融合定位算法对比试验

为对比本文所述所有定位算法的定位效果,在规划路径上将所有定位方法行走一圈。本文所述所有定位算法的定位轨迹如图6所示。对参考点已知坐标进行对比,得到如图7所示的定位误差序列。定位误差累积分布函数(cumulative distribution function,CDF)如图8所示,定位误差统计见表3。

表3 定位误差统计对比m

图6 定位轨迹对比

图7 定位误差对比

图8 误差累积分布对比

由图6—图8和表3可以看出,PDR+BLE+临近的定位效果明显好于单独一种定位方法或某两种定位方法的结合,并且从累积误差分布可知,PDR+BLE+临近的线处于最上方且是最先到达1的线,兼顾定位精度和稳健性。综上所述,在本文所介绍的定位方法中,PDR+BLE+临近共同结合的PDR+BLE融合定位系统定位精度最高。

4 结 语

当前,在室内定位研究领域已有多种行之有效的技术手段和方法,多种定位方法的融合能有效弥补单一定位手段所展现出的不足之处,进而提高定位精度。而对于与信号相关的定位方法仍然存在着如信号衰减和多径效应等影响定位精度的因素,因此与其他定位方法融合降低此类因素的影响至关重要。本文提出的基于EKF的自适应抗差PDR+BLE室内融合定位方法,考虑了PDR定位的短期相对稳定性,当BLE定位超过限差时降低其在EKF中的权值,抑制BLE定位粗差,达到两种定位方法融合并取长补短,进而提高融合定位精度的目的。试验结果表明,基于EKF的自适应抗差PDR+BLE融合定位方法,具有较好的定位精度和稳定性。但是,本文并没有详细分析地形因素对定位结果的影响,在今后的研究工作中,会考虑加入地图匹配等手段来辅助定位。