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

本发明专利技术提出一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法，所述方法包括计算旋转向量和平移向量的更新值，并将二者的更新值作为优化后的位姿。本发明专利技术通过修改位姿优化算法，减少ORB‑SLAM的运算量和耗时，为上层应用提供既精准又高效的定位，实现方法是用简单的卡尔曼滤波方法替换原来较为复杂的BA优化方法来降低计算量，显著地缩短了位姿优化部分的耗时。本发明专利技术所述方法与ORB‑SLAM相比，提高了实时性。

【技术实现步骤摘要】
一种基于单目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：计本文档来自技高网...

【技术保护点】
1.一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法，其特征在于：假设旋转向量和平移向量相互独立，所以建立两个相同的卡尔曼滤波器模型，过程噪声和观测噪声均服从零均值的高斯分布，过程噪声的方差和观测噪声的方差分别为R和Q；所述方法包括以下步骤：/n步骤1：旋转向量r卡尔曼滤波建模；/n步骤2：计算旋转向量r的预测值，将PnP求解的旋转向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；/n

【技术特征摘要】
1.一种基于单目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时刻...

【专利技术属性】
技术研发人员：马琳，吴婉晴，王彬，何晨光，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙;23