CN108648215B - 基于imu的slam运动模糊位姿跟踪算法 - Google Patents

Abstract

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

Description

基于IMU的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则被认为是特征点，循环上述步骤，对每一个像素执行相同的操作；

S12、计算BRIEF描述子，利用特征点的主方向为BRIEF描述子增加旋转不变特性，计算特征点的主方向，计算公式为，

其中，m₀₁、m₁₀为图像块的矩，

所述BRIEF描述子为由0和1组成的向量，其中，0和1编码了关键点附近两个像素p和q的大小关系，若p大于q，则取1，反之取0。

优选地，S2中所述利用匀速运动模型估计相机初始位姿，然后执行运动参数的捆集调整，包括如下步骤：

S21、更新匀速运动模型中的速度矩阵，根据匀速运动模型，当前帧的初始位姿T_c＝VT₁，其中V为速度矩阵，T_l为上一帧的相机位姿；

S22、对世界坐标系中3-D点和图像中的关键点进行匹配，执行运动参数的捆集调整优化相机位姿，包括方向R∈SO(3)和位置t∈R³,针对匹配的世界坐标系中的3-D点Xⁱ∈R³以及关键点

最小化它们之间的重投影误差，

其中，X为所有匹配点对的集合，ρ为鲁棒Huber代价函数，∑为与关键点尺度相关联的协方差矩阵，

投影函数π_s定义为，

其中，(f_x,f_y)为焦距，(c_x,c_y)为原点偏移量，b为基线。

优选地，所述估计位姿包括估计位置参数以及估计姿态参数，所述优化位姿包括优化位置参数以及优化姿态参数，所述最终位姿包括最终位置参数以及最终姿态参数。

优选地，S3中所述利用IMU运动方程得到估计位姿，然后利用扩展卡尔曼滤波器得到优化位姿，包括如下步骤：

S31、对惯性世界坐标系和IMU坐标系构建旋转矩阵，然后将该矩阵求导，得到旋转矩阵的微分方程，通过解微分方程，得到矩阵变量，最后转化成IMU的姿态四元数；

S32、通过坐标系之间的空间关系，能够利用IMU的位置、姿态得出相机的估计位姿；

S33、得到相机的估计位姿后，利用扩展卡尔曼滤波器对估计位姿进行优化，获得优化位姿。

优选地，所述S31具体包括如下步骤：

旋转矩阵C的表达式为，

其中，i，j，k为惯性世界坐标系的单位向量，i′，j′，k′为IMU坐标系的单位向量，对旋转矩阵c求导得到微分方程

解微分方程

得到矩阵变量，并转化成IMU姿态四元数

估计IMU位置，然后进行二次积分得到位移值，

其中，

为惯性世界坐标系下IMU的位置，

为其速度，g为世界坐标系下的引力向量。

优选地，所述S32具体包括如下步骤：

计算公式为，

其中，

为估计位置参数，

是由视觉坐标系到惯性世界坐标系的旋转，

为IMU的姿态，n_p为相机位置测量噪声，

为估计姿态参数，

表示由IMU坐标系到相机坐标系的旋转。

优选地，所述S33具体包括如下步骤：

测量矩阵为，

其中，

和

为线性化误差测量值的矩阵，

依据测量矩阵H，根据卡尔曼滤波过程更新估计值，由校正值计算得到优化位姿。

优选地，所述依据测量矩阵H，根据卡尔曼滤波过程更新估计值，具体包括如下步骤：

S331、计算余量

计算公式为，

其中，

S332、计算新息S，计算公式为，S＝HPH^T+R，

其中，R为相机平移与旋转测量噪声的协方差矩阵；

S333、计算卡尔曼增益K，计算公式为，K＝PH^TS^-1；

S334、计算校正值

计算公式为，

优选地，所述S3还包括，利用双目图像确定视觉尺度因子。

与现有技术相比，本发明的优点主要体现在以下几个方面：

本发明针对SLAM无法对图像序列中运动模糊段进行相机定位和跟踪丢失问题，结合并利用惯性测量单元运动学方程以及扩展卡尔曼滤波器计算并优化相机位姿，使得SLAM能获得连续可靠的相机位姿定位和跟踪。本发明不仅有效地提升了计算结果的准确性，同时也对计算过程进行了优化，降低了计算成本，提升了算法效率，为其后续的推广使用打下了基础。此外，本发明也为同领域内的其他相关问题提供了参考，可以以此为依据进行拓展延伸，运用于领域内其他算法的技术方案中，具有十分广阔的应用前景。

综上所述，本发明提出了一种基于IMU的SLAM运动模糊位姿跟踪算法，具有很高的使用及推广价值。

以下便结合实施例附图，对本发明的具体实施方式作进一步的详述，以使本发明技术方案更易于理解、掌握。

附图说明

图1为本发明的原理示意图。

具体实施方式

如图1所示，本发明揭示了一种基于IMU的SLAM运动模糊位姿跟踪算法，本发明基于ORB-SLAM2框架进行改进，结合并利用了IMU的测量值来跟踪相机位姿。

具体而言，包括如下步骤：

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则被认为是特征点，循环上述步骤，对每一个像素执行相同的操作；

S12、计算BRIEF描述子，利用特征点的主方向为BRIEF描述子增加旋转不变特性，计算特征点的主方向，计算公式为，

其中，m₀₁、m₁₀为图像块的矩，

所述BRIEF描述子为由0和1组成的向量，其中，0和1编码了关键点附近两个像素p和q的大小关系，若p大于q，则取1，反之取0。

S2中所述利用匀速运动模型估计相机初始位姿，然后执行运动参数的捆集调整，包括如下步骤：

S21、更新匀速运动模型中的速度矩阵，根据匀速运动模型，当前帧的初始位姿T_c＝VT₁，其中V为速度矩阵，T_l为上一帧的相机位姿；

S22、对世界坐标系中3-D点和图像中的关键点进行匹配，执行运动参数的捆集调整优化相机位姿，包括方向R∈SO(3)和位置t∈R³,针对匹配的世界坐标系中的3-D点Xⁱ∈R³以及关键点

最小化它们之间的重投影误差，

其中，X为所有匹配点对的集合，ρ为鲁棒Huber代价函数，∑为与关键点尺度相关联的协方差矩阵，

投影函数π_s定义为，

其中，(f_x,f_y)为焦距，(c_x,c_y)为原点偏移量，b为基线。上述这些都能从校准中获得。

所述估计位姿包括估计位置参数以及估计姿态参数，所述优化位姿包括优化位置参数以及优化姿态参数，所述最终位姿包括最终位置参数以及最终姿态参数。

S3中所述利用IMU运动方程得到估计位姿，然后利用扩展卡尔曼滤波器得到优化位姿，包括如下步骤：

S31、对惯性世界坐标系和IMU坐标系构建旋转矩阵，然后将该矩阵求导，得到旋转矩阵的微分方程，通过解微分方程，得到矩阵变量，最后转化成IMU的姿态四元数；

S32、通过坐标系之间的空间关系，能够利用IMU的位置、姿态得出相机的估计位姿；

S33、得到相机的估计位姿后，利用扩展卡尔曼滤波器对估计位姿进行优化，获得优化位姿。

所述S31具体包括如下步骤：

旋转矩阵c的表达式为，

其中，i，j，k为惯性世界坐标系的单位向量，i′，j′，k′为IMU坐标系的单位向量，对旋转矩阵c求导得到

则有微分方程

解微分方程

得到矩阵变量，并转化成IMU姿态四元数

估计IMU位置，即由加速度去除重力分量，然后进行二次积分得到位移值，

其中，

为惯性世界坐标系下IMU的位置，

为其速度，g为世界坐标系下的引力向量。

所述S32具体包括如下步骤：

计算公式为，

其中，

为估计位置参数，

是由视觉坐标系到惯性世界坐标系的旋转，

为IMU的姿态，n_p为相机位置测量噪声，

为估计姿态参数，

表示由IMU坐标系到相机坐标系的旋转。

所述S33具体包括如下步骤：

测量矩阵为，

其中，

和

为线性化误差测量值的矩阵，

依据测量矩阵H，根据卡尔曼滤波过程更新估计值，由校正值计算得到优化位姿。

所述依据测量矩阵H，根据卡尔曼滤波过程更新估计值，具体包括如下步骤：

S331、计算余量

计算公式为，

其中，

S332、计算新息S，计算公式为，S＝HPH^T+R，

其中，R为相机平移与旋转测量噪声的协方差矩阵；

S333、计算卡尔曼增益K，计算公式为，K＝PH^TS^-1；

S334、计算校正值

计算公式为，

所述S3还包括，利用双目图像确定视觉尺度因子。

本发明针对SLAM无法对图像序列中运动模糊段进行相机定位和跟踪丢失问题，结合并利用惯性测量单元运动学方程以及扩展卡尔曼滤波器计算并优化相机位姿，使得SLAM能获得连续可靠的相机位姿定位和跟踪。本发明不仅有效地提升了计算结果的准确性，同时也对计算过程进行了优化，降低了计算成本，提升了算法效率，为其后续的推广使用打下了基础。此外，本发明也为同领域内的其他相关问题提供了参考，可以以此为依据进行拓展延伸，运用于领域内其他算法的技术方案中，具有十分广阔的应用前景。

综上所述，本发明提出了一种基于IMU的SLAM运动模糊位姿跟踪算法，具有很高的使用及推广价值。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神和基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (1)

1.一种基于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则被认为是特征点，循环上述步骤，对每一个像素执行相同的操作；

S12、计算BRIEF描述子，利用特征点的主方向为BRIEF描述子增加旋转不变特性，计算特征点的主方向，计算公式为，

其中，m₀₁、m₁₀为图像块的矩，

所述BRIEF描述子为由0和1组成的向量，其中，0和1编码了关键点附近两个像素p和q的大小关系，若p大于q，则取1，反之取0；

S2中所述利用匀速运动模型估计相机初始位姿，然后执行运动参数的捆集调整，包括如下步骤：

S21、更新匀速运动模型中的速度矩阵，根据匀速运动模型，当前帧的初始位姿T_c＝VT_l，其中V为速度矩阵，T_l为上一帧的相机位姿；

S22、对世界坐标系中3-D点和图像中的关键点进行匹配，执行运动参数的捆集调整优化相机位姿，包括方向R∈SO(3)和位置t∈R³,针对匹配的世界坐标系中的3-D点Xⁱ∈R³以及关键点

最小化它们之间的重投影误差，

其中，X为所有匹配点对的集合，ρ为鲁棒Huber代价函数，∑为与关键点尺度相关联的协方差矩阵，

投影函数π_s定义为，

其中，(f_x,f_y)为焦距，(c_x,c_y)为原点偏移量，b为基线；

所述估计位姿包括估计位置参数以及估计姿态参数，所述优化位姿包括优化位置参数以及优化姿态参数，所述最终位姿包括最终位置参数以及最终姿态参数；

S3中所述利用IMU运动方程得到估计位姿，然后利用扩展卡尔曼滤波器得到优化位姿，包括如下步骤：

S31、对惯性世界坐标系和IMU坐标系构建旋转矩阵，然后将该矩阵求导，得到旋转矩阵的微分方程，通过解微分方程，得到矩阵变量，最后转化成IMU的姿态四元数；

S32、通过坐标系之间的空间关系，能够利用IMU的位置、姿态得出相机的估计位姿；

S33、得到相机的估计位姿后，利用扩展卡尔曼滤波器对估计位姿进行优化，获得优化位姿；

所述S31具体包括如下步骤：

旋转矩阵C的表达式为，

其中，i，j，k为惯性世界坐标系的单位向量，i′，j′，k′为IMU坐标系的单位向量，对旋转矩阵C求导得到微分方程

解微分方程

得到矩阵变量，并转化成IMU姿态四元数

估计IMU位置，然后进行二次积分得到位移值，

其中，

为惯性世界坐标系下IMU的位置，

为其速度，g为世界坐标系下的引力向量；

所述S32具体包括如下步骤：

计算公式为，

其中，

为估计位置参数，

是由视觉坐标系到惯性世界坐标系的旋转，

为IMU的姿态，n_p为相机位置测量噪声，

为估计姿态参数，

表示由IMU坐标系到相机坐标系的旋转；

所述S33具体包括如下步骤：

测量矩阵为，

其中，

和

为线性化误差测量值的矩阵，

依据测量矩阵H，根据卡尔曼滤波过程更新估计值，由校正值计算得到优化位姿；

所述依据测量矩阵H，根据卡尔曼滤波过程更新估计值，具体包括如下步骤：

S331、计算余量

计算公式为，

其中，

S332、计算新息S，计算公式为，S＝HPH^T+R，

其中，R为相机平移与旋转测量噪声的协方差矩阵；

S333、计算卡尔曼增益K，计算公式为，K＝PH^TS^-1；

S334、计算校正值

计算公式为，

所述S3还包括，利用双目图像确定视觉尺度因子。

Images