基于卡尔曼滤波的UWB室内定位技术研究

杨 会，沈微微，李传辉，杨 鹏，常智超

（宿迁学院 信息工程学院，江苏 宿迁 223800）

0 引 言

当今社会人们对位置服务的需求越来越迫切，越来越普遍地利用全球导航卫星系统进行室外定位，而在室内环境中存在多径效应、非视距传播和人员流动等因素会导致全球导航卫星系统对室内定位的效果不太理想[1]。目前，常用的室内定位技术主要有红外线定位[2]、超声波定位[3]、RFID近距离定位[4]、WiFi定位[5]、蓝牙定位[6]等，但这些定位技术仍存在稳定性较弱、安全性较差、成本较高、定位精度不够等问题。

超宽带定位技术（UWB）具有穿透力强、功耗低、多径分辨率高、安全性高、定位精度高等优点，在室内静止或者移动的物体和人的定位跟踪与导航的应用上有很大的优势[7-8]；应用场景非常广泛，可用于医院、养老院对医护人员、医疗设备、特殊病人以及老人等进行实时定位跟踪，用于智慧工厂、机场、司法监狱对重要的物品以及人员进行实时定位跟踪，还可用于对矿井下矿工、火灾现场消防员的定位等[9]。

基于UWB技术的室内定位，定位基站数量越多，定位精度越高，但是当定位基站达到一定数量时，无论再增加多少基站，定位精度的提高微乎其微[10]。因此，在选定基站数量并合理布局基站后，选取合适的定位测距方法及定位解算算法是UWB室内定位技术的关键。本文采用了TOF（Time Of Flight）双边双向三消息模式的测距方法，并针对非视距环境下误差较大的问题，提出了采用卡尔曼滤波算法进行优化，从而一定程度上提高了系统的定位精度。

1 系统结构设计

1.1 系统整体设计

基于卡尔曼滤波的UWB室内定位系统主要由可以移动的若干标签以及固定于周围环境中的不少于3个的基站组成，标签和基站之间采用TOF算法来实现系统的定位功能，并采用卡尔曼滤波算法对其进行优化，从而一定程度上提高了系统的定位精度。系统的工作原理如图1所示。

图1 系统的工作原理

系统运行时，首先由标签发送测距开始指令，当各个基站接收到测距指令后，开始进行测距并进行数据传输；最后将全部数据汇总至主基站，经主基站将数据上传至上位机，上位机通过位置解算算法，将标签坐标精确解算并显示出来。

1.2 系统硬件设计

定位系统中标签和基站的硬件设计是一致的，主要由STM32控制模块、UWB定位模块以及电源模块三部分组成，其硬件设计原理如图2所示。其中，STM32控制模块由单片机STM32F103C8T6实现，主要负责数据采集、存储并处理收发的数据；UWB模块主要负责标签和基站、基站和基站之间的数据通信，采用Decawave公司针对UWB信号设计的DW1000信号收发器，该信号收发器可以准确记录UWB信号的到达时间，时间精度达到15.6 ps，可以满足厘米级的定位要求；电源模块主要是为电路中各个模块实现供电管理。

图2 硬件设计

另外，可以通过软件设置的方式将系统配置成标签或者基站，标签以轮询形式与各个基站进行测距通信，最后将测得的各个数据汇总至主基站，由主基站上传至上位机进行显示。主基站还配有蓝牙模块，支持主基站到上位机的无线传输。

1.3 系统软件设计

1.3.1 TOF定位算法

本系统采用TOF定位算法，TOF算法又称飞行时间法，是一种双边双向测距技术，它通过测量UWB信号在基站与标签之间往返的飞行时间来计算距离。信号由标签发送出去时，如果有3个及以上基站接收到，要对每个接收器的到达时间进行测量，将发射点、接收点间距计算出来。然后以距离为半径进行圆形测量，三个圆形的交叉点即为标签位置。TOF定位算法如图3所示。

图3 TOF定位算法

图3中，基站A、B、C测得与标签的距离分别为d1、d2、d3，以各个基站间距作圆，3个圆的交点就是标签位置，设定标签的位置坐标为（x，y），N个基站的位置坐标为（xi，yi）。根据圆的几何意义，它们之间满足的关系为：

接着将公式展开并化简可得：

得到Y=AX后，结合实际需要，计算得出标签位置，即X值，以线性代数规则变换公式，可得

1.3.2 TOF测距原理及流程

TOF测距的过程就是Device A（标签）与Device B（基站）之间进行消息传送的过程，测距原理如图4所示。

图4 测距原理

以四消息为基础，应用三消息模式，能将Device B发送时间戳环节省略，当Device A接收到数据以后，会立刻返回响应，所得时间差分别有Tround1、Treply1、Tround2、Treply2。

整个测距流程大致为：Device A是测距指令发送起点，由Device B来响应。首先由标签发送测距指令，并记录发送时间TX；当基站接收到测距指令时，将测距信息向标签发送，此时记录接收时间RX，由标签来接收响应信息RX，由此将各类最终指令发送到基站。

由上述测距过程，可以测得飞行时间Tprop，函数关系整理为：

利用上述原理设计软件工作流程，如图5所示。第一步，将MCU、DWM1000等初始化处理，读取工作角色后进行测距，需要判断是否为测距目标，当否认时则要返回MCU外设。重新配置以后再判断是否为测距目标，这是单标签、多基站轮询形式的工作方式。当获取时间戳后，计算出来距离传送给卡尔曼滤波函数进行处理，最终将经过卡尔曼滤波算法处理过的数据传到上位机进行坐标解算，将标签的位置解算出来。

图5 软件设计流程

2 卡尔曼滤波算法对定位算法的改进

2.1 定位误差分析

TOF测距相对于其他定位算法精度较高，但是仍存在很大的误差，原因主要有以下三个方面：

（1）时钟同步精度

时间测量其实就是TOF测距时所依赖的重要数据，光速本身约为3×105km/s，所以即便是存在异常微小的误差，在后面计算时都会引起很大的误差，要确定精确的距离，就必须提高收发双方计时系统的标准。

（2）时间延迟

延迟类型主要有发送、接收、天线等。根据测距信号，以数字、模拟信号进行处理及调制时，传输过程中会通过无线电收发形成延迟误差。这就是天线的收发信号均存在显著延迟的原因。不仅如此，延迟还出现非均匀场景，是由同一辐射方向所产生的延迟与另一个辐射方向不一致引起的。

（3）多路径非视距传播

多路径非视距传播是提高信号特征测量值精度的重要基础。两点间无直接传播路径，信号通过信号反射、衍生等会流动到接收端，但第一个到达脉冲时间不一定能反映出真实值结果，因此非视距误差是明显存在的。

2.2 定位算法的改进

由于采用TOF算法定位存在上述误差，所以为了改进TOF算法的缺陷，选取Fang算法、Chan算法、卡尔曼滤波等常用的改进算法，对比这几种算法的优缺点。卡尔曼滤波算法在解决多址路径、时钟迁移问题方面有显著成效，最终确定选取它解决TOF算法的误差问题。

很多情况下都对测量数据有高度精确需求，这是得出状态最优成果的前提。卡尔曼滤波算法是一种系统性的状态估计法，它具有计算量小、容易实现和适应性强等特点，被广泛应用在工程中。可将其划分为两个步骤：（1）预测：将前时刻数据信息作为参考，需要考虑到每个时刻的计算精度；（2）更新：对前面已经估计出的结果，可将精度进一步提升。具体流程函数关系如下。

公式（5）属于状态预测，根据上一时刻的状态对当前时刻的状态进行预测，还要考虑外界环境对系统的作用。公式（6）属于误差矩阵预测，实施预测操作时除了有不确定性预测噪声增加因子Q，还要再考虑原有的不确定性。公式（7）是Kalman增益计算，根据预测结果的不确定性P和观测结果的不确定性R，计算卡尔曼增益（权重）。公式（8）属于状态校正，以加权平均方式处理预测、观测等结果，所得结果是当前时刻状态估计。输出结果表示Kalman滤波最终结果，公式（9）代表误差矩阵更新。式中各变量的含义见表1所列。

表1 各变量说明

由于卡尔曼滤波算法可以解决非视距信道传输的误差，还可以解决时钟延迟的问题，不受基站个数限制，因此卡尔曼滤波算法是最适合改进TOF定位算法的。

2.3 卡尔曼滤波算法的流程设计

利用三消息方式发送接收所得的四个时间差：Tround1、Treply1、Tround2、Treply2，计算出来飞行距离，当每个基站计算出来距离后，将距离参数传递给卡尔曼滤波函数，对数据进行卡尔曼滤波处理。卡尔曼滤波是根据上一时刻的状态和系统误差来预测下一时刻的状态与系统误差，然后根据系统的最优状态来计算，流程设计如图6所示。经过卡尔曼滤波后，将数据汇总至主基站，再上传至上位机进行位置解算。

图6 卡尔曼滤波流程

3 定位精度测试

3.1 卡尔曼滤波的MATLAB仿真

利用卡尔曼滤波算法对定位算法进行优化，将定位误差尽可能降低，为了进一步验证其可行性，采用MATLAB对该算法进行了仿真。选取三基站一标签定位系统，由该系统测得标签和各基站的距离，经MATLAB对所测得的数据进行处理，得到滤波前后的误差对比，如图7所示。

图7 滤波前后对比

图中灰色曲线是所测得的数据直接经TOA定位算法解算出来的位置与实际位置的误差，黑色曲线则是所测得的数据首先经卡尔曼滤波处理后再经TOA解算得到的位置与实际位置的误差。由图中可以看出，滤波前的误差较大，而经过卡尔曼滤波算法处理过的数据曲线更加平滑，误差较小。因此，卡尔曼滤波算法确实可以弥补定位算法所出现的不足，从而提高定位精度。

3.2 卡尔曼滤波前后实测系统数据的比较

3.2.1 测试系统搭建

选取宿迁学院现教中心一楼进行定位系统测试环境的搭建，如图8所示，该系统由三个基站、一个标签构成。此时基站坐标各自对应 A0（0，0）、A1（5，0）、A2（0，5），其中A0作为主基站与PC机相连，标签是可移动的，并在上位机软件中设定好基站的坐标。

图8 各节点的实际布局

静态标签的测试是为了展示加入卡尔曼滤波算法后定位的效果。按上一节方法布置好场景后，将标签依次放置在（0.5，0.5）、（1.5，1.5）、（2.0，2.0）、（2.5，2.5）、（3.0，3.0）、（3.5，3.5）、（4.0，4.0）、（4.5，4.5）、（5.0，5.0）进行定位测试。平面布局如图9所示。

图9 静态标签定位布局

3.2.2 数据比较

首先将未加入卡尔曼滤波算法的程序下载到标签和基站中，标签放置到指定位置，此时将上位机界面显示的坐标记录下来；采用同样的方法，再将加入卡尔曼滤波算法后的程序下载到标签和基站中，将上位机界面显示的坐标记录下来，其测量结果见表2所列。

表2 静态标签定位测试

最后将采集的数据导入MATLAB画出卡尔曼滤波前后数据对比图，如图10所示，可以明显看出滤波后的数据与实际轨迹更加一致。由此得出结论，卡尔曼滤波对TOF测距算法的改进是有效果的，该算法能够使得测算的坐标更加精确。

图10 卡尔曼滤波前后数据对比

4 结 语

本文设计的基于UWB室内定位系统实现了对室内人员及物品的静态定位和动态定位。为了进一步提高该室内定位系统的定位精度，采用了卡尔曼滤波算法对其定位算法进行优化。一方面，用MATLAB对卡尔曼滤波算法进行仿真，通过仿真结果可以明显看出卡尔曼滤波算法对定位精度的提高是非常明显的；另一方面，将卡尔曼滤波算法嵌入到定位系统的硬件程序中，进行了卡尔曼滤波前后的实验，并对实验数据进行比对。可以看出，加入卡尔曼滤波算法在一定程度上对 定位精度是有所改善的，对于提高基于UWB室内定位系统的定位精度有一定的参考价值。