基于Sage-Husa自适应UKF的UWB/INS融合定位方法

孙 伟，安晟均

基于Sage-Husa自适应UKF的UWB/INS融合定位方法

孙 伟，安晟均

（辽宁工程技术大学测绘与地理科学学院，辽宁阜新 123000）

针对室内环境中的非视距（NLOS）导致单一超宽带（UWB）定位出现较大误差的问题，提出基于无迹卡尔曼滤波（UKF）的超宽带/惯性导航系统（INS）紧组合定位方法。优化UWB量测方程，利用导航方程误差状态量简化卡尔曼滤波的预测和校正环节，通过萨格-胡萨（Sage-Husa）自适应估计并利用滤波器新息值对过程和测量噪声进行在线辨识，使得模型白噪声更接近真实情况，利用野值处理方法降低非视距对定位精度的影响。仿真实验结果表明，与传统扩展卡尔曼滤波（EKF）和UKF滤波方案相比较，本文提出基于UKF的组合定位模型能有效抑制非视距误差的影响并提高定位精度。

超宽带；无迹卡尔曼滤波；自适应滤波；室内定位；非视距

0 引言

超宽带（ultra-wide band，UWB）定位技术[1]中的无线信号由于多径效应和非视距（non-line-of-sight，NLOS）[2]干扰，在用于跟踪定位移动的人或物时，其运动轨迹会产生偏差。惯性导航系统（inertial navigation system，INS）在不需要任何额外附加条件下使用惯性测量单元（inertial measurement unit，IMU），以高频更新率估计目标位置并在短时间内可保持较高定位精度，但随时间推移系统定位误差会累积[3]。将二者紧耦合，可促使定位信息融合更加彻底，定位精度更高，且能为UWB定位数据处理进行假设性测验，有效处理NLOS和多径效应。

文献[4]提出基于鲁棒扩展卡尔曼滤波的UWB融合步行者航迹推算定位方法，通过处理系统状态和观测值间的马氏距离来抑制非视距对定位结果的影响，从而更新观测值协方差。但UWB测距噪声较强、信号中断较多的情况下，如何提高算法性能还有待进一步测试。文献[5]提出一种基于UWB的室内定位系统NLOS误差补偿方法。通过IMU固定标签的运动趋势得到虚拟惯性点坐标，采用环境因子作为加权系数将虚拟惯性点坐标与测量坐标融合，得到NLOS环境下的补偿坐标，以减少NLOS的影响。但没有考虑测量噪声误差和该方法在动态环境中下的表现。文献[6]提出了一种新的NLOS识别方法，该方法根据UWB信号在信道中的传播特征，结合支持向量机算法进行NLOS识别。文献[7]针对UWB定位样本具有类不平衡的特征，使用基于矩的不平衡二元分类方法来识别NLOS信号。该方法利用视距信号样本的2个第一矩的均值和协方差来表示其概率分布，然后利用概率分布和少量NLOS信号样本的总和建立分类模型。该方法不依赖于视距信号个数，适用于处理视距和非视距信号个数之间的不平衡分类。文献[8]提出使用距离测量的鲁棒定位算法。针对混合环境，把预测为异常值的距离观测值替换为使用最小中值算法获得的估计距离。随后，使用这些新的距离测量执行二步加权最小二乘方法。通过仿真证明了所提出方法的定位精度高于传统方法。文献[9]研究基于扩展卡尔曼滤波（extended Kalman filter，EKF）和无迹卡尔曼滤波（unscented Kalman filter，UKF）[10]的超宽带结合IMU的算法。采用3次多项式拟合法来补偿测距误差，提出适用于非视距环境下室内导航的误差互补扩展卡尔曼滤波算法。

为提高定位系统稳固性及位置输出平顺性，本文提出基于UKF的UWB/INS紧组合滤波模型后，采用萨格-胡萨（Sage-Husa）自适应滤波对其进行优化和野值剔除，完成在线对非视距干扰的有效辨别，提升紧组合定位方法在室内环境下的精度及鲁棒性。

1 组合滤波建模

1.1 状态方程

常规的INS一般都是以十五阶的状态量来建立系统的状态方程[11]。在室内环境中，对象节点在相对较小的定位范围内低速运动，可以不考虑地球曲率及自转，从而得出惯导状态方程为

1.2 量测方程

1.3 融合滤波

卡尔曼滤波通常对状态误差量来建模，一般不直接利用十五阶状态量。该方法可以分别进行航迹推算及滤波运算，即导航状态由惯导状态方程递归输出。而滤波器仅在量测发生时估计状态误差，并及时将其反馈给系统状态量。因此，滤波状态变量可表示为

继而得到先验误差协方差矩阵为

对于量测更新阶段，先对滤波器的增益进行估计，进而得到后验误差协方差矩阵为

2 基于Sage-Husa的自适应估计

在复杂的室内环境中，噪声的概率统计特性是有变化趋势的。比如：超宽带测距存在多路径干扰的情况；IMU容易受到温度变化的干扰，测量值会发生偏移。虽然不能准确地给出这样的随机噪声模型，但是可以通过对噪声的方差和平均进行适当的调节从而使模型近似的白噪声更加逼近实际的情况。卡尔曼滤波的自适应估计具有较好的在线辨识能力，能较好地区分出噪声的均值及方差，从而更好地适应各种噪声。由于IMU数据的噪声幅值变化较慢，所以只考虑了在线辨识噪声的均值及方差，从而使计算过程更简单[17]。

3 非视距处理

在超宽带定位系统中，NLOS误差影响非常明显。非视距误差与实际环境有密切关系，在电磁信号传播过程中若有障碍或多径效应影响，会导致测距不准确。上述情况是由于超宽带硬件接收到多径电磁信号，且把第一个到达的有时延信号判定为直路信号，这种情况在视距条件下基本无影响[19]。

在实际情况下判定超宽带硬件接收电磁信号是否为NLOS信号，可以依据接收非视距信道的信号功率会有很大的衰减这个特征，具体方法为

此外，紧耦合定位模型通过概率一致性检验证明数据合理性，卡尔曼滤波中的新息为白噪声序列，并且正交、时间不相关，则归一化新息方差为

以上2种方式配合，从物理特性和概率模型出发，提出了NLOS检测的基本原理，可有效地减少NLOS对定位系统的影响。在遇到非视距情况时，该系统会自动地选取惯性导航进行位置信息的解算，使其具有较好的稳定性。

4 仿真及结果分析

4.1 测试环境建立

将马尔可夫过程一阶模型对惯性传感器误差进行建模，而惯性部件的噪声包括高斯白噪声及高斯马尔可夫噪声。公式为：

表1 IMU参数配置表

视距情况下，用白噪声模型对超宽带测距伪距值噪声进行建模为

对于有NLOS干扰的场景，本文提出了以下的假定：

1）设时刻UWB测距存在NLOS误差的概率均匀分布，则表明在仿真环境下，各时刻有非视距干扰的可能性是相同的。

图1为模拟的室内定位场景示意图。仿真环境选择在矩阵实验室（matrix laboratory, MATLAB）R2020b下进行，实验场景设置在三轴为12 m×8 m×3 m的室内环境中，设定星号点表示基站（坐标如表2所示），圆圈点表示原点。设定对象历经匀速运动、加速运动、减速运动、直线运动、曲线运动等多种状态。并将目标节点的速度控制在1 m/s以内，采样的总时间为300 s。

图1 仿真实验场景

表2 UWB基站点号及坐标 m

4.2 结果及分析

为验证论文所提方案可行性，对比分析开展单一UWB定位、UWB/IMU组合定位测试；基于EKF和UKF滤波的组合定位对比测试。

4.2.1 单一UWB定位及其与IMU组合定位对比

如图2所示，待测目标点分别在单一UWB测距定位和组合定位EKF滤波方案下进行仿真实验。通过对比轨迹可以看出，单一UWB系统在运动中的目标节点跟随上不稳定，空间分辨率较低导致轴波动大。而组合定位EKF算法下的估计运动轨迹更接近于实际轨迹。

图2 UWB测距与EKF组合定位轨迹对比

如图3所示，单一UWB定位系统只使用测距值来解算位置。在非视距测距误差产生后，仅测距UWB系统输出不稳定，与实际的位置偏差较大。然而运用EKF算法的组合定位模型精度维持在0.3 m左右，表现出较好的效果。结果表明，利用EKF算法建立的模型可以很好地追踪到目标的位置，从而提高了单一UWB系统在NLOS干扰下的性能。

此外组合模型也能为物体的姿态、速度等精确地给出定位信息。如图4及图5所示，基于EKF算法实现组合定位速度与姿态跟踪曲线。从图4、图5中可明显看出，基于EKF算法的组合定位速度曲线跟随较好，姿态曲线略有起伏，但误差不大。

图3 UWB测距与EKF组合定位误差对比

图4 基于EKF的组合定位速度曲线

图5 基于EKF的组合定位姿态曲线

4.2.2 组合定位中的EKF和UKF滤波性能对比结果

由图6滤波器位置误差曲线可知，相比较之下，UKF算法的定位误差浮动范围维持在0.1 m之内，可以快速收敛到真实值，非线性近似精度更高。

图6 组合定位EKF与UKF误差对比图

EKF算法定位误差偏大的原因是：当UWB测距存在非视距误差时，滤波器不能识别这种情况，并对位置量进行了错误修正，从而导致其与实际值偏差；在新的视距伪距值产生后，由于测量更新过程中的非线性处理步长过大，使滤波产生了暂时性的波动。

5 结束语

本文提出基于UKF滤波的UWB/INS紧组合定位模型，可有效解决室内定位中由于NLOS干扰而影响定位精度的问题。利用Sage-Husa自适应估计分辨过程及测量噪声，并利用野值处理技术对NLOS干扰完成在线识别，使优化后模型的动态性能得以提升。测试结果表明，优于仅测距的UWB系统以及运用EKF算法的组合导航系统，误差可控制在10 cm以内，能有效提高非视距条件下的定位精度及稳定性。

[1] 李景文, 韦晶闪, 周俊芬, 等. 融合UWB+PDR的室内定位方法改进[J]. 测绘通报, 2022(3): 36-40.

[2] 高端阳, 李安, 傅军. 复杂环境下抗NLOS干扰的定位算法[J]. 导航定位学报, 2018, 6(1): 40-44,119.

[3] 姚子扬, 尚俊娜, 孙建强, 等. 一种多源融合室内外无缝定位方法[J]. 传感技术学报, 2022, 35(1): 72-83.

[4] LI X, WANG Y, KHOSHELHAM K. UWB/PDR tightly coupled navigation with robust extended kalman filter for NLOS environments[J]. Mobile Information Systems, 2018(3): 1-14.

[5] YANG X, WANG J, SONG D, et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system[J]. IEEE Sensors Journal, 2021, 21(9): 11203-11212.

[6] KOLAKOWSKI M, MODELSKI J. Detection of direct path component absence in NLOS UWB channel[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of the 201822nd International Microwave and Radar Conference (MIKION). Poznan Poland: IEEE, 2018: 247-250[2022-05-26].

[7] 宋波, 李生林, 刘恩启. 基于类别不平衡分类的超宽带非视距信号识别方法研究[J]. 信息工程大学学报, 2019, 20(2): 147-153.

[8] PARK C H, CHANG J H. Robust LMedS-based WLS and Tukey-based EKF algorithms under LOS/NLOS mixture conditions[J]. IEEE Access, 2019, 7: 148198-148207.

[9] WANG G, QIN S, QING L, et al. UWB and IMU system fusion for indoor navigation[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of the 201837nd Chinese Control Conference (CCC). Piscataway: IEEE, 2018: 4946-4950[2022-05-26].

[10] 余怀瑾, 依那, 蒋伟, 等. 基于UKF算法的高机动机载平台多普勒频移估计[J]. 北京大学学报(自然科学版), 2017, 53(6): 1003-1010.

[11] 徐爱功, 刘韬, 隋心, 等. UWB/INS紧组合的室内定位定姿方法[J]. 导航定位学报, 2017, 5(2): 14-19.

[12] 李月. UWB/INS联合室内定位方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2019.

[13] 孙伟, 李亚丹, 黄恒, 等. 基于级联滤波的建筑结构信息/惯导室内定位方法[J]. 仪器仪表学报, 2021, 42(3): 10-16.

[14] 黄剑雄, 刘小雄, 章卫国, 等. 基于视觉/惯导的无人机组合导航算法研究[J]. 计算机测量与控制, 2021, 29(2): 137-143, 149.

[15] MOGHTADAIEE V, ALIKHANI N, GHORASHI S A. Low-cost localisation considering LOS/NLOS impacts in challenging indoor environments[J]. International Journal of Sensor Networks, 2020, 32(1): 15-24.

[16] SUN W, LIU J Z. Design of robust cubature fission particle filter algorithm in multi-source cooperative navigation[J]. Scientific Reports, 2022, 12(1): 1-13.

[17] SUN W, WU J J, DING W, et al. A robust indirect Kalman filter based on the gradient descent algorithm for attitude estimation during dynamic conditions[EB/OL]. [2022-05-26]. https: //ieeexplore. ieee. org/stamp/stamp. jsp?tp=&arnumber=9099275. DOI: 10. 1109/ACCESS. 2020. 2997250.

[18] 盛坤鹏, 王坚, 李晨辉, 等. 非视距误差改正的超宽带定位模型研究[J]. 测绘科学, 2021, 46(3): 40-47.

[19] 尹烨佳, 华惊宇, 李枫, 等. 非视距环境中的无线网络残差加权定位算法[J]. 传感技术学报, 2019, 32(5): 749-755.

Fusion localization method of UWB/INS based on Sage-Husa adaptive UKF

SUN Wei, AN Shengjun

（School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China）

An ultra-wide band/inertial navigation system (INS) tight integrated positioning method based on unscented Kalman filter (UKF) was proposed to solve the problem of significant error of single ultra-wide band (UWB) localization caused by non-line-of-sight (NLOS) in the indoor environment．The UWB measurement equation was optimized，the error state of the navigation equation was used to simplify the prediction and correction of the Kalman filtering，and the process and measurement noise were identified online through the Sage-Husa adaptive estimation and the filter’s innovation value，so that the white noise of the model was closer to a real condition，and the outlier processing method was used to reduce the influence of non-line-of-sight on the positional accuracy．The results of the emulation experiment indicate that contrast with the conventional extended Kalman filter (EKF) and UKF filtering schemes，the integrated positioning model based on UKF can effectively suppress the influence of non-line-of-sight error and improve the positional accuracy．

ultra-wide band；unscented Kalman filter；adaptive filtering；indoor localization；non-line-of-sight

P228

A

2095-4999(2022)06-0068-07

孙伟，安晟均. 基于Sage-Husa自适应UKF的UWB/INS融合定位方法[J]. 导航定位学报, 2022, 10(6): 68-74.（SUN Wei，AN Shengjun. Fusion localization method of UWB/INS based on Sage-Husa adaptive UKF[J]. Journal of Navigation and Positioning, 2022, 10(6): 68-74.）

10.16547/j.cnki.10-1096. 20220609.

2022-07-08

2019辽宁省“兴辽英才计划”青年拔尖人才项目（XLYC1907064）；辽宁工程技术大学学科创新团队资助项目（LNTU20TD-06）；2018年度辽宁省“百千万人才工程”人选科技活动资助项目（辽百千万立项[2019]45号）。

孙伟（1984—），男，黑龙江萝北县人，博士，教授，研究方向为惯性及多源信息融合导航技术。

安晟均（1998—），男，辽宁营口人，硕士研究生，研究方向为惯性导航与组合导航方法。