基于UKF的环境自适应UWB/DR室内定位方法①

周 琳,李晓明,江先志

(浙江理工大学 机械与自动控制学院,杭州 310018)

随着无线定位技术的不断发展,基于位置服务的研究越来越受到了人们的关注[1].目前,全球定位系统(Global Positioning System,GPS)已被广泛应用于室外定位[2],但由于墙体等建筑物对卫星信号遮挡,其在室内环境下定位效果较差.因此,室内定位技术不断涌现,比如Zigbee、UWB、WIFI、RFID 等定位技术[3].其中UWB 是一种穿透力强、抗干扰能力强的无线定位技术,比WIFI、ZigBee、RFID 等室内无线定位技术有着更高的定位精度,在视距(los of sight,LOS)情况下可达厘米级别定位精度且不存在误差累积,然而在复杂的室内环境中,UWB 信号在传播过程中不可避免地产生多径效应,从而导致明显的NLOS 定位误差[4–8].而DR 是一种具有自主定位导航能力并且短时定位高精度的技术,但其定位结果将会随着时间的推移发生飘移,即存在定位误差累积的问题[9].

针对上述问题,本文提出了一种基于UKF 的环境自适应UWB/DR 室内定位方法,将UWB 定位和DR定位的优势联合,用DR 定位降低UWB 定位出现的NLOS 误差,用UWB 定位更新DR 定位的当前位置,减少DR 定位的累积误差.该方法通过建立自适应UKF 滤波模型,将UWB 定位信息和DR 定位信息进行融合.在此过程中,利用新息和高斯分布的 3σ原则来对UWB 定位结果进行非视距检测,再通过新息的实时估计协方差和理论协方差来构建环境适应系数,进而用此系数动态修正UWB 定位的观测噪声,使得观测噪声自适应真实环境,更大程度地降低NLOS 误差对融合定位结果的影响.为了验证本文定位方法的有效性和稳定性,设计差速移动机器人在多遮挡室内环境中进行折线运动实验并实时采集和处理数据.实验效果表明,该方法能够有效减小UWB 定位的NLOS误差和DR 定位的累积误差,并且由于环境适应系数的创新引入,比UKF 定位方法和PF 定位方法具有更高的定位精度和更强的抗NLOS 误差性能.

1 DR 定位

DR 定位是一种精度较高的自主式定位方法,尤其适用于短时短距定位.其核心思想可概述为根据当前时刻的位置、速度和航向推算下一时刻的位置.航位推算的实现原理如图1所示.

已知当前时刻的初始位置 (xk,yk)、航向角θk和速度vk,移动机器人在下一时刻移动到(xk+1,yk+1)时,航向角的变化为 ωk,根据当前的速度vk乘以间隔时间tk+1−tk得到的航行距离lk,从而tk+1时刻的位置和航向角可以表示为:

图1 航位推算原理图

采用的数据采集移动平台如图2所示.该移动平台采用差速驱动,左右两个为直流电机驱动的主动轮,前后两个为用于支撑的万向轮.本文通过安装在电机轴端的光电编码器输出来进行航位推算.

设车轮的直径为D,两车轮间距为L,左右两个驱动轮的转速分别为ωl,ωr,根据差速移动机器人的运动学关系[10]可知速度和航向变化的计算表达式为:

图2 数据采集移动平台图

因此k时刻根据的位置和航向推得的下一时刻的位置和航向可表达为:

其中,ωRk,ωLk分 别表示左右驱动轮ti时刻的转速,由光电编码器测得.

2 基于TOA 的UWB 定位

到达时间方法(Time Of Arrive,TOA)的核心在于精确地测量出信号从发射机到达接收机的飞行时间.已知信号的传播速度即可精确地计算出发送机和接收机的之间的距离,但是发射机和接收机采用不同的时钟源,即两者存在影响飞行时间测量值的时钟差,又因为信号传播速度非常快,TOA 所测距离准确度难以保证.因此采用DS 测距(Double-Sided two-way ranging)消除时钟差影响,其原理图如图3所示.

图3 DS 测距原理图

图3中,Tc1,Tc2分别表示设备1 和设备2 发出信号到收到回应信号所耗费的时间,Tr1,Tr2则分别表示设备1 和设备2 处理收到的信号所耗费的时间,c为光速,从而可得待测节点到四周各个基站的距离为:

该方法等效于分别用设备发射机和接收机的时钟计算一次来回路程花费的时间,再四均等分总时间求取测量时间.这可以有效减少不同设备之间的时钟差导致的飞行时间误差,极大提高了TOA 的精度.

本文所采用的UWB 定位系统的原理如图4所示,移动站安装在数据采集移动平台上,定位系统的基准锚点为四周的4 个定位基站,基站基于TOA 方法实时获取其到移动站的距离.

令UWB 移动站(Moving Station,MS)和基站(Base Station,BS)的位置分别为(x,y)和(xi,yi),MS 和BS 的距离为di,因此可得方程组为:

图4 UWB 定位系统原理图

用该方程组的最后一个方程减去前n–1 个方程即可得方程组如下:

然而在实际情况下,通过测量所得d1,d2···di都是存在误差的,因此使用最小二乘法去估计待测节点的位置,则可以得到式(6)的解为:

3 基于UKF 的环境自适应UWB/DR 定位

3.1 选用UKF 的合理性分析

线性系统叠加原理表明线性系统的多个输入信号的总响应等于各个信号单独作用产生响应的代数和.由式(3)可知,DR 定位涉及三角函数运算,因此本文提出的室内定位模型是非线性的,不符合叠加原理.一般采用扩展卡尔曼滤波(Extended Kalman Filter,EKF)或无迹卡尔曼滤波处理非线性模型的滤波问题.其中EKF 通过将非线性函数进行Taylor 级数展开,并保留其一阶近似项实现线性化,难免引入线性化误差,而UKF 利用无迹变换(Unscented Transform,UT)处理均值和协方差的非线性传递问题,直接基于非线性模型进行解算,免除线性化误差,精确度更高[11–14].因此本文选用UKF 方法来进行融合滤波.

3.2 UKF 预测方程和观测方程确定

根据DR 定位建立系统的状态预测方程如下:

式中,X=[x,y,θ]T为运动状态,(x,y)为机器人当前位置,θ为当前航向,ωRk−1,ωLk−1为电机编码器测得的电机转速,作为运动状态的控制变量,Qk为三维的过程噪声.根据式(5)可得f(Xk−1,ωRk−1,ωLk−1)的表达式为:

根据UWB 的TOA 定位方法可以获得位置坐标,根据电子罗盘可测得航向,因此以位置和航向作为观测量从而建立测量方程如下:

式中,Rk为三维得到观测噪声,H为观测矩阵,其表达式为:

3.3 UKF 方法

无迹卡尔曼滤波的大体思路是结合Kalman 线性滤波框架和UT 变换来处理均值和协方差的非线性传递问题,提高非线性问题的滤波效果.UKF 计算步骤如下:

(1)初始化

计算系统状态和方差的初始值,在导航开始阶段多次采集UWB 定位坐标和电子罗盘航向来计算系统状态和方差的初始值.

(2)Sigma 点和对应权值计算

其中,λ=α2(n+κ)−n,n为系统状态X的维数,α 和κ 确定采样点在均值附近的分布,通常 α取一个较小的正值,κ=3−n,P(k|k)i为协方差矩阵的第i列.β 是非负的权系数,如果x服从高斯分布,β 最优值为2.

(3)状态预测

(4)量测预测

(5)状态更新

3.4 环境适应系数构建

由于室内环境复杂,往往存在着诸多障碍,采用UWB 进行室内定位时,其信号传播过程中将会发生反射和折射,从而导致NLOS 误差,此时LOS 状态下确认的UWB 定位结果的观测噪声已无法正确反馈.而利用稳定噪声统计特性的UKF 滤波不能得知这种变化,因而大大影响其滤波的精度和稳定性.为了优化滤波的效果,构建环境适应系数来对观测噪声实时修正,使得观测噪声能根据环境变化自适应.

新息定义为:

理论上,最优滤波的残差服从均值为零的高斯分布,其协方差为:

但当BS 和MS 出现NLOS 环境的时候,残差将会发生显著的变化.因此可以根据高斯分布的 3σ原则,利用新息的理论协方差的均方根构建校验信息来对UWB 得到的定位结果进行非视距检测.

结合开窗估计法和极大似然估计法可得到新息的实时估计协方差为[15,16]:

则利用实时估计协方差和理论协方差构建环境适应系数为:

式中,RLOS为UWB 在LOS 情况下定位结果的协方差,则UWB 定位的观测噪声在每一次进行无迹卡尔曼滤波的时候都进行动态修正为:

4 实验分析

实验环境如图5所示,在场地内共放置了4 个UWB 基站,坐标分别为A(0,0),B(840,0),C(0,840),D(840,840),单位为cm.数据采集移动平台在移动过程中的可以实时采集航向和车速信息,并通过ESP8266不断向外传输,UWB 基站A 通过串口不断向外发送移动站和各个基站的距离.通过自主开发的C#软件同频率实时记录以上数据.

图5 实验环境图

为了更好验证所提出方法的定位效果,设计途经NLOS 误差路段的折线路径实验,即采集平台在空间阻挡的区域中按计划路径匀速运行时的信息.由于NLOS 误差的存在,观测噪声模型必然呈现非高斯特性.至此可知,室内定位模型是非线性且非高斯的.而粒子滤波是一种适于处理此类模型的成熟滤波方法.因此本文分别通过UKF、PF 和环境自适应UKF 方法处理数据,并比对和分析定位效果得出实验结果.

实验结果如图6～图9所示,分别分析基于最小二乘法得到的测量值、UKF 方法、PF 方法以及本文方法的运动轨迹复现、定位误差、定位累计误差以及定位误差累积概率分布.

图6对比了不同方法的运动轨迹复现结果,可以看出,在LOS 情况下测量值也有着较高的定位精度,此时基于非视距检测构建的环境适应系数为I,即环境自适应UKF 和UKF 具有相同滤波效果;当存在NLOS测距异常值时,测量值的定位精度明显降低,UKF、PF 和环境自适应UKF 方法均表现出较强的抗NLOS性能.

图6 运动轨迹复现

图7和图8中50～60 s 可以看出,当出现NLOS 误差的时候,测量值的定位误差明显增大,UKF 方法和PF 方法一定程度抑制了误差,本文方法能够显著提高NLOS 情况下的定位精度,使之与LOS 情况下的定位误差变化与累计误差增长趋势相近.

图7 定位误差

图8 定位累积误差

图9所示为不同方法的误差累积概率分布,进一步验证了在有遮挡的路径下,本文方法相比于其它方法具有更优的定位性能.

表1是NLOS 误差量化表,从中可以看出在NLOS情况下,本文方法的最大误差值、平均误差值相较其他方法最低,NLOS 降低率最高.此外用Matlab 统计得,UKF、PF、本文方法的运算时间分别是0.0117 s,0.6094 s,0.0125 s.因此PF 方法定位效果略优于UKF方法,但运算效率过低;环境自适应UKF 通过消耗低成本运算效率换得显著优于UKF 方法的定位效果,进一步表明在NLOS 情况下,本文方法相比于其它方法具有更好的性能.

图9 定位误差累积概率分布

表1 NLOS 误差量化表

总之,实验数据和结果表明,本文方法在LOS 情况下定位效果与UKF 方法、PF 方法相近;在NLOS情况下,由于环境适应系数的动态修正,比UKF 方法和PF 方法具有更高的定位性能.

5 结论

针对复杂室内环境下UWB 信号传播的非视距NLOS 误差问题,本文提出基于UKF 的环境自适应UWB/DR 室内定位方法.该方法从UWB 定位环境的误差产生原因出发,通过建立环境自适应UKF 滤波模型,在UWB/DR 定位融合过程中对UWB 定位信息进行非视距检测并且引入环境适应系数动态修正UWB定位观测噪声.实验效果表明,该方法具有较高的抗NLOS 误差性能,定位精度优于PF 方法和UKF 方法,是一种具有较强实用性且高精度的室内定位方法.