WiFi与行人航迹推算自适应无迹卡尔曼滤波融合定位算法

2020-11-09余成波成科宏

科学技术与工程 2020年27期

余成波，成科宏

(重庆理工大学远程测试与控制研究所, 重庆 400054)

近年来，人们对位置服务的需求极速增长，传统GPS和北斗卫星定位系统在商场、地下停车场等结构复杂的建筑区，性能急剧下降而无法满足人们位置需求，因此室内定位与导航成为研究热点。目前的室内定位技术主要包括射频识别、ZigBee、WiFi、低功耗蓝牙、行人航迹推算、超宽带(ultra wideband，UWB)等方法[1]。单一技术定位受其固有劣势影响，定位精度和鲁棒性都较低，所以通过研究融合技术获取更多数据信息降低不确定性，提高定位精度和鲁棒性具有重要意义。文献[2]提出了行人航迹推算(pedestrian dead reckoning，PDR)+地磁的组合定位方法，将PDR定位结果的2个步长作为地磁搜索区，再利用相关地磁匹配算法得到最佳估计点，但地磁受室内其他设备产生磁场影响较大，该算法仅仅使用PDR的粗略位置缩小地磁匹配空间，未考虑噪声因素影响，系统鲁棒性不强。文献[3]提出了使用容积卡尔曼滤波(CKF)模型及地图匹配进行PDR和WiFi数据的融合，能够大幅度提高定位精度，虽然CKF利用容积点进行非线性函数传递更加精确，但在系统状态纬度小于4维时，二者估计精度和稳定性相当，且CKF计算量更大。文献[4]讨论了粒子滤波方法对PDR和室内平面地图进行了融合，并在融合过程中结合地图匹配信息抑制了粒子退化问题，但需要对室内地图环境进行分析并设定标准物，前期工作量较大。文献[5]提出了使用全球定位系统(global positioning system，GPS)和PDR的室内外定位模式，该方法通过分析户外行人运动状态建立数据集，将数据集分类后应用于GPS异常的室内定位，该方法结构简单，但室内定位精度有待提高。文献[6]采用相关的冗余融合和互补融合对传感器数据和WiFi数据进行了融合，其方法有效抑制了WiFi信号的振荡性，但忽略了传感器数据的不确定性。文献[7]重点研究了行人状态估计算法的精确度，并考虑室内环境的干扰进行了算法增强，有效量化了姿态估计误差，但单一PDR累积概率误差仍然对系统影响较大。

针对WiFi信号易受室内复杂环境干扰造成定位误差较大，鲁棒性不强，而单一PDR存在累积误差，传统融合方法精度与鲁棒性难以良好兼容。本文设计了一种加入自适应调整因子的UKF算法融合Wifi定位信息与PDR定位信息，推算用户的最优估计位置。

1 行人航迹推算(PDR)

航迹推算基于手机惯性传感器单元。当已知行人在某一时刻的初始位置，根据加速度计，方向罗盘传感器数据获取步伐、步长和航向信息推算行人下一时刻的位置[8]。PDR原理图如图1所示。

图1 PDR原理图Fig.1 PDR schematic diagram

由图1所示，S0为行人初始时刻的位置坐标S0(N0,E0)，Ln为k时刻的步长，θk为k时刻航向角，则下一时刻位置Sk(Nk,Ek)为

(1)

1.1 步数及步长估计

行人水平运动时，可以从手机加速度传感器数据中检测到加速度值的周期性变化，其近似正弦曲线，利用文献[9]中阈值检测模型实现步数估计。步长估计常见有两种方法：一种是根据行人身体的特征在步行过程中设定固定的步长; 另一种是基于人类行走特征采用线性或非线性动态步长[10]。其计算公式如下：

L=A+BSf+CSv+ws

(2)

(3)

式中：L为步长；Sf为第k步步频；Sv为该步加速度方差；ws为高斯白噪声；amax、amin为三轴加速度的最大值和最小值；A、B、C、K为常数。由于采用变长会增加算法时间复杂度，只需要在短时间内利用传感器数据进行融合，定值步长的误差在短时间内对系统整体精度的影响极为有限，所以步长采用定值，L取60 cm[11]。

1.2 航向角估计

行人方向的识别一般由手机中陀螺仪测量的角速度积分获得，但是陀螺仪随着时间的增加产生累积误差并且随着传感器温度升高偏置漂移误差也会增加[12]。方向罗盘能够直接获得角度信息，所以使用方向罗盘估计行人航向角。

2 WiFi室内定位模型

基于WiFi信号强度的位置指纹识别是目前WiFi室内定位最常用的方法之一，其分为两步：一是位置指纹库的离线建立；二是指纹库的在线匹配。

2.1 WiFi信号强度指纹库建立

离线阶段：假设实验区域有M个AP，表示为AP1,AP2,…,APM。在实验环境中每隔一定距离设定N个采样点(RP)，表示为RP1,RP2,…,RPN。对于每个RP，接收来自不同AP的信号强度(rssi)值矢量S=(rssi1,rssi2,…,rssiM)与其对应位置坐标(xi,yi)，i=1,2,…,N存储在指纹库中用于在线匹配。

2.2 在线匹配算法

在线阶段：用户在待测点接收来自不同AP的信号强度值矢量R=(Rrssi1,Rrssi2,…,RrssiM)，通过计算实时测量的R向量与指纹库向量S的差异来进行用户位置估计，通常比较数据点之间的欧几里德距离(ED)[13]。因此，使用传统加权k-近邻(WKNN)的算法考虑其最接近的k个数据点的加权值来估计待测点位置。待测点位置计算为

(4)

(5)

(6)

式(6)中：wi表示k个最接近待测点位置RPi的权值；(xi,yi)表示RPi的位置坐标；(x,y)表示待测点估计位置坐标。

3 WiFi与PDR融合定位算法

单一的WiFi室内定位会因为噪声、同频干扰、多径效应和室内环境等因素的影响，造成RSSI的严重波动而导致定位结果不准确[1]。基于多传感器的PDR虽然在短时间内精度高，但随着时间推移会产生累积误差和传感器漂移误差，同时单一的PDR导航也需要初始位置对准。卡尔曼滤波在导航、制导和控制等领域应用广泛，传统的卡尔曼滤波(Kalman filtering)利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计[14]。然而现实中大多存在非线性系统，UKF则常用于非线性系统的目标状态的最优估计[15]。

3.1 UKF算法原理

非线性系统模型：

(7)

式(7)中：X(k)为k时刻的系统过程状态；Z(k)为k时刻的系统测量状态；f(·)为系统方程；h(·)为测量方程；W(k)为系统过程噪声；V(k)为测量噪声；W(k)、V(k)均为高斯白噪声。系统噪声协方差矩阵为Q，W(k)～N(0,Q)。测量噪声协方差矩阵为R，V(k)～N(0,R)。UKF算法详细步骤如下。

第1步状态向量均值、方差初始化。

(8)

(9)

第2步通过UT变换得到2N+1个Sigma点。

(10)

第3步计算Sigma点相应的权值。

(11)

式(11)中：i表示第i个Sigma点；下标m表示均值；下标c表示协方差；λ为缩放比例参数；α为参数控制采样点的分布状态；β为一个非负权系数。

第4步时间更新：

Xi(k+1|k)=f[Xi(k|k)]+W(k)

(12)

(13)

(14)

Zi(k+1|k)=h[Xi(k+1|k)]

(15)

(16)

式中：Xi(k+1|k)为第i个点k+1时刻的过程状态；Zi(k+1|k)为第i个点k+1时刻的测量状态。

第5步测量更新：

(17)

(18)

Kk(k+1)=PxzPzz-1

(19)

(20)

P(k+1|k+1)=P(k+1|k)-Kk(k+1)Pzz×

Kk(k+1)T

(21)

式中：Pzz为Sigma点测量状态与测量均值的协方差；Pxz为Sigma点过程状态与测量均值的协方差；Kk为卡尔曼增益系数；P(k+1|k+1)为k+1时刻系统协方差更新。

3.2 自适应调整因子的改进UKF算法

融合系统中观测值的异常会造成UKF融合系统功能退化，导致最优估计的精度和稳定性下降。残差表示实际测量值与基于滤波器内部模型的最佳测量预测值之间的差异，可用于调节滤波器[16]。残差的理论协方差与实际协方差差异可用于确定滤波器发散程度。使用残差rk的理论协方差与实际协方差作为条件引入自适应调整因子ρ，调整状态向量和观测向量的协方差从而调整卡尔曼增益参数，增强系统鲁棒性和提高定位精度。系统残差表示为

(22)

残差rk的理论协方差为

(23)

残差rk的实际协方差为

(24)

式(24)中：n表示估计窗口；i0=k-n+1。引入自适应调整因子ρ，如式(25)所示：

(25)

diff=|tr(Ck+1)-tr(C′k+1)|

(26)

式中：ddiff为残差的理论协方差与实际协方差之间的差异；tr(·)为协方差矩阵的迹，引入自适应调整因子ρ后协方差及卡尔曼增益表示为

(27)

Kk(k+1)=ρPxzPzz-1

(28)

3.3 融合系统模型建立

根据3.2节所述融合算法原理，融合系统状态方程及测量方程建立如下：

(29)

式(29)中：xk+1、yk+1为k+1时刻行人航迹估算二维坐标；L为行人步长；θk+1为k+1时刻航向角。

(30)

式(30)中：x′k+1、y′k+1为WiFi指纹匹配结果的坐标。

改进UKF融合算法系统流程图如图2所示。

图2 UKF融合系统流程Fig.2 UKF fusion system process

4 实验设计及数据分析

4.1 实验环境

红色线路代表路径1；绿色线路代表路径2图3 实验环境平面图Fig.3 Experimental environment plan

4.2 仿真数据分析

从图4～图7中路径1、2的实验结果对比可以看出，WiFi信号易受环境影响，信号波动大，造成定位点跳点和堆积现象；基于多传感器的PDR算法由于累积误差的存在，随着运动步数的增加导致漂移逐渐增大。标准UKF会因为WiFi定位结果(观测值)的异常值出现而导致系统融合效果出现不佳状况，而加入自适应调整因子后会有效的平衡融合系统中状态信息和观测信息的正确比例达到更优的估计。

图8可以看出标准UKF算法在1.5 m以内的概率约为22%，本文算法位置精度在1.5 m以内的概率达到了约80%，提高了约58%，其算法鲁棒性获得大幅度提高。