CN111044054A - 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法 - Google Patents

一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法 Download PDF

Info

Publication number
CN111044054A
CN111044054A CN202010009922.4A CN202010009922A CN111044054A CN 111044054 A CN111044054 A CN 111044054A CN 202010009922 A CN202010009922 A CN 202010009922A CN 111044054 A CN111044054 A CN 111044054A
Authority
CN
China
Prior art keywords
vector
value
moment
predicted value
translation vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010009922.4A
Other languages
English (en)
Other versions
CN111044054B (zh
Inventor
马琳
吴婉晴
王彬
何晨光
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202010009922.4A priority Critical patent/CN111044054B/zh
Publication of CN111044054A publication Critical patent/CN111044054A/zh
Application granted granted Critical
Publication of CN111044054B publication Critical patent/CN111044054B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

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

Description

一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法
技术领域
本发明属于计算机视觉技术领域，特别是涉及一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法。
背景技术
同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)，提出于1988年，三十多年来一直是移动机器人领域研究的热门方向。当今，SLAM算法常用于室内场景下的移动机器人定位，也常用于小型嵌入式系统中。2015年Raul Mur-Artal等人提出的ORB-SLAM，是基于特征点法的SLAM算法。ORB-SLAM的主体继承了PTAM(Parallel Trackingand Mapping)，用于单目相机，是特征点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求解的旋转向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；
Figure BDA0002356761010000011
Figure BDA0002356761010000021
其中，
Figure BDA0002356761010000022
为卡尔曼滤波器的旋转向量的预测值；r′k为PnP求解的旋转向量；
Figure BDA0002356761010000023
为k时刻旋转向量r的协方差矩阵的预测值；Pr,k-1为k-1时刻旋转向量r的协方差矩阵；R为卡尔曼滤波器的过程噪声的方差；
步骤3：计算旋转向量r的更新值，通过观测噪声的方差和旋转向量r的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的旋转向量、当前时刻旋转向量的预测值和卡尔曼滤波增益值，计算当前时刻的旋转向量；
Figure BDA0002356761010000024
Figure BDA0002356761010000025
Figure BDA0002356761010000026
其中，Kr,k为k时刻旋转向量r的卡尔曼滤波增益；Q为卡尔曼滤波器的观测噪声的方差；rk为k时刻旋转向量的滤波值；rk-1为k-1时刻旋转向量的滤波值；Pr,k为k时刻旋转向量r的协方差矩阵的滤波值；
步骤4：平移向量t的卡尔曼滤波建模；
步骤5：计算平移向量t的预测值，将PnP求解的平移向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；
Figure BDA0002356761010000027
Figure BDA0002356761010000028
其中，
Figure BDA0002356761010000029
为卡尔曼滤波器的平移向量的预测值；t′k为PnP求解的平移向量；
Figure BDA00023567610100000210
为k时刻平移向量t的协方差矩阵的预测值；Pt,k-1为k-1时刻平移向量t的协方差矩阵；
步骤6：计算平移向量t的更新值，通过观测噪声的方差和平移向量t的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的平移向量、当前时刻平移向量的预测值和卡尔曼滤波增益值，计算当前时刻的平移向量；
Figure BDA00023567610100000211
Figure BDA00023567610100000212
Figure BDA0002356761010000031
其中，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求解的旋转向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；
Figure BDA0002356761010000041
Figure BDA0002356761010000042
其中，
Figure BDA0002356761010000043
为卡尔曼滤波器的旋转向量的预测值；r′k为PnP求解的旋转向量；
Figure BDA0002356761010000044
为k时刻旋转向量r的协方差矩阵的预测值；Pr,k-1为k-1时刻旋转向量r的协方差矩阵；R为卡尔曼滤波器的过程噪声的方差；
步骤3：计算旋转向量r的更新值，通过观测噪声的方差和旋转向量r的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的旋转向量(认为是前一时刻的真值)、当前时刻旋转向量的预测值和卡尔曼滤波增益值，计算当前时刻的旋转向量；
Figure BDA0002356761010000045
Figure BDA0002356761010000046
Figure BDA0002356761010000047
其中，Kr,k为k时刻旋转向量r的卡尔曼滤波增益；Q为卡尔曼滤波器的观测噪声的方差；rk为k时刻旋转向量的滤波值；rk-1为k-1时刻旋转向量的滤波值；Pr,k为k时刻旋转向量r的协方差矩阵的滤波值；
步骤4：平移向量t的卡尔曼滤波建模；
步骤5：计算平移向量t的预测值，将PnP求解的平移向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；
Figure BDA0002356761010000048
Figure BDA0002356761010000049
其中，
Figure BDA00023567610100000410
为卡尔曼滤波器的平移向量的预测值；t′k为PnP求解的平移向量；
Figure BDA00023567610100000411
为k时刻平移向量t的协方差矩阵的预测值；Pt,k-1为k-1时刻平移向量t的协方差矩阵；
步骤6：计算平移向量t的更新值，通过观测噪声的方差和平移向量t的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的平移向量(认为是前一时刻的真值)、当前时刻平移向量的预测值和卡尔曼滤波增益值，计算当前时刻的平移向量；
Figure BDA0002356761010000051
Figure BDA0002356761010000052
Figure BDA0002356761010000053
其中，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优化运行时间均值对比
算法 运行时间均值/ms
Kalman滤波 2.149
BA优化 7.208
首先，使用本发明所述方法的系统能够正常运行，在室内环境下能够顺利完成实时的相机位姿跟踪，地图构建，回环检测与纠正等功能。通过实验结果数据的对比，可以看出用卡尔曼滤波优化位姿显著减少了系统的运算量，将该部分的程序运行时间减小到原来的30％。可以得出结论，在ORB-SLAM中用卡尔曼滤波优化位姿，用卡尔曼滤波建模优化位姿，减小了系统位姿优化的运行时间和计算时所占用CPU资源，提高了系统的运行效率，与ORB-SLAM系统相比，提高了实时性，达到了预期效果。
以上对本发明所提出的一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (1)

1.一种基于单目SLAM算法利用卡尔曼滤波进行位姿优化的方法，其特征在于：假设旋转向量和平移向量相互独立，所以建立两个相同的卡尔曼滤波器模型，过程噪声和观测噪声均服从零均值的高斯分布，过程噪声的方差和观测噪声的方差分别为R和Q；所述方法包括以下步骤：
步骤1：旋转向量r卡尔曼滤波建模；
步骤2：计算旋转向量r的预测值，将PnP求解的旋转向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；
Figure FDA0002356760000000011
Figure FDA0002356760000000012
其中，
Figure FDA0002356760000000013
为卡尔曼滤波器的旋转向量的预测值；r′k为PnP求解的旋转向量；
Figure FDA0002356760000000014
为k时刻旋转向量r的协方差矩阵的预测值；Pr,k-1为k-1时刻旋转向量r的协方差矩阵；R为卡尔曼滤波器的过程噪声的方差；
步骤3：计算旋转向量r的更新值，通过观测噪声的方差和旋转向量r的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的旋转向量、当前时刻旋转向量的预测值和卡尔曼滤波增益值，计算当前时刻的旋转向量；
Figure FDA0002356760000000015
Figure FDA0002356760000000016
Figure FDA0002356760000000017
其中，Kr,k为k时刻旋转向量r的卡尔曼滤波增益；Q为卡尔曼滤波器的观测噪声的方差；rk为k时刻旋转向量的滤波值；rk-1为k-1时刻旋转向量的滤波值；Pr,k为k时刻旋转向量r的协方差矩阵的滤波值；
步骤4：平移向量t的卡尔曼滤波建模；
步骤5：计算平移向量t的预测值，将PnP求解的平移向量作为卡尔曼滤波器的预测值，并带入经典卡尔曼滤波公式计算协方差矩阵；
Figure FDA0002356760000000018
Figure FDA0002356760000000019
其中，
Figure FDA0002356760000000021
为卡尔曼滤波器的平移向量的预测值；t′k为PnP求解的平移向量；
Figure FDA0002356760000000022
为k时刻平移向量t的协方差矩阵的预测值；Pt,k-1为k-1时刻平移向量t的协方差矩阵；
步骤6：计算平移向量t的更新值，通过观测噪声的方差和平移向量t的协方差矩阵的预测值计算卡尔曼滤波增益，并通过前一时刻的平移向量、当前时刻平移向量的预测值和卡尔曼滤波增益值，计算当前时刻的平移向量；
Figure FDA0002356760000000023
Figure FDA0002356760000000024
Figure FDA0002356760000000025
其中，Kt,k为k时刻平移向量t的卡尔曼滤波增益；Q为卡尔曼滤波器的观测噪声的方差；tk为k时刻平移向量的滤波值；tk-1为k-1时刻平移向量的滤波值；Pt,k为k时刻平移向量t的协方差矩阵的滤波值；
步骤7：用旋转向量r的更新值和平移向量t的更新值作为优化后的位姿。
CN202010009922.4A 2020-01-06 2020-01-06 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法 Active CN111044054B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010009922.4A CN111044054B (zh) 2020-01-06 2020-01-06 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010009922.4A CN111044054B (zh) 2020-01-06 2020-01-06 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法

Publications (2)

Publication Number Publication Date
CN111044054A true CN111044054A (zh) 2020-04-21
CN111044054B CN111044054B (zh) 2023-02-03

Family

ID=70243755

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010009922.4A Active CN111044054B (zh) 2020-01-06 2020-01-06 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法

Country Status (1)

Country Link
CN (1) CN111044054B (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN103617622A (zh) * 2013-12-10 2014-03-05 云南大学 一种位姿估计的正交迭代优化算法
CN106500693A (zh) * 2016-12-07 2017-03-15 中国电子科技集团公司第五十四研究所 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN107561503A (zh) * 2017-08-28 2018-01-09 哈尔滨工业大学 一种基于多重渐消因子的自适应目标跟踪滤波方法
WO2018184423A1 (zh) * 2017-04-07 2018-10-11 深圳岚锋创视网络科技有限公司 一种全景视频防抖的方法、系统及便携式终端
CN108648215A (zh) * 2018-06-22 2018-10-12 南京邮电大学 基于imu的slam运动模糊位姿跟踪算法
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置
CN110490085A (zh) * 2019-07-24 2019-11-22 西北工业大学 点线特征视觉slam系统的快速位姿估计算法
CN110595466A (zh) * 2019-09-18 2019-12-20 电子科技大学 轻量级的基于深度学习的惯性辅助视觉里程计实现方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN103617622A (zh) * 2013-12-10 2014-03-05 云南大学 一种位姿估计的正交迭代优化算法
CN106500693A (zh) * 2016-12-07 2017-03-15 中国电子科技集团公司第五十四研究所 一种基于自适应扩展卡尔曼滤波的ahrs算法
WO2018184423A1 (zh) * 2017-04-07 2018-10-11 深圳岚锋创视网络科技有限公司 一种全景视频防抖的方法、系统及便携式终端
CN107561503A (zh) * 2017-08-28 2018-01-09 哈尔滨工业大学 一种基于多重渐消因子的自适应目标跟踪滤波方法
CN108648215A (zh) * 2018-06-22 2018-10-12 南京邮电大学 基于imu的slam运动模糊位姿跟踪算法
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置
CN110490085A (zh) * 2019-07-24 2019-11-22 西北工业大学 点线特征视觉slam系统的快速位姿估计算法
CN110595466A (zh) * 2019-09-18 2019-12-20 电子科技大学 轻量级的基于深度学习的惯性辅助视觉里程计实现方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
AZARTASH, HALEH等: ""An integrated stereo visual odometry for robotic navigation"", 《ROBOTICS AND AUTONOMOUS SYSTEMS》 *
WENG ZHENGKUI等: ""Real-time facial feature point location and tracking algorithm based on prediction mechanism"", 《COMPUTER ENGINEERING AND APPLICATIONS》 *
王睿,等: ""基于扩展卡尔曼滤波的舰机相对位姿估测"", 《北京航空航天大学学报》 *
马琳,等: ""基于图像关键帧的Visual-Depth Map建立方法"", 《哈尔滨工业大学学报》 *

Also Published As

Publication number Publication date
CN111044054B (zh) 2023-02-03

Similar Documents

Publication Publication Date Title
CN112181971B (zh) 一种基于边缘的联邦学习模型清洗和设备聚类方法、系统
CN109559320B (zh) 基于空洞卷积深度神经网络实现视觉slam语义建图功能的方法及系统
CN103440645B (zh) 一种基于自适应粒子滤波和稀疏表示的目标跟踪算法
CN109064514B (zh) 一种基于投影点坐标回归的六自由度位姿估计方法
CN110264471A (zh) 一种图像分割方法、装置、存储介质及终端设备
Chen et al. A full density stereo matching system based on the combination of CNNs and slanted-planes
CN111178261A (zh) 一种基于视频编码技术的人脸检测加速方法
CN111044054B (zh) 一种基于单目slam算法利用卡尔曼滤波进行位姿优化的方法
CN111640129B (zh) 一种应用于室内墙面施工机器人的视觉砂浆识别系统
CN111914832A (zh) 一种rgb-d相机在动态场景下的slam方法
WO2022095583A1 (zh) 一种基于流卷积的目标检测方法
WO2022095584A1 (zh) 一种基于流卷积的图像识别方法
Zhang et al. Dcs-slam: A semantic slam with moving cluster towards dynamic environments
WO2022120988A1 (zh) 基于混合2d卷积和伪3d卷积的立体匹配方法
CN114022520A (zh) 一种基于卡尔曼滤波与孪生网络的机器人目标跟踪方法
CN111611996A (zh) 一种点云特征点描述子的计算方法
Wang et al. Dynamic object separation and removal in 3d point cloud map building
Lai et al. Yo-slam: A robust visual slam towards dynamic environments
Zhang et al. On the Way from Lightweight to Powerful Intelligence: A Heterogeneous Multi-Robot Social System with IoT Devices
Qin et al. OPR-SLAM: A semantic SLAM with occluded point recovery for dynamic environments
Meng et al. A Self-Navigation Method with Monocular Plane Discovery
Wang et al. A 39pJ/label 1920x1080 165.7 FPS Block PatchMatch Based Stereo Matching Processor on FPGA
CN117556273B (zh) 一种通过多图形处理器计算对比损失的方法和装置
Li et al. The Research of Mean-Shift UAV Tracking Algorithm by TWH and FB-Error Restriction
Tian et al. Semi-direct RGB-D slam algorithm for mobile robot In dynamic indoor environments

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant