基于卡尔曼滤波的超宽带定位技术应用

刘永昌 龚元明

摘  要：利用超宽带定位技术，改进了基于三边定位技术的超宽带室内定位技术应用模型。该模型基于TW-TOF（双向飞行时间法）采集定位数据，利用三边定位原理解算坐标位置，结合卡尔曼滤波算法，解决了定位模型中由于设备自身误差带来的定位误差大和定位点漂移等问题，进一步提高了定位精度。经现场实验检验，定位模型的定位坐标精度可达厘米级，误差低于12 cm，滤波后定位均方差减少了38%以上。该模型具有定位算法简单可靠、定位结果误差小等优势，为室内定位技术的应用提供了参考。

关键词：室内定位;超宽带;卡尔曼滤波;应用模型

中图分类号：TP391     文献标识码：A

Application of Ultra-wideband Positioning Technology based on Kalman Filter

LIU Yongchang， GONG Yuanming

（College of Automotive Engineering， Shanghai University of Engineering Science， Shanghai 201620， China）

yongchangsd@163.com; gongyuanming@tsinghua.org.cn

Abstract： This paper proposes to use ultra-wideband （UWB） positioning technology to improve the application model of ultra-wideband indoor positioning technology based on trilateral positioning technology. In this model， positioning data is collected based on TW-TOF （two-way time-of-flight method） and the trilateral positioning principle is used to calculate the coordinate position. Combined with Kalman filter algorithm， it solves the problems of large positioning error and positioning point drift caused by device's errors in the positioning model， further improving the positioning accuracy. On-site experiment shows that positioning accuracy of the positioning model can reach centimeter level， the error is less than 12 cm， and the mean square error of positioning after filtering is reduced by more than 38%. The proposed model has the advantages of simple and reliable positioning algorithm and small positioning error. This model provides a reference for application of indoor positioning technology.

Keywords： indoor positioning; ultra-wideband; Kalman filter; application model

1   引言（Introduction）

我国各个行业产业信息化、智能化等技术飞速发展，高精度室内定位技术的应用拥有广阔的发展前景。随着物联网渗透到人们生活的方方面面，日常使用的定位技术中，特别是室内定位技术在人们的生活中扮演着越来越重要的角色[1]，但是相对成熟的应用技术还很少。比如，在大型室内停车场，由于室内信号的屏蔽作用导致卫星定位技术无法施展，车辆识别定位系统还没有得到有效地开发。又如，由于山体遮挡，对于现代交通道路中的长隧道道路，目前还没有有效地解决车辆定位的办法;大型立交桥立体空间的车辆定位也是亟待解决的问题。我国大力发展智慧产业等技术[2]，智慧农业、智慧交通、智慧物流、智慧矿山等都需要高精度的定位技术来支撑智能设备在现场的应用。比如，对隧道里矿工定位，可以对施工工人进行实时自动位置跟踪，全面监测隧道里人员的活动轨迹和位置分布。建立双向报警系统机制，在紧急情况下双向报警。当隧道施工或者矿井开采遇到突发事故时可以迅速找到被困人员的位置，最大可能保障生命安全。与此同时，也会在企业管理方面提高生产施工效率等。

目前，对比市场上各种定位技术，超宽带已逐渐成为无线通信领域研究与开发的热点[1]。首先，近几年超宽带技术的发展使得设备成本得以降低，为科研发展提供了前提条件。同时，融合和结合其他定位技术，超宽带定位技术在算法上有了很多突破，比如，超寬带定位的三种常用定位算法，包括到达时间差（Time Difference Of Arrival， TDOA）定位算法[3]、飞行时间差（Time Of Flight， TOF）定位算法、到达时间（Time Of Arrival， TOA）定位算法[4]等。另外，超宽带定位技术相比较其他室内定位技术具有定位范围广、定位速度快，定位精度高、同时容纳的定位设备数量多等优势。

目前，国内外对超宽带定位技术的研究还处于初级阶段，大规模的超宽带应用还没有成熟的案例。在理论研究方面，国内各个高校对超宽带定位技术及其各种优化算法有比较深入的研究。例如，哈尔滨工程大学的杨俊杰用神经网络算法来补偿一些定位中的不确定因素造成的测距偏差，并利用系统输出的距离数据和系统的测距偏差数据训练了神经网络参数[5]。阿坝师范学院电子信息与自动化学院的莫建麟提出，利用RSSI中的Shadowing传播模型来估算待定位节点与发送节点之间的距离，建立关于待定位节点和发送节点距离的目标方程，通过最小二乘算法来估计方程中的系数[6]等。

2   超宽带定位原理（UWB positioning principle）

2.1   几何定位原理

在常见的定位系统中，测量相对位置通常有三种方法：测量角度、测量距离和测量距离差[3-4]。其本质是测量基站和标签相对位置，通过几何位置和算法确定相对位置。本文应用的超宽带定位模组采用的是双向飞行时间法（TW-TOF）。双向飞行时间法属于常用定位系统中的到达时间法，这种算法可以测量出标签到基站间的距离。

如图1所示，表示该基站从发送信号时刻到重新接收到信号时刻的时间间隔，表示标签从接收到信号的时刻到发送信号时刻的时间间隔。假设基站发出一组信号，经过时间后标签收到信号，同样地标签也发送一组信号，经过时间后基站收到信号。因为两次信息交换标签移动的距离很短或者没有移动，所以两次互传信息的时间间隔相同。

在基站或者标签系统中，为电磁波波速，和可以根据模组自身时钟系统计算时间差得到，即模组已经知道自己连续发送两次的时间间隔。在TW-TOF定位系统中降低了对基站的时钟同步要求，减小定位误差。

2.2   三边测量法的原理与计算方法

根据以上已经测量出来的基站到基站和标签到基站的数据，把基站和标签组成的点连接成三角形，运用三角学原理推算标签的坐标。三边测量法的原理如图2所示，以三个节点、、为圆心作圆，、、即为基站位置，、、与交点的距离分别为、、，假设交点的坐标为。

坐标分别为、、，这三个圆周相交于一点，交点即为移动标签：

（4）

由式（4）可以得到标签的坐标为：

（5）

根据上一节的论述可以计算出标签坐标的具体范围，但是由于系统硬件误差和测量的精度问题，测出的位置坐标范围其实是一个服从正态分布的坐标区域。从图3可以看出，三边定位算法组成的定位系统存在系统误差，误差范围较大，定位坐标不断漂移，致使定位精度下降。其中该正态分布数学期望为，方差为，可以看出单纯依靠三边定位算法计算出来的数据，静态定位坐标落于±12 cm范围内，定位坐标漂移较大，数据不能在高精度场所应用。

3   建立定位模型（Establishing positioning model）

3.1   卡尔曼滤波算法

卡尔曼滤波算法[7-8]是目前应用最为广泛的滤波算法，在导航、制导、通信等领域得到了较好的应用。卡尔曼滤波的核心思想就是：根据当前仪器的测量值以及上一时刻的预测值和误差，计算得到当前最优量，再預测下一时刻的数值。卡尔曼滤波算法具体步骤如下：

（1）先验估计

由现代控制理论知识，假设某一状态方程如下：

（6）

其中，为先验估计，是根据状态方程计算出的数值，为状态矩阵;为控制矩阵。

（2）先验误差协方差矩阵

（7）

其中，

（8）

值得注意的是，在先验估计过程中和测量过程中都存在误差和噪声，而式（7）就是将先验估计中的过程误差考虑进来。

（3）卡尔曼增益

（9）

式（9）表明，将测量误差和先验误差进行融合，转换为卡尔曼增益。当测量误差的方差较大且较小时，卡尔曼增益越小;反之，当测量误差的方差较小且较大时，卡尔曼增益越大。

（4）后验估计

（10）

其中，为观测值，是测量出来的数值，其中，为观测矩阵;为测量噪声。后验估计值由卡尔曼增益权衡，越小，后验估计值越接近先验估计;反之越大，后验估计值越接近测量值。

（5）更新误差协方差矩阵

（11）

3.2   建立基于卡尔曼滤波的定位模型

根据式（6）—式（11）卡尔曼滤波的基本公式，建立以Python为基础的滤波应用模型，实现函数代码如下：

def KalmanFilter（Z_measure）：

global x_last，start_flag， Kk， p_last #定义过程参数

if start_flag == 1：      #初始化先验估计值

x_last = Z_measure

start_flag=2

x_mid=x_last             #先验估计

p_mid=np.add（p_last，Q）    #计算先验误差协方差矩阵

Kk1=np.add（p_mid，R）

Kk=np.divide（p_mid，Kk1）   #计算卡尔曼增益

Sm1=np.subtract（Z_measure，x_mid）

Sm1=np.multiply（Kk，Sm1）

Sm=np.add（x_mid，Sm1）       #计算后验估计

Kk2=np.subtract（[1，1]，Kk）

p_now=np.multiply（Kk2，p_mid） #更新误差协方差矩阵

p_last=p_now

x_last=Sm

return Sm

需要指出的是，建立卡尔曼滤波模型之前要对测量误差的方差矩阵和过程误差方差矩阵赋初值。该模型在初始阶段对先验估计进行赋值，减少了模型从零更新到可靠数值的时间，使得模型更加具有实用性。测量误差的方差矩阵由系统决定，对于不同的设备和测量环境测量误差有所不同，后期精确测试实验要求现场采集，以获得更加精确的定位数据。过程误差方差矩阵可以现场调试。

4   实验测试与结果（Test and results）

4.1   超宽带定位模组与环境搭建

该模块采用STM32F103T8U6单片机为主控芯片，外围电路包括DW1000芯片、天线模组电源转换模块、LED灯指示模块、复位电路等。模块通过串口与电脑通信，通信距离可达300 m。基站和标签实物如图4所示。

本次测试在室内进行，测试基站布置如图5所示，完全模拟现实中室内环境。测试使用的UWB定位模组采用三基站的组网方式。其中，基站标记为A0、A1、A2，移动标签标记为T0，实验时将标签安装在无人智能小车上，为了实验方便简洁，只采用一个标签验证实验结果。采集的定位数据由基站A0通过串口通信方式发送到计算机上。计算机对接收到的数据进行解析和解算，显示相应的坐标位置。

4.2   测试结果与分析

测试分为静态数据测试和动态数据测试。

4.2.1   静态数据测试

在静态测试环境下，智能小车保持不动，持续采集定位数据，采样间隔为100 ms。对采集的定位数据进行标记，图形化显示如图6所示，并且对定位数据进行计算，得出未经过卡尔曼滤波的定位数据期望及均方差和经过卡尔曼滤波的定位数据期望及均方差，如表1所示。分别对横坐标和纵坐标方差进行分析，卡尔曼滤波处理后的定位数据方差明显减小，其中X轴均方差降低47%，Y轴均方差降低38%，数据离散程度降低，明显优于未经滤波的原始定位数据。

4.2.2   动态数据测试

动态数据采集中，为了数据分析方便，现将小车携带标签按照设定轨迹直线行驶，持续观测定位数据，采样间隔为100 ms。图7蓝色曲线是原始定位数据，红色曲线是滤波后的定位数据，很明显滤波后的行驶轨迹更加规则，其定位精度高于5 cm，没有经过卡尔曼滤波的定位数据定位精度高于20 cm。对采集到的数据进行统计得到表2，可以看出滤波前定位数据方差为，而滤波后的定位数据离散程度的定位方差为，方差减少38%以上。卡尔曼滤波明显减小了数据离散程度，提高了定位的精度。

5   結论（Conclusion）

基于超宽带的室内定位技术能很好地定位室内标签，定位精度在厘米级别，对室内定位技术的应用有借鉴意义。本文提出的结合三边定位算法的卡尔曼滤波定位模型定位可靠，在去除噪声和优化定位精度上效果明显。在定位初期将初始值赋给先验估计，减少了初始滤波时间，提高了效率。在该模型中，现场测量的误差协方差大小会影响滤波的响应时间，如果要提高滤波响应时间就要以降低滤波精度为代价。值得一提的是，室内定位技术的应用还应考虑更加复杂的环境，比如遮挡物带来的影响，必要时要进行趋势分析和补偿，进一步提高该模型应用的鲁棒性。

参考文献（References）

[1] 吉华芳.超宽带无线通信系统中同步技术的研究[D].南京：东南大学，2005.

[2] 邓世燕，郭承军.智慧城市建设中的室内定位现状与发展趋势分析[C]//中科北斗汇（北京）科技有限公司.第十一届中国卫星导航年会论文集——S02导航与位置服务.成都：第十一届中国卫星导航年会，2020：131-137.

[3] 余央.NLOS条件下TOA/TDOA定位优化方法[D].成都：电子科技大学，2018.

[4] PORCINOD H W. Ultra-wideband radio technology： Potential and challenges ahead[J]. IEEE Communications Magazine， 2003， 41（7）：66-74.

[5] 杨俊杰.基于UWB的室内定位系统设计[D].哈尔滨：哈尔滨工程大学，2018.

[6] 莫建麟.基于卡尔曼滤波的室内定位方法设计[J].长春师范大学学报，2019，38（04）：43-47.

[7] 岳崇伦，曾苑，郭云开.提高卫星导航精度的卡尔曼滤波算法应用研究[J].测绘工程，2021，30（02）：60-64，71.

[8] 蔡明明，王运锋.基于扩展卡尔曼滤波的时差定位算法[J].现代雷达，2020，42（04）：50-54.

作者简介：

刘永昌（1993-），男，硕士生.研究领域：汽车电子.

龚元明（1964-），男，博士，教授.研究领域：汽车电子与控制，总线与仪表，物联网和工业自动化.