具有全局速度约束的惯性／编码器／视觉／激光融合定位方法：IEVL-Fusion

doi:10.13973/j.cnki.robot.210287

摘要
图/表
参考文献
相关文章 (15)

全文: PDF (2580 KB) HTML ( ) 输出: BibTeX \| EndNote (RIS)
引用本文:
武东杰, 仲训昱, 崔晓珍, 庄明溪, 彭侠夫. 具有全局速度约束的惯性／编码器／视觉／激光融合定位方法：IEVL-Fusion[J]. 机器人, 2022, 44(4): 443-452.DOI: 10.13973/j.cnki.robot.210287. WU Dongjie, ZHONG Xunyu, CUI Xiaozhen, ZHUANG Mingxi, PENG Xiafu. IEVL-Fusion: An Inertial/Encoder/Vision/Laser Fusion Positioning Method with Global Velocity Constraint. ROBOT, 2022, 44(4): 443-452. DOI: 10.13973/j.cnki.robot.210287.

摘要针对卫星拒止环境中移动机器人高精度定位需求，提出一种基于误差状态扩展卡尔曼滤波（ES-EKF）的激光定位子系统／视觉定位子系统／全局速度测量子系统松耦合融合定位方法，并设计了一个误差低漂移的组合定位系统。首先，根据向量加法和矩阵乘法对系统状态的误差进行最小表示，建立误差形式的卡尔曼滤波模型，以误差状态的最优估计对系统状态的估计值进行补偿。然后，针对激光和视觉定位子系统位姿不确定度未知的问题，将位姿输出依时间戳转化为位姿增量，并建立位姿增量观测模型。其次，针对组合定位系统缺少全局速度约束的问题，利用姿态航向参考系统（AHRS）和正向运动学模型构建全局速度测量子系统，并建立全局速度观测模型。最后，在街道和野外两个场景中进行测试，本文算法的相对定位误差小于0.4%，相较于仅受局部速度约束的EKF和ES-EKF融合定位算法降低了约40%。实验结果表明，所提出的算法有效提升了定位系统的精度。

	服务

	把本文推荐给朋友
	加入我的书架
	加入引用管理器
	E-mail Alert
	RSS
	作者相关文章
	武东杰
	仲训昱
	崔晓珍
	庄明溪
	彭侠夫

关键词 ：卫星拒止, 多传感器融合, 位姿增量融合, 全局速度约束, 误差状态扩展卡尔曼滤波, 轮式机器人, 移动机器人定位

Abstract：Answering the high-precision positioning requirements of mobile robot in satellite-denied environments, an error-state extended Kalman filter (ES-EKF) based positioning method is proposed with loosely coupled fusion of subsystems of laser positioning, visual positioning, and global velocity measurement, and an integrated positioning system with low error drift is designed. Firstly, the error of system state is represented with vector addition and matrix multiplication in a minimum form, and a Kalman filter model in the error form is established, in which the optimal estimation of error state is used to compensate the estimated value of system state. Then, the pose output is transformed into the pose increment according to the time stamp, and the pose increment observation model is established, to deal with the problem of unknown pose uncertainty of the laser and visual positioning subsystems. Secondly, a global velocity measurement subsystem is constructed by using the attitude heading reference system (AHRS) and forward kinematics model, and a global velocity observation model is established, to make up the lack of global velocity constraint in the integrated positioning system. Finally, tests are carried out in street and field scenes, and results show that the relative positioning error of the proposed algorithm is less than 0.4%, which is about 40% lower than that of EKF and ES-EKF positioning algorithms with local velocity constraint. Experimental results demonstrate that the proposed method effectively improves the accuracy of the positioning system.

Key words： satellite-denied multi-sensor fusion incremental pose fusion global velocity constraint error-state extended Kalman filter (ES-EKF) wheeled mobile robot mobile robot positioning

收稿日期: 2021-06-28 录用日期: 2021-11-13

TP242

基金资助:装备预研领域基金（61405180205）.

通讯作者: 仲训昱，zhongxunyu@xmu.edu.cn E-mail: zhongxunyu@xmu.edu.cn

作者简介: 武东杰（1990-），男，博士生。研究领域：多源融合定位与自主导航。
仲训昱（1980-），男，博士，副教授。研究领域：多源融合定位与自主导航。
崔晓珍（1997-），女，硕士生。研究领域：组合导航。

链接本文:

https://robot.sia.cn/CN/10.13973/j.cnki.robot.210287 或 https://robot.sia.cn/CN/Y2022/V44/I4/443

[1] 丁文东,徐德,刘希龙,等.移动机器人视觉里程计综述[J].自动化学报, 2018, 44(3):385-400.Ding W D, Xu D, Liu X L, et al.Review on visual odometry for mobile robots[J].Acta Automatica Sinica, 2018, 44(3):385-400.
[2] Li D C, Li Q, Tang L W, et al.Invariant observer-based state estimation for micro-aerial vehicles in GPS-denied indoor environments using an RGB-D camera and MEMS inertial sensors[J].Micromachines, 2015, 6(4):487-522.
[3] Layh T, Larson J, Gebre-Egziabher D, et al.GPS-denied navigator for small UAVs[R].Minneapolis, USA:UAV Laboratory, University of Minnesota, 2014.
[4] 赖际舟,袁诚,吕品,等.不依赖于卫星的无人系统视觉/激光雷达感知与自主导航技术[J].导航定位与授时, 2021, 8(3):1-14.Lai J Z, Yuan C, Lü P, et al.Unmanned system visual/LiDAR perception and navigation technology independent of GNSS[J].Navigation Positioning and Timing, 2021, 8(3):1-14.
[5] Zhang J, Singh S.LOAM:Lidar odometry and mapping in real-time[C]//Robotics:Science and Systems.Cambridge, USA:MIT Press, 2014.DOI:10.15607/RSS.12014.X.15007.
[6] Geiger A, Lenz P, Urtasun R.Are we ready for autonomous driving?The KITTI vision benchmark suite[C]//IEEE Conference on Computer Vision and Pattern Recognition.Piscataway, USA:IEEE, 2012:3354-3361.
[7] Shan T X, Englot B.LeGo-LOAM:Lightweight and groundoptimized LiDAR odometry and mapping on variable terrain[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway, USA:IEEE, 2018:4758-4765.
[8] 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.
[9] Qin T, Pan J, Cao S Z, et al.A general optimization-based framework for local odometry estimation with multiple sensors[DB/OL].(2019-1-11)[2020-10-10].https://arxiv.org/pdf/1901.03638v1.pdf.
[10] Zhang J, Singh S.Visual-lidar odometry and mapping:Lowdrift, robust, and fast[C]//IEEE International Conference on Robotics and Automation.Piscataway, USA:IEEE, 2015:2174-2181.
[11] Kubelka V, Oswald L, Pomerleau F, et al.Robust data fusion of multimodal sensory information for mobile robots[J].Journal of Field Robotics, 2015, 32(4):447-473.
[12] Moore T, Stouch D.A generalized extended Kalman filter implementation for the robot operating system[M]//Advances in Intelligent Systems and Computing, Vol.302.Berlin, Germany:Springer, 2016:335-348.
[13] Jones E S, Soatto S.Visual-inertial navigation, mapping and localization:A scalable real-time causal approach[J].International Journal of Robotics Research, 2011, 30(4):407-430.
[14] 刘强,段富海,桑勇,等.复杂环境下视觉SLAM闭环检测方法综述[J].机器人, 2019, 41(1):112-123,136.Liu Q, Duan F H, Sang Y, et al.A survey of loop-closure detection method of visual SLAM in complex environments[J].Robot, 2019, 41(1):112-123,136.
[15] Sutoh M, Iijima Y, Sakakieda Y, et al.Motion modeling and localization of skid-steering wheeled rover on loose terrain[J].IEEE Robotics and Automation Letters, 2018, 3(4):4031-4037.
[16] Simanek J, Reinstein M, Kubelka V.Evaluation of the EKFbased estimation architectures for data fusion in mobile robots[J].IEEE/ASME Transactions on Mechatronics, 2015, 20(2):985-990.
[17] Chilian A, Hirschmuller H, Görner M.Multisensor data fusion for robust pose estimation of a six-legged walking robot[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway, USA:IEEE, 2011:2497-2504.
[18] Shuster M D.Survey of attitude representations[J].Journal of the Astronautical Sciences, 1993, 41(4):439-517.
[19] Sola J.Quaternion kinematics for the error-state Kalman filter[DB/OL].(2017-11-3)[2020-10-10].https://arxiv.org/pdf/1711.02508.pdf.
[20] 郑威,彭刚.基于惯性/磁力传感器的行人3维轨迹跟踪技术[J].机器人, 2016, 38(4):444-450.Zheng W, Peng G.3D pedestrian trajectory tracking based on inertial/magnetic sensors[J].Robot, 2016, 38(4):444-450.
[21] Ma J, Bajracharya M, Susca S, et al.Real-time pose estimation of a dynamic quadruped in GPS-denied environments for 24-hour operation[J].International Journal of Robotics Research, 2016, 35(6):631-653.
[22] Wan G W, Yang X L, Cai R L, et al.Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]//IEEE International Conference on Robotics and Automation.Piscataway, USA:IEEE, 2018:4670-4677.
[23] Zhang J, Kaess M, Singh S.On degeneracy of optimizationbased state estimation problems[C]//IEEE International Conference on Robotics and Automation.Piscataway, USA:IEEE, 2016:809-816.
[24] Hartley R, Jadidi M G, Grizzle J W, et al.Contact-aided invariant extended Kalman filtering for legged robot state estimation[C]//Robotics:Science and Systems.Cambridge, USA:MIT Press, 2018.DOI:10.15607/RSS.2018.XIV.050.