基于超宽带阵列与里程计的多机器人相对定位

刘冉; 曹志强; 邓天睿; 邓忠元; 肖宇峰

doi:10.11999/JEIT210812

基于超宽带阵列与里程计的多机器人相对定位

doi: 10.11999/JEIT210812

西南科技大学信息工程学院绵阳 621000

基金项目: 国家自然科学基金 (12175187)，国家重点研发计划 (2019YFB1310805)

详细信息

作者简介:
刘冉：男，副研究员，研究方向为机器人导航定位、室内定位、SLAM

曹志强：男，硕士生，研究方向为室内定位、多机器人定位

邓天睿：男，硕士生，研究方向为室内定位

邓忠元：男，硕士生，研究方向为室内定位

肖宇峰：男，教授，研究方向为特种机器人

通讯作者:
刘冉　ran.liu.86@hotmail.com

中图分类号: TP301.6; TP242.6
计量
- 文章访问数: 1050
- HTML全文浏览量: 599
- PDF下载量: 132
- 被引次数: 0
出版历程
- 收稿日期: 2021-08-10
- 修回日期: 2021-10-01
- 网络出版日期: 2021-11-19
- 刊出日期: 2022-10-19

Multiple Robots Localization Based on the Fusion of Ultra-Wideband Array and Odometry

School of Information Engineering, Southwest University of Science and Technology, Mianyang 621000, China

Funds: The National Natural Science Foundation of China (12175187), The National Key Research Program (2019YFB1310805)

摘要

摘要: 精准的相对定位是实现多机器人协作与编队控制的关键。在弱全球定位系统(GPS)的室内环境中，视觉或激光雷达(LiDAR)通过特征匹配的方式确定机器人间相对位置，但在非视距环境下难以工作。针对这一问题，该文提出一种基于多超宽带(UWB)节点的移动机器人相对定位方法。首先，利用每个机器人携带的多个UWB节点构成UWB阵列，通过非线性优化实现移动机器人间相对姿态估计。为进一步提升估计精度，利用里程计对非线性优化结果进行约束，通过图优化算法对滑动窗口内的相对位姿与里程计进行优化，保证了算法的实时性。然而，图优化过程中难以确定相对位姿估计的误差，对定位结果影响较大。因此，利用粒子滤波融合里程计和滑动窗口优化后的相对位姿，进一步提升相对姿态估计的精度。实验结果表明，该方法在12×6 m的室内环境中，能够达到0.312 m的平均定位误差以及4.903°平均角度误差，且具有良好的实时性。
- 相对定位 /
- 非线性优化 /
- 图优化 /
- 滑动窗口 /
- 粒子滤波
Abstract: An accurate relative localization is critical for multiple robots to realize collaboration and formation control. Visual or Light Detection And Ranging (LiDAR)-based approaches use feature matching to determine the relative pose between robots in indoor environments with Global Positioning System (GPS)-denied, but which is challenging in non-line-of-sight environment. To solve this problem, a relative positioning approach of mobile robots based on multiple Ultra WideBand (UWB) nodes is proposed. First, multiple UWB nodes carried by mobile robot are used to form UWB array, and the relative pose estimation between robots is realized through nonlinear optimization algorithm. To improve further the localization accuracy, the results of non-linear optimization are constrained through odometry measurements. In addition, in order to meet the real-time requirement, the relative pose and the odometry in the sliding window are optimized through the graph optimization algorithm. However, the uncertainty of the relative pose from the non-linear optimization is not known, thus it will affect the optimization accuracy. Therefore, this paper uses particle filtering to integrate the odometry and relative pose from sliding window to improve further the accuracy. The experimental results show that the proposed approach provides an average positioning error of 0.312 m and orientation error of 4.903° in an indoor environment with a size of 12×6 m, and has good real-time performance.
- Relative localization /
- Nonlinear optimization /
- Graph optimization /
- Sliding window /
- Particle filtering

HTML全文

图 1 多机器人相对定位示意图

下载: 全尺寸图片幻灯片

图 2 算法框架图

下载: 全尺寸图片幻灯片

图 3 粒子滤波算法流程框图

下载: 全尺寸图片幻灯片

图 4 数据采集平台

下载: 全尺寸图片幻灯片

图 5 不同实验的设置

下载: 全尺寸图片幻灯片

图 6 实验1-b的估计轨迹

下载: 全尺寸图片幻灯片

图 7 实验1-a的评估结果

下载: 全尺寸图片幻灯片

图 8 实验2估计轨迹

下载: 全尺寸图片幻灯片

图 9 计算时间：不同估计方法的时间消耗

下载: 全尺寸图片幻灯片

表 1 实验1：不同估计方法在不同的UWB节点配置下，位置误差以及角度误差的评估结果

估计方法	UWB节点距离配置
	0.3 m		0.5 m		0.7 m
	位置误差(m)	角度误差(º)	位置误差(m)	角度误差(º)	位置误差(m)	角度误差(º)
里程计	0.96±0.43	10.01±4.54	0.79±0.32	6.27±3.54	0.62±0.23	3.71±2.38
非线性优化(仅UWB)	0.48±0.26	7.57±5.57	0.39±0.24	5.27±4.34	0.37±0.22	5.05±3.63
滑动窗口(w=5)	0.42±0.18	7.02±5.53	0.35±0.17	4.77±4.15	0.33±0.18	4.69±3.95
滑动窗口(w=30)	0.38±0.14	4.46±2.24	0.33±0.13	3.75±2.36	0.32±0.13	3.19±2.63
滑动窗口(w=80)	0.33±0.11	2.99±2.33	0.30±0.10	2.55±2.06	0.30±0.12	2.35±2.56
滑动窗口(w=160)	0.37±0.18	3.49±1.86	0.30±0.14	2.91±1.76	0.31±0.15	3.09±2.34
粒子滤波(UWB距离+里程计)^[22]	0.35±0.20	5.31±3.91	0.49±0.23	6.26±3.84	0.35±0.22	4.84±3.12
粒子滤波(滑动窗口+里程计)	0.25±0.10	2.91±2.76	0.22±0.10	2.70±2.27	0.25±0.09	2.02±1.47

下载: 导出CSV

表 2 实验2：不同估计方法在w=30及UWB间距0.5 m情况下评估结果

估计方法	位置误差(m)	角度误差(º)
里程计	1.466	22.076
非线性优化(仅UWB)	0.528	7.413
滑动窗口(w=30)	0.424	5.648
粒子滤波(UWB距离+里程计)^[22]	0.534	7.234
粒子滤波(滑动窗口+里程计)	0.312	4.903

下载: 导出CSV

参考文献(27)

[1]	GUO Xiansheng, ANSARI N, HU Fangzi, et al. A survey on fusion-based indoor positioning[J]. IEEE Communications Surveys & Tutorials, 2020, 22(1): 566–594.
[2]	MOON S, CHOI Y, KIM D, et al. Outdoor swarm flight system based on RTK-GPS[J]. Journal of KIISE, 2016, 43(12): 1315–1324. doi: 10.5626/JOK.2016.43.12.1315
[3]	SAKURAMA K, KOSAKA Y, and NISHIDA S I. Formation control of swarm robots with multiple proximity distance sensors[J]. International Journal of Control, Automation and Systems, 2018, 16(1): 16–26. doi: 10.1007/s12555-016-0741-z
[4]	PIASCO N, MARZAT J, and SANFOURCHE M. Collaborative localization and formation flying using distributed stereo-vision[C]. 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 2016: 1202–1207.
[5]	PREISS J A, HONIG W, SUKHATME G S, et al. Crazyswarm: A large nano-quadcopter swarm[C]. 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 2017: 3299–3304.
[6]	杨海, 李威, 张禾, 等. 复杂坏境下基于SINS/UWB的容错组合定位技术研究[J]. 仪器仪表学报, 2017, 38(9): 2177–2185. doi: 10.3969/j.issn.0254-3087.2017.09.011 YANG Hai, LI Wei, ZHANG He, et al. Fault tolerant integrated positioning system based on SINS/UWB in complex environment[J]. Chinese Journal of Scientific Instrument, 2017, 38(9): 2177–2185. doi: 10.3969/j.issn.0254-3087.2017.09.011
[7]	胡久松, 刘宏立, 肖郭璇, 等. 应用于WiFi室内定位的自适应仿射传播聚类算法[J]. 电子与信息学报, 2018, 40(12): 2889–2895. HU Jiusong, LIU Hongli, XIAO Guoxuan, et al. Adaptive affine propagation clustering algorithm for WiFi indoor positioning[J]. Journal of Electronics &Information Technology, 2018, 40(12): 2889–2895.
[8]	LIU Ran, YUEN C, DO T N, et al. Cooperative positioning for emergency responders using self IMU and peer-to-peer radios measurements[J]. Information Fusion, 2020, 56: 93–102. doi: 10.1016/j.inffus.2019.10.009
[9]	SCARAMUZZA D, ACHTELIK M C, DOITSIDIS L, et al. Vision-controlled micro flying robots: From system design to autonomous navigation and mapping in GPS-denied environments[J]. IEEE Robotics & Automation Magazine, 2014, 21(3): 26–40.
[10]	SASKA M, BACA T, THOMAS J, et al. System for deployment of groups of unmanned micro aerial vehicles in GPS-denied environments using onboard visual relative localization[J]. Autonomous Robots, 2017, 41(4): 919–944. doi: 10.1007/s10514-016-9567-z
[11]	WANG Xinzuo and LI Wei. Design of an accurate yet low-cost distributed module for vehicular relative positioning: Hardware prototype design and algorithms[J]. IEEE Transactions on Vehicular Technology, 2019, 68(5): 4494–4501. doi: 10.1109/TVT.2019.2901743
[12]	TOMIC S, BEKO M, and DINIS R. RSS-based localization in wireless sensor networks using convex relaxation: Noncooperative and cooperative schemes[J]. IEEE Transactions on Vehicular Technology, 2015, 64(5): 2037–2050. doi: 10.1109/TVT.2014.2334397
[13]	XU Hao, WANG Luqi, ZHANG Yichen, et al. Decentralized visual-inertial-UWB fusion for relative state estimation of aerial swarm[C]. 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 2020: 8776–8782.
[14]	GHANEM E, O’KEEFE K, and KLUKAS R. Testing vehicle-to-vehicle relative position and attitude estimation using multiple UWB ranging[C]. 2020 IEEE 92nd Vehicular Technology Conference (VTC2020-Fall), Victoria, Canada, 2020: 1–5.
[15]	吴玉秀, 孟庆浩, 曾明. 基于声音的分布式多机器人相对定位[J]. 自动化学报, 2014, 40(5): 798–809. WU Yuxiu, MENG Qinghao, and ZENG Ming. Sound based relative localization for distributed multi-robot systems[J]. Acta Automatica Sinica, 2014, 40(5): 798–809.
[16]	FENG Daquan, WANG Chunqi, HE Chunlong, et al. Kalman-filter-based integration of IMU and UWB for high-accuracy indoor positioning and navigation[J]. IEEE Internet of Things Journal, 2020, 7(4): 3133–3146. doi: 10.1109/JIOT.2020.2965115
[17]	赵晨, 乔钢, 周锋. 基于正交移动双水下自主潜航器的水下合作目标定位方法[J]. 电子与信息学报, 2021, 43(3): 834–841. doi: 10.11999/JEIT200570 ZHAO Chen, QIAO Gang, and ZHOU Feng. Underwater cooperative target localization method based on double orthogonal moving autonomous underwater vehicles[J]. Journal of Electronics &Information Technology, 2021, 43(3): 834–841. doi: 10.11999/JEIT200570
[18]	BAI Nan, TIAN Yuan, LIU Ye, et al. A high-precision and low-cost IMU-based indoor pedestrian positioning technique[J]. IEEE Sensors Journal, 2020, 20(12): 6716–6726. doi: 10.1109/JSEN.2020.2976102
[19]	SAN MARTÍN J, CORTÉS A, ZAMORA-CADENAS L, et al. Precise positioning of autonomous vehicles combining UWB ranging estimations with on-board sensors[J]. Electronics, 2020, 9(8): 1238. doi: 10.3390/electronics9081238
[20]	MAGNAGO V, CORBALÁN P, PICCO G P, et al. Robot localization via odometry-assisted Ultra-wideband ranging with stochastic guarantees[C]. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019: 1607–1613.
[21]	GUO Kexin, QIU Zhirong, MENG Wei, et al. Ultra-wideband based cooperative relative localization algorithm and experiments for multiple unmanned aerial vehicles in GPS denied environments[J]. International Journal of Micro Air Vehicles, 2017, 9(3): 169–186. doi: 10.1177/1756829317695564
[22]	LIU Ran, YUEN C, DO T N, et al. Cooperative relative positioning of mobile users by fusing IMU inertial and UWB ranging information[C]. 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 2017: 5623–5629.
[23]	HARDY J, STRADER J, GROSS J N, et al. Unmanned aerial vehicle relative navigation in GPS denied environments[C]. 2016 IEEE/ION Position, Location and Navigation Symposium (PLANS), Savannah, USA, 2016: 344–352.
[24]	CHOU C, WANG Di, SONG Dezhen, et al. On the tunable sparse graph solver for pose graph optimization in visual SLAM problems[C]. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019: 1300–1306.
[25]	ASCHER C, KESSLER C, WANKERL M, et al. Dual IMU indoor navigation with particle filter based map-matching on a smartphone[C]. 2010 International Conference on Indoor Positioning and Indoor Navigation, Zurich, Switzerland, 2010: 1–5.
[26]	THRUN S, BURGARD W, and FOX D. Probabilistic Robotics[M]. Cambridge: The MIT Press, 2005.
[27]	SCHOUTEN G and STECKEL J. RadarSLAM: Biomimetic SLAM using ultra-wideband pulse-echo radar[C]. 2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Sapporo, Japan, 2017: 1–8.