[发明专利]一种基于RGB-D和IMU信息融合的位姿估计方法

摘要

本发明提供一种基于RGB‑D和IMU信息融合的位姿估计方法，包括：S1在RGB‑D相机数据和IMU数据的时间同步之后，对RGB‑D相机采集的灰度图像和深度图像以及IMU采集的加速度、角速度信息进行预处理，获取世界坐标系下相邻帧匹配的特征点和IMU状态增量；S2依据位姿估计系统的系统外参，对系统中视觉惯导装置进行初始化；S3根据初始化后的视觉惯导装置的信息和世界坐标系下相邻帧匹配的特征点、IMU状态增量构建系统的最小二乘优化函数，使用优化方法迭代求解出最小二乘优化函数的最优解，将最优解作为位姿估计状态量；进一步地，进行回环检测，获取全局一致的位姿估计状态量。由此，使得特征点深度估计更加准确，提高系统的定位精度。

主权项

1.一种基于RGB‑D和IMU信息融合的位姿估计方法，其特征在于，包括：S1、在RGB‑D相机数据和IMU数据的时间同步之后，对RGB‑D相机采集的灰度图像和深度图像以及IMU采集的加速度、角速度信息进行预处理，获取世界坐标系下相邻帧匹配的特征点和IMU状态增量；S2、依据位姿估计系统的系统外参，对位姿估计系统中视觉惯导装置进行初始化，以恢复陀螺仪bias、尺度、重力加速度方向和速度；S3、根据初始化后的视觉惯导装置的信息和世界坐标系下相邻帧匹配的特征点、IMU状态增量构建位姿估计系统的最小二乘优化函数，使用优化方法迭代求解出最小二乘优化函数的最优解，将最优解作为位姿估计状态量；进一步地，如果位姿估计系统在预设时间段内回到之前的位置时，采用当前时刻RGB‑D相机的帧数据和之前位置时刻RGB‑D相机的帧数据作为约束条件，对预设时间段内位姿估计系统的位姿估计状态量进行全局优化，获取全局一致的位姿估计状态量。