基于两步Kalman 滤波的NLoS 误差抑制算法

贾时蕾，王潜心

( 中国矿业大学环境与测绘学院, 江苏 徐州 221116 )

0 引言

随着GPS 和北斗卫星导航系统(BeiDou Navigation Satellite System，BDS)的迅猛发展，目前户外定位和导航问题已经得到了很好地解决[1-2]. 而在室内定位方面，虽然广大学者已经提出了各种技术手段和定位方法，但是仍未得出通用的解决方案. 现代人类室内活动时间增加以及室内定位技术在救援、精准营销、智能停车、智慧城市等领域具有的广泛应用价值使得人们对于高精度的室内定位需求日益迫切[3-4].相较于当前使用的其他定位方法，超宽带(ultra wide band，UWB)具有大带宽、时间分辨率高和抗多径干扰等优点，被认为是最有前途的无线协作室内定位解决方案之一[5-7]. 但UWB 信号在传输过程中受到遮挡无法直线传播时会产生非视距(non line of sight,NLoS)误差，该误差会因在基于信号到达时间(time of arrival, TOA)距离估计算法中引入正偏差而严重影响定位性能[8-10]，特别是在复杂的室内环境中. 因此，减少NLoS 误差对定位结果的影响是UWB 定位过程中的主要目标.

针对NLoS 误差抑制问题，近年来各种文献已经提出了许多卓有成效的定位算法，其中最广为流行的方法主要分为消极加权和视距(line of sight, LoS)重构两种[11]. 文献[12]提出一种基于泰勒展开的加权最小二乘估计方法，虽然适用性较强，但需要用于确定权重的统计信息. 文献[13]提出一种残差加权算法，虽然不需要关于权重的统计信息，但需要较多的基站参与定位. 文献[14]利用TOA 测量值的时间历史对LoS 环境下的TOA 测量值进行重构，定位误差较小但不能实时定位. 文献[15]对Kalman 滤波器进行改进，将 NLoS 误差作为状态向量的一部分引入到Kalman 滤波中，在一定程度上改善了定位精度.文献[16]将测距残差与Kalman 滤波结合，并将残差值引入Kalman 滤波迭代过程中以修正Kalman 滤波的异常值，有效抑制了NLoS 误差的影响. 但由于NLoS 误差随机性的特点，上述方法在LoS/NLoS 混合环境下的实际使用性能并不稳定.

从传统方法的不足与局限性出发，本文提出了一种基于两步Kalman 滤波的NLoS 误差抑制方法：在第一步Kalman 滤波中，将NLoS 误差作为第三个参数加入待估计的状态变量中，借助NLoS 误差与测量误差之间的相互独立性直接估计出 NLoS 误差，并将其与设定阈值比较以鉴别NLoS 误差是否存在，若存在，则将初始TOA 测距值减去NLoS 误差初步估计后的值作为第二步Kalman 滤波的输入，第二步Kalman 滤波针对受NLoS 误差影响较大的测距值进行二次NLoS 误差估计、鉴别和消除，并将减去二次NLoS 误差估计后的输入值作为最终的定位解算依据. 通过对NLoS 误差二次估计和二次剔除从而实现室内复杂环境下的精准实时定位.

1 NLoS 误差对TOA 定位的影响

假定LoS 环境下移动站在k时刻与第i个基站之间的TOA 测距值为ri(k)，其通常可表示为

式中：di(k) 为k时刻移动站和第i个基站之间的真实距离；ni(k)为标准时间所造成的测量误差，一般被认为是服从0 均值高斯分布的高斯随机噪声[17-18]. 而在NLoS 环境下测距值除了包含测量误差外，还包括NLoS 传播环境引起的随机测量误差，即NLoS误差. 此时TOA 测距值可表示为

式中，非视距误差 NLoS(k)仅与信号传播环境有关，在不同的信道环境中其可能服从指数分布，均匀分布或高斯分布[19]. NLoS 误差存在的情况下，移动站与基站之间的测距值会被引入正值的误差分量，将该测距值用于定位解算必然会导致定位精度的降低，如图1 所示.

图1 NLoS 误差对TOA 定位的影响

LoS 环境下的TOA 定位解算应为三圆交点，但由于NLoS 误差的存在，该点拓展为三圆交叠的区域. 因此想要获取高精度的UWB 定位服务，NLoS误差是必须解决的一个关键问题.

2 基于两步Kalman 滤波的NLoS 误差抑制

2.1 Kalman 滤波基本原理

Kalman 滤波问题实质上是以某种最优方式联合求解未知状态方程和测量方程的问题[20]. 在动态定位方面，Kalman 滤波及其变体形式能够为抑制NLoS传播误差提供可行的解决途径，从而改善无线环境中信号TOA 的估计[21-22].

Kalman 滤波器的状态方程和测量方程分别为：

式中：s(k)和z(k)分别为第k时刻的状态向量和观测向量；A和G分别为状态转移矩阵和观测向量系数矩阵；w(k)和v(k)分别是第k时刻的过程噪声和测量噪声.

Kalman 滤波主要包括预测和更新两个阶段：在预测阶段，滤波器根据上一时刻的估计值对当前时刻的状态进行估计；在更新阶段，滤波器使用当前时刻的测量值来优化预测阶段获得的预测值[23]. Kalman滤波的具体迭代过程如下：

2.2 NLoS 误差识别与抑制

由(2)式可知，NLoS 环境下的TOA 测距值由真实测距值、测量误差以及NLoS 误差三者叠加而成.鉴于标准测量误差与随机NLoS 误差产生的原因不同，可以针对误差独立性对NLoS 误差进行分离，并重构测距值. 在第一步Kalman 滤波过程中，将NLoS 误差作为第三个分量加入状态方程，此时各向量矩阵设置如下：

式中：r(k)为第k时刻基站与移动站间的距离；r˙(k)为第k时刻移动站相对基站的移动速度； NLoS(k)为NLoS 误差；T为采样间隔；β为实验选取NLoS 误差系数参数；ω为过程噪声分量；n为测量误差. 考虑到NLoS 误差的非负性特点，在迭代过程中要保证每一时刻的NLoS 误差估计都符合实际. 因此每得到一个状态估计就需要对NLoS 状态分量进行一次符号判断 ，当其不符合实际情况时，需将其设置为0.

完成一次迭代过程后，将该过程估计的NLoS误差状态分量与给定阈值 θ进行比较以鉴别NLoS误差是否存在. 阈值取值对于NLoS 误差的消除十分关键，若设置过大，则无法有效除去TOA 测距值中的NLoS 误差，若设置过小则可能会导致Kalman 滤波器不收敛的问题. 本文通过在多次实验中采用不同的门限值，考察估计结果的误差和收敛情况以确定最终的阈值取值. NLoS 误差鉴别过程如下：

由于NLoS 误差的随机性会引起NLoS 误差的剧烈变动，故而某些偏差较大的TOA 测距值在第一步Kalman 滤波时可能未被消除干净，考虑在第二步Kalman 滤波时再次对NLoS 误差进行估计并与给定阈值 θ进行比较以判断是否需要对输入值进行修正.

图2 展示了本文提出的基于两步Kalman 滤波的NLoS 误差抑制算法流程. 对每一个定位基站首先获取当前测距值，然后利用第一步Kalman 滤波对NLoS 误差进行估计并判断该测距值是否属于NLoS测距值，以对TOA 测距值进行合理修正，在第二步Kalman 滤波时则利用第一步Kalman 滤波得到的输入值对NLoS 误差进行二次估计、鉴别及修正以提高TOA 测距精度，最终实现UWB 精准定位.

图2 算法流程图

3 仿真实验及结果分析

为了验证本文所提方法的有效性及合理性，模拟LoS/NLoS 混合室内场景进行仿真验证. 模拟场景中布设四个UWB 模块，设定UWB 模块采样时间间隔为T(T=1 s)，移动站初始位置和区域内的三个UWB 参考基准站坐标已知，移动站以0.2 m/s 的速度沿固定路径匀速运动，路径全长20 m，前期运动过程中基准站均处于视距状态，后期由于移动障碍物的出现致使基站1 处于长达50 s 的NLoS 状态. 实验环境如图3 所示.

图3 实验环境示意图

假定实验过程中的测量误差服从均值为0，标准差 σ为0.2 的高斯分布，NLoS 误差服从指数分布[24]，

式中，τRMS为均方根(root mean square，RMS)时延拓展，作为与传播环境密切相关的随机量，其服从对数正态分布，并且可以通过四个环境依赖变量进一步表征为

式中：T1为 τRMS在1 km 处的中值；d为移动站与基站之间的距离；ε为介于0.5~1.0 的指数分量；ξ为对数正态随机变量(10logξ服从均值为0，标准差为 σξ的高斯分布). 具体参数设置如表1 所示，本文实验采用一般市区无线信道环境进行NLoS 误差模拟.

表1 不同环境下的模型参数

在进行NLoS 误差抑制实验之前，首先对本文所提方法的NLoS 误差估计性能进行测试. 图4 为同一基站在不同环境下对移动站观测100 s 后，利用本文算法对不同环境进行NLoS 误差估计得到的结果. 可以看到，LoS 环境下NLoS 误差估计值在前期由于初始给定的NLoS 误差分量略有波动，但随着滤波器的稳定，NLoS 误差估计值逐渐平稳地趋向于0. 同时NLoS 环境下滤波估计得到的NLoS 误差同实验仿真NLoS 误差趋势总体一致，特别是对较大的NLoS误差实现了良好的跟踪估计.

图4 NLoS 误差估计性能

将仿真实验环境下影响基站1 测距过程的仿真NLoS 误差与利用本文方法估计得到的NLoS 误差进行对比，以分析验证滤波器是否具有环境转变的感知能力，结果如图5 所示. 可以看出，运动前期滤波估计所得NLoS 误差整体分布较为稳定，幅值趋近于0 且均小于给定阈值，NLoS 误差判别效果与现实情况相符；在移动站运动第50 s 时滤波估计所得NLoS误差值开始出现波动，幅值相较于运动前期开始逐渐增大且总体趋势与实验仿真NLoS 误差保持一致. 该结果表明滤波器不仅具有良好的跟踪效果，对于LoS/NLoS 环境转变的感知也具有较强的灵敏性.

图5 仿真NLoS 误差与估计NLoS 误差对比

针对上述实验环境下基站1 所得测距值进行视距重构，重构效果如图6 所示. 可以看出在LoS 阶段，经本文算法滤波后测距值与原始测距值并无二异，而在NLoS 阶段，经过第一次Kalman 滤波后，NLoS 测距值相较于原始测距值整体有所降低，但一些受NLoS 误差影响较大的测距值仍需进一步修正；经过第二次Kalman 滤波后，距离估计值已经非常接近真实的测距值，相对于第一次Kalman 滤波，测距值修正结果得到了明显的改善. 该结果表明本文所提方法对于TOA 测距值具有良好的NLoS 误差抑制效果.

图6 TOA 测距值重构效果图

将本文NLoS 误差抑制算法与最小二乘定位算法结合以比较 NLoS 误差抑制前后定位性能的变化，实验结果如图7 所示. 可以看出，NLoS 测距值在应用本文算法后的定位性能逼近于LoS 环境下的理想状态，且定位收敛速度也与LoS 环境下的收敛速度相当，定位精度较高. 其中，定位误差小于1.25 m 的概率约为50%，定位误差小于3 m 的概率约为90%.

图7 使用本算法前后定位性能对比

4 总结

复杂环境下UWB 信号的NLoS 传播严重影响TOA 测距精度，进而损害UWB 定位的准确性. 针对该问题在已有NLoS 消除方法的基础上，提出了一种基于两步Kalman 滤波的NLoS 误差抑制方法. 该方法借助NLoS 误差与测量误差之间的相互独立性直接估计出 NLoS 误差，并将其与设定阈值比较以鉴别NLoS 误差是否存在. 通过Kalman 滤波对TOA 测距值进行二次估计、鉴别及修正以提高TOA 测距精度，从而实现室内复杂环境下的UWB 精准实时定位. 仿真实验表明，该方法不仅能够对NLoS 误差实现良好的跟踪估计，对LoS/NLoS 环境转变也具有较强的灵敏感知能力，此外，NLoS 误差测距值在应用本文算法后的定位性能逼近于LoS 环境下的理想状态，且定位收敛速度与无NLoS 误差情况下的收敛速度相当.