[发明专利]一种基于视觉惯导信息融合的定位方法及系统

摘要

本发明公开了一种基于视觉惯导信息融合的定位方法及系统，其中，所述方法包括：对采集的传感器信息进行预处理，所述传感器信息包括：深度视觉传感器的RGB图像和深度图像信息，惯性测量单元IMU数据；获取深度视觉传感器和惯性测量单元所属系统的外部参数；采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息；基于回环检测方式对所述位姿信息进行校正，获得校正后的全局一致的位姿信息。上述方法在定位过程中鲁棒性好，提升定位准确性能。

主权项

1.一种基于视觉惯导信息融合的定位方法，其特征在于，包括：/n步骤A1、对采集的传感器信息进行预处理，所述传感器信息包括：深度视觉传感器的RGB图像和深度图像信息，惯性测量单元IMU的数据；/n步骤A2、获取深度视觉传感器和惯性测量单元所属系统的外部参数；/n步骤A3、采用IMU预积分模型和深度相机模型处理所述预处理后的传感器信息、外部参数，获得位姿信息；/n步骤A4、基于回环检测方式对所述位姿信息进行校正，获得校正后的位姿信息并输出；/n所述系统的外部参数为惯性测量单元和深度视觉传感器之间的相对位姿；/n所述处理后的传感器信息包括：传感器测量值和系统状态向量；/n所述A3具体包括：/n根据IMU预积分模型计算测量值与系统状态的残差项和协方差；/n根据深度相机模型计算特征点从前一个关键帧到当前关键帧去畸变后的重投影误差和协方差；/n初始化滑动窗口，当滑动窗口为空时，则将所述残差项与所述重投影误差的马氏范数和加到滑动窗口中；/n对滑动窗口中的残差范数构造非线性最小二乘式，通过高斯牛顿法迭代求解，得到系统状态向量的最优解；/n边缘化滑动窗口中的残差项；/n所述传感器测量值为IMU积分得到的位置、姿态和速度以及使用LK光流跟踪法在当前帧中跟踪到与上一帧中的特征点匹配的特征点。/n