UWB精确定位自适应无迹卡尔曼滤波算法

王孜洲，臧利国，唐一鸣，贾鹏，王德海

（1.211167江苏省 南京市 南京工程学院 汽车与轨道交通学院；2.211167江苏省 南京市 南京工程学院 自动化学院；3.211167江苏省 南京市 南京工程学院 电力工程学院）

0 引言

随着位置服务的快速发展，人们对高精度定位技术的需求越来越高。定位技术广泛存在于生活的方方面面，在工业生产、日常生活中发挥着巨大的作用。超宽带定位技术（UWB）是近年来兴起的一项室内定位技术，以非正弦波窄负脉冲传输信号。其抗多径能力强，定位精度高，且对信道衰弱不敏感，难以截获，成为最受关注的定位技术之一。

然而在实际应用中往往存在障碍物遮挡、金属干扰等情况，会给UWB测距带来较大误差（NLOS误差），严重影响UWB定位精度。为抑制NLOS误差，提高定位精度，鲍小雨[1]等提出了一种改进的扩展卡尔曼滤波（IEKF）算法，通过实际观测值和预测值的差值大小，确定是否存在NLOS误差，若存在误差，则将滤波增益设置为零，从而减小NLOS误差对定位的影响。试验显示，该系统一定程度减小了NLOS误差，但该方法依赖于阈值的设定，且误警率较高，难以应用于实际工程中；李旭虹[2]等提出一种改进的无迹卡尔曼滤波算法，以联合定位的初始坐标作为滤波观测值，并在无迹卡尔曼滤波的更新方程中直接减去NLOS误差项，避免了UKF滤波的发散。此种方法在建模时，将NLOS误差假设为服从指数分布，然而实际测量中，NLOS误差并非完全服从指数分布，因而滤波精度有所下降。

针对上述问题，为保证误差抑制效果，本文首先对信道环境进行鉴别，若为LOS环境，则进行无迹卡尔曼滤波；若存在NLOS误差，则针对性地引入自适应因子，对协方差进行修正，从而减小误差。

1 TOA的定位原理和误差模型

1.1 TOA定位原理

基于到达时间（TOA）定位利用测量信号在基站和标签之间的到达时间，求出每个基站与标签之间的距离，进而通过距离实现定位[3]。一般采用3个基站进行定位，其定位模型如图1所示。

图1 TOA定位原理Fig.1 Positioning principle of TOA

图1中，A0，A1，A2为UWB基站位置；MS为标签位置；测得基站A0，A1，A2与标签MS的距离分别为d0，d1，d2；分别以基站为圆心、距离为半径作3个圆，三圆交点即为标签MS的位置。设3个基站坐标为（x0，y0），（x1，y1），（x2，y2），标签坐标为（x，y），根据定位模型可列如下方程：

求解可得

1.2 误差模型

在NLOS环境下，直达单径分量DP到达基站需经过障碍物，由于障碍物的介电系数大于1，会给测距模型带来附加时延[4]。通常认为，此附加时延会导致正均值的随机误差，定位结果由点坐标变为如图2所示的阴影区域，大大增加了定位的不可信度，同时提高了计算的复杂程度，导致计算效率降低。

图2 定位误差模型Fig.2 Positioning error model

设阴影部分3个圆两两相交的3个交点为B0，B1，B2，坐标分别为，，为减小计算量，将基站设置于等边三角形的3个顶点处，以两点 A1，A2连线为X轴，连线中点为原点建立坐标系。

交点坐标可由两圆联立方程求解：

同理，可求出B1，B2的坐标，根据坐标算出 B0，B1，B2三点与实际标签位置的距离为T0，T1，T2。距离公式：

由此得出，NLOS环境下引入的定位误差可达max（T0，T1，T2），具有极大的不确定性，大大减小了UWB的定位精度。

2 NLOS信道鉴别

由式（5）可知，NLOS误差的存在会给UWB定位带来极大的干扰。为保证误差抑制效果，本文采用峭度作为参数鉴别UWB信道环境，若峭度参数异常，则说明存在NLOS误差，便可引入自适应因子，有针对性地剔除NLOS误差，从而保证定位精度。

UWB信号在IEEE802.15.4a标准下，信道冲激响应（CIR）可表示为[5]

式中：l——第l条多径；L——多径总数；al，τ1——第l条路径的幅度增益和时延；δ（x）——单位能量的单径脉冲函数，其性质如下：

为鉴别是否存在NLOS环境，提出使用峭度作为衡量信道状态的参数。峭度K定义为波形四阶矩和二阶矩平方的比值，即

式中：μ——t时刻O（t）的均值，峭度作为信号的归一化四阶矩，对信号中的冲击特征尤其敏感。

四阶矩定义数据的峰度表征数据在均值处的峰值高低；二阶矩即方差，表征数据的离散程度。峭度作为四阶矩与二阶矩的比值，可以表征数据相对于正态分布是陡峭还是平坦。对于高K的数据，倾向于在均值附近有更明显的峰值，下降更迅速，数据更聚拢；对于低K的数据，倾向于在均值附近有更平坦的峰值，下降缓慢，数据更为离散[6]。一般LOS环境中因为没有阻挡，数据变化更为剧烈，倾向于有更高的峭度；NLOS环境中数据变化较为缓慢，倾向于有更低的峭度。故峭度可作为衡量NLOS误差的参数，表示如下；

式中：thresh——设定的阈值，因K=3时，波形具有正常峰值（零峭度），故取thresh=3。

3 自适应无迹卡尔曼滤波算法

3.1 定位模型

根据TOA定位原理建立定位模型。将基站A0，A1，A2固定，以A1，A2连线的中点作为原点O建立空间直角坐标系。标签MS的状态向量分别由标签的平面坐标(x,y)和在方向上的速度vx、vy构成，可列状态方程如下：

为使UWB的精度提高，可采用数字滤波算法优化定位数据。

3.2 标准UKF算法

卡尔曼滤波（KF）是目前应用广泛的数字滤波技术，其利用上一系统状态的数据和量测数据，通过线性系统状态方程求出当前状态最优估计值，在递归过程中提高预估精度[7]，但是，由于计算时必须通过线性系统状态方程，所以，只适用于高斯线性模型。

对于非线性滤波问题，在卡尔曼滤波的基础上主要有两种发展：扩展卡尔曼滤波（EKF）算法和无迹卡尔曼滤波（UKF）算法。其中，EKF的核心是通过泰勒展开来处理非线性状态方程和观测方程，只取泰勒展开式的一阶项来实现线性化[8]。但EKF也只适用于线性化较弱的系统方程，否则同样会导致滤波器性能下降，影响其收敛性。而UKF的核心是利用UT（Unscented Transform）变换来对均值和协方差进行处理，从而避免线性化带来的误差。相比EKF,UKF不需要计算非线性函数的Jacobi矩阵，计算量大大减少，且UKF没有忽略高阶项，计算精度更好，适用性也更强（例如：UKF可以应用于复杂的NLOS环境）。

首先对滤波进行初始化：

选取2n+1个Sigma样本点，n由研究对象的状态维数决定，本文主要研究UWB定位模型，可取n=2，选取方式如下：

式中：χ——调整逼近的精度的尺度参数，适当调整可提高精度，且其中，α——一个较小的正数，取值范围10-4≤α＜1，常取0.001，确定周围Sigma样本点的分布情况；β——有关状态分布的参数，在高斯分布时最优取值为2；λ——比例因子，通常取0或3-n。对于上述α，λ，β这3个参数，都可以适当调节来提高计算过程中的估计值和方差的精度[9]。

UKF算法主体主要分为3个步骤，分别是预测更新、迭代更新和量测更新[10]。预测更新中，先对Sigma样本点进行非线性变换，接着根据变换后的Sigma样本点计算预测估计值和预测协方差

式中：Wim，Wic——计算均值和协方差的权值，计算方法如下：

3.3 自适应UKF

对于标准UKF，虽然相比EKF可以更好地处理非线性系统，但是，如果存在系统数学模型误差或者系统噪声特性不精确等情况，极有可能影响滤波的精度，甚至导致滤波的发散。此外，初始化的取值对UKF算法也存在影响，如果取值存在误差会导致滤波估值的精度下降、收敛速度慢。针对以上存在的问题，本文采用了一种自适应UKF算法[11]。

首先，针对UKF算法在不同测量环境需要自适应的程度不同，需要一个随环境变化的自适应因子，为此构建了测量残差向量：

式中：yk——该时刻UWB的真实测量值；测量的估计值。当系统预测出现异常，或与系统模型不符，为使滤波保持精度，可引入自适应因子[12]，构建如下：

式中：μk——所构建的自适应因子（0＜μk≤1），可判别滤波中是否存在状态模型误差，并且对误差有自适应调节的作用。

将构建的自适应子加入标准UKF中，可对其进行修正

由以上分析可知，当系统模型等都正常时，μk=1，对滤波结果没有影响；当初始值的选取可能存在异常或是状态模型存在偏差时，μk＜1，即滤波结果对预测的估计值信任程度下降；当状态模型错误时，μk无穷接近于0，与其相关的值可完全舍弃[13]。因此，当滤波结果出现异常时，系统能做出自适应调节，减少异常值造成的影响，使最终得出的滤波结果相对准确。

恰当设置自适应因子，能够对状态方程预测值和测量值的权比进行自适应平衡，也能降低系统模型误差对滤波的影响。此外，自适应因子在滤波的递归过程中不断对增益矩阵进行修正和调节，在此过程中UKF算法的抗差能力也得到提高。

4 试验与分析

为验证本文算法对UWB定位数据的优化效果和对NLOS误差的修正能力，在10 m×10 m的实验室环境中，进行了模拟NLOS试验。试验采用典型带宽为500 MHz的DWM1000射频模块，通信速率6.8 m/s，发射功率为-62～-35 dBm/MHz。

UWB的定位环境如图3所示。基站 A0，A1，A2沿边长为6 m的等边三角形顶点放置，虚线表示标签MS运动轨迹。

图3 试验环境Fig.3 Test environment

在基站A0与轨迹之间插入一块金属块，当标签、金属块和基站A0三者在同一直线上时，介电系数大于1的金属块会干扰直达单径分量DP的传播，形成强NLOS环境。分别采用传统定位算法和本文算法进行定位试验，试验结果见图4。

图4 基站测距值Fig.4 Base station ranging value

图4为基站 A0，A1，A2与标签MS的测距值，从图中可看出第30次到第40次测距中，基站A0受金属块影响，测距值出现突变，表现在定位结果中即如图5所示。

图5 原始定位结果Fig.5 Positioning results

图5为传统算法定位结果，可看出数据在y=-1 500 mm处开始出现异常，随后定位出现较大误差，最大误差值达到了335.89 mm；图6为本文算法定位结果，显然数据波动更小，并且原NLOS误差处相对平缓，最大误差仅为158.83 mm，相比传统算法最大误差下降了52.7%。为对比整体定位结果，计算了RMSE（均方根误差）值与MAE（平均绝对误差）值。参见表1。

其计算方法如下:

式中：x*——标签MS在x方向的真实距离；xi——第i次定位中本文算法解算出的标签MS在x方向的测量值；n——测量总次数；RMSE值——误差平方均值的平方根，一般用来表示定位数据的离散程度；MAE值——误差绝对值的平均，可用来表示定位数据的整体误差。

图6 本文算法定位结果Fig.6 Positioning results of the algorithm in this paper

表1 试验结果Tab.1 Test results

试验表明，本文提出的算法对UWB的数据优化明显，并且能够准确鉴别并修正NLOS误差。

5 结语

UWB作为当前精度最高的室内定位技术，其发射信号功率谱密度低，截获率低，但受环境影响显著。本文根据UWB的TOA定位原理，分析了UWB在NLOS环境下的误差模型。并通过计算峭度判别UWB信道环境，对于参数正常的情况，判定为LOS环境，采用适用于非线性模型的标准无迹卡尔曼滤波算法进行滤波；对于参数异常的情况，判定为NLOS环境，在UKF算法的基础上加入自适应因子修正增益矩阵，进而减小NLOS误差。经试验验证，该算法有效实现了对定位结果的优化和对NLOS误差的修正，提高了UWB的定位精度。