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

权利要求书

1.一种基于RGB-D和IMU信息融合的位姿估计方法，其特征在于，包括：

S1、在RGB-D相机数据和IMU数据的时间同步之后，对RGB-D相机采集的灰度图像和深度图像以及IMU采集的加速度、角速度信息进行预处理，获取世界坐标系下相邻帧匹配的特征点和IMU状态增量；

S2、依据位姿估计系统的系统外参，对位姿估计系统中视觉惯导装置进行初始化，以恢复陀螺仪bias、尺度、重力加速度方向和速度；

S3、根据初始化后的视觉惯导装置的信息和世界坐标系下相邻帧匹配的特征点、IMU状态增量构建位姿估计系统的最小二乘优化函数，使用优化方法迭代求解出最小二乘优化函数的最优解，将最优解作为位姿估计状态量；

进一步地，如果位姿估计系统在预设时间段内回到之前的位置时，采用当前时刻RGB-D相机的帧数据和之前位置时刻RGB-D相机的帧数据作为约束条件，对预设时间段内位姿估计系统的位姿估计状态量进行全局优化，获取全局一致的位姿估计状态量；

所述步骤S1包括：

S11、对图像数据和IMU数据进行时间同步，使用改进的基于RANSAC的光流跟踪算法跟踪上一帧的关键点，并提取当前灰度图像新的特征点；

S12、对提取的特征点像素坐标进行畸变校正，并通过内参模型得到特征点在相机坐标系下的归一化坐标；

S13、利用IMU加速度和角速度信息，使用预积分技术得到两个相邻图像帧之间的IMU状态增量；

步骤S3包括：将IMU状态增量、系统外参和特征点的逆深度作为优化变量，最小化边缘化残差、IMU预积分测量残差和视觉测量残差和深度残差，以构建成最小二乘问题，使用高斯牛顿法迭代求解得到系统状态的最优解；

RANSAC算法的实现过程如下：

初始：假设S是N个特征点对应的集合；S为ORB初始确定的描述子；

开启循环：

1)随机选择集合S中8个特征点对；

2)通过使用8个特征点对拟合出一个模型；

3)使用拟合出的模型计算S集合内每个特征点对的距离；如果距离小于阈值，那么该点对为内点；存储内点集合D；

4)返回1)重复进行，直到达到设定的迭代次数；

选择内点最多的集合作为最后输出的ORB的描述子，所述ORB的描述子中每一特征点的深度值为对应深度图像中的像素值。

2.根据权利要求1所述的方法，其特征在于，所述步骤S11包括：

对RGB-D相机采集的灰度图像中选取代表性的特征点进行处理；

特征点的选取包括：

具体地，给定图像I和关键点(u,v)，以及该关键点的转角θ；

描述子d表示为：d＝[d₁,d₂,...,d₂₅₆]；

对任意i＝1,...,256,d_i的计算为：取(u,v)附近的任意两点p,q，并按照θ进行旋转：

其中u_p,v_p为p的坐标，对q同样处理，记旋转后的p,q为p′,q′,那么比较I(p′)和I(q′),若I(p′)大，记为d_i＝0，反之为d_i＝1；得到ORB的描述子。

3.根据权利要求1所述的方法，其特征在于，IMU状态量包括：位置、旋转、速度、加速度bias、陀螺仪bias；

系统外参包括：相机到IMU的旋转和平移；

或者，

系统外参的获取方式：通过离线的外参标定算法来获取，或者通过在线外参标定算法来获取。

4.根据权利要求1所述的方法，其特征在于，所述步骤S1之前，所述方法还包括：

同步相机数据和IMU数据的时钟，具体包括：

在位姿估计系统中，以IMU的时钟作为系统的时钟，首先创建2个缓冲区用于存储图像消息和同步消息；

图像消息的数据结构包含当前图像的时间戳、帧号和图像信息；

同步消息包含当前图像的时间戳、帧号和图像信息；

每当相机捕获一张图片，一个同步消息就产生；且同步消息的时间戳更改为和IMU最近的时间作为同步消息的时间戳，此时，实现了相机数据和IMU数据的时间同步。