一种单目VIO/UWB室内组合定位方法

隋 心，张 杰，陈志键，王思语，张宏庆，张 聪，徐爱功

一种单目VIO/UWB室内组合定位方法

隋 心，张 杰，陈志键，王思语，张宏庆，张 聪，徐爱功

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

针对单目视觉惯性里程计（VIO）在室内环境下易漂移、定位误差较大的问题，提出了一种基于单目VIO/超宽带（UWB）组合的室内高精度定位方法。该方法结合单目VIO输出的位置信息和UWB的测距信息，利用自适应卡尔曼滤波进行数据融合，考虑到UWB测距值易受非视距（NLOS）误差的影响，通过新息向量构建组合系统的抗差模型，以减小异常测距值对组合系统的影响，同时引入萨格-胡萨（Sage-Husa）滤波对系统噪声进行实时估计和修正。实验结果表明该定位方法能够有效缓解单目VIO的累积漂移和UWB的非视距误差，相较于基于扩展卡尔曼滤波器（EKF）的单目VIO/UWB组合方法，其平面均方根误差降低了52.3%，有效提高了组合系统的定位精度和鲁棒性。

单目视觉惯性里程计；超宽带；自适应抗差卡尔曼滤波；非视距；室内定位

0 引言

随着时代的发展，人们对于定位精度的要求越来越高[1]。日常生产生活80%～90%的时间处于室内，因此更多的定位导航和位置服务需求来自室内。室内环境复杂，采用单一传感器很难满足当下的定位需求，因此多传感器组合方式已经成为高精度定位领域的研究热点[2]。由于相机和惯性测量单元（inertial measurement unit，IMU）传感器之间的优势互补性，基于不同融合方式的视觉惯性里程计（visual inertial odometry，VIO）已经成为解决此问题的主流方法[2]。VIO的融合方式主要分基于滤波和基于优化2种方式[3-4]：前者融合框架简单，但是线性化误差较大；后者定位精度更加精确，但是对硬件计算力要求较高。尽管VIO在短时间内能够获得较高的定位精度，但由于IMU本身定位易发散以及相机易受光照、纹理等因素的影响而导致跟踪失败，VIO自身误差会随着时间的推移而累积，进而导致定位结果的累积误差逐渐变大[5-6]，因此需要进一步引入其他辅助传感器来降低VIO的定位误差。

目前常用的室内无线定位技术有超宽带（ultra-wide band, UWB）、无线保真（wireless fidelity, WiFi）、超声波、蓝牙等[7]，其中UWB定位技术因具有功率谱密度低、脉冲宽度窄、时间分辨率高的特点，能够获得较高的测距精度，从而被广泛应用于室内高精度定位[8]。但UWB定位技术在脉冲信息传播的过程中，障碍物的存在会遮挡UWB的脉冲信号，使信号在传播过程中发生反射、折射以及穿透的现象，构成非视距（none line of sight，NLOS）环境，形成NLOS误差，导致UWB定位系统的精度和稳定性大幅度降低，严重时甚至无法测距定位[9]。文献[10]提出了一种将穿墙模型与对数衰减模型相融合的UWB测距值处理方法，减少了信号在穿墙时的能量损失，提高了测距值的准确性，并通过最小二乘法减少测距时的累计误差，提高了UWB的定位精度。文献[11]提出了一种UWB定位系统自主完好性监测方法，根据最小二乘位置估计获得距离残差来构建检验因子，采用基于数理统计的卡方检验进行全局检验以达到剔除异常测距值的目的。UWB的定位解算精度和可靠性提升效果明显。

综上分析可知，在室内环境下单独依靠VIO或者UWB进行定位很难获得高时效和高精度的定位结果，因此，近几年已有一些专家学者针对VIO和UWB的数据融合展开研究。文献[12]提出了一种VIO和UWB的紧耦合融合方案，将VIO的测量数据和UWB的量测数据进行融合，从而获得机器人的位姿。文献[13]提出了一种利用UWB辅助单目VIO的方法，通过扩展卡尔曼滤波（extend Kalman filter，EKF）进行融合解算，以获得改进的无漂移全局位姿估计。文献[14]提出了一种双目VIO和UWB的松耦合方案，分别利用双目VIO输出的位置信息和UWB输出的位置信息，通过EKF进行数据融合得到机器人的最优位置估计，该方案采用的是松耦合的融合方式，组合系统的鲁棒性偏低。虽然上述3种融合方案在良好环境下都能获得较好的定位结果，但都并未充分考虑UWB NLOS误差对组合系统的影响。因此针对组合系统存在UWB NLOS误差影响的问题，本文提出了一种基于自适应抗差卡尔曼滤波（adaptive robust Kalman filter，ARKF）的单目VIO/UWB室内组合定位方法，以期通过该方法，能够有效抑制NLOS误差对组合系统的影响，提高组合系统的定位精度。

1 UWB测距模型及单目VIO定位算法原理

1.1 TW-TOF测距模型

在UWB的测距模型中，基于信号双向传播时间（two-way time-of-flight，TW-TOF）的测距模型既不需要UWB基准站与流动站之间保持时间同步[15]，也不需要UWB基准站互相之间保持时间同步，消除了因为时间不同步所导致的测距误差，使得UWB的测距值更加精确。

基于TW-TOF的测距模型是通过流动站发送脉冲信号到基准站产生响应脉冲信号的时间差来计算二者之间的距离，测距原理如图1所示。

图1 TW-TOF测距原理示意

UWB流动站与基准站之间的TW-TOF测距模型为

1.2 单目VIO定位算法原理

单目VIO的代表性算法主要有2种：一种是基于滤波的多状态约束扩展卡尔曼滤波器（multi-state constraint Kalman filter，MSCKF）；另一种是基于非线性优化的视觉惯性状态估计器（visual-inertial state estimator system，VINS）。MSCKF算法在状态向量中只能维护前几帧相机位姿，无法做到利用全局的信息进行全局位姿优化，而VINS利用滑窗优化算法融合预积分后的IMU测量值和相机的特征观测值，能够在短时间内获得较高精度的定位结果。因此本文选择VINS实现单目VIO定位，VINS框架中单目VIO主要包括以下3个模块：

1）数据预处理。数据预处理主要分为2部分：第一部分是相机图像处理，提取图像哈里斯角点，利用金字塔光流法跟踪相邻帧，基于随机抽样一致算法去除异常点；第二部分是IMU数据预处理，计算当前帧的位姿作为初始值，以及相邻帧的预积分增量和预积分误差的雅可比矩阵及协方差。

2）初始化。初始化模块分为相机初始化和相机惯导联合初始化：相机初始化主要是计算滑动窗口内所有帧的位姿和路标点的深度；相机惯导联合初始化主要是利用旋转约束估计陀螺仪的零偏，利用平移约束估计重力以及尺度初始值，求解世界坐标系和初始相机坐标系的外参矩阵。

3）后端非线性优化。VINS后端优化采用的是基于滑动窗口的光束法平差（bundle adjustment，BA）优化。优化模型主要分为3个部分：第一部分是由于滑动窗口删除位姿和特征点约束所引起的先验残差部分；第二部分是预积分所引起的IMU残差部分；第三部分是视觉重投影误差所引起的视觉残差部分。优化变量为

2 单目VIO/UWB组合定位算法

根据上述UWB测距模型和单目VIO定位算法原理，本文首先采用机器人操作系统（robot operating system, ROS）时间标签和四参数转换模型对不同传感器之间进行时空同步，然后基于ARKF算法将UWB输出的测距信息和单目VIO输出的位置信息进行融合，进而获得高精度和高稳定性的定位结果。单目VIO/UWB组合系统定位算法流程如图2所示。

图2 单目VIO/UWB组合定位系统流程

2.1状态模型构建

在室内定位中，平面定位的需求多于高程定位[16]，且对平面定位精度要求更高，所以本文单目VIO/UWB组合系统采用平面定位，利用ARKF算法进行融合解算，获得整个组合系统的位置更新。单目VIO/UWB组合系统的更新模型为

单目VIO/UWB组合系统的状态方程为

2.2 量测模型构建

单目VIO/UWB组合系统的观测模型为：

对式（5）、式（6）进行线性化可得：

则单目VIO/UWB组合系统的量测方程为

2.3 自适应抗差卡尔曼滤波

由式（4）和式（9）可知，本文的VIO/UWB组合系统是线性系统，其标准卡尔曼滤波算法预测过程为

更新过程为

3 实验与结果分析

3.1 实验平台

实验过程中首先将相机、IMU、UWB、棱镜固定在组合定位实验平台上，通过外参标定完成空间坐标系的统一。组合定位实验平台如图4所示。数据采集及处理所采用的电脑系统均为利努克斯（Linux）系统乌本图（Ubuntu）18.04版本，单目相机、IMU、UWB所采集的数据均采用ROS时间标签进行时间同步。

图4 单目VIO/UWB组合定位实验平台

本次实验共采用4个UWB模块，3个作为基准站使用，1个固定在组合定位实验平台上作为流动站使用。同时将全站仪架设在实验场地的空旷处，使其能够全过程观测到实验平台的棱镜，为实验提供真实可靠的参考轨迹。如图5所示，点为实验平台移动的起始位置，点、点、点为UWB基准站的架设位置，点为全站仪的架设位置。实验平台以接近匀速的移动速度采集数据，顺时针采集2圈数据。实验时长共计10 min。

图5 实验设计示意

3.2 结果与分析

对实验过程中单目VIO采集到的数据进行解算，将组合定位解算结果与全站仪的定位结果进行对比，如图6所示。可以看出：单目VIO在移动的初期阶段，定位精度较高；但随着时间的推移，定位轨迹逐渐出现漂移，其中点处的漂移最为严重，最大定位误差为0.384 m。

图6 单目VIO解算轨迹与参考轨迹对比

利用单目VIO/UWB-EKF组合模型进行参数解算，将组合定位解算结果与全站仪定位结果进行对比，如图7所示。可以看出，在实验平台移动的初始阶段，组合系统的定位精度较高，在、、、四点处的漂移明显减小，整体的移动轨迹更加贴近参考轨迹。但由于实验平台在移动过程中受到障碍物的遮挡，导致组合系统存在NLOS误差的影响，随着时间的推移，误差累积仍然越来越大，其最大定位误差为0.269 m。

图7 单目VIO/UWB-EKF组合系统轨迹与参考轨迹对比

利用单目VIO/UWB-ARKF组合模型进行参数解算，将组合定位解算结果与全站仪定位结果进行对比，如图8所示。可以看出，利用ARKF解算的单目VIO和UWB组合系统的定位精度较好，不仅能够解决单目VIO在、、、四点处容易出现漂移的问题，同时有效消除了UWB的NLOS误差对组合系统的影响，使得组合系统的定位轨迹更加贴近参考轨迹。组合系统的最大定位误差为0.157 m。

图8 单目VIO/UWB-ARKF组合系统轨迹与参考轨迹对比

图9给出了单目VIO、单目VIO/UWB-EKF组合模型和单目VIO/UWB-ARKF组合模型定位误差的统计结果。可以看出：单目VIO定位误差随着时间逐渐增大，在引入UWB后，定位精度明显提高；但由于UWB的NLOS误差的存在，对组合系统的定位精度仍然存在较大影响。而ARKF算法的引入有效抑制了UWB的NLOS误差的影响，提高了组合系统的定位精度和鲁棒性。

图9 单目VIO、单目VIO/UWB-EKF、单目VIO/UWB-ARKF定位误差

表1 3种定位方法误差统计 m

4 结束语

针对单目VIO在室内环境下易漂移、定位误差较大，及UWB测距值易受NLOS误差影响的问题，本文采用ARKF算法对UWB原始测距值和单目VIO所解算的位置信息进行融合，根据新息向量构建抗差模型，同时引入Sage-Husa滤波实时估计并修正系统噪声协方差矩阵，有效减小了UWB NLOS误差对组合系统的影响，解决了单目VIO在室内环境下定位误差较大的问题，提高了单目VIO/UWB组合系统的鲁棒性和定位精度，平均定位精度可达到亚分米级。

[1] XU H, WANG L, ZHANG Y, et al. Decentralized visual-inertial-UWB fusion for relative state estimation of Aerial Swarm[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris, France: IEEE, 2020: 8776-8782[2022-03-21].

[2] 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, 2013, 70(1): 461-476.

[3] NGUYEN T H, NGUYEN T-M, XIE L. Tightly-coupled single-anchor ultra-wideband-aided monocular visual odometry system[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris, France: IEEE, 2020: 665-671[2022-03-21].

[4] 杨观赐, 王霄远, 蒋亚汶, 等. 视觉与惯性传感器融合的SLAM技术综述[J]. 贵州大学学报(自然科学版), 2020, 37(6):1-12.

[5] BRYSON M, SUKKARIEH S. Building a robust implementation of bearing-only inertial SLAM for a UAV[J]. Journal of Field Robotics, 2007, 24(1/2): 113-143.

[6] LUTZ P, SCHUSTER M J, STEIDLE F. Visual-inertial SLAM aided estimation of anchor poses and sensor error model parameters of UWB radio modules[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 201919th International Conference on Advanced Robotics (ICAR). Montreal, Canada: IEEE, 2019: 739-746[2022-03-21].

[7] XIA M, XIU C, YANG D, et al. A novel PDR aided UWB indoor positioning method[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 2018 Ubiquitous Positioning, Indoor Navigation and Location-Based Services (UPINLBS). Wuhan, China: IEEE, 2018: 1-7[2022-03-21].

[8] SHI Q, CUI X, LI W, et al. Visual-UWB navigation system for unknown environments[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of the 31st International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS+ 2018). Miami, American: ION GNSS, 2018: 3111-3121[2022-03-21].

[9] 徐爱功, 闫可新, 高嵩, 等. 一种单目视觉里程计/UWB组合室内定位方法[J]. 导航定位学报, 2021, 9(5): 82-88.

[10] 宰昶丰, 郭英, 叶瑾, 等. 基于UWB穿墙定位中NLOS误差的优化方法[J]. 电子测量技术, 2021, 44(19): 128-133.

[11] 徐爱功, 史政旭, 高嵩, 等. 一种UWB定位系统自主完好性监测方法[J]. 导航定位学报, 2021, 9(6): 9-15.

[12] NGUYEN T H, NGUYEN T-M, XIE L. Range-focused fusion of camera-IMU-UWB for accurate and drift-reduced localization[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 1678-1685.

[13] NYQVIST H E, SKOGLUND M A, HENDEBY G, et al. Pose estimation using monocular vision and inertial sensors aided with ultra wide band[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 2015 International Conference on Indoor Positioning and Indoor Navigation (IPIN). Banff, Canada: IEEE, 2015: 1-10[2022-03-21].

[14] 申炳琦, 张志明, 舒少龙. 移动机器人UWB与VIO组合室内定位算法[J]. 计算机应用，2022(1): 1-8.

[15] 隋心, 高嵩, 黄俊尧, 等. UWB辅助INS的室内组合定位方法[J]. 无线电工程, 2020, 50(8): 624-630.

[16] LIU H, DARABI H, BANERJEE P, et al. Survey of wireless indoor positioning techniques and systems[J]. IEEE Transactions on Systems, Man, and Cybernetics, Part C (Applications and Reviews), 2007, 37(6): 1067-1080.

[17] 魏伟, 秦永元, 张晓冬, 等. 对Sage-Husa算法的改进[J]. 中国惯性技术学报, 2012, 20(6): 678-686.

A monocular VIO/UWB indoor integrated positioning method

SUI Xin, ZHANG Jie, CHEN Zhijian, WANG Siyu, ZHANG Hongqing, ZHANG Cong, XU Aigong

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

Aiming at the problems of monocular visual inertial odometry (VIO) easy to drift and large positioning error in indoor environment, this paper proposes an indoor high-precision positioning method based on monocular VIO/ultra-wide band (UWB) integration. This method integrates position information output by VIO and the ranging information about (UWB), and uses adaptive kalian filter for data fusion. Considering that the UWB ranging value is susceptible to non line of sight (NLOS) error, the robust model of the integrated system is constructed by the new interest vector to reduce the influence of the abnormal ranging value on the integrated system, and Sage-Husa filter is introduced to estimate and correct the system noise in real time. The experimental results show that the positioning method can effectively alleviate the cumulative drift of monocular VIO and the non-line-of-sight error of UWB. Compared with the monocular VIO/UWB integrated method based on extend Kalman filter (EKF), the root mean square error is reduced by 52.3%, and the positioning accuracy and robustness of the integrated system are effectively improved.

monocular visual inertial odometry; ultra-wide band; adaptive robust Kalman filtering; non line of sight; indoor positioning

P228

A

2095-4999(2022)06-0001-08

隋心，张杰，陈志键，等. 一种单目VIO/UWB室内组合定位方法[J]. 导航定位学报, 2022, 10(6): 1-8 .（SUI Xin, ZHANG Jie, CHEN Zhijian, et al. A monocular VIO/UWB indoor integrated positioning method[J]. Journal of Navigation and Positioning, 2022, 10(6): 1-8.）

10.16547/j.cnki.10-1096.20220601.

2022-04-11

国家自然科学基金项目（42074012）；辽宁省重点研发计划项目（2020JH2/10100044）；辽宁省“兴辽英才计划”项目（XLYC2002101，XLYC2008034）；辽宁省教育厅基础研究项目（LJ2020JCL016）。

隋心（1981—），男，辽宁沈阳人，博士，副教授，研究方向为基于多传感器数据融合的室内外无缝定位。

张杰（1995—），男，四川巴中人，硕士研究生，研究方向为室内外导航定位。