面向超宽带的室内小车动态跟踪方法

匡 兵，陈凤冉，孙毛毛，曾宪锋，景 晖

（桂林电子科技大学 机电工程学院，广西 桂林 541000）

超宽带（ultra wide band，UWB）作为一种室内无线定位技术，因其信号隐蔽性强、数据传输效率高、多径分辨率好等优点［1］，越来越多地应用在室内移动机器人的导航定位中。目标跟踪技术作为UWB室内导航定位领域中的一项重要关键技术，是目前相关高校及企业关注的难题之一。经典卡尔曼滤波器（kalman filter，KF）在线性系统中要求状态空间变量和观察到的变量是连续的、高斯分布［2］。但在UWB室内目标跟踪过程中，由于周围环境存在噪声和测量误差干扰，并非理想中的线性高斯环境，因此KF已经不再适用非线性室内目标跟踪的研究中［3］。

非线性室内目标跟踪的研究中，处理非线性目标跟踪问题尽管存在很大的改进空间，但作为基础研究方法，粒子滤波（particle filter，PF）仍具有很大的研究价值［4］。PF作为非线性贝叶斯估计问题的一种数值逼近方法，目前已有较为成熟的理论和实际应用［5］。2014年，Li X等［6］对传统PF目标跟踪过程中存在的粒子退化问题，研究引入了重采样技术，实验结果得出PF具有更好的实时跟踪性能及跟踪精度。2017年，Peter Rohal等［7］创建了一个粒子滤波器，可以观察二维空间中随机飞行物体的轨迹，实验结果得出：与经典卡尔曼滤波跟踪相比，粒子滤波对雷达实测轨迹跟踪效果更好。

针对非视距室内超宽带目标动态定位跟踪不精确的问题，本文利用适用于线性模型的卡尔曼滤波对测距部分进行滤波降噪处理，有效降低了测距误差。在此基础上采用适用于非线性模型的PF对目标小车进行动态定位跟踪研究，并与EKF跟踪结果进行对比分析。通过实验得出，PF对于小车的定位跟踪效果更好，跟踪轨迹更加接近真实轨迹。

1 UWB测距定位算法

1.1 UWB测距原理

本文选用的UWB传感器主要定位模块，是Decawave公司于2012年推出的基于DW1000定位芯片的DWM1000模块。由于DWM1000模块抗多径衰落能力强、支持高数据速率通信、功耗低等优点，在室内UWB定位领域被广泛应用。为了减小超宽带在测距过程中因时钟偏移导致不必要的过大测距误差，采用DW1000推出的DS测距算法（double-sided two-way ranging，DS）。

为了弥补因时钟偏移而导致的误差，DS测距算法在SS测距（single-sided two-way ranging，SS）原理基础上增加了一次通讯。其测距原理如图1所示。

DS算法的基本测距过程为：标签A首先向基站B发送一个无线电信息并记录下发送时间T1；基站B需要提前准备接收此信号，并在接收到信号之后记录下时间T2；基站B接收到信号在一个特定的延迟时间Tr1后发送回复信号给标签A，此时基站B记录发送时间T3；标签A收到回复信号并记下接收时间T4；标签A在一个特定延迟时间Tr2后发送最终信号给基站B，并记录下时间T5；基站B接收到最终信号，记录时刻T6。

由标签A与基站B之间的测距过程，可计算出信号飞行时间为：

式（1）中：TR1＝T4-T1；TR2＝T6-T3；Tr1＝T3-T2；Tr2＝T5-T4。

由式（1）可知，DS测距得出电磁波在A与B之间的飞行时间，假设电磁波在空气中的传播速度与光速相近为c，则两节点之间的距离为：

1.2 UWB定位原理

无线传感器网络的位置定位算法可分为基于距离（range-based）的定位算法和基于无距离（range-free）的定位算法2种。前者能够利用节点之间的距离等测量值来计算出标签的运动位置，对传感器节点依赖低、定位精度高；后者对传感器设备的要求较小，不需要节点之间的测量信息，但定位误差精确度不高［8］。通过对比分析，基于距离的定位算法更能满足实验要求。

现有技术中，因不同的数学处理方法使得基于测距的定位技术略有不同，如三角测量、三边定位算法及多重网格［9］。其中，三边定位算法由于能减小不必要的计算且容易实现，是无线传感器网络定位的基本测距方法［10］，其定位原理如图2所示。

由图2可知，3个已知基站P1、P2、P3的坐标分别为（x1，y1，z1）、（x2，y2，z2）、（x3，y3，z3）；未知的标签坐标设为（x0，y0，z0）；标签到三基站的距离为各圆半径，分别为di（i＝1，2，3）。

则由三边定位原理得到标签的距离公式为：

将式（3）方程组中各个方程平方之后得到一组非线性方程组，利用数学原理得到其线性化方程为：

利用最小二乘法，通过解算计算出标签的坐标：

由式（3）～式（5）可知，由于无线传感器网络三边定位算法得到的测量定位方程并不是线性的，因此在超宽带定位下，为了得到更加准确的定位跟踪信息，本文采用适用于非线性系统下的PF对目标进行跟踪研究分析。

2 基于UWB室内小车跟踪方法

针对超宽带在非视距室内目标动态跟踪不精确的问题，本文提出一种基于超宽带的室内小车动态跟踪方法。该方法利用KF对超宽带测距部分进行降噪处理，有效降低了因非视距及信号干扰等原因造成的测距误差影响。利用适用于非线性非高斯情况下PF对室内小车进行定位跟踪研究，有效提高了跟踪精度。

2.1 卡尔曼滤波

本文实验场景为小面积室内多障碍物复杂条件，采集到的UWB测距值主要受非视距误差影响严重，波动性较大，通过实际测量得出小车动态情况下的定位均方根误差为18.586 cm。为了得到小车更加精准的位置信息，针对非视距等因素的影响，有必要对UWB测距部分进行降噪处理。

2019年，王川阳等［11］针对UWB定位系统中由非视距等因素导致的测距误差，分别采用小波变换、Vondrak滤波及卡尔曼滤波3种方法对UWB原始观测数据和定位结果2种方案进行降噪处理，实验结果得出卡尔曼滤波方法的降噪效果最优。作为线性模型中的一种经典滤波降噪方法，KF具有处理传感器噪声能力强及计算复杂度较低等优点，能很大程度上降低超宽带室内非视距等影响，减小测距误差。

综上所述，超宽带在室内测距过程中受非视距影响，使得测距数据精度严重受到影响，产生过大的误差，对后续目标跟踪研究极为不利，因此考虑对超宽带测距数据首先进行KF滤波。短时间内，标签与基站之间的距离可认为是均匀变化［12］，距离公式为：.

则基于超宽带测距的KF的状态方程与观测方程离散形式为：

式（6）～（7）中：F、H是系统的状态转移矩阵；T0为数据采样周期；xk为状态向量；wk，vk分别为由协方差矩阵Q，R组成的高斯白噪声。

KF预测方程及状态更新方程定义为：

KF采用线性递推的方式，通过最小均方差来获得最佳线性估计或滤波［13］，并在对多个参数与数据进行处理时，能够实现实时状态更新。由于KF是在已知方差的情况下估计系统状态［11］，因此设

2.2 粒子滤波

超宽带定位过程并不是线性分布，传统的线性跟踪方法在非线性系统中局限性较大，使跟踪精度受到较大影响。PF打破了传统的基于KF延伸的非线性跟踪定位思想，对系统的观测及状态方程不再有更多的线性需求，因此其在非线性系统的跟踪性及适应性更强，跟踪精度及收敛速度更加满足工程需求。

PF实质上是蒙特卡洛方法里增加了一层重要性采样，其依然属于贝叶斯估计的理论范畴［14］。该方法的基本思想是用同一组随机变量（粒子）来递归近似系统状态的后验概率分布，使用近似估计来逼近非线性系统的状态，大数定律表明这个近似估计收敛于真实分布［15］。用大量随机样本建立的系统，基本的后验概率分布为：

式中：N为独立分布的样本数；ωk为k时刻的粒子权重，满足并采用重要性采样（importance sampling，IS）进行选取；σk为Delta函数。

PF预测部分中，粒子通过系统模型在传播过程中添加了噪声，导致传播中的粒子出现了不确定性，从而影响系统模型的准确性。为了解决因噪声问题导致的不准确性，权重函数被用来给每一个粒子分配一个权重值，即ωk。其中，IS的基本原则就是给每个粒子分配权重，即通过当前的分布提取新的粒子，使得重量较轻的粒子消失，重量较大的粒子会存活下来并可能取代较轻的粒子，这实际上就是PF的更新部分［15］。PF的跟踪过程如下：

PF预测过程为：

PF更新过程为：

PF需要大量的粒子才能对非线性系统进行正确的建模，但随着粒子数量的增加，计算复杂度与运算耗费时间也随之增加［16］。如果减少粒子的数目，则会导致有效粒子的数目越小，使得粒子之间权重值差距增大，出现粒子退化现象。针对PF跟踪过程中出现的这种问题，研究在重要性采样之后采用重采样方法。即在保持粒子数目情况下，用一组新的由权重较大的粒子，重新根据自身的权重值按比例分配所得的粒子代替权重小的粒子。

重采样舍弃了权重较小的粒子，并代之以权重较大的粒子，降低了重要性采样算法中存在的权值退化现象。重采样过程在满足条件下，将初始粒子集合更新为即：

初始粒子集经过重采样后，权重较大的粒子代替了较小的粒子，增加了粒子集的可靠性。同时粒子的权重被重新分配为最终PF后验概率分布更新为：

综上所述，超宽带室内导航定位经过PF算法重采样及状态更新后，计算出k时刻系统状态估计值为：

3 实验与结果对比分析

3.1 UWB测距卡尔曼滤波

本文实验场地选用的是500 cm×600 cm室内非空旷场地，3个定位基站的坐标分别为A（0，0，0），B（0，300，0），C（450，550，0），动态运动下目标标签的速度为20 cm／s。为了减小UWB因信号干扰等因素导致的测距数据精度低的问题，在其测距部分首先采用KF进行滤波处理，效果如图3所示。

由于实验所用到的场地并非视距环境下，因此标签在运动过程中，与基站之间的信号通讯随着环境变化而呈现不同频率的波动，导致原始测量得到的数据并非理想中的平滑曲线。由图3可以看出，原始测量数据d1、d2、d3曲线并不光滑，某些采样点的测量数据偏离较为严重。经过KF处理后，曲线kfd1、kfd2、kfd3能有效地减弱原始数据的偏离度，使得采样曲线更加接近于理想平滑曲线，有效降低测距带来的误差影响。

3.2 EKF与PF的动态目标跟踪性能对比

由于实验过程中处于非视距小面积的室内场地，面对复杂的室内环境，实验得出PF的计算复杂度及计算时间，尽管随着粒子数目的增多而增加，但其定位跟踪精度却并非随之严格单调递增。为了得到更加直观的跟踪精度变化，本文定位指标的性能评定标准选用均方根误差（root mean square error，RMSE）。图4为在同一条件下，实验分别采用粒子数目为i（i＝100，500∶500∶5 000）进行PF跟踪时所得到的系统RMSE。

图5为Matlab对PF运算时间拟合结果，其中，PF运算时长为对所采用的不同粒子总数i分别进行10次运算所求取的PF定位跟踪平均运算的时长；图6为EKF跟踪运算时间。

由图4可得，当i＝2 000时，PF在对标签进行动态定位跟踪时RMSE相对较小；由图5、图6可得，通过Matlab对PF跟踪运算时间拟合结果得出，PF的跟踪运算时间随着粒子总数的增多近似呈指数增长。且当i＝2 000时，PF定位跟踪的运算时间为199.189 ms，尽管PF跟踪速度相对EKF较慢，但在非视距室内实验过程中，目标运动速度为20 cm／s情况下，其由重采样技术等引起的运算复杂度所导致的跟踪性误差为3.984 cm，远小于UWB因非视距等因素导致的测量误差，满足实验实时性跟踪需求。

因此，以下PF实验均采用粒子数i＝2 000时所得到的实验结果与EKF进行对比分析。图7与图8为EKF与PF的动态目标跟踪轨迹，从图7、图8可以看出：与原始测量轨迹相比，EKF与PF的跟踪轨迹更加接近真实轨迹。

为了更加清晰地显现出EKF与PF两者之间的跟踪差异性，绘制了两者在X方向与Y方向的跟踪波动偏差对比及滤波轨迹与真实轨迹的RMSE曲线图，如图9～图11所示，其定位跟踪精度统计计算如表1所示。

表1 X、Y方向波动及定位跟踪精度 cm

从表1可以看出，与UWB原始测量定位相比，EKF及PF对原始测量轨迹进行跟踪后，其定位跟踪精度均有不同程度地提高。通过实际计算可以看出，EKF跟踪后的X方向波动性与测量相比降低了9.772%，Y方向波动性降低了7.346%，定位精度提高了6.968%；PF跟踪后的X方向波动性与测量相比降低了15.447%，Y方向波动性降低了22.736%，定位精度提高了20.074%；PF跟踪后的X方向波动性与EKF相比降低了6.290%，Y方向波动性降低了16.610%，定位精度提高了14.088%。

由实验最终定位跟踪精度得出，PF的跟踪效果更好，跟踪实时性满足实验需求。与EKF跟踪相比，其在X方向与Y方向的跟踪波动更小，定位跟踪精度相比提高了14.088%，与真实轨迹更加接近。

4 结论

通过对UWB测距原理进行分析，采用适用于线性模型的KF对测距部分进行了滤波降噪处理，有效克服了UWB因室内非视距、信号干扰等因素导致的部分测量数据误差较大的情况。利用PF对标签目标进行动态跟踪，建立了超宽带室内小车非线性动态跟踪系统模型。通过实验对比分析了EKF与PF两者基于超宽带室内目标动态定位跟踪效果。实验结果得出：PF的跟踪效果更好，在X方向与Y方向的跟踪波动更小，与真实轨迹更加接近，可为室内非线性系统模型下超宽带目标定位跟踪提供参考。