朱润驰，王洪源，陈慕羿

(沈阳理工大学 信息科学与工程学院，沈阳110159)

军用机器人中最为常见的是轮式移动机器人，可代替人类进行侦察、排爆、作战等工作，其在运动方面具有移动速度快、机动能力强、控制方式简易等特点[1]。考虑到军用机器人的特殊性及室内弱GPS环境对定位的影响较大，对其室内精确定位稳定性、准确性及智能性均提出较高要求[2]。目前对军用机器人精确定位的研究大多为仿真模拟，缺少实验对比。

超宽带(Ultra Wide Band，UWB)定位技术是一种无线载波通信技术，具有区别于其他定位技术的时间分辨率高、抗多径能力强、穿透性能好等优点，其响应频率能达到30Hz以上，确保了运动体实时定位的关键性需求。但由于其超高频段，加之真实应用环境复杂多变，当系统所处环境中存在障碍物遮挡信号等情况出现非视距(Non Line of Sight，NLOS)误差时，会使测量精度大大降低，影响定位结果[3]。

消除NLOS误差成为目前精确定位研究的一个重要方向，许多学者针对如何改善NLOS误差进行了大量研究。文献[4]建立了卷积神经网络(Convolutional Neural Networks，CNN)定位模型，采集数据后进行训练，通过扩展卡尔曼滤波进行目标位置估计。文献[5]采用最小二乘法区分NLOS环境，用泰勒级数展开计算视距，采用Chan氏定位算法计算NLOS传输情况，然后进行组合定位。文献[6]重新定义了卡尔曼增益矩阵，使时钟漂移得到补偿，从而使主从基站保持时钟同步，并仿真分析了带宽与频谱密度对于算法的影响。上述方法各有优劣，但多为仿真分析，未考虑基于NLOS误差建立的定位模型中也存在影响定位结果的参数。

本文设计一种改进的粒子滤波定位模型，将标准粒子滤波算法(Particle Filter，PF)运用于UWB定位系统，针对原算法的重采样环节存在样本贫化的问题，使用马克可夫链蒙特卡洛方法(Markov Chain Monte Carlo，MCMC)优化重采样，避免样本贫化；通过残差平方和(Residual Sum of Squares，RSS)算法判定当前系统受到NLOS误差影响程度，设置阈值对测量数据进行选择性剔除；设计UWB测距模块进行实验，将实验结果通过均方根误差(Root Mean Squared Error，RMSE)进行算法精度分析。

1 超宽带定位技术原理

UWB定位技术的最大优势是采用了时间间隔在1ns内的脉冲进行通信，主要原理是通过系统主基站与定位标签之间的信号往返传输飞行时间(Time Of Flight，TOF)来计算距离，从而得到更准确的测距结果。图1所示为双边双向飞行时间法，即采用两个往返时间来计算飞行时间的方法，目的是消除时钟误差和偏差误差[7]。

图1 双边双向飞行时间法

UWB系统用于标签定位的基础算法通常使用到达时间差(Time Difference of Arrival，TDOA)方法，其原理是标签通过检测脉冲信号到达两个基站的时间差值来确定此时的位置，而非检测信号到达目标基站的绝对时间。TDOA的三基站定位图如图2所示。

图2 TDOA三基站定位图

将到达时间的差值乘以电磁波传播速度c，得到距离差值，即

(1)

式中：li，j为定位标签到编号i基站与定位标签到编号j基站的距离差值，i≠j；i，j=1，2，…，k；li、lj分别为UWB标签到编号i和编号j基站的测距值；k为系统布置的基站总数；Ti、Tj分别为信号在编号i和编号j基站与标签之间传输所用时间。

将系统中编号为z的基站设置为系统中心基站，TDOA双曲线模型可表示为

(2)

式中：li，z为标签到i基站与标签到中心基站z的距离差；(x，y)表示标签真实位置坐标；(xi，yi)表示i基站的平面坐标；(xz，yz)表示中心基站的平面坐标。

实际应用中，UWB系统会受到很多因素的干扰，影响其定位的精度。研究发现，影响UWB系统定位的最主要因素是环境存在的NLOS误差，其产生的原因是标签在进行信号收发的路径上存在障碍物，导致系统传输信号无法进行视距传播，而在系统环境中反射或折射传播，定位精度会受到较大影响。

2 改进粒子滤波定位模型

2.1 标准粒子滤波算法

PF算法的核心思想是通过随机抽取一组样本(粒子)来代替当前系统状态的后验概率分布。当系统处于非线性环境时，相比于其他算法，使用该算法具有更好的滤波性能与处理非高斯噪声影响的能力，故在定位系统中应用越来越广泛[8]。本文将粒子滤波算法运用至UWB系统，估计t时刻运动体的定位坐标Xt，其系统模型由两个非线性函数表示为

Xt+1=f(Xt，ut)

(3)

zt=e(Xt)+vt

(4)

式中：Xt+1表示系统的下一时刻定位坐标；zt为当前时刻标签与各基站的测量距离；ut、vt分别为系统存在的NLOS误差和测量噪声；f为运动位置转移函数；e为观测函数。

标准粒子滤波算法在每个时刻都会生成离散的权重粒子集，即

(5)

利用st对定位坐标的后验概率密度p(Xt|zt)进行求解，过程主要分为三个阶段。

(1)初始化

(2)序贯重要性采样

先对参考分布函数q(Xt|Xt-1，zt，ut-1)中加权的粒子进行随机采样。利用贝叶斯原理，系统的后验概率密度p(X0∶t|zt)变换为

(6)

式中p(zt|z1∶t-1)为常数。式(6)可变换为

p(X0∶t|zt)∝p(zt|Xt)p(Xt|Xt-1)p(X0∶t-1|z1∶t-1)

(7)

观测模型p(zt|Xt)表示在当前定位坐标Xt处，UWB标签测量信息为zt的概率，计算式为

(8)

式中：δ表示过程中的测量噪声方差；k表示方差维度。

对每个粒子重新分配权重，即

(9)

归一化权重为

(10)

(3)重采样

当UWB定位系统周围环境发生变化出现NLOS误差时，由于步骤(2)中概率密度的选择未考虑当前时刻的实测值，故先验概率密度函数和真实的后验密度函数的样本会出现较大不同。算法结果会随着时间不断迭代，导致权重粒子集st中少数粒子的权重逐渐增大，其余粒子权重逐渐降低，该现象称为粒子退化现象[9]。引入采样有效阈值neff来处理该问题，即

(11)

neff越小，说明退化现象越严重。当neff小于限定值时，通过重采样的方法来处理退化现象，对加权粒子集使用“优胜劣汰”原则去除小权重粒子。

2.2 辅助重采样算法

重采样能够在一定程度上改善退化现象，使UWB系统在出现NLOS误差时的定位精度不受较大影响。但由于粒子的逐渐替代更新会导致模型出现样本贫化现象，使加权粒子集的多样性逐渐减弱，UWB定位结果出现持续偏移与误差累积等状况。

针对模型样本贫化问题，解决方式可以在重采样阶段加入MCMC方法。核心是建立稳态分布为π的马尔可夫链，从中得到π的样本，其具有与所期望目标密度等价的稳态分布，能够进行各种统计推断[10]。PF-MCMC实现步骤如下。

(12)

(4)状态更新

(13)

(14)

2.3 NLOS误差判定

实际应用出现较大NLOS误差时，会将原始测量值直接输入到算法模型中，导致滤波发散，使定位误差逐渐增大。判定系统的定位NLOS误差通常有两种方式：一是对系统测得的数据直接进行处理，达到减弱NLOS误差的目的；二是先识别系统信号传输过程中是否存在NLOS误差，再通过优化算法处理误差产生的影响。

考虑到实际应用中不能实时获得真实定位坐标等问题，本文将NLOS误差判定方法进行改进，通过已知的中心基站坐标(xz，yz)，利用RSS判断当前时刻的系统定位精度受NLOS影响程度，即

(15)

具体判定步骤为

(1)将Dt与dt带入判定式；

(2)设置阈值Δ；

(3)当SSR≥Δ时，表示原始定位偏移过大，剔除此时存在较大NLOS误差的测距数据。

阈值由多次测试实验分析后计算得出，其值过小将使系统失去部分有效测距数据。

3 实验测试与结果分析

3.1 实验设备与环境

实验采用DecaWave公司生产的DWM1000芯片，设计其相关外部电路，开发了搭载于Arduino UNO核心板的超宽带模块。硬件模块的具体工作流程为：上电后，基站模块检测周围是否有标签模块；当检测到环境周围有标签模块时启动连接程序，与标签进行连接数据传输，实现测距。本文设计UWB测距模块的印制电路板(Printed Circuit Board，PCB)与实物设计如图3所示。

图3 UWB测距模块

测试场景选取占地面积4m2的空间环境，为保证系统在测量时空间的统一性，将超宽带模块安装在平台的正中心位置。同时，为保证各模块空间位置的准确性，使用激光水平仪与激光测距仪测量场地的空间距离。实验环境如图4所示。通过实际环境的位置制定测试环境的相对位置坐标，其中超宽带基站模块的坐标分别为基站1(0，0，10)、基站2(200，0，10)、基站3(100，173.2，10)(坐标单位：cm)。

图4 实验环境

3.2 实验结果分析

为验证算法的可行性及相比于原算法的优势，分别进行两组实验。第一组实验为空旷环境中的定位效果；第二组实验在环境中放置障碍物来模拟遮挡信号等非视距情况。区域内机器人的实际行进路线设置为

A(50，0)→B(150，0)→C(150，86.6)→D(50，86.6)→A(50，0)(坐标单位：cm)。

每组分别进行10次重复实验，通过3σ准则去除各次实验数据的较大误差值，然后将剩余数据取平均值，作为该点的定位坐标。

第一组实验为无NLOS影响时定位效果，直接使用TDOA原始算法的定位轨迹与偏差度如图5所示。

图5 实验一定位轨迹对比图

第二组实验为受NLOS影响时定位效果，在环境中加入体积为2500cm3的木制阻挡物，来模拟NLOS环境。TDOA原始算法定位、PF算法定位、PF-MCMC算法定位的轨迹对比结果及与真实轨迹的偏差度如图6所示。

图6 实验二定位轨迹对比图

由图5和图6定位结果对比可知，在无阻挡物的环境中，TDOA原始算法的定位效果良好；处于NLOS环境中时，直接使用TDOA原始算法进行定位受NLOS干扰较大，造成定位误差较大。

为更直观地显示各算法之间的差异，采用RMSE衡量各算法定位结果与真实定位结果的偏差(保留三位小数)，对定位精度进行定量分析。

(16)

式中：(xt，yt)为使用各算法时得到的相应定位坐标；(xturet，yturet)为该时刻直接测量得到的真实坐标。

各算法定位结果的精度定量分析RMSE曲线如图7所示。

图7 RMSE对比曲线

由图7可以看出，使用标准粒子滤波算法后，优化了系统定位，但随着运动过程中出现NLOS干扰，样本逐渐贫化，误差逐渐增大；在标准粒子滤波算法中加入MCMC步骤，能够在系统出现

NLOS干扰时，增大部分随机粒子的权重值，保证模型粒子的多样性，避免局部最优的情况出现，使最终的模型定位结果更加接近真实轨迹。

4 结论

针对标准粒子滤波算法存在的粒子贫化、计算量大等问题进行了改进，提出了一种基于MCMC的改进粒子滤波定位模型。该定位模型能够实时更新随机粒子权重，优化在NLOS环境下出现的定位偏移与误差累积问题。设计基于DWM1000的UWB硬件模块系统，通过实验对比验证了改进算法可以减弱粒子贫化现象，进而达到精确定位的效果。结果表明，本文模型能够在非视距的环境中有效提高定位精度，满足军用机器人的室内定位需求。但文中只考虑了二维环境中的系统定位效果，而未考虑到Z轴方向上的位移对系统定位准确度的影响，对三维环境中的定位是今后需要进一步研究的重要内容。