基于ISRUKF的UWB/INS组合室内定位方法研究

邹 强，陆甫光，兰馗博，窦 彦

基于ISRUKF的UWB/INS组合室内定位方法研究

邹 强1, 2，陆甫光1, 2，兰馗博1，窦 彦3, 4

(1. 天津大学微电子学院，天津 300072；2. 天津市物联网国际联合研究中心，天津 300072；3. 天津衣联网生态科技有限公司，天津 300350；4. 天津海尔洗涤电器有限公司，天津 300350)

超宽带(UWB)定位技术是室内定位领域中极具竞争力的技术，但是在室内非视距(NLOS)环境下，UWB定位技术会出现精度低和稳定性差等问题，难以满足较高的室内定位精度需求．UWB定位技术和惯性导航系统(INS)结合，可以减轻NLOS环境对UWB定位的影响，因此提出了一种基于改进平方根无迹卡尔曼滤波(ISRUKF)的UWB/INS组合室内定位方法．UWB/INS融合定位系统工作时，首先，通过INS输出定位目标的定位信息，但是单独的INS解算结果误差很大．然后，利用由零速更新(ZUPT)、零角速率更新(ZARU)和启发式漂移消减(HDR)组成的INS优化框架初步消除INS的累积误差．最后，基于ISRUKF以紧耦合方式深度融合UWB定位信息和INS定位信息，得到融合定位结果．其中，ISRUKF在标准平方根无迹卡尔曼滤波(SRUKF)的基础上增加了UWB伪距信息平滑、UWB伪距信息离群值修正以及状态协方差矩阵更新策略．在室内NLOS环境下进行了一系列实验，实验结果表明所提方法既可以补偿UWB定位技术在NLOS环境下的精度丢失，也可以进一步限制INS的累积误差．另外，相较于扩展卡尔曼滤波(EKF)和标准SRUKF，ISRUKF分别提高了9.48%和4.59%的定位精度，具有更好的滤波效果和鲁棒性．

室内定位；超宽带定位技术；惯性导航系统；改进平方根无迹卡尔曼滤波；紧耦合

随着无线通信技术和移动设备的发展，室内定位服务逐渐成为现代社会生产生活中的重要内容．传统的室外定位技术，如GPS、GALILEO、GLONASS和北斗卫星导航系统等在面对新兴的室内定位场景时，容易受墙体、家具和装饰品等障碍物的影响，无法达到室内定位的精度标准[1]．这就推动了包括Wi-Fi、蓝牙、可见光、ZigBee和RFID等室内定位技术的发展[2]．虽然这些技术在室内定位领域中应用比较广泛，但是仍然存在定位精度较差和成本较高的问题．近年来，UWB定位技术凭借其精度高、功耗低和系统容量大等优点从众多室内定位技术中脱颖而出，成为室内定位领域中的关键技术[3]．然而UWB定位技术容易受到室内NLOS环境的干扰，定位结果会出现较大误差．因此如何识别并抑制NLOS环境对UWB定位的影响是一个亟待解决的问题．传统上的解决方法均是利用UWB自身信息来识别NLOS条件并进行NLOS测距误差补偿[4-6]．尽管这类方法起到一定作用，但是单一UWB定位的性能终究是有限的，因此利用UWB和其他定位技术实现多源信息融合定位成为该领域的研究重点．Bargshady等[7]提出了一种UWB/Wi-Fi联合定位方法，但是Wi-Fi部署复杂且需要时刻联网．李中道等[8]提出了一种基于UWB/LiDAR的融合定位算法，但是LiDAR价格昂贵且体积较大．

INS是一种自主式导航系统，无需依赖任何外部信息就能提供定位目标的位置、速度和姿态等，不受NLOS环境的影响．INS的核心器件是惯性测量单元(inertial measurement unit，IMU)，IMU具有体积小、重量轻、成本低和可靠性高等优点[9]．INS随着时间推移会出现误差累积现象，通常INS可以利用自身信息来抑制累积误差，如ZUPT[10]、ZARU[11]、HDR[12]和地磁偏航[13]等，但是该类方法效果有限.另一种更有效的方法是引入无线定位技术，如UWB、Wi-Fi、RFID和GPS等．

当前，UWB/INS融合定位是实现室内行人高精度定位的主流方案．过去已经有相关人员做过了大量研究：Hol等[14]提出了一种UWB和IMU紧耦合的集成方法，该方法信息融合彻底，相较于松耦合提高了定位精度．紧耦合的融合方式通常涉及到非线性滤波．Yuan等[15]提出了一种基于EKF的UWB/INS数据融合方法，然而EKF一阶泰勒级数展开忽略了二阶及以上的高阶项，对于非线性系统可能导致滤波发散，并且涉及到的雅可比矩阵计算复杂，容易出错．Chen等[16]提出了一种基于UKF的UWB/PDR联合定位系统，较好解决了非线性系统中滤波发散的问题，但是高维数据下UKF容易因为协方差矩阵非正定而崩溃．Tian等[17]提出了一种使用粒子滤波(particle filter，PF)进行行人跟踪的INS/UWB融合定位系统，通过粒子重置的方法，解决了跟踪丢失的问题，但是PF计算量大．文献[18-19]在原有UWB/INS的基础上进一步集成了其他传感器或数据，虽然获得了更高的定位精度，但是相应的系统成本和复杂度也有所增加．

综合考虑精度、成本和鲁棒性等因素，本文提出了一种基于ISRUKF的UWB/INS组合室内定位方法，该方法主要应用于室内行人动态定位．在该UWB/INS融合定位系统中，通过分析各部分模型，设计了一种KF-EKF-ISRUKF架构．其中，KF用于状态转移模型，EKF用于INS优化框架，ISRUKF用于UWB伪距更新．本文验证了上述方法在室内NLOS环境下对于行人动态定位的有效性，实验结果表明ISRUKF的定位精度高于传统的EKF和SRUKF．

1 UWB/INS融合定位系统

图1显示了UWB/INS融合定位系统的总框架.该系统主要分为3个部分，第1部分是INS解算，IMU通过内置的加速度计和陀螺仪分别采集加速度数据和角速度数据，经惯性导航方程解算后输出定位目标的位置、速度和姿态[9]；第2部分是INS优化框架，当定位目标满足静止条件时，通过基于EKF的ZUPT、ZARU和HDR来限制INS的累积误差；第3部分是UWB伪距更新，当定位目标满足UWB伪距更新条件时，通过ISRUKF实现UWB/INS的融合定位．其中，ISRUKF由标准SRUKF、UWB伪距信息平滑、UWB伪距信息离群值修正和状态协方差矩阵更新策略组成．

图1 UWB/INS融合定位系统总框架

1.1 惯性导航方程

图2 INS定位原理示意

本文中INS的导航坐标系(n系)采取“东-北-天”坐标系，机体坐标系(b系)采取“左-前-上”坐标系．在数学上，INS的导航过程可以描述为

1.2 状态转移模型

本文中，无论是基于EKF的INS优化框架还是基于ISRUKF的UWB/INS融合定位，其状态向量均是15维的状态误差向量，即

因为状态转移模型是线性的，所以状态转移过程采用标准卡尔曼线性滤波形式．离散时间下系统状态转移模型定义为

1.3 系统测量模型

系统测量模型由两部分组成，分别是INS优化框架和UWB伪距更新．

1.3.1 INS优化框架

通过观察人类行走时的步态特点，可以将人类行走过程简单地划分为静止阶段和移动阶段．当人类处于静止阶段时，理论上人脚的速度和角速度几乎为零，并且航向也基本不会发生变化．但是实际情况中，受限于MEMS-IMU自身的精度，此时计算得到的速度并不为零、测量得到的角速度并不为零，航向也会有所变化．如果不对这个阶段的相关数据进行处理，那么随着时间推移，INS的累积误差会持续增大，严重时甚至导致INS无法定位．

考虑到室内环境中地磁不稳定，所以本文仅选取ZUPT、ZARU和HDR来组成INS优化框架．如图3所示，INS优化框架首先基于广义似然比检测算法来检测静止阶段，然后根据设定的条件分别提取出误差观测向量，最后通过EKF将这些误差观测向量用于更新状态误差向量，并最终融入惯性导航方程中．

图3 基于EKF的INS优化框架示意

1.3.2 UWB伪距更新

UWB技术是近年来新兴的一种无载波通信技术．通过发射纳秒至皮秒级的极窄脉冲，UWB技术可以实现快速的数据传输．基于UWB技术的定位方案具有功耗低、传播速率快、安全性好和定位精度高等优点[3]．本文中的UWB定位采用的是到达时间(time of arrival，TOA)定位方法．TOA定位方法通过测量UWB信号从UWB待测标签到达UWB基站的传输时间来计算待测标签与基站间的距离．针对TOA定位方法中存在因基站和待测标签时钟不同步而导致的测距误差的问题，本文决定采用双边双向测距(double-sided two-way ranging，DS-TWR)算法来实现TOA定位．如图4所示，二维平面内，基于TOA定位方法的UWB定位至少需要3个UWB基站和1个UWB待测标签，以UWB基站为圆心，以TOA定位方法计算的距离为半径作圆，理想条件下，三圆的交点就是定位目标的位置，其具体计算过程为

紧耦合方式下，用UWB伪距信息和INS解算位置相对于UWB基站距离的距离差来作为状态误差向量的观测值，定义观测值为

式中：为当前时刻第个基站测得的伪距信息；为当前时刻INS解算出来的位置；为第个UWB基站的位置．

2 改进的平方根无迹卡尔曼滤波

卡尔曼滤波理论是动态系统状态估计领域中的重要技术，广泛应用于参数估计、系统控制和定位导航等方面．SRUKF是标准UKF的改进，其通过QR分解和Cholesky因子更新确保协方差矩阵的半正定性[20]，有效解决了UKF稳定性差的问题．但传统的SRUKF没有考虑测量数据离群值的影响以及状态协方差矩阵在复杂系统下仍然容易产生非正定的问题.因此本文提出的ISRUKF在标准SRUKF的基础上，引入了UWB伪距信息平滑、UWB伪距信息离群值修正和状态协方差矩阵更新策略，提高了UWB/INS融合定位系统在NLOS环境下的容错性和鲁棒性．

2.1 UWB伪距信息平滑

UWB待测标签通常固定在人体上，人在运动的过程中难免会使UWB待测标签产生位移，进而导致UWB定位系统所测得的伪距信息波动．针对这一问题，在加固UWB待测标签的传统方法上，提出了一种UWB伪距信息窗口平滑方法，该方法的窗口大小设置为7．经过该方法处理过的原始UWB数据会变得更平滑且准确．其公式为

2.2 UWB伪距信息离群值修正

严重的NLOS环境会造成TOA定位方法的测距误差较大．如何降低NLOS测距误差对定位精度的影响是室内定位系统的重要挑战．针对NLOS环境的影响，通过设定某一阈值来鉴别NLOS误差．对于当前时刻下的位置，可以得到两组距离信息：一组是UWB基站直接测量UWB待测标签位置所得的距离信息；另一组是由INS的位置相对于UWB基站位置解算出来的距离信息．将二者相减并取绝对值，如果该绝对值大于所设定的阈值，那么便判定此时的UWB信号受到NLOS环境干扰，该组UWB伪距信息不能直接参加UWB伪距更新过程．其计算式为

之后需要对该组UWB伪距信息进行修正，将UWB伪距离群值依次减去NLOS误差经验值便可得到较为正确的新UWB伪距信息，从而完成对NLOS误差的抑制．修正公式为

2.3 平方根无迹卡尔曼滤波

标准SRUKF的流程如下．

步骤1 Sigma点权值设置．

步骤2 初始化．

步骤3 计算Sigma点．

步骤4 时间更新．

步骤5 再次计算Sigma点.

步骤6 观测更新.

步骤7 滤波更新．

步骤8 状态协方差矩阵更新．

2.4 状态协方差矩阵更新策略

3 实验结果及分析

在这项工作中，首先，进行了UWB在视距(line-of-sight，LOS)和NLOS环境下的定位实验，分析了NLOS环境对于UWB定位的影响；然后，进行了单一的INS定位实验和加入INS优化框架后的INS定位实验，分析了INS优化框架对于INS定位的影响；最后，在NLOS环境下，进行了UWB/INS融合定位实验，其中重点比较了ISRUKF、SRUKF和EKF的融合性能，实验结果表明本文所提出的ISRUKF具有优越性．

3.1 实验器材及环境

实验中所涉及的器材主要包括UWB基站4个、UWB待测标签1个、MEMS-IMU 1个、笔记本电脑2台、三角支架4个、白板1块、胶带1卷、移动电源和数据线若干．其中，UWB是DWM1000模块，工作频率设置为10Hz，其具体性能参数如表1所示；MEMS-IMU是博世BMI-055，工作频率设置为100Hz，其内置的加速计和陀螺仪的具体性能参数分别如表2和表3所示．

如图5所示，实验场景是长10m、宽6m的室内矩形区域．障碍物是长1.55m、宽0.1m、高1.8m的白板，当需要制造NLOS环境时将其放置在矩形区域中心．4个UWB基站分别垂直固定在高为1.7m的三角支架上，1个UWB待测标签垂直固定在定位目标头部，MEMS-IMU水平固定在定位目标右脚背上.

表1 UWB性能参数

Tab.1 UWB performance parameters

表2 加速度计性能参数

Tab.2 Accelerometer performance parameters

表3 陀螺仪性能参数

Tab.3 Gyroscope performance parameters

图5 实验环境平面示意

3.2 UWB定位实验结果及分析

图6显示了UWB分别在LOS和NLOS环境下的定位结果．图中的黑色点线代表人为预设的真实路径，蓝色虚线代表LOS环境下的UWB定位轨迹，红色曲线代表NLOS环境下的UWB定位轨迹．从图中可以看出，LOS环境下，UWB定位轨迹比较平滑且基本贴近于真实路径；而NLOS环境下，UWB定位轨迹不够平滑且偏离真实路径明显，在真实路径的4个直角处甚至出现了比较大的“毛刺状”凸起．

图6 LOS和NLOS环境下的UWB定位结果比较

表4是LOS和NLOS环境下的UWB定位误差的数据分析．由表4可知，相较于LOS环境，NLOS环境下的UWB定位最大误差增长了83.87%，平均绝对误差增长了54.85%，均方根误差增长了46.46%，这同样印证了图6的结论．

表4 LOS和NLOS环境下的UWB定位误差

Tab.4 UWB positioning errors under LOS and NLOS environment 　m

3.3 INS定位实验结果及分析

图7是原始的INS和经过优化后的INS的定位结果比较．图中黑色点线是人为预设的真实路径，蓝色虚线是原始的INS定位轨迹，红色曲线是加入INS优化框架后的INS定位轨迹．由图7可知，原始的INS定位轨迹在起始阶段就发生了巨大漂移，无法实现正常的定位．而引入INS优化框架后的INS定位，其定位轨迹平滑且形状基本与真实路径形状相吻合，但是受累积误差影响，轨迹整体偏移的趋势依然严重.

图7 INS和INS优化框架定位结果比较

3.4 UWB/INS融合定位实验结果及分析

UWB和INS的融合定位方案中，在NLOS环境下分别进行了基于EKF、SRUKF和ISRUKF的融合定位实验．如图8所示，UWB/INS融合定位结果明显优于图6中的UWB-NLOS定位结果，但是3种融合定位算法的定位轨迹基本重合，肉眼较难分辨．

图8 UWB/INS融合定位结果

图9是误差累积分布函数．由图9可知，EKF、SRUKF和ISRUKF中80%的定位误差分别在0.2017m、0.1988m和0.1901m以下，均小于UWB-NLOS的0.2200m．这表明融合定位方案在NLOS环境下具有突出性能，其中以ISRUKF的定位效果最好．

图9 误差累积分布函数

表5是3种融合定位算法的误差分析，以均方根误差为例，NLOS环境下，ISRUKF的定位精度相较于SRUKF提高了4.59%，相较于EKF提高了9.48%．3种融合定位算法中，本文提出的ISRUKF定位精度最高．另外，通过比较表４和表５可知，ISRUKF的定位精度相较于UWB-NLOS提高了24.28%．

表5 UWB/INS融合定位误差

Tab.5 UWB/INS fusion positioning errors 　m

4 结 语

针对UWB定位技术容易受NLOS环境影响的问题，本文提出了一种基于ISRUKF的UWB/INS组合室内定位方法．在该方法中，首先通过INS优化框架来初步限制INS的累积误差，使得INS输出的位置、速度和姿态等信息较为准确；然后利用TOA定位方法输出UWB定位的坐标和伪距信息；最后基于ISRUKF实现UWB和INS的融合定位，其中ISRUKF在标准SRUKF的基础上增加了UWB伪距信息平滑、UWB伪距信息离群值修正以及状态协方差矩阵更新策略．实验结果表明，基于ISRUKF的UWB/INS组合室内定位方法提高了融合定位系统的定位性能和鲁棒性，在NLOS环境下，以均方根误差为例，ISRUKF的定位精度相较于SRUKF提高了4.59%，相较于EKF提高了9.48%，相较于UWB-NLOS提高了24.28%．

［1］ Yassin A，Nasser Y，Awad M，et al. Recent advances in indoor localization：A survey on theoretical approaches and applications[J]. IEEE Communications Surveys & Tutorials，2017，19(2)：1327-1346.

［2］ Zafari F，Gkelias A，Leung K K. A survey of indoor localization systems and technologies[J]. IEEE Communications Surveys & Tutorials，2019，21(3)：2568-2599.

［3］ Abdulrahman A，Abdulmalik A S，Mansour A，et al. Ultra wideband indoor positioning technologies：Analysis and recent advances[J]. Sensors，2016，16(5)：1-36.

［4］ Yan L B，Lu Y，Zhang Y R. An improved NLOS identifica-tion and mitigation approach for target tracking in wireless sensor networks[J]. IEEE Access，2017，5：2798-2807.

［5］ Yang X F，Zhao F，Chen T J. NLOS identification for UWB localization based on import vector machine[J]. AEU-International Journal of Electronics and Communications，2018，87：128-133.

［6］ Yu K G，Wen K，Li Y B，et al. A novel NLOS mitigation algorithm for UWB localization in Harsh indoor environments[J]. IEEE Transactions on Vehicular Technology，2019，68(1)：686-699.

［7］ Bargshady N，Pahlavan K，Alsindi N A. Hybrid WiFi/UWB，cooperative localization using particle filter[C]//2015 International Conference on Computing，Networking and Communications(ICNC). Garden Grove，USA，2015：1055-1060.

［8］ 李中道，刘元盛，常飞翔，等. 室内环境下UWB与LiDAR融合定位算法研究[J]. 计算机工程与应用，2021，57(6)：260-266.

Li Zhongdao，Liu Yuansheng，Chang Feixiang，et al. Research on UWB and LiDAR fusion positioning algorithm in indoor environment[J]. Computer Engineering and Applications，2021，57(6)：260-266(in Chinese).

［9］ 王 巍. 惯性技术研究现状及发展趋势[J]. 自动化学报，2013，39(6)：723-729.

Wang Wei. Status and development trend of inertial technology[J]. Acta Automatica Sinica，2013，39(6)：723-729(in Chinese).

［10］ Foxlin E. Pedestrian tracking with shoe-mounted inertial sensors[J]. IEEE Computer Graphics and Applications，2005，25(6)：38-46.

［11］ Jiménez A R，Seco F，Prieto J C，et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C]//2010 7th Workshop on Positioning，Navigation and Commu-nication(WPNC). Dresden，Germany，2010：135-143.

［12］ Borenstein J，Ojeda L，Kwanmuang S. Heuristic reduction of gyro drift for personnel tracking systems [J]. Journal of Navigation，2009，62(1)：41-58.

［13］ Zhang W C，Li X H，Wei D Y，et al. A foot-mounted PDR system based on IMU/EKF＋HMM＋ZUPT＋ZARU+HDR+compass algorithm[C]//2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN). Sapporo，Japan，2017：1-5.

［14］ Hol J D，Dijkstra F，Luinge H，et al. Tightly coupled UWB/IMU pose estimation[C]// 2009 IEEE Interna-tional Conference on Ultra-Wideband(ICUWB). Vancouver，Canada，2009：688-692.

［15］ Yuan X，Chen X Y，Jin C，et al. Improving tightly-coupled model for indoor pedestrian navigation using foot-mounted IMU and UWB measurements[C]//2016 IEEE International Instrumentation and Measurement Technology Conference Proceedings. Taipei，China，2016：1-5.

［16］ Chen P Z，Kuang Y，Chen X Y. A UWB/improved PDR integration algorithm applied to dynamic indoor positioning for pedestrians[J]. Sensors，2017，17(9)：2065.

［17］ Tian Q L，Wang I K，Salcic Z. A resetting approach for ins and uwb sensor fusion using particle filter for pedestrian tracking[J]. IEEE Transactions on Instru-mentation and Measurement，2020，69(8)：5914-5921.

［18］ Benini A，Mancini A，Longhi S. An IMU/UWB/vision-based extended Kalman filter for mini-UAV localization in indoor environment using 802.15.4a wireless sensor network[J]. Journal of Intelligent & Robotic Systems，2012，70(1/2/3/4)：461-476.

［19］ Liu F，Wang J，Zhang J X，et al. An indoor localization method for pedestrians base on combined UWB/ PDR/floor map[J]. Sensors(Basel，Switzerland)，2019，19(11)：2578-2596.

［20］ Merwe R，Wan E A. The square-root unscented Kalman filter for state and parameter-estimation[C]//2001 IEEE International Conference on Acoustics，Speech，and Signal Processing(ICASSP). Salt Lake City，USA，2001：3461-3464.

UWB/INS Combined Indoor Positioning Method Based on ISRUKF

Zou Qiang1, 2，Lu Fuguang1, 2，Lan Kuibo1，Dou Yan3, 4

(1. School of Microelectronics，Tianjin University，Tianjin 300072，China；2. International Joint Research Center for Internet of Things，Tianjin University，Tianjin 300072，China；3. Tianjin Internet of Clothes Ecological Technology Co.，Ltd.，Tianjin 300350，China；4. Tianjin Haier Washing Electric Appliance Co.，Ltd.，Tianjin 300350，China)

Ultra-wide band(UWB)positioning technology is a highly competitive technology in the field of indoor positioning；however，its low accuracy and poor stability in the indoor non-line-of-sight(NLOS)environment makes it difficult to meet the high indoor positioning accuracy requirements. Combining UWB positioning technology and inertial navigation system(INS)can reduce the impact of the NLOS environment on UWB positioning；therefore，a UWB/INS combined indoor positioning method based on improved square root unscented Kalman filter(ISRUKF)is proposed in this study. The positioning information of the positioning target was first output through the INS when the UWB/INS fusion positioning system was in operation；however，the single INS results contain very large errors. Then，to initially eliminate the INS cumulative errors，an INS optimized framework consisting of zero velocity update(ZUPT)，zero angular rate update(ZARU)，and heuristic drift reduction(HDR) was utilized. Finally，based on ISRUKF，the UWB and INS positioning information was deeply fused in a tightly coupled manner，and the fusion positioning results were obtained. Using the standard square root unscented Kalman filter(SRUKF)，the ISRUKF added UWB pseudo-range information smoothing，UWB pseudo-range information outlier correction，and state covariance matrix update strategy. A series of experiments were completed in the indoor NLOS environment. The experimental results showed that the proposed method could compensate UWB positioning technology accuracy loss in the NLOS environment and further eliminate the INS cumulative errors. In addition，the ISRUKF improves the positioning accuracy by 9.48% and 4.59%，compared to the EKF and SRUKF，respectively，and it has a better filtering effect and robustness.

indoor positioning；ultra-wide band positioning technology；inertial navigation system；improved square root unscented Kalman filter；tightly coupled

10.11784/tdxbz202105006

TN967.2

A

0493-2137(2022)05-0496-08

2021-05-05；

2021-06-29.

邹 强（1978—  ），男，博士，副教授，zouqiang@tju.edu.cn.

陆甫光，m15161133133@163.com.

国家自然科学基金资助项目(6167010973).

Supported by the National Natural Science Foundation of China(No. 6167010973) .

(责任编辑：孙立华)