[发明专利]基于IMU的SLAM运动模糊位姿跟踪算法

说明书

本发明揭示了一种基于IMU的SLAM运动模糊位姿跟踪算法，包括如下步骤：S1、对输入的图像序列进行ORB特征提取，根据特征点数目判断图像序列种类，根据判断结果，择一进行S2或S3步骤；S2、若为正常图像，利用匀速运动模型估计相机初始位姿，执行运动参数的捆集调整；S3、若为运动模糊图像，利用IMU运动方程得到估计位姿，利用扩展卡尔曼滤波器得到优化位姿，将估计位姿与优化位姿进行合并，得到相机的最终位姿。本发明针对SLAM无法对图像序列中运动模糊段进行相机定位和跟踪丢失问题，结合并利用惯性测量单元运动学方程以及扩展卡尔曼滤波器计算并优化相机位姿，使得SLAM能获得连续可靠的相机位姿定位和跟踪。

技术领域

本发明涉及一种跟踪算法，尤其涉及一种基于IMU的SLAM运动模糊位姿跟踪算法，属于计算机视觉和机器人领域。

背景技术

同步定位与地图构建(Simultaneous localization and mapping,SLAM)是近年来计算机视觉和机器人领域的一个研究热点。SLAM技术能够构建和更新未知环境中的地图，并实时跟踪定位。早期的SLAM方法利用滤波解决问题，Davinson等提出了实时单个相机的MonoSLAM方法，其以扩展卡尔曼滤波为后端，追踪前端非常稀疏的特征点。Eade等提出了尺度可变的单目SLAM方法，它利用粒子滤波和自顶向下的搜索以实现实时绘制大数目的路标点。这些方法多是用滤波器处理图像帧以关联估计地图点的位置和相机位姿，由于连续图像帧只含有少量的新信息，因此这些方法的计算成本高昂，并且会累积线性误差，计算结果的准确性较低。

之后Strasdat等证明了在同等计算成本下，基于关键帧的SLAM方法比滤波方法更精确。Klein等基于关键帧技术提出了PTAM，它实现了跟踪与建图过程的并行化，并且是第一个使用非线性优化，而不是使用传统的滤波器作为后端的方案。Mur-Artal等提出了ORB-SLAM2，它支持单目、双目、RGB-D三种模式；整个系统围绕ORB特征进行计算，包括视觉里程计与回环检测的ORB字典；ORB的回环检测是其亮点；创新式地使用了三个线程完成SLAM，并且围绕特征点进行了不少的优化。

为了顺应上述发展趋势，目前也出现了采用基于关键帧的ORB-SLAM2跟踪相机位姿的相关技术。然而其中仍存在一些问题值得研究，例如结合利用视觉特征与IMU信息来更好的进行相机位姿跟踪任务等。

综上所述，如何提供一种基于IMU的SLAM运动模糊位姿跟踪算法，以解决SLAM无法对图像序列中运动模糊段进行相机定位和跟踪丢失问题，也就成为了行业内技术人员所共同期望解决的问题之一。

发明内容

鉴于现有技术存在上述缺陷，本发明的目的是提出一种基于IMU的SLAM运动模糊位姿跟踪算法。

具体而言，一种基于IMU的SLAM运动模糊位姿跟踪算法，包括如下步骤：

S1、对输入的图像序列进行ORB特征提取，根据提取到的特征点数目判断图像序列属于正常图像还是运动模糊图像，根据判断结果，择一进行S2或S3步骤；

S2、若判断结果为正常图像，利用匀速运动模型估计相机初始位姿，然后执行运动参数的捆集调整；

S3、若判断结果为运动模糊图像，利用IMU运动方程得到估计位姿，然后利用扩展卡尔曼滤波器得到优化位姿，最后将估计位姿与优化位姿进行合并，得到相机的最终位姿。

优选地，S1中所述对输入的图像序列进行ORB特征提取，包括如下步骤：

S11、FAST角点检测，在图像中选取像素p，假设它的亮度为I_p，设置一个阈值T，以像素p为中心，选取半径为3的圆上的16个像素点，若选取的圆上有连续的N个点的亮度大于I_p+T或小于I_p-T，那么像素p则被认为是特征点，循环上述步骤，对每一个像素执行相同的操作；