基于KF/PF测距滤波的UWB精确定位试验研究

姚 健，刘送永，崔玉明，顾聪聪

(中国矿业大学 机电工程学院，江苏 徐州 221116)

1 概 述

我国煤炭开采已从机械化、自动化正逐步向智能化迈进[1]，发展智慧煤矿是我国煤炭工业发展的必由之路[2]，井下精确定位是智慧煤矿精准控制的基础[3]，UWB(Ultra Wide Band)技术因其功耗低、传输速率高[4-5]、穿透能力强[6]、高灵活性、高可靠性以及高精度测距等特性，在井下高精度定位领域得到广泛的应用。

在UWB定位系统中，当移动目标不能满足视距条件时，信号的传播只能通过折射和衍射等非视距方式，此时就会造成一定的NLOS(Non-Line-of- Sight)误差[7]，如图1所示，在发射端信号无法直接到达接收端，会以反射和折射的方式绕过障碍物，得到的测距值就会大于真实值，从而造成测距和定位误差，另外定位过程中的环境噪声以及其它干扰都会对UWB定位结果产生影响，产生测距异常值[8]，因此，亟需对UWB定位过程中，NLOS误差和噪声的抑制进行研究。

在UWB定位方面，国内外学者已经进行了大量的研究，唐春玲等人[9]将井下人员定位作为研究对象，对比了UWB以及另外三种定位技术的环境适用性，结果表明，UWB定位技术在井下定位方面有着非常大的发展潜力；赵文生等人[10]结合井下工作面实际工作环境，对超宽带脉冲信号模型和测距原理进行了深入研究，通过对通信参数的优化，将UWB测距误差控制在50cm以内；Guvenc等[11]提出一种基于多径信道统计信息的NLOS识别技术，开发了联合似然比，实现LOS/NLOS的有效识别；肖竹等人[12，13]对UWB定位研究的发展历程和研究现状进行了详细的分析，指出了UWB定位仍存在的问题和发展趋势，通过对NLOS信道模型的仿真，证明基于最强路径检测的TOA估计方法，可以有效抑制NLOS误差；张健铭等人[14]提出一种UWB/PDR融合定位方案，利用支持向量机回归模型对复杂环境中UWB定位误差进行预测，并在扩展卡尔曼滤波的基础上添加自适应调节系数，有效抑制了NLOS误差，提高了UWB定位的精度和可靠性；Gonzalez等[15]将UWB测距值和里程计测距值通过粒子滤波器进行融合，有效抑制了由于多径效应而产生的定位误差；朱带先等人[16]采用粒子滤波算法对UWB定位进行估计，并采用TDOA/RSSI联合定位技术进行位置修正，有效降低了多径和NLOS误差的影响。

现有的研究多集中于对UWB整个定位系统以及联合外部定位方式进行数据融合，进而实现定位误差的修正。本文在UWB定位原理的基础上，从UWB测距值入手，采用卡尔曼滤波和粒子滤波对UWB测距信息进行滤波处理，从而抑制NLOS误差和随机噪声的影响，以提高UWB定位精度和可靠性。

2 UWB定位原理

UWB定位一般要先获取测距信息，然后建立位置坐标解算模型，以测距信息作为数学模型的输入，求解坐标信息，测距定位原理图如图2所示，设目标的坐标为P(x，y)，定位区域内有n个基站，坐标分别为A0(x0，y0)，A1(x1，y1)，A2(x2，y2)，A3(x3，y3)….An(xn，yn)，通过UWB测距可以得到目标点距离各基站的距离，设为d1，d2…dn，则有：

对式(1)进行分解移项可得：

AX=b

(6)

X=(ATA)-1ATb

(7)

由此即可求得目标点的坐标。

3 测距滤波

UWB实现精确定位的基础是精确的测距信息，在实际工作过程中，测距结果经常因为NLOS和噪声干扰出现较大的误差，因此亟需对测距值进行滤波，以提高测距精度。以UWB基站和目标节点间的测距值以及测距变化率作为状态参数X=[dd′]T，则测距模型的状态方程为：

Xk+1=FXk+wk

(8)

测距模型的观测方程为：

Yk=HXk+vk

(9)

3.1 卡尔曼滤波

在工程实际中，观测信号中不可避免的夹杂着噪声，状态变量的真实值往往不能直接得到，而卡尔曼滤波(Kalman Filter，KF)能有效降低噪声影响，且其计算过程较为复杂的缺点，也已随着计算机技术的发展得到有效解决[17，18]，目前卡尔曼滤波在目标跟踪、导航和制导等高科技领域得到越来越广泛的应用。

卡尔曼滤波通过对系统状态进行最优估计，抑制观测数据中噪声和干扰的影响，从而获得更加准确的状态估计。

卡尔曼滤波的一般过程为：

式中，Pk为k时刻协方差矩阵，G是噪声驱动矩阵，卡尔曼滤波首先对状态进行一步预测，然后预测协方差，求滤波增益矩阵，而后再进行状态更新，协方差跟新，如此循环计算，就可以有效抑制NLOS误差和随机噪声干扰。

3.2 粒子滤波

卡尔曼滤波是高斯模型条件下的状态估计，而粒子滤波(Particle Filter，PF)是非线性非高斯模型条件下的状态估计算法，是一种基于蒙特卡洛的近似贝叶斯滤波，从概率角度出发，以样本均值作为最终的参数估计[17，19，20]。在一些精度要求较高而经典的分析方法又解决不了的场合，有着巨大的发展潜力。目前，粒子滤波在机器人定位、目标跟踪等领域已经得到较为广泛的应用。

粒子滤波的一般过程为：

2)重要性采样。

3)粒子权值更新及归一化权值更新：

权值归一化：

4)重采样。重要性采样会带来粒子重要性加权退化、有效粒子数减少的问题，因此必须进行重采样，重采样首先将粒子进行排序，然后用权值大的粒子代替权值小的粒子。

5)状态估计。

3.3 测距滤波

KF/PF测距滤波误差如图3所示，UWB测距结果中包含较多的噪声以及误差，静态测距波动明显，误差波动范围大于5cm，其中因为噪声的干扰，出现多个测距异常值，测距误差超过15cm，经过卡尔曼滤波以及粒子滤波处理后，有效抑制了测距异常值，且测距误差明显减小，卡尔曼滤波后最大测距误差小于3cm，粒子滤波的效果明显好于卡尔曼滤波的效果，测距误差小于1cm。

图3 KF/PF测距滤波误差

4 实验与分析

实验中使用的UWB定位模块为深圳空循环科技有限公司生产的Link-Track UWB定位模块，模块标签与基站一体，具体参数见表1，由四个基站和一个标签组成UWB定位系统。

表1 UWB定位模块参数

实验场景为地下车库，如图4所示，使用四个基站，以基站A0为坐标原点，A0-A3为定位坐标系的X轴正方向，A1、A2在Y轴正方向一侧，基站坐标见表2。

图4 实验场景

表2 基站坐标

4.1 静态定位

图5 KF/PF静态定位

静态定位如图5所示，五角星所示点为静态定位点的实际坐标，直接解算得到的静态定位结果跳动范围更大，经滤波处理后的定位跳动范围明显减小，且PF测距滤波的范围最小，KF次之。定位误差见表3，UWB直接根据测距结果计算得到的定位结果存在较大的定位误差，最大定位误差为9.41cm，测距定位跳动较大，X方向最大误差2.94cm，Y方向最大测距误差为7.43cm，在经过卡尔曼滤波和粒子滤波之后定位精度明显提高，定位跳动误差明显减小，卡尔曼滤波后X方向最大定位误差1.28cm，相比UWB测距直接解算降低了56.4%，Y方向最大定位误差3.83cm，相比UWB测距直接解算降低了48.5%，最大定位误差为4.51cm，相比UWB测距直接解算降低了39.3%，粒子滤波的静态定位效果最好，X最大定位误差小于1cm，相比UWB测距直接解算降低了70.7%，相比卡尔曼滤波降低了32.8%，Y方向最大定位误差1.18cm，相比UWB测距直接解算降低了84.1%，相比卡尔曼滤波降低了69.1%，最大定位误差为1.83cm，相比UWB测距直接解算降低了75.4%，相比卡尔曼滤波降低了59.4%。同时，受UWB基站布置形式的影响，X方向上的静态定位误差明显小于Y方向上的误差。

表3 静态定位误差

4.2 动态定位

手持目标节点，按预设轨迹行走一圈，四个基站与移动节点间的动态测距结果如图6所示，行走过程中因为人体的遮挡存在一定的NLOS误差，又因为测距过程中随机噪声的影响，UWB测距值存在明显的抖动现象，在经卡尔曼和粒子滤波处理之后，抖动明显减少，曲线更加光滑。

图6 动态测距滤波

动态定位结果如图7所示，按预定行走过程中，因为人体遮挡产生的NLOS误差和噪声干扰，直接根据测距值解算得到的UWB定位结果存在明显的定位跳动现象，经KF以及PF测距滤波处理后，得到的定位结果相比直接解算得到的结果，轨迹更加平滑，更加贴近真实轨迹。定位误差见表4，UWB直接测距解算的定位结果在整个行走过程相较于参考轨迹存在明显的跳动误差，X方向最大误差12.12cm，Y方向最大测距误差为24.85cm，最大定位误差为36.41cm，在经过卡尔曼以及粒子测距滤波之后，UWB定位跳动误差明显减少，定位曲线更加光滑，卡尔曼滤波后，X方向最大误差11.31cm，Y方向最大误差为21.77cm，最大定位误差22.13cm，相较于UWB直接测距解算，X方向降低了6.68%，Y方向降低了12.39%，最大定位误差降低39.21%，粒子滤波后，X方向最大误差8.94cm，Y方向最大测距误差为10.92cm，最大定位误差为18.03cm，相较于UWB直接测距解算，X方向降低了26.24%，Y方向降低了56.1%，最大定位误差降低了50.48%，相较于卡尔曼滤波，X方向降低了20.9%，Y方向降低了49.8%，最大定位误差降低了18.5%。同时，受UWB基站布置形式的影响，X方向上的动态定位误差明显小于Y方向上的误差。

图7 KF/PF测距滤波动态定位

表4 动态定位误差

5 结 论

1)采用卡尔曼和和粒子滤波方式，可以有效抑制UWB测距过程中的NLOS误差和随机噪声干扰，粒子滤波相较于卡尔曼滤波抑制效果更好。

2)静态定位中，卡尔曼和粒子滤波都可以很好的抑制随机噪声的干扰，卡尔曼测距滤波后的定位精度可以达到4.51cm，粒子测距滤波相较于卡尔曼滤波静态定位精度提高59.4%。

3)采用卡尔曼和粒子测距滤波的方式进行UWB动态定位，可以有效降低NLOS误差和噪声的影响，提高UWB定位的精度和可靠性，卡尔曼测距滤波动态定位精度可以达到22.13cm，粒子测距滤波动态定位精度相较于卡尔曼滤波提高18.5%。

4)UWB基站布置方式会对UWB定位精度有产生很大影响，针对基站布置方式与定位误差之间的关系，还需进行更加深入的研究。