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

说明书

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

技术领域

本发明涉及多传感器融合技术，特别是一种基于RGB-D和IMU信息融合的位姿估计方法。

背景技术

多传感器信息融合的位姿估计技术是指不同的传感器在相近的时间段获取的数据进行结合，利用相关算法进行数据结合，优势互补，从而得到更可信的分析结果。由于相机低廉的价格和具备丰富的信息、惯性测量单元短时间内积分准确的特性，以相机和惯性测量单元的融合逐渐成为研究的热点。

目前相机和惯性测量单元数据融合的位姿估计技术主要分为两类：基于滤波器的方法和基于优化的方法。根据是否把图像特征信息加入到状态变量进行联合优化，可以进一步分为松耦合和紧耦合的方法。

基于滤波器的松耦合方法以ETHZ的ssf方法为代表，ETHZ实验室使用携带单目摄像头和IMU的无人机进行了松耦合方法的实验，取得了较高精度的位姿估计。与松耦合算法恰恰相反的是紧耦合算法，基于滤波的紧耦合算法中系统的优化变量不仅包含IMU在世界坐标系下的位姿、旋转、加速度bias和陀螺仪bias，而且还包含地图点在世界坐标系下的坐标。采用紧耦合方法的另一个算法是ETHZ的ROVIO算法。两种算法都是基于EKF框架。其中ROVIO算法将系统外参也加入到系统优化变量中，此外把特征点在世界坐标系下的三维坐标参数化为二维的相机归一化坐标和特征点的逆深度(即深度的倒数)，另外为了减小计算规模，加速计算，算法对系统代价函数的雅克比进行QR分解。由于紧耦合算法将特征点的坐标也考虑到系统优化变量中，相比松耦合算法能够获得更高的定位精度。

和基于滤波器的方法相比，基于捆集优化的方法能够获得更高的精度，虽然增加了计算量，但是随着近年来处理器算力的快速增长，当前基于视觉惯导融合的位姿估计方法多采用基于优化的方法。

目前国内外流行的基于优化的位姿估计算法有：VINS-Mono。该算法后端优化的变量包含系统在世界坐标系下的位置、姿态、IMU加速度和陀螺仪的bias、系统外参以及特征点的逆深度。算法最小化IMU测量残差和视觉测量残差，从而得到系统状态的最优估计。该算法的创新点是视觉惯导的初始化和后端优化。同时该系统加入回环检测，如果检测到回环，系统进行4个自由度的全局优化进行消除累计误差。该算法在实际环境测试中发现系统在回到原始位置时，进行全局优化后，整体系统位姿变化比较大，这说明系统位姿估计精度不高。另外，提出的OKVIS算法将IMU测量残差和相机重投影误差的范数的和作为最小二乘代价函数进行联合优化，从而得到系统的实时位姿，通过采用滑动窗口方法进行约束计算量，以及通过采用边缘化方法不丢失历史状态的约束信息。由于该算法没有加入回环检测机制，所以从本质上来说是一个视觉惯导里程计，如果进行长时间的位姿估计的话，累计误差不能纠正。融合IMU数据的ORBSLAM2算法是一个完整的视觉惯导SLAM系统。系统加入了回环检测，能够进行全局优化，从而消除累积的误差。该算法的创新点之一是视觉惯导系统的初始化。首先利用运动恢复结构(Struct From Motion,缩写为SFM)得到连续几个关键帧的相对位姿，将该结果作为IMU的约束，进一步优化得到尺度、速度、IMU的陀螺仪和加速度计的bias以及重力方向。由于该初始化方法需要经过一定的时间进行系统尺度的收敛，对于实时系统，比如无人机的定位导航来说会出现一定的问题。

发明内容