UWB-VIO integrated indoor positioning algorithm for mobile robots

doi:10.11772/j.issn.1001-9081.2021101778

Journal of Computer Applications ›› 2022, Vol. 42 ›› Issue (12): 3924-3930.DOI: 10.11772/j.issn.1001-9081.2021101778

• Frontier and comprehensive applications • Previous Articles

UWB-VIO integrated indoor positioning algorithm for mobile robots

Bingqi SHEN¹^,², Zhiming ZHANG¹(), Shaolong SHU¹

^1.College of Electronics and Information Engineering，Tongji University，Shanghai 200092，China
^2.College of Control Science and Engineering，Zhejiang University，Hangzhou Zhejiang 310027，China

Received:2021-10-18 Revised:2021-12-16 Accepted:2021-12-23 Online:2021-12-31 Published:2022-12-10
Contact: Zhiming ZHANG
About author:SHEN Bingqi，born in 1999， M. S. candidate. His research interests include simultaneous localization and mapping， autonomous mobile robot.
SHU Shaolong，born in 1980， Ph. D.， professor. His research interests include analysis and control of cyber physical systems.
Supported by:
Science and Research Innovation Program of Shanghai Municipal Education Commission(202101070007E00098);University-Industry Collaborative Education Program of Ministry of Education of China(201902016059);Double First-Class Guidance Project of Tongji University(4250145304)

移动机器人超宽带与视觉惯性里程计组合的室内定位算法

申炳琦¹^,², 张志明¹(), 舒少龙¹

^1.同济大学电子与信息工程学院，上海 200092
^2.浙江大学控制科学与工程学院，杭州 310027

通讯作者: 张志明
作者简介:申炳琦（1999—），男，河南安阳人，硕士研究生，CCF会员，主要研究方向：同步定位与地图构建、自主移动机器人
舒少龙（1980—），男，湖北黄石人，教授，博士，主要研究方向：信息物理系统的分析与控制。
基金资助:
上海市教育委员会科研创新计划项目(202101070007E00098);教育部产学合作协同育人项目(201902016059);同济大学双一流引导专项(4250145304)

Abstract

Abstract:

For the positioning task of mobile robots in indoor environment， the emerging auxiliary positioning technology based on Visual Inertial Odometry （VIO） is heavily limited by the light conditions and cannot works in the dark environment. And Ultra-Wide Band （UWB）-based positioning methods are easily affected by Non-Line Of Sight （NLOS） error. To solve the above problems， an indoor mobile robot positioning algorithm based on the combination of UWB and VIO was proposed. Firstly， S-MSCKF （Stereo-Multi-State Constraint Kalman Filter） algorithm/DS-TWR （Double Side-Two Way Ranging） algorithm and trilateral positioning method were used to obtain the position information of VIO output/positioning information resolved by UWB respectively. Then， the motion equation and observation equation of the position measurement system were established. Finally， the optimal position estimation of the robot was obtained by data fusion carried out using Error State-Extended Kalman Filter （ES-EKF） algorithm. The built mobile positioning platform was used to verify the combined positioning method in different indoor environments. Experimental results show that in the indoor environment with obstacles， the proposed algorithm can reduce the maximum error of overall positioning by about 4.4% and the mean square error of overall positioning by about 6.3% compared with the positioning method only using UWB， and reduce the maximum error of overall positioning by about 31.5% and the mean square error of overall positioning by about 60.3% compared with the positioning method using VIO. It can be seen that the proposed algorithm can provide real-time， accurate and robust positioning results for mobile robots in indoor environment.

Key words: indoor positioning, mobile robot, Ultra-Wide Band (UWB), Visual-Inertial Odometry (VIO), Kalman filter

摘要：

对于移动机器人在室内环境的定位任务，新兴的基于视觉惯性里程计（VIO）的辅助定位技术受光线条件限制大，无法在黑暗环境中工作，且超宽带（UWB）定位易受非视距（NLOS）误差影响。针对以上问题，提出一种UWB与VIO组合的室内移动机器人定位算法。首先，采用立体视觉多状态约束下的Kalman滤波器（S-MSCKF）算法/双边双向测距（DS-TWR）算法和三边定位法，分别得到VIO输出的位置信息/UWB解算的定位信息；然后，建立位置测量系统的运动方程与观测方程；最后，通过误差状态扩展卡尔曼滤波（ES-EKF）算法来进行数据融合，得到机器人的最优位置估计。使用搭建的移动定位平台在不同的室内环境下对组合定位方算法进行验证。实验结果表明在有障碍物的室内环境下，与单一UWB定位方法相比，所提算法的总体定位的最大误差减小了约4.4%，均方误差减小了约6.3%；与VIO定位方法相比，所提算法的总体定位的最大误差减小了约31.5%，均方误差减小了约60.3%。可见所提算法可为室内环境下的移动机器人提供实时、精确且鲁棒的定位结果。

关键词: 室内定位, 移动机器人, 超宽带, 视觉惯性里程计, 卡尔曼滤波

CLC Number:

TP301.6

Bingqi SHEN, Zhiming ZHANG, Shaolong SHU. UWB-VIO integrated indoor positioning algorithm for mobile robots[J]. Journal of Computer Applications, 2022, 42(12): 3924-3930.

申炳琦, 张志明, 舒少龙. 移动机器人超宽带与视觉惯性里程计组合的室内定位算法[J]. 《计算机应用》唯一官方网站, 2022, 42(12): 3924-3930.

Figures/Tables 10

Fig. 1 UWB positioning system

Fig. 2 Principles of UWB positioning algorithms

Fig. 3 Flow of VIO positioning module S-MSCKF algorithm

Fig. 4 Flow of ES-EKF algorithm

Fig.5 Overall architecture of UWB-VIO integrated positioning system

Fig. 6 Scenes of experiment sites

Fig. 7 Positioning effect comparison in obstacle-free small site

Tab.1 Error comparison for results of different positioning algorithms in obstacle-free small site

坐标轴方向	RMSE				MAX
坐标轴方向	UWB/m	VIO/m	UWB/VIO via ES-EKF/m	性能提升/%	UWB/m	VIO/m	UWB/VIO via ES-EKF/m	性能提升/%
总体	0.130 5	0.869 2	0.319 8	-145.0	0.490 0	1.372 3	0.512 4	-4.6
x方向	0.084 9	1.172 3	0.423 4	-398.7	0.180 0	1.372 3	0.512 4	-184.6
y方向	0.164 7	0.347 3	0.152 3	7.5	0.490 0	1.153 0	0.393 0	19.8

Fig. 8 Positioning effect comparison in large site with obstacles

Tab. 2 Error comparison for results of different positioning algorithms in large site with obstacles

坐标轴方向	RMSE				MAX
坐标轴方向	UWB/m	VIO/m	UWB/VIO via ES-EKF/m	性能提升/%	UWB/m	VIO/m	UWB/VIO via ES-EKF/m	性能提升/%
总体	0.359 5	0.848 6	0.337 0	6.3	1.240 0	1.730 0	1.185 9	4.4
x方向	0.229 4	0.201 9	0.130 9	37.5	1.050 0	0.408 3	0.607 5	-48.8
y方向	0.486 9	1.296 2	0.501 3	-2.9	1.240 0	1.730 0	1.185 9	4.4

References 20

1	CADENA C， CARLONE L， CARRILLO H， et al. Past， present， and future of simultaneous localization and mapping： toward the robust-perception age［J］. IEEE Transactions on Robotics， 2016， 32（6）：1309-1332. 10.1109/tro.2016.2624754
2	BRESSON G， ALSAYED Z， YU L， et al. Simultaneous localization and mapping： a survey of current trends in autonomous driving［J］. IEEE Transactions on Intelligent Vehicles， 2017， 2（3）：194-220. 10.1109/tiv.2017.2749181
3	丁斗建，赵晓林，王长根，等. 基于视觉的机器人自主定位与障碍物检测方法［J］. 计算机应用， 2019， 39（6）：1849-1854. 10.11772/j.issn.1001-9081.2018102187
	DING D J， ZHAO X L， WANG C G， et al. Autonomous localization and obstacle detection method of robot based on vision［J］. Journal of Computer Applications， 2019， 39（6）：1849-1854. 10.11772/j.issn.1001-9081.2018102187
4	房德国，王伟，李自然，等. VIO-SLAM综述［J］. 电光与控制， 2020， 27（12）：58-62， 100. 10.3969/j.issn.1671-637X.2020.12.013
	FANG D G， WANG W， LI Z R， et al. VIO-SLAM review［J］. Electronics Optics and Control， 2020， 27（12）：58-62， 100. 10.3969/j.issn.1671-637X.2020.12.013
5	MOURIKIS A I， ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］// Proceedings of the 2007 International Conference on Robotics and Automation. Piscataway： IEEE， 2007： 3565-3572. 10.1109/robot.2007.364024
6	LEUTENEGGER S， LYNEN S， BOSSE M， et al. Keyframe-based visual-inertial odometry using nonlinear optimization［J］. The International Journal of Robotics Research， 2015， 34（3）：314-334. 10.1177/0278364914554813
7	QIN T， LI P L， SHEN S J. VINS-Mono： a robust and versatile monocular visual-inertial state estimator［J］. IEEE Transactions on Robotics， 2018， 34（4）： 1004-1020. 10.1109/tro.2018.2853729
8	SUSKI W， BANERJEE S， HOOVER A. Using a map of measurement noise to improve UWB indoor position tracking［J］. IEEE Transactions on Instrumentation and Measurement， 2013， 62（8）： 2228-2236. 10.1109/tim.2013.2256714
9	PROROK A， MARTINOLI A. Accurate indoor localization with ultra-wideband using spatial models and collaboration［J］. The International Journal of Robotics Research， 2014， 33（4）：547-568. 10.1177/0278364913500364
10	HANSSENS B， PLETS D， TANGHE E， et al. An indoor variance-based localization technique utilizing the UWB estimation of geometrical propagation parameters［J］. IEEE Transactions on Antennas and Propagation， 2018， 66（5）： 2522-2533. 10.1109/tap.2018.2810340
11	XU H， WANG L Q， ZHANG Y C， et al. Decentralized visual-inertial-UWB fusion for relative state estimation of aerial swarm［C］// Proceedings of the 2020 IEEE International Conference on Robotics and Automation. Piscataway： IEEE， 2020： 8776-8782. 10.1109/icra40945.2020.9196944
12	NGUYEN T H， NGUYEN T M， XIE L H. Range-focused fusion of camera-IMU-UWB for accurate and drift-reduced localization［J］. IEEE Robotics and Automation Letters， 2021， 6（2）：1678-1685. 10.1109/lra.2021.3057838
13	WANG C， ZHANG H D， NGUYEN T M， et al. Ultra-wideband aided fast localization and mapping system［C］// Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway： IEEE， 2017： 1602-1609. 10.1109/iros.2017.8205968
14	NEIRYNCK D， LUK E， McLAUGHLIN M. An alternative double-sided two-way ranging method［C］// Proceedings of the 13th Workshop on Positioning， Navigation and Communications. Piscataway： IEEE， 2016：1-4. 10.1109/wpnc.2016.7822844
15	JEON J， JUNG S， LEE E， et al. Run your visual-inertial odometry on NVIDIA Jetson： benchmark tests on a micro aerial vehicle［J］. IEEE Robotics and Automation Letters， 2021， 6（3）：5332-5339. 10.1109/lra.2021.3075141
16	MA S J， BAI X H， WANG Y L， et al. Robust stereo visual-inertial odometry using nonlinear optimization［J］. Sensors， 2019， 19（17）： No.3747. 10.3390/s19173747
17	SUN K， MOHTA K， PFROMMER B， et al. Robust stereo visual inertial odometry for fast autonomous flight［J］. IEEE Robotics and Automation Letters， 2018， 3（2）： 965-972. 10.1109/lra.2018.2793349
18	ZENG Q X， LIU D H， LV C D. UWB/binocular VO fusion algorithm based on adaptive Kalman filter［J］. Sensors， 2019， 19（18）： No.4044. 10.3390/s19184044
19	SHAUKAT N， ALI A， JAVED IQBAL M， et al. Multi-sensor fusion for underwater vehicle localization by augmentation of RBF neural network and error-state Kalman filter［J］. Sensors， 2021， 21（4）： No.1149. 10.3390/s21041149
20	MADYASTHA V， RAVINDRA V， MALLIKARJUNAN S. Extended-Kalman filter vs. error state Kalman filter for aircraft attitude estimation［C］// Proceedings of the 2011 AIAA Guidance， Navigation， and Control Conference. Reston， VA： AIAA， 2011： No.6615. 10.2514/6.2011-6615

[1]	Lu ZHANG, Jiapeng LIU, Dongmei TIAN. Application of Stacking-Bagging-Vote multi-source information fusion model for financial early warning [J]. Journal of Computer Applications, 2022, 42(1): 280-286.
[2]	LI Kairong, LIU Shuang, HU Qianqian, TANG Yiyuan. Improved ant colony optimization algorithm for path planning based on turning angle constraint [J]. Journal of Computer Applications, 2021, 41(9): 2560-2568.
[3]	LU Guoqing, SUN Hao. Two-dimensional mapping of swarm robot based on random walk [J]. Journal of Computer Applications, 2021, 41(7): 2121-2127.
[4]	LUAN Jianing, ZHANG Wei, SUN Wei, ZHANG Ao, HAN Dong. High-accuracy localization algorithm based on fusion of two-dimensional code vision and laser lidar [J]. Journal of Computer Applications, 2021, 41(5): 1484-1491.
[5]	Erchao LI, Kuankuan QI. Robot path planning based on B-spline curve and ant colony algorithm [J]. Journal of Computer Applications, 2021, 41(12): 3558-3564.
[6]	ZHANG Jianming, SHI Yuanhao, XU Zhengyi, WEI Jianming. Adaptive UWB/PDR fusion positioning algorithm based on error prediction [J]. Journal of Computer Applications, 2020, 40(6): 1755-1762.
[7]	LIU Ang, JIANG Jin, XU Kefeng. Robot path planning based on improved ant colony and pigeon inspired optimization algorithm [J]. Journal of Computer Applications, 2020, 40(11): 3366-3372.
[8]	WANG Shouhua, LI Yunke, SUN Xiyan, JI Yuanfa. Dual-antenna attitude determination algorithm based on low-cost receiver [J]. Journal of Computer Applications, 2019, 39(8): 2381-2385.
[9]	ZHANG Bang, ZHU Jinxin, XU Zhengyi, LIU Pan, WEI Jianming. Bayesian network-based floor localization algorithm [J]. Journal of Computer Applications, 2019, 39(8): 2468-2474.
[10]	WANG Kun, ZENG Guohui, LU Dunke, HUANG Bo, LI Xiaobin. Path planning of mobile robot based on improved asymptotically-optimal bidirectional rapidly-exploring random tree algorithm [J]. Journal of Computer Applications, 2019, 39(5): 1312-1317.
[11]	CHEN Ruonan, WEN Congcong, PENG Ling, YOU Chengzeng. Application of improved A^* algorithm in indoor path planning for mobile robot [J]. Journal of Computer Applications, 2019, 39(4): 1006-1011.
[12]	HUANG Chao, LIANG Shengtao, ZHANG Yi, ZHANG Jie. Path planning of mobile robot based on multi-objective grasshopper optimization algorithm [J]. Journal of Computer Applications, 2019, 39(10): 2859-2864.
[13]	DING Jianli, MU Tao, WANG Huaichao. Fast indoor positioning algorithm of airport terminal based on spectral regression kernel discriminant analysis [J]. Journal of Computer Applications, 2019, 39(1): 256-261.
[14]	TANG Qi, HE Lamei. Unscented Kalman filtering method with nonlinear equality constraint [J]. Journal of Computer Applications, 2018, 38(5): 1481-1487.
[15]	LUO Rui, SHI Wuxi, LI Baoquan. Active disturbance rejection control for mobile robot with skidding and slipping [J]. Journal of Computer Applications, 2018, 38(5): 1517-1522.

UWB-VIO integrated indoor positioning algorithm for mobile robots

移动机器人超宽带与视觉惯性里程计组合的室内定位算法

RichHTML

PDF

Knowledge

Abstract

Cite this article

share this article

Figures/Tables 10

References 20

Related Articles 15

Recommended Articles

Metrics