基于ToA技术与扩展卡尔曼滤波的室内无线定位算法设计∗

刘明佳 骆 曦 刘志新

（1.秦皇岛港股份有限公司第六港务分公司，秦皇岛 066000）（2.燕山大学电气工程学院 秦皇岛 066004）

基于ToA技术与扩展卡尔曼滤波的室内无线定位算法设计∗

刘明佳1骆 曦2刘志新2

（1.秦皇岛港股份有限公司第六港务分公司，秦皇岛 066000）（2.燕山大学电气工程学院 秦皇岛 066004）

无线室内定位是物联网技术的重要应用之一，为提高对于运动目标的定位和跟踪的精度，论文设计了基于TOA技术的扩展卡尔曼滤波算法。提出利用双边双向测距算法进行节点间的距离测定，将时钟误差及测距误差视为状态噪声，设计扩展卡尔曼滤波器，获得递推估计算法。在不同运动轨迹和不同噪声参数下的仿真测试结果表明，所设计算法可有效提高定位精度，改善目标跟踪效果。

室内定位；扩展卡尔曼滤波；跟踪误差；去噪

1 引言

室内无线定位技术是物联网应用的关键技术之一，近年来受到国内外研究人员的广泛关注［1～2］。相比于室外定位技术而言，由于室内通信环境更为复杂，无线信号在传输过程中存在不同程度的信号衰减、反射、多径效应的影响，使得定位精度、定位范围均受到较大程度的影响。利用无线信号进行室内定位的手段主要包括超声波、红外线、超宽带（UWB）、射频识别技术（RFID）、WiFi、ZigBee等［3～5］。不同的定位技术具有不同的定位精度和适用场合，如超声波定位、UWB定位、RFID定位均可达到较高的定位精度，但其受视距测量、设备成本高等因素的限制。WiFi技术和ZigBee技术利用免费的ISM频段虽具有成本低易于实现的优点，但无线信号易受干扰，使得测距定位信息存在误差而影响定位精度［6～8］。

对室内目标的定位问题一般可利用三边定位方法实现，而前提是获得锚节点与目标间的距离信息。因此测量节点间的距离是定位实现过程中直接影响性能的关键所在，目前应用较为广泛的室内测距技术主要有基于到达时间（TOA：Time of Arriv⁃al）、基于到达时间差（TDOA：Time Difference of Ar⁃rival）、基于信号到达角度（AOA：Angle of Arrival）、基于接收信号强度（RSSI：Received Signal Strength Indicator）等方法［9～10］。其中，TOA 方法简单易于实现，他通过测量发射信号在空间上的飞行时间来测得距离，可分为单程测距和双程测距两种方式［3］。单程测距为信号从锚节点到达目标节点后不返回，双程测距为信号从源节点到达目标节点后，又从目标节点返回源节点。两个节点之间使用单向测距要时钟同步时间，而双向测距不必考虑这个问题，但本地时间引起的偏差及测量噪声也会影响定位精度［11～12］。为了解决室内定位中测距干扰和数据噪声导致的定位与跟踪误差较大的问题，本文提出了一种基于扩展卡尔曼滤波的室内定位算法。首先使用TOA这一方法进行测距，然后再使用卡尔曼滤波算法去除测量及计算噪声，最终达到提高室内定位系统的性能的目的。

2 定位原理

如前所述，为避免复杂的时钟同步问题，本文采用对称双边双向测距算法（Symmetrical Dou⁃ble-Sided Two-Way Ranging，SDS-TWR）测量端点间的距离。其原理如图1所示。

该过程分为两次测量，首先，节点A向节点B发送数据。此时节点A开始计时，当节点B接收到数据后，向节点A返回数据，记录下节点B的处理时间T2。节点B开始计时，当节点A接收到从节点B返回的数据后，停止计时，记录下节点A的读数T1。然后再次向节点B发送数据，记录下节点A的处理时间T4，当节点B接收到数据后，停止计时，记录下节点B的读数T3。然后节点B返回初始状态，并向节点A返回数据，节点A接收到返回的数据后也返回初始状态。节点A到节点B之间的单向时间由以下公式得到：

由于无线信号在空气介质中的传播速度一定，易得两点间的距离。若目标与至少三个锚节点间距离已知，则可利用三边定位法确定目标节点的位置，如图2所示。若已知锚节点A，B，C的位置以及锚节点到待测物体的距离，则每一个目标节点的位置，都可以通过以三个锚节点为圆心的圆唯一地确定，从而确定该目标节点D的位置。

节点间的距离可表示为

其中：Zk=[d1kd2kd3k]T，表示目标节点到三个锚节点间的距离，vk是测量噪声，方差为R。对于运动目标进行连续测距和定位即可实现目标跟踪。

使用SDS-TWR算法虽不需要两个节点进行时钟同步，而且由于进行了两次测量，对测得时间的平均处理又使得因为本地时钟造成的距离误差减小。但由于无线信道的复杂性，使得接收信号易受到各种噪声的影响，这里归结为噪声项vk。去除测数据噪声干扰的方法很多，基本上分为硬件滤波和软件滤波两种。本文采用的扩展卡尔曼滤波（Extended Kalman Filter，EKF）是软件滤波的一种方法。EKF的基本流程为：首先使用状态函数对上一时刻状态进行迭代，求出先验状态和误差矩阵。然后求出卡尔曼增益，根据TOA测得的距离估算目标节点状态，然后使用目标节点的状态对先验状态进行校正得到目标节点的后验状态，然后代到下一个时刻进行迭代。从而实现移动目标的连续跟踪。

3 算法设计

假设运动目标的非线性离散系统的方程如下：

其中：Xk是状态变量，f和h均为非线性函数，分别表示前一时刻状态与当前时刻状态、状态变量与观测值之间的关系。wk和vk分别为状态和观测噪声，假设为高斯噪声，其方差分别为Qk和Rk。

EKF分为两个部分：估计部分、校正部分。预估阶段是根据系统状态模型及前一时刻的估计值预估当前系统的状态，同时求出预估状态误差的协方差矩阵；校正阶段是通过预估状态、当前时刻的观测值以及测量模型对当前的系统状态进行校正，并得到当前状态误差的协方差矩阵，并将当前时刻的状态变量代入到下一时刻。

在已知初始状态的后验估计X0及其方差P0的条件下，k-1时刻状态变量的值及其方差分别为X̂k-1和Pk-1，则按如下步骤计算第k时刻的先验估计。

时间更新方程：

状态更新方程：

本书建立在作者多年顶级物流企业的从业基础上，根据中国电商企业存在的物流相关共性问题，结合中国的现实环境和商业趋势，最终希望解决的是电商物流的现实问题，并提供一套行之有效的方法，同时也为想要了解电商物流的普通读者奉上了最好的读物。

下面建立模型并代入到扩展卡尔曼滤波方程中，系统状态模型可表示为

然后根据三边定位原理建立的观测模型，已知第n个锚节点的坐标是(xn，yn)，k时刻目标节点到三个锚节点距离的测量值为(d1k，d2k，d3k)，等式右边应为状态方程中得到的坐标(xk，yk)到三个锚节点的距离，测量距离时受到的噪声用vk表示。目标节点和锚节点间距离公式如下：

vk是高斯噪声，方差为R。

由于观测模型非线性的，因此需要求hk(Xk)的雅可比矩阵：

因此通过第k-1时刻的状态估计，估算k时刻状态的EKF算法如下：

其具体流程如图3所示。

4 仿真分析

为测试算法在不同环境下的定位性能，分别设定待定位目标做直线运动，圆周运动和不规则运动，并且分析在不同的噪声干扰下的定位效果。我们假设系统受到零均值高斯噪声的影响，设观测噪声的方差R=1，状态噪声的方差Q=0.003。在运动直线运动下定位和跟踪效果如图4所示，并与传统的最小二乘法进行了误差对比，如图5所示。

从图4、图5看见，EKF对于直线运动具有较好的跟踪效果。虽在初始阶段EKF算法产生较大的误差，但在迭代一定次数后定位误差明显减小。两种算法下的平均误差分别为0.3581m和1.5414m。与最小二乘法得到的估计相比，EKF得到的跟踪轨迹更加平滑，误差更小，其滤波去噪的作用更加有效。

对于待跟踪目标做圆周运动和不规则运动的跟踪效果分别如图6和图7所示。与直线运动跟踪结果相似，EKF算法均表现出较好的滤波效果，有效提高了定位精度。在三种不同运动轨迹下的平均误差如表1所示，可见EKF算法对于规则运动的目标跟踪效果更好，其主要原因是在状态估计的过程中，若两个观测点状态的相关性较强，则后验估计的效果会更加准确。类似的，当改变不同的状态干扰噪声来模拟在测距过程中产生的距离误差，定位结果如表2所示。可见EKF算法对于抑制状态噪声依然具有较好的效果，需要指出的是，随着噪声方差的加大，定位误差也将随之增大，这也符合一般的认知规律。

表1 不同算法下定位平均误差（单位：m）

表2 不规则运动下不同参数下定位平均误差（单位：m）

5 结语

本文研究了无线室内定位过程中如何降低测量噪声对定位精度影响的问题，提出利用扩展卡尔曼滤波算法，对测量距离产生的噪声干扰加以抑制。设计了基于TOA技术的滤波算法，分析了目标运动轨迹以及噪声方差对定位和跟踪性能的影响。仿真结果表明卡尔曼滤波算法可有效降低定位误差。

［1］H.Liu，H.Darabi，P.Banerjee，J.Liu.Survey of wireless indoor positioning techniques and systems［J］.IEEE Transactions on Systems，Man，and Cybernetics，Part C：Applications and Reviews，2007，37（6）：1067-1080.

［2］ J.Jiang，X.Zheng，Y.Chen，et al.A distributed RSS-based localization using a dynamic circle expanding mechanism［J］.IEEE Sensors Journal，2013，13（10）：3754-3766.

［3］张勇，黄杰，徐科宇.基于PCA-LSSVR算法的WLAN室内定位方法［J］.仪器仪 表学报，2015，36（2）：408-414.

［4］石柯，陈洪生，张仁同.一种基于支持向量回归的802.11无线室内定位方法［J］.软件学报，2014，25（11）：2636-2651

［5］J.Prieto，S.Mazuelas，A.Bahillo，et al.Adaptive data fu⁃sion for wireless localization in harsh environments［J］，IEEE Transactions on Signal Processing，2012，60（4）：1585-1596.

［6］谢代军，胡捍英，孔范增.基于分布重叠和特征加权的无线局域网室内定位算法［J］.计算机科学，2013，40（11）：38-42.

［7］张勇，李飞腾，王昱洁.基于KDDA和SFLA-LSSVR算法的WLAN室内定位算法［J］.计算机研究与发展，2017，54（5）：979-985

［8］吕亚娟，刘珊，胡荣贻，张文浩.一种基于多天线簇的新型LTE室内定位系统［J］.电信科学，2017（3）：67-75

［9］王沁，何杰，张前雄，等.测距误差分级的室内TOA定位算法［J］.仪器仪表学报，2011，32（12）：2851-2856.

［10］H.Tian，S.Wang，H.Xie.Localization using coopera⁃tive AOA approach［C］//International Conference on Wireless Communications，Networking and Mobile Com⁃puting，2007：2416-2419.

［11］朱明强，侯建军，刘颖，苏军峰.一种基于卡尔曼数据平滑的分段曲线拟合室内定位算法［J］.北京交通大学学报，2012，36（5）：95-99

［12］张桀，沈重，联合TDOA改进算法和卡尔曼滤波的UWB室内定位研究［J］.现代电子技术，2016，29（13）：1-5.

Design of Indoor Wireless Location Algorithm Based on ToA Technique and Extended Kalman Filtering

LIU Mingjia1LUO Xi2LIU Zhixin3
（1.Qinhuangdao Port Group Co.，Ltd，Qinhuangdao 066000）（2.Institute of Electrical Engineering，Yanshan University，Qinhuangdao 066004）

Indoor wireless location is one of the most important applications of internet of things.To improve the precision of lo⁃cation and tracking of mobile object，the extended Kalman filter algorithm is proposed based on Time of Arrival（TOA）method in this paper.The symmetrical double-sided two-way ranging algorithm is used to measure the distance between the nodes.The errors of timer and distance are considered as state noise.The extended Kalman filter is designed and the recursive estimation algorithm is achieved.The simulations under different mobile tracking and noise parameters are done，and the results show that the proposed al⁃gorithm can improve the location precision and tracking performance effectively.

indoor location，extended kalman filter，tracking error，denoising

TP301

10.3969/j.issn.1672-9730.2017.11.022

Class Number TP301

2017年5月17日，

2017年6月23日

国家自然科学基金项目（编号：61473247）资助。

刘明佳，男，工程师，研究方向：工业企业自动化技术与应用。骆曦，男，硕士，实验师，研究方向：无线通信网络及应用。刘志新，男，博士，教授，研究方向：无线通信技术及应用。