基于快速不变卡尔曼滤波的视觉惯性里程计-控制与决策.PDF

第xx卷 第x期 控 制 与 决 策 Vol.xx No.x xxxx年 x 月 Control and Decision xxx. xxxx DOI: 10.13195/j.kzyjc.2018.0239 基于快速不变卡尔曼滤波的视觉惯性里程计 黄伟杰 , 张国山 (1. 天津大学电气自动化与信息工程学院，天津300072) 摘 要: 针对相机定位问题, 设计了基于深度相机和惯性传感器的视觉惯性里程计, 里程计包含定位部分和重 定位部分. 定位部分使用不变卡尔曼滤波融合多层迭代最近点(ICP)的估计值和惯性传感器的测量值来获得精 确的相机位姿, 其中ICP 的估计误差使用费舍尔信息矩阵进行量化. 由于需要使用海量的点云作为输入, 本文采 用GPU并行计算来快速实现ICP估计和误差量化的过程. 当视觉惯性里程计出现定位失败时, 结合惯性传感器数 据建立恒速模型, 并基于此模型改进了随机蕨定位方法, 实现视觉惯性里程计的重定位. 实验结果表明设计的视 觉惯性里程计可以获得准确追踪相机并且可以进行有效的重定位. 关键词: 视觉惯性里程计; 不变卡尔曼滤波; 多层迭代最近点; 随机蕨; GPU并行计算; 惯性传感器 中图分类号: TP24 文献标志码: A Visual-Inertial Odometry Based On Fast Invariant Kalman Filter HUANG Weijie , ZHANG Guoshan (1. School of Electrical Engineering and Information Engineering，Tianjin University ，Tianjin 300072 ，China) Abstract: This paper designs a visual-inertial odometry based on a depth camera and an inertial sensor. The odometry contains camera localization and camera relocalization. Camera localization uses Invariant Extended Kalman Filter (IEKF) to fuse multilevel Iteration Closest Point (ICP) estimates with the measurements from the inertial sensor for obtaining an accurate camera pose, in which, the estimated error of Multilevel ICP is quantized by Fisher Information Matrix. GPU parallel computing is used to fast implement multilevel ICP estimation and its error quantization because massive points are set as the inputs. When the odometry tracks camera failed, a constant velocity model is constructed with the data from the inertial sensor, and the random fern method is improved ba