一种基于卡尔曼滤波的超宽带室内定位算法

闫保芳, 毛庆洲

(武汉大学 测绘遥感信息工程国家重点实验室,湖北 武汉 430079)

一种基于卡尔曼滤波的超宽带室内定位算法

闫保芳, 毛庆洲

(武汉大学测绘遥感信息工程国家重点实验室,湖北武汉430079)

为了减小室内环境中障碍物对超宽带(UWB)传感器测距结果的影响,提出了一种基于卡尔曼滤波(KF)的超宽带室内定位算法。利用超宽带接收信号的信噪比区分视距和非视距环境,给出了超宽带传感器测距性能最小二乘标定模型,减小测距系统误差;判断相邻测距差分是否在阈值范围内,否则用卡尔曼滤波先验估计替代后验估计处理测距结果,由此减弱多径效应和非视距误差对测距的影响;用扩展卡尔曼滤波器(EKF)实现室内定位。实验结果表明:算法在复杂室内环境中可达到亚米级的动态实时定位精度。

超宽带; 室内定位; 标定; 卡尔曼滤波; 扩展卡尔曼滤波

0 引 言

目前,常用的超宽带[1～4](ultra-wideband,UWB)技术室内定位方法有到达角(angle of arrival，AOA),到达时间(time of arrival，TOA),接收信号强度(receive signal strength，RSS),到达时间差(time difference of arrival，TDOA),一般采用其中的一种或几种混合作为定位方法[5]。Irahhauten Z等人[6]提出了基于信号达到角度和时间(TOA/AOA)的室内定位方法,算法只需要一个锚节点(anchor node,AN),非视距(non-line of sight,NLOS)定位估计精度较高,而视距(line of sight,LOS)定位估计精度较传统TOA定位方法低。Wang T等人[7]增加参考节点(refe-rence node,RN),计算移动节点 (mobile node,MN)和RN到AN的TDOA的定位方法,获得较高的室内定位精度,但是需要的节点数目较多,且定位算法复杂。Suski W等人[8]建立了测量噪声地图,在粒子滤波框架下将UWB室内定位导航精度提高了30%,但建立测量噪声地图需要测大量数据且环境普适性不强。

上述定位方法关于测量值的方程组通常情况下均为非线性的,为了求出最优解,提出了具有不同精度和复杂度的定位算法。常见的有:Chan定位算法[9],在LOS环境中定位精度较高,但NLOS环境下需要靠增加AN改善定位精度;Taylor级数展开法[10],计算速度较Chan算法快,NLOS环境下定位精度也有所改善,但是Taylor算法初值精度低时会发散;径向基函数(radial basis function,RBF)神经网络定位算法[11]与前两者相比,在NLOS环境下远距离的定位精度较高,而一般室内测量距离较短,因此并不适用。

相较而言,卡尔曼滤波(Kalman filtering,KF)利用某一时刻测量值和上一时刻估计值来预测当前值,具有良好的动态跟踪定位的效果,递归计算复杂度低[12]。本文采用带有观测量差分阈值判断后验估计的KF以减小测距过程中环境、电气等引入的噪声,最后利用扩展卡尔曼滤波(extended Kalman filtering,EKF)实现动态跟踪定位。

1 UWB传感器测距原理与标定模型

本文选用P410超宽带传感器,利用超宽带信号在2只传感器之间的双向飞行时间(two-way time of flight,TWTOF)进行测距[13]。

UWB传感器TWTOF测距原理如图1所示。传感器A在τtxA时刻发送测距请求信号,传感器B在τtxB时刻接收到信号后,在τtxB时刻发送测距响应信号,传感器A在τtxA时刻接收并解析响应信号数据包,计算信号在两个节点之间的飞行时间再乘以光速c,即可得两节点间距AB。TWTOF测距方程为

(1)

LOS和NLOS环境中影响UWB测距精度的因素不同,需分别标定。本文选择一次函数作为传感器标定模型[14,15]。

1.1 LOS环境

受电气转换时间、天线和天线连接器电气延迟等因素的影响,测距响应时延τdelay存在误差。首先需要标定UWB传感器的LOS测距性能。假设传感器P0与Pi,i=1,2,3之间距离的测量值0,i和真值d0,i关系如下

(2)

式中v0,i(d0,i)为与真实值相关的绝对误差函数,假设v0,i为一次函数

v0,i(d)=a0,id+b0,i

(3)

则

d0,i=(0,i-b0,i)/(1+a0,i)

(4)

1.2 NLOS环境

NLOS条件下,信号直接路径上存在障碍物,一方面影响信号的检测精度,另一方面影响信号穿过障碍物时的速度,使图1所示的τrxB和τrxA数值偏大。障碍物对信号的衰减作用使NLOS环境接收信号强度较LOS环境弱很多,可以将接收信号的信噪比作为测距条件的判断依据。对于相同材质的障碍物,NLOS误差可以视为常数,在LOS标定后的基础上式(4)可以改写为

(5)

式中SNR为接收信号信噪比;TSNR为测距条件判定阈值;vNLOS为非视距误差常数,需要实地测量确定。

2 测距值KF

首先将标定后的传感器数据进行卡尔曼滤波处理,获得精度较高的连续测距值,方可进行定位。短时间内,MN与AN的距离rk可视为均匀变化

(6)

由式(6)构造离散卡尔曼滤波的状态方程为

xk=Axk-1+wk-1

(7)

离散卡尔曼滤波的观测方程为

zk=Hxk+vk

(8)

式中zk为第k个采样点距离测量值;wk和vk分别为过程噪声和观测噪声;H=[1,0]。由文献[16]可得

(9)

(10)

(11)

(12)

(13)

创新生态系统核心企业主导能力测度研究—基于扎根理论的探索…………………………边伟军，密淑泉，罗公利，王玉英（1，24）

(14)

3 EKF跟踪定位

TOA定位原理如图1所示,三个锚节点位于同一水平面上,以P101为原点O,P101—P102为X轴,P101—P103为Y轴,垂直OXY方向为Z轴,建立空间直角坐标系。

图1 TOA定位原理

设MNP100坐标为(x,y,z),ANP101,P102,P103坐标为(xi,yi,zi),i=1,2,3,TOA定位原理如图1所示,则MN和AN之间的距离为

(15)

式(15)所示的MN坐标(x,y,z)和观测距离ri之间的关系是非线性的,即测量方程非线性,需要将预测和状态估计线性化后再应用于KF。因此,为了提高MN动态定位精度,本文采用EKF来估计MN坐标。

假设P100运动状态模型为

Xk=fk-1(Xk-1)+Wk-1,Wk～N(0,Qk)

(16)

非线性测量模型为

Zk=hk(Xk)+Vk,Vk～N(0,Rk)

(17)

式中Xk为MN坐标和相应坐标轴运动速度向量,Xk=[x(k),y(k),z(k),vz(k),vy(k),vz(k)]T,vi(k)=(i(k)-i(k-1))/T,i=x,y,z;Qk为预测噪声向量Wk的协方差矩阵;Zk为MN距AN观测距离向量,Zk=(r1(k),r2(k),r3(k))T;Rk为观测噪声向量Vk的协方差矩阵;T为采样时间间隔。则扩展卡尔曼滤波方程为

(18)

(19)

(20)

(21)

(22)

4 实验与结果分析

实验流程如下:1)用最小二乘法标定传感器;2)采用带有观测量差分阈值判断后验估计的KF提高测距精度,减小随机误差对测距的影响;并估计含粗大误差的采样点,减弱粗大误差影响;3)通过非线性EKF估计MN位置

4.1 UWB传感器测距标定实验

在室内LOS环境中,从1～22m分别每隔1m静态测量2000次距离,取2000次测量均值作为该距离下的测量值,用最小二乘拟合测距误差与真实距离值间的函数关系,LOS环境标定结果如图2所示。

图2 LOS环境标定

从图2可以看出:真实距离小于3m时,测距绝对误差较大。由于距离太近,UWB信号在空中飞行时间过短,使图1中传感器A接收的响应信号被自己发射的测距请求信号干扰,标定UWB传感器测距值可以减小这种干扰引入的误差。

在室内LOS和NLOS条件下从1～16m分别每隔1m静态测距1000次,取1000次测量均值,用一次函数最小二乘拟合信噪比(signal to noise ratio,SNR)与测距的关系,如图3所示。其中,NLOS环境中2只传感器间隔厚度约为250mm的空心砖墙。

图3 SNR与测距关系

由图3知:传感器测量值相同时,LOS环境中的SNR较NLOS环境大,且随着距离的增加,二者之间的差值逐渐增大。本文取LOS与NLOS环境中,SNR和传感器测量值拟合曲线的角平分线作为测距环境的决策阈值曲线。获得传感器测距值后,根据决策阈值曲线求出此测距值对应的环境决策SNR阈值,即式(5)中的TSNR,对传感器进行标定。

4.2 测距卡尔曼滤波

为了提高测距精度,减小多径效应与NLOS的影响,用KF处理测距,结果如图4所示。

图4 卡尔曼滤波测距处理

LOS环境中以多径效应测距误差为主,UWB信号具有良好抗多径效应的能力,从图5(a)可以看出整体测距误差较小,动态实时定位过程中,差分阈值判断先验估计代替后验估计的KF可以很好估计个别含有粗差的测距值,削减突刺平滑测距采样数据,KF处理后曲线较原始测量曲线平滑,且覆盖了原来的测距曲线,具有良好跟踪效果。

MN在NLOS环境运动过程中,与AN之间环境变化导致接收到的UWB信号强度和飞行时间剧烈变化,如图5(b)所示,测距采样曲线有很多毛刺平滑度低。KF处理后,测距采样波形整体较平滑,有效减弱了NLOS误差的影响。

4.3 EKF定位实验

采用EKF能减弱测距误差对定位的影响,动态定位效果如图5所示。

图5 EKF定位

图5(a)为图4(a)对应的定位轨迹,在56m×7.8m的室内LOS环境中进行实验,MN实际运动轨迹为一个长方框。图5(b)为在26m×7m的室内LOS环境中实验结果,MN实际运动轨迹为回字形。图5(c)为图4(b)对应的定位轨迹,在室内NLOS环境中沿着厚度约为250mm的空心红砖墙绕行。

从图5可以看出:EKF后的轨迹较三边测量定位轨迹平滑,受测距误差影响较小,且更贴近实际运动轨迹。UWB在室内LOS环境中定位精度较高,尺寸较大的室内平均动态定位精度达到1m,小尺寸室内最大动态定位误差在0.2m以内,室内NLOS环境中动态定位精度在1m左右。

5 结束语

本文通过分析实际测量误差值,采用一次函数作为传感器测距的误差校正模型,减小了系统误差的影响,并利用KF,减弱了多径效应和非视距误差。这两步数据处理有效地提高了测距精度,最后采用EKF提高了定位精度。本文仅用3个锚节点,达到了1m的综合室内动态定位精度,适用于机场、火车站和会展等室内公共场所的导航定位。由于本文仅使用UWB传感器,缺少多余观测量,因此,对于误差较大的数据,KF估计效果不理想。后续工作中可以添加加速度计、陀螺仪等多传感器组合导航,进一步提高室内定位精度。

[1] Zhang J,Orlik P V,Sahinoglu Z,et al.UWB systems for wireless sensor networks[C]∥Proceedings of the IEEE,2009:313-331.

[2] 王小辉,汪云甲,张 伟.基于RFID的室内定位技术评述[J].传感器与微系统,2009,28(2):1-7.

[3] 刘小康,张 翔,方 爽,等.基于ZigBee无线传感器网络的一种室内定位新方法[J].传感器与微系统,2013,32(11):29-36.

[4] 张令文,杨 刚.超宽带室内定位关键技术[J].数据采集与处理,2013(6):706-713.

[5] Irahhauten Z,Nikookar H,Klepper M.A joint ToA /DoA technique for2D /3D UWB localization in indoor multipath environment[C]∥Internatimal Conference on Communications (ICC),Ottawa,ON,2012.

[6] 欧阳昱.基于超宽带的室内定位算法研究[D].南京:南京邮电大学,2013.

[7] Wang T,Chen X,Ge N,et al.Error analysis and experimental study on indoor UWB TDOA localization with reference tag[C]∥APCC,Denpasar,2013.

[8] Suski W,Banerjee S,Hoover A.Using a map of measurement noise to improve UWB indoor position tracking[J].IEEE Tran-sactions on Instrumentation and Measurement,2013,62(8):2228-2236.

[9] 章坚武,唐 兵,秦 峰.Chan定位算法在三维空间定位中的应用[J].计算机仿真,2009,26(1):323-326.

[10] 蒋文美,王 玫.一种基于TOA的UWB直接Taylor复合定位算法[J].桂林电子工业学院学报,2006,26(1):1-5.

[11] 毛永毅,李明远,张宝军.一种 NLOS 环境下的 TOA/AOA 定位算法[J].电子与信息学报,2009,31(1):37-40.

[12] Zhang Y,Zhang R,Miao W,et al.An indoor positioning algorithm for mobile objects based on track smoothing[C]∥2014IEEE7th International Conference on Advanced Infocomm Technology (ICAIT),IEEE,2014:252-259.

[13] 孟宏伟.超宽带无线定位方法研究[D].长春:吉林大学,2007.

[14] Monica S,Ferrari G.An experimental model for UWB distance measurements and its application to localization problems[C]∥2014 IEEE International Conference on Ultra-WideBand (ICUWB),IEEE,2014:297-302.

[15] Zoltan K,Charles T,Dorota G,et al.Performance analysis of UWB technology for indoor positioning[C]∥International Technical Meeting of The Institute of Navigation,San Diego,California,2014.

[16] Mohinder S Grewal,Angus P Andrews.Kalman filtering:Theory and practice using Matlab[M].2nd ed.New York:A Wiley-Interscience Publication,2001:170-184.

AnUWBindoorpositioningalgorithmbasedonKalmanfiltering

YAN Bao-fang, MAO Qing-zhou

(StateKeyLaboratoryofInformationEngineeringinSurvey,MappingandRemoteSensing,WuhanUniversity,Wuhan430079,China)

In order to eliminate the effects of indoor obstacles lead to ultra-wideband(UWB) ranging result,present an UWB indoor localization algorithm based on Kalman filtering.By using signal noise ratio of UWB to distinguish line of sight from non-line of sight environment,give least squares calibration models for ultra-wideband sensor ranging performance to reduce the distance measuring system error.To weaken multipath effects and effect of non-line of sight error on ranging,use Kalman filtering priori estimation to replace posterior estimation to process ranging result.Extended Kalman filter is used to achieve indoor positioning.Experimental results prove that,the algorithm has sub-meter high dynamic realtime positioning precision in complex indoor environments.

ultra wide band(UWB); indoor positioning; calibration; Kalman filtering(KF); extended Kalman filtering(EKF)

10.13873/J.1000—9787(2017)10—0137—04

2016—11—02

TN 929

A

1000—9787(2017)10—0137—04

闫保芳(1992-),女,硕士研究生,研究方向为室内导航、3S集成。