1、基金项目:国家地方联合工程实验室基金项目(GSYSJ2018012);陕西省教育厅专项科学研究计划(17JK0381)收稿日期:2021-05-19 修回日期:2021-06-05 第 40 卷 第 4 期计 算 机 仿 真2023 年 4 月 文章编号:1006-9348(2023)04-0439-05NLOS 环境下基于 UWB 的定位算法研究徐淑萍,郭 宇,王 双,张玉西(西安工业大学计算机学院,陕西 西安 710000)摘要:为解决室内非视距(NLOS)环境下定位及机器人定位过程中由于非视距环境下,噪声存在异常值误差过大的问题,提出了两种基于 Chan-Taylor 协同算法来抑制 N
2、LOS 误差的方案。一种为结合了卡尔曼滤波的抑制方案,另一种为依据测量特征统计的距离重构方案,从多种定位算法组合出发,有效避免了高噪声和 NLOS 误差的影响,并通过动静态定位测试分析两种方案的定位精度。结果表明,两种方案均能降低 NLOS 环境下的定位误差,但 NLOS 误差抑制算法的效果较优,适应性更强。关键词:室内定位;非视距;卡尔曼滤波中图分类号:TN911.7 文献标识码:BResearch on Location Algorithm Based onUWB in NLOS EnvironmentXU Shu-ping,GUO Yu,WANG Shuang,ZHANG Yu-xi(S
3、chool of computer science,Xian University of technology,Shaanxi,Xian 710000,China)ABSTRACT:In order to solve the problem of excessive outlier error due to noise in the indoor non-line-of-sight(NLOS)environment and robot positioning process due to the non-line-of-sight environment,two solutions based
4、on the Chan-Taylor collaborative algorithm to suppress the NLOS error are proposed.The first is a suppressionscheme combined with Kalman filtering.The other is a distance reconstruction scheme based on measurement featurestatistics.Starting from the combination of multiple positioning algorithms,the
5、 effects of high noise and NLOS errorsare effectively avoided,and the positioning accuracy of the two solutions is analyzed through dynamic and static posi-tioning tests.The results show that both schemes can reduce positioning errors in the NLOS environment,but theNLOS error suppression algorithm i
6、s more effective and more adaptable.KEYWORDS:Indoor positioning;NLOS;Kalman filter1 引言随着移动机器人的发展,室内定位技术的需求也在不断增加,但由于室内环境存在复杂性、多变性,不可避免的存在遮挡物等非视距(Non Line of Sight,NLOS)环境,造成定位精度下降,而单一的定位算法又无法适用1。UWB 技术有穿透性能强、功耗低、定位精高的优势2。TDOA 技术复杂度低等特点成为常用的估计方法3。Chan 算法在视距(Line ofSight,LOS)环境下定位精度较高,但在 NLOS 环境下定位效果很
7、差4。Taylor 算法5需要设置合理的初始值,否则会导致定位算法不收敛,Chan 和 Taylor 的协同方法6,将 Chan方法的定位结果作为 Taylor 算法的初始位置,可满足其收敛性。文献7基于 Chan-Taylor 协同定位,但没考虑 NLOS 误差。文献8是抛弃利用定位结果残差鉴别受到 NLOS 污染严重的测距数据,定位效果较好,但一定程度上消去了表征定位目标的有效信息。文献9利用信噪比区分视距和非视距环境,需要判断测距阈值范围。本文在在 TDOA 技术的基础上,考虑到 Chan 算法与Taylor 算法的优劣势,采用 Chan-Taylor 协同算法,识别 NLOS误差并分别
8、结合卡尔曼滤波和距离重构,设计出两个定位方案,并进行静动态实验分析。2 TDOA 定位算法描述依第一个基站作为主基站构建双曲线模型,TDOA 可表934示为ri,1=ri-r1=(xi-x)2+(yi-y)2-(x1-x)2+(y1-y)2(1)其中,ri,1为第 i 个基站与主基站距离与主基站到标签距离的差,(x,y)表示目标标签的实际坐标,(xi,yi)表示第 i 个基站的坐标。2.1 Chan 氏定位算法Chan 算法是一种求解双曲线方法的非递归算法10。在其求解中需要两次利用最小二乘法加权估计结果。系统在服从高斯分布噪声情况下,误差相对较小,但在 NLOS 环境下定位性能将受到严重影响
9、。2.2 Taylor 级数展开算法Taylor 算法是一种需要初始值的递归算法11。先前求解的结果作为当前的初始值,利用最小二乘法不断的迭代修正,直到求解的误差小于设定的阈值,该算法定位精度较高,但易受初始值的影响。2.3 Chan-Taylor 协同定位算法将 Chan 算法定位估计值作为 Taylor 算法的初始估计坐标,不断修正迭代,直到满足最小二乘法求解的误差值小于先前设定阈值,则迭代停止,最终输出一个更加精确定位估计值。图 1 Chan-Taylor 协同算法流程3 NLOS 误差识别在非视距环境下,标签的信号到达基站会存在一个较大延时引起的正向误差值,ti时刻定位标签到基站之间距
10、离的误差观测模型表示为12rm(ti)=dm(ti)+nm(ti)+NLOSm(ti),i=1,2k(2)dm(ti)=(xt i-xm)2+(yt i-ym)2(3)其中,rm(ti)为基站与待测标签在 NLOS 环境下 ti时刻的距离测量值,dm(ti)表示为 LOS(Line of Sight,LOS,视距)环境在在 ti时刻基站与待测标签的距离,nm(ti)为系统测量误差,NLOSm(ti)为 NLOS 环境下 ti时刻的测距误差,平滑处理定位标签到各个基站的测量距离值,即rm(ti)=N-1n=1am(n)tni(4)上式(4)中多项式的阶数为 N-1,可由最小二乘法获得多项式的系数
11、am(n),假设获得的结果为 am(n),系统测量值经过平滑处理的值为sm(ti)=N-1n=1am(n)tni(5)rm(ti)的样本标准差可以计算出来,则表示为m=1kk-1i=0(sm(ti)-rm(ti)2(6)上式(6)中测距样本数量用 k 表示。由式(2)可以看出由于系统测量值数据中既有 NLOS 信号传播的误差也有测量噪声的误差,因此,测量值的标准差要大于平滑值的标准差。假设 LOS 环境下标签到第 m 个基站系统测量误差的标准差为m,在 LOS 环境下统计多次测量值分析可获得m,是否 NLOS 环境传播就可以用m与m的差值判别,即H0:m=m,LOS 环境H1:m m,NLOS
12、 环境(7)残差值为em(ti)=rm(ti)-dm(ti)(8)NLOS 环境下的测量误差确定后,结合卡尔曼滤波进行优化,之后采用定位算法确定目标最优估计位置。4 NLOS 误差的抑制与重构4.1 卡尔曼滤波抑制算法卡尔曼滤波能够有效对数据进行平滑处理,使之降低异常值的影响,当存在 NLOS 误差时,后一时刻的卡尔曼滤波计算时可加入测距残差值,由式(8)可令测距残差表示为 C=em(ti),从而考虑到非零残差值 C 的卡尔曼滤波推到公式表示为:估计测量值观测向量 Yk为Yk/k-1=Hk/k-1+C(9)Yk与估计值 Yk/k-1之间的差为ek/k-1=Yk-Yk/k-1=Yk-HkXk/k
13、-1-C(10)状态向Xk/k-1利用误差 ek/k-1进行纠正Xk/k-1=Xk/k-1+KkYk-HkXk/k-1-C()(11)NLOS 环境下一种新的卡尔曼滤波器表示为Xk/k-1=AXk-1/k-1(12)Pk/k-1=APk-1/k-1AT+k,k-1Qk-1Tk,k-1(13)Kk=Pk-1/k-1HTkHkPk/k-1HTk+Rk-1(14)Xk/k-1=Xk/k-1+KkYk-HkXk/k-1-C()(15)Pk/k=I-KkHkPk/k-1(16)由文献13可得,状态向量用 Xk=rkvrk,A=1T01|,044Hk=1 0,rk移动标签与基站之间的距离,vrk移动标签相
14、对基站的速度,T 为间隔时间,k,k-1表示为状态噪声系数矩阵,状态噪声序列的协方差矩阵为 Q,Kk表示卡尔曼增益,I表示为单位矩阵。利用新的卡尔曼滤波器对 NLOS 环境下的测量数据进行 LOS 优化重构,之后带入到 Chan-Taylor 协同算法中再次对 LOS 优化重构数据进行定位处理,最终得到最优的目标位置。4.2 NLOS 误差距离重构算法由移动标签运动是一个不断连续的过程,因此当前时刻的状态可使用先前时刻的状态进行表示14。基于距离重构的算法种 NLOS 的距离重构是利用先前时刻的 LOS 环境下的距离值和距离变化速率。k 时刻的距离使用 k-1 时刻的距离进行表示,表示为?rk
15、i=dk-1i+dk-1iT+nki(17)式(17)中,?rki表示为 NLOS 环境下的距离重构,k 时刻标签与第 i 个基站之间的系统误差用 nki表示,dk-1i表示为 k-1 时刻标签到第 i 个基站之间距离变化的速率dki=dkiT(18)dki=?xk-1m()2+?yk-1m()2T+?xk-1m(xi-xkm)+?yk-1m(yi-ykm)(xi-xkm)2+(yi-ykm)2(19)综上,要想获得当前时刻的距离的信息,必须知道上一时刻的距离信息和移动标签的状态信息,最后完成的 NLOS环境下的距离重构,之后带入到 Chan-Taylor 协同算法中,获取最终估计值。5 仿真
16、分析选择 8mx8m 的室内复杂 NLOS 场地,随机设置三个个不均匀的坐标为 B0、B1、B2,以 0.15s 频率采集数据,每个坐标点采集约 100 个数据点左右的数据集,在静态情况下分别对4 个不同坐标点进行定位,动态情况下对移动机器人的真实轨迹进行定位,利用2 种 NLOS 误差抑制方案,分别对测量观测数据进行定位结果分析,实验测试分析的方案如图 2所示。两种方案静态定位结果如下图 3、4、5、6 所示。表 1 两种方案静态定位精度结果(m)标签测试点方案 1RMSE方案 2RMSET1(2.3,2.53)0.0520.103T2(2.42,2.6)0.0450.095T3(2.5,2.6)0.0390.087T4(2.22,2.5)0.0410.112 由图 3、4、5、6 可看出方案 1 的四个测试点定位精度相图 2 实验方案图 3 测试点 T1(2.3,2.53)图 4 测试点 T2(2.42,2.6)图 5 测试点 T3(2.5,2.6)比方案 2 明显提高,且从表 1 可以看出方案 1 均方根误差与方案 2 相比明显较小,定位效果更好。从图 7、8 可以看出方案 1
