复杂环境下的UWB/PDR紧组合定位方法

代晓霁，李 敏，徐天河，江 楠，许 艳

复杂环境下的UWB/PDR紧组合定位方法

代晓霁1，李 敏2，徐天河2，江 楠2，许 艳2

（1. 长安大学地质工程与测绘学院，西安 710054；2. 山东大学空间科学研究院，山东威海 264209）

室内定位；超宽带；行人航位推算；紧组合；复杂环境

0 引言

随着定位导航服务的不断发展和新技术在定位导航领域的广泛使用[1]，室内定位导航技术受到了人们极大的关注。众多学者开展了室内定位相关的研究工作，研究成果包括蓝牙[2]、无线保真（wireless fidelity, WiFi）[3]、第5代移动技术（the fifth generation of mobile technologies, 5G）[4]、超宽带（ultra wide band, UWB）[5]、惯性导航[6]（inertial navigation system, INS）、视觉定位[7]、音频定位[8]、地磁定位[9]等单一传感器定位技术及多种传感器融合定位技术。文献[10]在城市街道、长距离隧道等复杂非暴露空间，采用全球卫星导航系统（global navigation satellite system, GNSS）/INS/里程计融合的定位方法，极大增加了定位的成功率，除长距离隧道外，定位精度可保持优于2 m。文献[11]首次将UWB与INS进行了组合并用于确定组合系统的姿态。文献[12]利用UWB定位技术和行人航位推算（pedestrian dead reckoning, PDR）算法结合，采用约束卡尔曼滤波器使最终行人动态定位精度达到亚米级。文献[13]针对UWB无线信号易受干扰的问题，提出基于平方根容积卡尔曼滤波先更新后修正测量噪声的方法，能有效提高UWB/PDR组合的定位精度和稳定性。文献[14]利用扩展卡尔曼滤波对UWB和PDR进行融合，有效提升了室内定位精度，可以达到分米级。基于UWB的室内单点定位精度高但易受非视距（non line of sight, NLOS）误差影响，定位结果不稳定，而PDR算法短距离内定位精度高但存在航向漂移，因此将UWB和惯性传感器组合起来，组成UWB/PDR融合定位系统，二者具有很好的互补性。现阶段对UWB/PDR融合定位的研究多集中在削弱UWB NLOS误差上，如何保持复杂环境下的定位精度和稳定性是一直存在的难题。

在UWB NLOS环境下，UWB/PDR松组合会将NLOS误差引入UWB定位结果，而在UWB基站信号缺失的环境下，UWB会存在位置信息缺失的情况，从而降低整体融合精度。为了更加充分利用UWB测距信息，保持复杂环境下UWB/PDR融合系统定位的稳定性和精度，本文利用UWB测距及惯性传感器信息，提出了一种改进的UWB/PDR紧组合的定位方法。与松组合不同，该方法直接对UWB测距值进行融合，不须单独解算UWB位置信息，在UWB NLOS环境下和UWB基站信号缺失等复杂环境下定位效果都要优于UWB/PDR松组合，而且增加了系统的鲁棒性。

1 UWB测距及定位

1.1 UWB测距

UWB 基于到达时间（time of arrival, TOA）测距是通过记录超宽带脉冲信号从UWB移动站到基准站传播的时间来计算UWB移动站到基准站之间的距离。对于室内定位，由于实验环境等限制，UWB基站高程方向上差异较小造成高程方向出现明显的定位误差[15]。所以本文研究主要针对平面位置解算，将UWB基准站固定，保持高度不变；实验在室内小范围内进行，UWB移动站高度变化也可忽略不计。计算过程中，在原始TOA测距信息中减去UWB基准站与移动站之间的高差得到平距，其计算公式为

UWB基准站与移动站之间的TOA测距模型为

1.2 UWB定位

由UWB TOA测距可得UWB定位表达式为

变形可得UWB定位线性观测方程

设置迭代阈值，其最小二乘算法解为

式中为权阵。

最终得到时刻UWB的位置信息为

2 行人航位推算

PDR是一种针对行人平面定位导航的相对定位算法[16]，其基本原理如图1所示：利用加速度计和陀螺仪信息，实时探测行人步频，估计行人步长，同时解算行人航向角；基于行人前一步的位置信息，结合当前时刻的步长和航向角，推算当前时刻行人的位置信息。

图1 行人航位推算原理

基本公式为

考虑到惯性传感器的精度和与步长估计算法的同步性，步频探测算法选取文献[17]提出的基于“滑动窗口+峰值探测+零点交叉”的算法。此算法先对加速度的重力分量进行剔除，再进行预设窗口的平滑，可有效抑制加速度计随机噪声的影响，得到更加明显的行人步态特征。进行加速度峰值探测时，应保证连续2个加速度峰值之间的时间差大于1个阈值，该阈值大概为行人步行一步的时间，消除同一步里出现加速度多峰造成错误的步频判断。零点交叉主要用于确定每一跨步的开始点和结束点，即加速度前一时刻小于零，当前时刻大于或等于零的时间。

行人步长模型采用经典步长线性模型[18]：

初始航向角的获取则利用轨迹相似性原理[19]，基于行人1个跨步内航向基本保持不变，通过匹配UWB和PDR的位移向量对MEMS进行航向的初始对准，无须引入额外传感器。

3 改进的UWB/PDR紧组合算法

目前常用的融合估计方法是利用卡尔曼滤波（Kalman filter，KF）[20]算法对系统进行滤波处理。KF是以一种“预测-更新”的模式对状态向量进行估计。在预测阶段，根据系统状态方程，递推状态预测向量；在量测更新阶段，根据系统状态协方差矩阵和量测噪声协方差矩阵来计算增益矩阵，然后利用增益矩阵对参数验前估值和新的观测值加权平均进行状态参数更新，从而获得最优解。然而，在现实应用中很多问题并不能用线性函数表述，通常采用扩展卡尔曼滤波（extend Kalman filter, EKF），即式（10）解决非线性函数问题，主要思想是对非线性函数泰勒级数展开，忽略高阶项，将非线性函数近似为线性函数求解，即

3.1 状态方程

在改进的UWB/PDR 紧组合定位系统中，以UWB建立的坐标系为导航坐标系，以位置误差、步长误差和航向角误差作为系统的状态变量，即

对式（8）进行微分得到UWB/PDR紧组合的状态方程为

3.2 量测方程

当UWB系统测距信息更新时，以UWB的测距信息和PDR推算的测距信息的差值作为系统量测信息，UWB/PDR紧组合定位系统的量测方程为

3.3 UWB NLOS检验

图2 显著性水平与阈值之间的关系

图3 UWB/PDR紧组合定位流程

4 实验与结果分析

4.1 实验条件

图4 实验场地

图5 原始UWB测距和惯性传感器信息

4.2 UWB/PDR紧组合

图6 不同定位方法的定位轨迹结果

图7展示了上述几种定位方法的定位误差。图8展示了根据定位误差求得的平面位置误差累积分布函数（cumulative distribution function，CDF）曲线。表1展示了行人室内定位实验总体误差的统计结果。上述结果表明：在视距环境下，改进的UWB/PDR紧组合定位精度与UWB/PDR松组合定位、UWB单独定位基本一致；而在NLOS环境下，改进的UWB/PDR紧组合最大定位误差和平均定位误差分别为0.727和0.669 m，相比于单独UWB定位的最大误差和平均误差分别降低了75.1%和70.1%，相比于PDR定位分别降低了74.5%和62.0%，相比于UWB/PDR松组合分别降低了51.4%和47.7%。因此，改进的UWB/PDR紧组合方法更加充分利用了UWB的测距信息，整体定位效果优于UWB/ PDR松组合，具有较强的鲁棒性。

图7 不同定位方法的的定位误差

图8 不同定位方法的位置误差CDF

表1 不同定位方法的的定位误差 m

此外，实验还模拟计算了不同UWB基站数量情况下的UWB/PDR紧组合定位效果。图9展示了不同UWB基站数量情况下UWB/PDR紧组合定位的轨迹。图10展示了不同UWB基站数量情况下UWB/PDR紧组合的定位误差。表2展示了不同UWB基站数量情况下UWB/PDR紧组合定位误差的统计结果。结果显示：当UWB基站为4个或3个时，定位效果基本一致；当UWB基站为2个时，部分时刻UWB/PDR紧组合定位效果不如UWB基站为4个或3个时，但整体定位效果还是能够满足基本的平面位置估计要求。但由于UWB平面位置解算至少需要3个UWB基站，此时UWB/PDR松组合就不能发挥作用，再次说明UWB/PDR紧组合的鲁棒性。当UWB基站数量为1个时，UWB/PDR紧组合定位也会失去对PDR漂移误差的约束作用，定位误差也会迅速发散。

图9 不同基站数量情况下的紧组合定位轨迹结果

图10 不同基站数量情况下的紧组合定位误差

表2 不同基站数量情况下的紧组合定位误差

最后实验评估了UWB基站数量缺失情况下行人改进UWB/PDR紧组合的室内动态定位精度。图11展示了UWB基站数量缺失情况下UWB/PDR松、紧组合定位的轨迹结果。图12展示了UWB基站数量缺失情况下UWB/PDR松、紧组合定位方法的误差。结果表明，在UWB基站数量缺失情况下，UWB/PDR紧组合定位效果明显优于UWB/PDR松组合，特别是在当UWB基站数量为1或2个时。此时，松组合定位退化为单PDR定位，定位误差迅速增大；UWB/PDR紧组合定位可以充分利用UWB的测距信息，定位精度依然保持在0.6 m左右，具有较强的抗干扰性。图13展示了根据定位误差求得的平面位置误差CDF曲线，结果表明：在UWB基站数量缺失情况下，UWB/PDR紧组合整体定位性能要优于UWB/PDR松组合和单独PDR；在UWB基站数量变化情况下，UWB/PDR紧组合最大定位误差和平均定位误差分别为1.283和0.566 m，相比于UWB/PDR松组合，定位误差RMS分别降低了18.7%和24.7%，更适合复杂环境的位置估计。

图11 基站数量缺失情况下的定位轨迹结果

图12 基站数量缺失情况下的定位误差

图13 基站数量缺失情况下的位置误差累积分布函数

5 结束语

[1] 刘公绪, 史凌峰. 室内导航与定位技术发展综述[J]. 导航定位学报, 2018, 6(2): 7-14.

[2] ZHUANG Y,YANG J,LI Y,et al.Smartphone-based indoor localization with bluetooth low energy beacons[J].Sensors, 2016, 16(5): 596.

[3] SHU Y,BO C,SHEN G,et al.Magicol:indoor localization using pervasive magnetic field and opportunistic WiFi sensing[J].IEEE Journal on Selected Areas in Communications, 2015, 33(7): 1443-1457.

[4] 张平, 陈昊. 面向5G的定位技术研究综述[J]. 北京邮电大学学报, 2018, 41(5): 1-12.

[5] 熊海良. 超宽带无线通信与定位关键技术研究[D]. 西安: 西安电子科技大学, 2011.

[6] BARSHAN B,DURRANT-WHYTE H F.Inertial navigation systems for mobile robots[J].IEEE Transactions on Robotics and Automation, 1995, 11(3): 328-342.

[7] YAN Z,CHU S, DENG L. Visual SLAM based on instance segmentation in dynamic scenes[J]. Measurement Science and Technology, 2021, 32(9): 095113.

[8] DOSTÁLEK P,DOLINAY J, VAŠEK V. Embedded system for audio source localization based on beamforming[J]. International Journal of Circuits: Systems and Signal Processing, 2012, 6(6): 367-375.

[9] LEE S,CHAE S,HAN D.ILoA:indoor localization using augmented vector of geomagnetic field[J].IEEE Access, 2020, 8: 184242-184255.

[10] 刘飞. 多传感器融合的高精度无缝定位模型与方法研究[J]. 测绘学报, 2021, 50(12): 1780.

[11] HOL J D,DIJKSTRA F, LUINGE H, et al. Tightly coupled UWB/IMU pose estimation[C]//The Institute of Electrical and Electronic Engineers (IEEE). 2009 IEEE International Conference on Ultra-wideband. Palms Springs: IEEE, 2009: 688-692.

[12] LI Z K,ZHAO L,QIN C B,et al.WiFi/PDR integrated navigation with robustly constrained Kalman filter[J].Measurement Science and Technology, 2020, 31(8): 084002.

[13] TANG C,HE C,DOU L.An IMU/ODM/UWB Joint localization system based on modified cubature Kalman filtering[J].Sensors, 2021, 21(14): 4823.

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

[15] 文铠.基于超宽带的室内定位与定向关键技术研究[D]. 武汉: 武汉大学, 2020.

[16] 旷俭. 基于捷联PDR和磁场特征匹配的智能手机多源室内定位算法研究[D]. 武汉: 武汉大学, 2019.

[17] 陈伟.基于GPS和自包含传感器的行人室内外无缝定位算法研究[D]. 合肥: 中国科学技术大学, 2010.

[18] 陈锐志, 郭光毅, 叶锋, 等. 智能手机音频信号与MEMS传感器的紧耦合室内定位方法[J]. 测绘学报, 2021, 50(2): 143-152.

[19] CHEN Q J,LIN H,GUO R,et al.Rapid and accurate initial alignment of the low-cost MEMS IMU chip dedicated for tilted RTK receiver[J].GPS Solutions, 2020, 24(4): 1-13.

[20] 徐恩松, 陆文华, 刘云飞, 等. 基于卡尔曼滤波的数据融合算法与应用研究[J]. 计算机技术与发展, 2020, 30(5): 143-147.

[21] 苗岳旺, 周巍, 田亮, 等. 基于新息检测的扩展抗差卡尔曼滤波及其应用[J]. 武汉大学学报(信息科学版), 2016, 41(2): 269-273.

[22] 杨元喜, 任夏, 许艳. 自适应抗差滤波理论及应用的主要进展[J]. 导航定位学报, 2013, 1(1): 9-15.

[23] 陈国良, 李飞, 张言哲. 一种基于自适应波峰检测的MEMS计步算法[J]. 中国惯性技术学报, 2015, 23(3): 315-321.

Tight integration positioning method of UWB/PDR in complex environments

DAI Xiaoji1, LI Min2, XU Tianhe2, JIANG Nan2, XU Yan2

（1. School of Geological and Surveying Engineering, Chang’an University, Xi’an 710054, China;2. Institute of Space Science, Shandong University, Weihai, Shandong 264209, China）

indoor positioning; ultra wide band; pedestrian dead reckoning; tight integration; complex environments

P228

A

2095-4999(2022)06-0018-09

代晓霁，李敏，徐天河，等. 复杂环境下的UWB/PDR紧组合定位方法[J]. 导航定位学报, 2022, 10(6): 18-26.（DAI Xiaoji, LI Min, XU Tianhe, et al. Tight integration positioning method of UWB/PDR in complex environments[J]. Journal of Navigation and Positioning, 2022, 10(6): 18-26.）

10.16547/j.cnki.10-1096.20220603.

2022-06-17

国家重点研发计划项目（2020YFB0505800，2020YFB0505804）。

代晓霁（1995—），男，河南驻马店人，硕士研究生，研究方向为室内定位。

徐天河（1975—），男，湖北黄冈人，博士，教授，研究方向为卫星导航、卫星重力、海洋大地测量基准、海洋自适应融合导航。