深度相机与微机电惯性测量单元松组合导航算法

doi:10.3969/j.issn.1000-1093.2021.01.018

摘要/Abstract

摘要： 针对室内非结构环境下，全球定位系统、无线电定位等定位手段使用困难，轮式里程计在楼梯等场合易出现打滑或空转而误差较大，单一视觉传感器或微机电系统（MEMS）惯性测量单元（IMU）很难实现高精度自主定位，以及传统的视觉与MEMS IMU组合导航算法复杂、计算量大、导航精度低等问题，提出一种适用于室内的深度相机与MEMS IMU松组合导航算法。MEMS IMU预积分结果作为改进迭代最近点（ICP）算法的迭代初值，大幅减少了迭代次数；通过深度相机和MEMS IMU分别计算载体的位置并作差运算，将位置差值作为量测信息，使用扩展卡尔曼滤波估计MEMS IMU的导航误差，修正航位推算的结果；利用Kinect v1深度相机和MTI 100-IMU搭建的平台进行实验验证。结果表明，基于MEMS IMU辅助的改进ICP算法能够减少迭代次数约50%，基于位置差值的深度相机与MEMS IMU松组合算法导航定位误差小于总里程的10%.

关键词: 深度相机, 惯性测量单元, 迭代最近点, 组合导航

Abstract: GPS, radio positioning and other positioning methods are difficult to use in the indoor environment. Wheel odometer is easy to slip or idle in stairs and other occasions. Single vision sensor or micro-electromechanical system (MEMS) inertial measurement unit (IMU) is difficult to achieve high-precision autonomous positioning. The traditional vision/MEMS IMU integrated navigation algorithm is complex, computationally expensive and of low accuracy. In order to solve these problems, a loose coupled navigation algorithm based on depth camera/MEMS IMU is proposed for indoor application. The pre-integration results of MEMS IMU are used as the initial value of the iterative closest point (ICP) algorithm, which greatly reduces the number of iterations. The position of a carrier is calculated by depth camera and MEMS IMU, respectively. The position difference is obtained by subtracting the two positions. The errors of MEMS IMU is estimated and the result of dead reckoning is corrected by using the position difference as the measuring information of EKF. The results of experiments with Kinect v1 and MTI 100-IMU show that the improved ICP algorithm based on MEMS IMU can effectively reduce the number of iterations by about 50%. The depth camera/MEMS IMU loose coupled algorithm based on position difference can make the position errors less than 10% of the total mileage.

Key words: depthcamera, inertialmeasurementunit, iterativeclosestpoint, integratednavigation

中图分类号:

TP242.3

张福斌，林家昀. 深度相机与微机电惯性测量单元松组合导航算法[J]. 兵工学报, 2021, 42(1): 159-166.

ZHANG Fubin， LIN Jiayun. Integrated Navigation Algorithm of Depth Camera/MEMS IMU[J]. Acta Armamentarii, 2021, 42(1): 159-166.

参考文献

［1］ ZHANG Q, LI B P, XU G Q, et al. Indoor environment applications for mobile robots using Kinect2.0[C]∥Proceedings of the 11th World Congress on Intelligent Control & Automation. Shen-yang, China:IEEE, 2014.
[2］ JWO D J, WANG S H. Adaptive fuzzy strong tracking extended Kalman filtering for GPS navigation[J]. IEEE Sensors Journal, 2007, 7(5): 778-789.
[3］ KLEEMAN L, CHONG K S. Accurate odometry and error modelling for a mobile robot[C]∥Proceedings of IEEE International Conference on Robotics and Automation. Albuquerque, NM, US: IEEE, 1997: 2783-2788.
[4］ MADGWICK S, HARRISON A, VAIDYANATHAN R, et al. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]∥Proceedings of IEEE International Conference on Rehabilitation Robotics. Zurich, Switzerland: IEEE, 2011: 1-7.
[5］ MUR-ARTAL R, TARDOS J D. Visual-inertial monocular SLAM with map reuse[J]. IEEE Robotics and Automation Letters, 2017, 2(2):796-803.
[6］ TAKETOMI T, UCHIYAMA H, IKEDA S, et al. Visual SLAM algorithms: a survey from 2010 to 2016[J]. IPSJ Transactions on Computer Vision and Applications, 2017, 9(1): 16.
[7］ YOUSIF K, TAGUCHI Y, RAMALINGAM S. MonoRGBD-SLAM: simultaneous localization and mapping using both mono-cular and RGBD cameras[C]∥Proceedings of IEEE International Conference on Robotics and Automation. Singapore:IEEE, 2017: 4495-4502.
[8］ ENGEL J, STUECKLER J, CREMERS D. Large-scale direct SLAM with stereo cameras[C]∥Proceedings of IEEE/RSJ International Conference on Intelligent Robots & Systems. Hamburg, Germany: IEEE, 2015: 1935-1942.
[9］ 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］ QAYYUM U, AHSAN Q, MAHMOOD Z, et al. IMU aided RGB-D SLAM[C]∥Proceedings of International Bhurban Conference on Applied Sciences and Technology. Islamabad, Pakistan: IEEE, 2017: 337-341.
[11］ LAIDLOW T, BLOESCH M, LI W B, et al. Dense RGB-D-inertial SLAM with map deformations[C]∥Proceedings of Intelligent Robots and Systems. Vancouver, BC, Canada: IEEE, 2017: 6741-6748.
[12］孙彤, 秦文虎, 孙立博. 基于松组合的视觉惯性SLAM方法[J]. 测控技术, 2019, 38(4):22-26.
SUN T, QIN W H, SUN L B. Visnal-aid inertial SLAM method based on loose couple[J]. Measurement & Control Technology, 2019, 38(4):22-26.(in Chinese)
[13］ LAIDLOW T, BLOESCH M, LI W B, et al. Dense RGB-D-inertial SLAM with map deformations[C]∥Proceedings of 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver, Canada: IEEE, 2017.
[14］ QIN T, LI P, SHEN S. Vins-mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.
[15］ 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.
[16］ BRUNETTO N, SALTI S, FIORAIO N, et al. Fusion of inertial and visual measurements for RGB-D SLAM on mobile devices[C]∥Proceedings of the IEEE International Conference on Computer Vision Workshops. Santiago, Chile: IEEE, 2015: 148-156.
[17］ KIM D H, HAN S B, KIM J H, et al. Visual odometry algorithm using an RGB-D sensor and IMU in a highly dynamic environment[J]. Revista de Informática Teórica e Aplicada, 2015, 34(5): 11-26.
[18］ ZHU Z J, XU F, YAN C, et al. Real-time indoor scene reconstruction with RGBD and inertial input[C]∥Proceedings of 2019 IEEE International Conference on Multimedia and Expo. Shanghai, China:IEEE, 2019: 7-12.
[19］ MOURIKIS A I, ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]∥Procee-dings of IEEE International Conference on Robotics and Automation. Roma, Italy: IEEE, 2007.
[20］ YANG Z, GAO F, SHEN S. Real-time monocular dense mapping on aerial robots using visual-inertial fusion[C]∥Procee-dings of 2017 IEEE International Conference on Robotics and Automation. Singapore:IEEE, 2017: 4552-4559.

[1]	沈世斌，谢非，赵静，钱伟行，康国华，刘锡祥. 基于相位控制的惯性与卫星超紧组合导航系统信号解调方法[J]. 兵工学报, 2020, 41(3): 495-506.
[2]	胡杰，严勇杰，王子卉. 基于速度约束与模糊自适应滤波的车载组合导航[J]. 兵工学报, 2020, 41(2): 231-238.
[3]	张梦得, 李开龙, 胡柏青, 吕旭. 辅助信息源短时失效下的捷联惯性基组合导航算法[J]. 兵工学报, 2020, 41(10): 2008-2015.
[4]	董春梅，任顺清，陈希军，李巍. 一种激光陀螺捷联惯性导航系统级标定方法[J]. 兵工学报, 2019, 40(8): 1618-1626.
[5]	徐昊玮，廉保旺，刘尚波. 基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法[J]. 兵工学报, 2019, 40(4): 807-819.
[6]	李群生，赵剡，王进达. 一种适用于高动态强干扰环境的视觉辅助微机械捷联惯性导航系统/全球定位系统超紧组合导航系统[J]. 兵工学报, 2019, 40(11): 2241-2249.
[7]	刘生攀, 王文举, 饶兴桥. 单轴旋转捷联惯性导航系统误差分析与转位方案研究[J]. 兵工学报, 2018, 39(9): 1749-1755.
[8]	黄欣，熊智，许建新，徐丽敏. 基于零速/航向自观测/地磁匹配的行人导航算法研究[J]. 兵工学报, 2017, 38(10): 2031-2040.
[9]	王洁, 熊智, 邢丽, 戴怡洁, 华冰, 刘建业. 基于新息自适应滤波的惯性测量单元误差在线标定方法研究[J]. 兵工学报, 2016, 37(7): 1203-1213.
[10]	张福斌，马朋，王智辉. 基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航算法[J]. 兵工学报, 2016, 37(7): 1229-1235.
[11]	潘加亮，熊智，王丽娜，郁丰，赵慧，林爱军. 一种简化的发射系下SINS/GPS/CNS组合导航系统无迹卡尔曼滤波算法[J]. 兵工学报, 2015, 36(3): 484-491.
[12]	吴有龙, 王晓鸣, 曹鹏. 抗差估计及 Allan 方差在车载组合导航系统中的应用研究[J]. 兵工学报, 2013, 34(7): 889-895.
[13]	李杰, 刘俊. 制导弹药用微惯性测量单元结构设计[J]. 兵工学报, 2013, 34(6): 711-717.
[14]	刘明雍，胡俊伟，李闻白. 一种基于改进无迹卡尔曼滤波的自主水下航行器组合导航方法研究[J]. 兵工学报, 2011, 32(2): 252-256.
[15]	张萌，马铁华，曹咏弘，祖静. 旋转弹导航系统研究[J]. 兵工学报, 2008, 29(7): 793-797.