改进的Chan-粒子滤波算法超宽带室内三维定位

李 鹏，向宇翔，荣冬成，凌智琛,李 朋

(湘潭大学自动化与电子信息学院，湖南湘潭 411100)

0 引言

随着人们生活水平的不断提高，对室内位置服务的要求也越来越高，室内定位的精度问题也成为了室内位置服务中的重中之重。超宽带(Ultra-Wide Band，UWB)技术作为近年来室内定位中非常热门的一种定位方式，凭借时间分析率高、穿透性强、覆盖范围广、系统复杂度低、抗多径效应良好等优势，越来越受到广大公司和研究者的重视。但是传统的UWB定位算法大多都有相应的缺点，如Chan算法、Fang算法、泰勒级数展开法等，这些算法要想得到较高的定位精度，就必须要求系统满足测量误差服从理想高斯分布。而在现实环境中，处处都存在非视距(Non Line of Sight，NLOS)误差，而NLOS误差的存在又会导致定位时产生多径效应，从而无法进行准确定位，最终影响室内定位精度。

在到达时间差(Time Difference of Arrival，TDOA)算法中，由测量值建立的标签与基站之间的关系方程是非线性的，而粒子滤波(Particle Filter，PF)算法又是基于序贯蒙特卡罗算法结构，常用于非线性非高斯系统模型，因此在NLOS较强的环境中，PF算法相比扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)算法有更好的估计性能。文献[10]提出了一种自适应卡尔曼滤波算法，能进一步提高室内视距环境下的定位精度，但是这种算法有着传统卡尔曼滤波算法的通病，即不能有效解决在NLOS环境中定位精度低的问题。文献[11]提出了将UWB定位中的包括到达时间(Time of Arrival，TOA)、接收信号强度指示(Received Signal Strength Indicator，RSSI)、到达角(Angle of Arrival，AOA)在内的多种算法得到的测距信息进行融合定位，从而达到抑制NLOS误差的效果，但是该方法成本较大。文献[12]提出了对UWB采集的原始数据进行两次滤波，从而实现高精度定位，但是该作者使用了TOA方法进行初步定位，该方法在实际环境中往往面临硬件无法实现时钟同步的问题，因而无法避免地会产生定位误差。文献[13]将TDOA 算法与UKF算法进行融合，提高了室内定位精度，但是UKF在非线性严重的环境中仍然无法进行准确定位。

针对上述问题，为了能够有效抑制在室内定位中存在的NLOS误差，实现室内高精度定位，满足智能时代人们对于高精度室内定位的需求，本文提出了一种Chan-PF UWB联合室内定位算法。首先，利用模拟退火算法能在解空间内随机寻找全局最优解的能力辅助Chan算法进行定位解算，由于Chan算法受NLOS影响较大，因此定位精度会随着室内障碍物的增多而大幅度降低；所以，第二步将Chan算法解算得到的坐标通过PF算法进一步提高精度，得到精确的标签坐标，从而最大限度地削弱NLOS误差的影响，实现高精度室内定位。

1 改进的Chan算法

1.1 TDOA定位原理

TDOA算法是对TOA算法的改进，与TOA定位算法相比，TDOA是用多个基站接收到信号的时间差来确定标签的位置，不需要实现硬件中的时钟同步，定位精度也更高。

TDOA定位原理如下：假设有个基站，基站1作为参考基站，已知基站(=1,2,…，)的坐标为(,),并假设标签的坐标为(,)、基站与标签的距离为。

已知信号到达参考基站1和第个基站的时间差及信号传播速度，可以得到不同基站与标签的距离差为-(=1,2,…，)。因此，可以建立以2个基站为焦点、距离差为长轴的双曲线方程来求解标签位置。

=-

(1)

=-

(2)

1.2 Chan算法

Chan算法是基于TDOA定位方法的一种定位算法。比起TDOA定位方法中的另外两种算法：Fang算法和泰勒算法，Chan算法计算量小，且在视距环境下能够取得良好的定位精度。但由于在NLOS环境下Chan算法的定位精度会大幅度下降，所以一般与其他算法协同定位。由TDOA方法中的几何关系可知

(3)

将式(1)、式(2)代入式(3)得

(4)

其中，,1=-，,1=-，,1=-。

以=[,,]作为未知变量，可建立误差矢量的方程组

=-

(5)

通过加权最小二乘法(Weighted Least Squar-es, WLS)，计算出第一次估计结果

(6)

1.3 改进的Chan算法

为了进一步提高定位精度，除了加入能解决NLOS误差的PF之外，本文还在Chan算法中加入了模拟退火算法，模拟退火算法具有局部搜索能力强、运行时间较短等优势。假设场所内共有个基站，个标签，对于每个待测目标，模拟退火算法的目标函数设置为

(7)

其中，为初始坐标(,,)；为目标与基站的距离估计值；′为测量值。目标函数的含义为：使用估计的待测目标坐标求出的与测量值′差距的绝对值越小，估计的坐标越准确。

(8)

算法的具体步骤如下：

1)设初始坐标为，同时计算目标函数()，设当前迭代次数=0，当前温度=，∈(0,1)用于控制降温退火。

2)在初始坐标周围生成新的坐标′，并再次计算目标函数(′)。

4)判断是否达到设置的迭代次数，如果没有则继续步骤2)。

5)若连续若干个新解都没有满足条件，则终止算法，满足的话输出最终结果，否则重置迭代次数，并降低初始温度=。

6)得到坐标初始估计值(′′′)。

7)将得到的初始值代入Chan算法中，求出式(6)中的，从而得到第一次最小二乘法的解。

2 Chan-PF联合定位算法

2.1 PF

PF的基本思想是利用一组粒子近似地表示系统的后验概率，从而估计系统的状态。与卡尔曼滤波和EKF相比，PF常用于非线性环境中，因此在NLOS环境中，本文使用PF对TDOA的测量值进行滤波。

PF的步骤为：

1)建立定位数学模型。

=[]

(9)

=[]

(10)

其中，为状态矩阵；为观测矩阵；、、分别为、、轴坐标；、、分别为、、方向的速度。

2)预测。

+1=()+()+

(11)

其中，为状态矩阵；为控制矩阵；为过程噪声；为状态转移矩阵；为控制方程。

3)更新。

(12)

其中，为粒子的权重，更新后通过后验概率判断粒子的权重，从而判断当前系统的状态。

4)重采样。

为降低粒子退化，对粒子进行重采样处理。重采样根据粒子的权重，在原有基础上生成新的粒子集，由于粒子集中的粒子数不多且多项式重采样易于实现，因此本文选择多项式重采样。多项式重采样算法直接从均匀分布(0,1]中，随机抽取个独立分布的随机样本，并将抽取到的样本与归一化权值的累计和进行比较来选取粒子。

5)提高样本多样性。

重采样过程虽然可以让粒子更加集中在有效区域，改善粒子的退化现象，但由于在重采样过程中复制了许多大权重粒子，导致降低了粒子的多样性。本文对样本集合中的每个粒子都加上一个零均值高斯噪声扰动，使粒子更加分散，提高粒子的多样性，即粒子粗化过程。

2.2 Chan-PF联合定位算法

在实际应用环境中，基站与标签之间常常会存在一些障碍物。这些障碍物导致UWB信号发生折射现象，从而产生NLOS误差。为了解决在复杂环境中较大的NLOS误差导致定位精度低的问题，本文提出了一种Chan-PF联合定位算法。

Chan-PF联合室内定位算法的步骤如下：

1)设状态方程为

()=(-1)+(-1)

(13)

式中，为状态转移矩阵；为过程噪声驱动矩阵；()为过程噪声。行人矢量为

()=[]

(14)

式中，,,为3个坐标轴的位置；,,为3个方向的速度。

行人观测方程为

()=(())+()

(15)

式中，()为观测噪声。

2)根据式(12)和式(14)，使用Chan算法进行初步定位，得到相应的测量值

=()

(16)

4)粒子集合按照1)中的运动方程进行运动，得到新的粒子集，并以Chan算法中所得测量值作为观测值，根据粒子集合与观测值之间的距离计算各粒子对应权值。

(17)

(18)

6)归一化权值。

3 仿真与实验

3.1 仿真结果与分析

假设行人在室内行走，轨迹为矩形，6个基站坐标分别为：(0,0,20cm)；(0,438cm,403cm)；(0,873cm,20cm)；(644cm,873cm,403cm)；(644cm,438cm,20cm)；(644cm,0,403cm)。标签初始位置(100cm,100cm,100cm)，初始速度为(25cm/s,0,0)，对真实轨迹、用Chan算法得到的标签轨迹和分别加了Chan-PF、Chan-EKF算法之后的标签轨迹进行比较。

3.1.1 视距环境下的仿真结果

在视距环境下，图1所示为行人的真实轨迹和预测轨迹，图2所示为三种算法在不同坐标轴下的误差值。

图1 三维跟踪轨迹Fig.1 3D tracking trajectory

(a)

(b)

(c)图2 三种算法的误差对比图Fig.2 Error comparison of three algorithms

图1中，红点表示基站坐标，橙色线条表示在Chan-PF算法下的行人轨迹，蓝色线条表示单一Chan算法下的行人轨迹，红色线条表示在Chan-EKF算法下的行人轨迹，三角形表示行人行走的真实轨迹。

由表1各个轴的误差均值可以看出，Chan、Chan-PF、Chan-EKF在方向的定位精度分别为5.72cm、4.11cm、4.67cm；在方向分别为7.87cm、4.15cm、6.23cm；在方向分别为9.73cm、4.52cm、7.32cm。由此可以看出，在视距环境下，Chan-PF算法和Chan-EKF算法均能小幅度改善定位精度。通过算法计算用时可以看出，Chan-PF算法用时相对更多，实时性上可能不如Chan算法，但是整体而言差距不大，Chan-PF算法在NLOS情况下的表现更好。

表1 视距环境下三种算法的误差对比

3.1.2 NLOS环境下的仿真结果

在NLOS干扰较大时，图3所示为行人的真实轨迹和预测轨迹。

图3 三维跟踪轨迹Fig.3 3D tracking trajectory

本文在标签与基站的测量值中加入一个服从指数分布的随机变量()表示NLOS误差。其条件概率密度函数为

(19)

式中，为均方根时延扩展，IEEE802.15.4a信道模型中该参数的均值和标准差通常为，nlos=27,,nlos=044。

通过图4可以直观地看出，在NLOS干扰较大时，Chan-PF算法能更加有效地降低定位误差，实现室内NLOS情况下的精确定位。

(a)

(b)

(c)图4 三种算法的误差比较图Fig.4 Error comparison of three algorithms

由表2可以看出，在受到NLOS误差干扰时，Chan-PF算法的优化作用更加明显，尤其在方向和方向，定位精度有了较大的改善。在轴方向，Chan算法平均定位误差为14.23cm,Chan-PF算法定位误差为7.725cm,Chan-EKF算法定位误差为9.88cm；在轴方向，Chan算法平均定位误差为17.33cm,Chan-PF算法定位误差为7.12cm,Chan-EKF算法定位误差为9.28cm。

表2 NLOS环境下三种算法的误差对比

3.2 实验验证

实验采用UWB模块为DecaWave公司的DW1000芯片，采用串口通信，实验场景如图5所示。

图5 UWB测试平台及测试场景Fig.5 UWB test platform and test scenario

实验地点为室内，实验场景选择在实验室，其中，4个基站设在室内，2个基站设在室外。主要的NLOS因素为信号在多面墙壁之间的折射反射，基站坐标为：(0,885cm,180cm)，(0,1435cm,180cm)，(325cm,1435cm,180cm)，(325cm,885cm,180cm)，(560cm,1245cm,180cm)，(560cm,0,180cm)。标签起点为(560cm,1000cm,180cm)，终点为(325cm,1400cm,180cm)。测试人员手持标签沿预设轨迹缓慢移动，总步数为40，分别采用Chan-PF算法和Chan-EKF算法进行解算。定位误差如图6所示，从上到下分别为Chan-PF算法、Chan-EKF算法和Chan算法。表3所示为坐标平均误差。

(a)

(b)

(c)图6 实际场景轨迹误差Fig.6 Actual scene trajectory error

表3 三种算法误差情况

由以上图表可以得出：在NLOS环境下， Chan-PF算法的定位精度有了较大的提升。虽然三种算法在实验中的定位精度相较于在仿真中都有了不同程度的降低，但考虑到实际环境中的NLOS误差不仅仅有信号的折射，还存在着墙壁的阻隔，因此该算法还是能够很好地提高定位精度。

4 结论

针对室内NLOS条件下Chan算法定位精度低的问题，本文提出了一种UWB室内混合三维定位算法。首先用模拟退火算法辅助Chan算法处理目标距离基站较近时的初始值估计，寻找最优解；其次用PF对改进Chan算法进行滤波，降低在NLOS环境下的定位误差，得到标签的精确位置。通过仿真实验可以得出结论：

1)在视距环境中，由于NLOS误差影响小，所以应该优先选择效率高、实时性最好的算法。

2)当室内存在NLOS误差或NLOS误差较大时，单一算法得到的定位效果并不理想。本文提出的改进Chan-PF算法能很好地处理NLOS误差的情况，从而使定位效果得到有效改善。经过实验验证，也证明了该方法具有良好的工程应用价值。在后续的工作中可以通过加入惯导，进一步提高室内复杂环境下的定位精度。