一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法与流程

本发明属于计算机视觉
技术领域：
，特别是涉及一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法。
背景技术：
：同时定位与地图构建(simultaneouslocalizationandmapping，slam)，提出于1988年，三十多年来一直是移动机器人领域研究的热门方向。当今，slam算法常用于室内场景下的移动机器人定位，也常用于小型嵌入式系统中。2015年raulmur-artal等人提出的orb-slam，是基于特征点法的slam算法。orb-slam的主体继承了ptam(paralleltrackingandmapping)，用于单目相机，是特征点slam的一个顶峰，具有优秀的回环检测和纠正算法，并首次提出用三个并行的线程完成slam，且优化算法十分完善，这使orb-slam比其他算法稳健得多。2017年，该团队又提出orb-slam2，完善了orb-slam，增加了双目相机和rgb-d相机模式，扩展了应用范围。由于orb-slam构建的地图是稀疏的，而稀疏地图只能用于定位，但是仅用orb-slam处理定位问题又显得“大材小用”了。现在slam的一个发展方向是沿着轻量级发展，即减少运算量，缩短运行时间。这样，slam也支持在低端处理器上运行，对于中高端处理器可以节省资源用于其他计算。技术实现要素：本发明目的是为了解决现有技术中的问题，提出了一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法。本发明是通过以下技术方案实现的，本发明提出一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法，假设旋转向量和平移向量相互独立，所以建立两个相同的卡尔曼滤波器模型，过程噪声和观测噪声均服从零均值的高斯分布，过程噪声的方差和观测噪声的方差分别为r和q；所述方法包括以下步骤：步骤1：旋转向量r卡尔曼滤波建模；步骤2：计算旋转向量r的预测值，将pnp求解的旋转向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；其中，为卡尔曼滤波器的旋转向量的预测值；r′k为pnp求解的旋转向量；为k时刻旋转向量r的协方差矩阵的预测值；pr,k-1为k-1时刻旋转向量r的协方差矩阵；r为卡尔曼滤波器的过程噪声的方差；步骤3：计算旋转向量r的更新值，通过观测噪声的方差和旋转向量r的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的旋转向量、当前时刻旋转向量的预测值和卡尔曼滤波增益值，计算当前时刻的旋转向量；其中，kr,k为k时刻旋转向量r的卡尔曼滤波增益；q为卡尔曼滤波器的观测噪声的方差；rk为k时刻旋转向量的滤波值；rk-1为k-1时刻旋转向量的滤波值；pr,k为k时刻旋转向量r的协方差矩阵的滤波值；步骤4：平移向量t的卡尔曼滤波建模；步骤5：计算平移向量t的预测值，将pnp求解的平移向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；其中，为卡尔曼滤波器的平移向量的预测值；t′k为pnp求解的平移向量；为k时刻平移向量t的协方差矩阵的预测值；pt,k-1为k-1时刻平移向量t的协方差矩阵；步骤6：计算平移向量t的更新值，通过观测噪声的方差和平移向量t的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的平移向量、当前时刻平移向量的预测值和卡尔曼滤波增益值，计算当前时刻的平移向量；其中，kt,k为k时刻平移向量t的卡尔曼滤波增益；q为卡尔曼滤波器的观测噪声的方差；tk为k时刻平移向量的滤波值；tk-1为k-1时刻平移向量的滤波值；pt,k为k时刻平移向量t的协方差矩阵的滤波值；步骤7：用旋转向量r的更新值和平移向量t的更新值作为优化后的位姿。本发明在orb-slam系统的基础上，实现了一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法，通过修改位姿优化算法，减少orb-slam的运算量和耗时，为上层应用提供既精准又高效的定位，实现方法是用简单的卡尔曼滤波方法替换原来较为复杂的ba优化方法来降低计算量，显著地缩短了位姿优化部分的耗时。本发明所述方法与orb-slam相比，提高了实时性。附图说明图1为本发明所述方法实现单目slam算法结构框图；图2为位姿优化部分卡尔曼滤波和ba优化运行时间对比图。具体实施方式下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。图1为本发明所述方法实现单目slam算法结构整体框图，整体主要沿用了orb-slam的思想，分为跟踪、建图和闭环三个线程。本发明主要针对单目slam的位姿优化部分提出，目的是减小耗时，提高整体的实时性。对于卡尔曼滤波，本发明构建的运动模型为恒速恒角速度模型，即认为相邻两帧间的运动速度是恒定的，这并不意味着相机一直以恒定的速度移动，只是在时间步长中的运动统计模型是平均地，期望用高斯分布来表示未确定的加速度。在跟踪线程中，位姿优化之前用局部地图跟踪在当前帧和局部地图之间找到了较多的匹配点对，设为n对，用pnp算法求解出位姿。pnp算法是通过n对2d-3d匹配点对计算当前相机位姿的算法，利用该算法可以实现测算相机/物体的空间姿态，也可以用来空间定位。本发明提出一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法，假设旋转向量和平移向量相互独立，所以建立两个相同的卡尔曼滤波器模型，过程噪声和观测噪声均服从零均值的高斯分布，过程噪声的方差和观测噪声的方差分别为r和q；所述方法包括以下步骤：步骤1：旋转向量r卡尔曼滤波建模；步骤2：计算旋转向量r的预测值，将pnp求解的旋转向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；其中，为卡尔曼滤波器的旋转向量的预测值；r′k为pnp求解的旋转向量；为k时刻旋转向量r的协方差矩阵的预测值；pr,k-1为k-1时刻旋转向量r的协方差矩阵；r为卡尔曼滤波器的过程噪声的方差；步骤3：计算旋转向量r的更新值，通过观测噪声的方差和旋转向量r的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的旋转向量(认为是前一时刻的真值)、当前时刻旋转向量的预测值和卡尔曼滤波增益值，计算当前时刻的旋转向量；其中，kr,k为k时刻旋转向量r的卡尔曼滤波增益；q为卡尔曼滤波器的观测噪声的方差；rk为k时刻旋转向量的滤波值；rk-1为k-1时刻旋转向量的滤波值；pr,k为k时刻旋转向量r的协方差矩阵的滤波值；步骤4：平移向量t的卡尔曼滤波建模；步骤5：计算平移向量t的预测值，将pnp求解的平移向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；其中，为卡尔曼滤波器的平移向量的预测值；t′k为pnp求解的平移向量；为k时刻平移向量t的协方差矩阵的预测值；pt,k-1为k-1时刻平移向量t的协方差矩阵；步骤6：计算平移向量t的更新值，通过观测噪声的方差和平移向量t的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的平移向量(认为是前一时刻的真值)、当前时刻平移向量的预测值和卡尔曼滤波增益值，计算当前时刻的平移向量；其中，kt,k为k时刻平移向量t的卡尔曼滤波增益；q为卡尔曼滤波器的观测噪声的方差；tk为k时刻平移向量的滤波值；tk-1为k-1时刻平移向量的滤波值；pt,k为k时刻平移向量t的协方差矩阵的滤波值；步骤7：用旋转向量r的更新值和平移向量t的更新值作为优化后的位姿。对用卡尔曼滤波和ba优化方法的orb-slam位姿优化部分程序的运行时间进行对比，用matlab散点图形式绘制，并求均值。如图2所示，蓝色点为卡尔曼滤波每次的运行时间，红色点为ba优化每次的运行时间，绿色线和黄色线为运行时间的均值，表1列出了卡尔曼滤波和ba优化运行时间均值的对比。表1位姿优化部分卡尔曼滤波和ba优化运行时间均值对比算法运行时间均值/mskalman滤波2.149ba优化7.208首先，使用本发明所述方法的系统能够正常运行，在室内环境下能够顺利完成实时的相机位姿跟踪，地图构建，回环检测与纠正等功能。通过实验结果数据的对比，可以看出用卡尔曼滤波优化位姿显著减少了系统的运算量，将该部分的程序运行时间减小到原来的30％。可以得出结论，在orb-slam中用卡尔曼滤波优化位姿，用卡尔曼滤波建模优化位姿，减小了系统位姿优化的运行时间和计算时所占用cpu资源，提高了系统的运行效率，与orb-slam系统相比，提高了实时性，达到了预期效果。以上对本发明所提出的一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。当前第1页12