CN111156984A - 一种面向动态场景的单目视觉惯性slam方法 - Google Patents

一种面向动态场景的单目视觉惯性slam方法 Download PDF

Info

Publication number
CN111156984A
CN111156984A CN201911313842.1A CN201911313842A CN111156984A CN 111156984 A CN111156984 A CN 111156984A CN 201911313842 A CN201911313842 A CN 201911313842A CN 111156984 A CN111156984 A CN 111156984A
Authority
CN
China
Prior art keywords
imu
frame
frames
representing
coordinate system
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
CN201911313842.1A
Other languages
English (en)
Other versions
CN111156984B (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201911313842.1A priority Critical patent/CN111156984B/zh
Publication of CN111156984A publication Critical patent/CN111156984A/zh
Application granted granted Critical
Publication of CN111156984B publication Critical patent/CN111156984B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Theoretical Computer Science (AREA)
  • Biophysics (AREA)
  • General Health & Medical Sciences (AREA)
  • Biomedical Technology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computational Linguistics (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种面向动态场景的单目视觉惯性SLAM方法，首先视觉前端提取ORB特征点，并使用YOLO‑v3神经网络进行目标识别，进而提取出潜在的静态特征点集合，再结合本质矩阵的RANSAC外点剔除，筛选出最终的静态特征点并进行跟踪；同时，为了提高数据的处理效率，对IMU测量值进行预积分；然后进行初始化，计算包括：姿态、速度、重力向量、陀螺仪偏置的初值；随后进行视觉惯性紧耦合的非线性优化，并建立地图；同时进行回环检测和重定位，最后进行全局的位姿图优化。本发明将深度学习，和视觉惯性SLAM相融合，能够一定程度上消除动态物体对于SLAM定位和建图的影响，提升了系统的长时间工作的稳定性。

Description

一种面向动态场景的单目视觉惯性SLAM方法
技术领域：
本发明涉及一种面向动态场景的单目视觉惯性SLAM方法，属于移动机器人中的同时定位和地图构建技术领域。
背景技术：
近年来计算机视觉和机器人成为一个热点的研究方向，而其中，最为基础和底层的研究便是要解决机器人自身的定位问题。其中可以使用的传感器种类十分丰富，包括GPS,UWB,激光雷达，轮式里程计等等，其中视觉传感器凭借价格低廉，和人类的定位方式相仿等特点脱颖而出。而由于单独的视觉传感器容易受画面模糊，光照等外界条件的影响，往往使用多种传感器进行融合，进而提高鲁棒性，而其中的IMU和视觉最为互补。传统的视觉惯性SLAM主要在静态场景下运行，无法处理复杂的动态场景；近年由于深度学习的不断发展，出现了一批适合图像处理的神经网络。因此，将深度学习和视觉惯性SLAM相结合，改善系统在动态场景下的鲁棒性。
发明内容
本发明为了解决在现有的机器人SLAM系统在动态场景下的稳定性不足的问题，提出了一种面向动态场景的单目视觉惯性SLAM方法。
上述目的可通过以下技术方案实现：
一种面向动态场景的单目视觉惯性SLAM方法。具体包括以下步骤：
S1：使用数据集训练YOLO-v3神经网络，得到的网络权重参数；
S2：对相机获得的图片提取特征点并进行匹配，结合S1中训练的YOLO-v3神经网络筛选出静态特征点；
S3：将IMU获得的测量值进行预积分，并构建误差状态传递方程，计算其误差协方差矩阵和jocobian矩阵；
S4：进行视觉和惯性联合初始化；
S5：构建视觉和IMU的残差方程，进行视觉和IMU的紧耦合非线性优化模型，优化位姿，优化变量为位姿，逆深度，加表和陀螺仪偏置；
S6：使用DBoW2词袋模型，对关键帧数据库中的帧进行回环检测，当检测到回环帧后进行重定位，再利用位姿图优化得到全局一致的位姿并输出。
进一步，所述步骤S1具体包括以下过程：
S1.1采集样本数据集；
S1.2手工标记样本的数据类型以及目标位置信息；
S1.3使用预训练的参数darknet53.conv.74作为网络参数的初始值；
S1.4使用上述的数据集对YOLO-v3神经网络进行训练；
S1.5使用产生的最终的权重文件作为网络的参数，对测试集进行测试，对验证集进行验证。
进一步，所述步骤S2具体包括以下过程：
S2.1对于相机获得的照片首先提取ORB特征点，并计算描述子，进而利用描述子进行特征匹配，求取相对运动。具体步骤如下：
(1)根据对极几何约束：
Figure BDA0002324884100000021
其中x1,x2为匹配的两个像素在归一化平面上的坐标，t为两帧之间的平移，R为两帧之间的旋转。E为本质矩阵。
(2)使用八点法来求取本质矩阵E，设一对匹配点的归一化坐标为：
x1＝[u1 v1 1],x2＝[u2 v2 1]
根据对极几何约束，可以得到：
Figure BDA0002324884100000022
将方程转化为线性形式为：
Figure BDA0002324884100000023
其中
Figure BDA0002324884100000024
表示第i对匹配的特征点中的第j个匹配点归一化的坐标。
带入8对匹配点的坐标，即可求解出本质矩阵E。
(3)对本质矩阵进行SVD分解求解出旋转t和平移R.设E的SVD分解为:
E＝UΣVT
有两种可能的旋转和平移于其对应：
Figure BDA0002324884100000031
Figure BDA0002324884100000032
根据先验知识，从中选择出正确的旋转和平移。
S2.2使用训练好的YOLO-v3神经网络对图片进行目标识别，可以得到识别的目标区域，定义检测框的集合为R＝{r1,r2…},单个检测框定义为：r(x,y,w,h)∈R
其中，(x,y)为检测框左上角在帧中的坐标，(w,h)为检测框的宽和高。对特征点的集合进行判断：
Figure BDA0002324884100000033
以此将当前的特征点划分为潜在动态特征点集合P和非潜在动态特征点集合
Figure BDA00023248841000000313
S2.3利用S2.1求得的本质矩阵，将S2.2中求得的上一帧非潜在动态特征点使用RANSAC进行外点剔除，得到最终的静态特征点集合。
进一步，所述步骤S3具体包括以下过程：
S3.1对IMU获得的测量值进行预积分：
Figure BDA0002324884100000034
Figure BDA0002324884100000035
Figure BDA0002324884100000036
其中：
Figure BDA0002324884100000037
Figure BDA0002324884100000038
Figure BDA0002324884100000039
其中
Figure BDA00023248841000000310
表示拍摄第k帧图片时体坐标系到世界坐标系的相对旋转；
Figure BDA00023248841000000311
分别表示拍摄第k+1帧图片时体坐标系相对于世界坐标系的位置，速度和旋转，其中q表示旋转对应的四元数；α,β,γ分别表示预积分项；
Figure BDA00023248841000000312
表示IMU的测量值。ba和bω分别表示加表和陀螺仪的偏置。na和nω分别表示加速度计和陀螺仪的噪声。
对于预积分项的求解需要使用到数值积分，这里使用欧拉积分：
Figure BDA0002324884100000041
Figure BDA0002324884100000042
Figure BDA0002324884100000043
根据以上的欧拉积分可以求取每个时刻的预积分值。
S3.2构建连续时间的误差状态传递方程：
Figure BDA0002324884100000044
其中θ表示四元数的虚部部分。
S3.3误差协方差更新公式为：
Figure BDA0002324884100000045
其中：初始的协方差矩阵
Figure BDA0002324884100000046
Q是噪声的对角线协方差矩阵
Figure BDA0002324884100000047
S3.4一阶jocobian的迭代计算公式如下：
Jt+δt＝(I+Ftδt)Jt t∈[k,k+1]
其中初始jocobian
Figure BDA0002324884100000048
由于加表和陀螺仪的偏置也会影响预积分的值，当偏置被更新之后，需要更新预积分项，当偏置的增量很小时，我们可以使用预积分项的一阶近似来更新：
Figure BDA0002324884100000049
Figure BDA00023248841000000410
Figure BDA00023248841000000411
进一步，所述步骤S4具体包括以下过程：
S4.1利用S2中得到的静态特征点集合S进行纯视觉的SFM：利用八点法计算相对平移和旋转，如S2.1所示。对这两帧中的所有静态特征点进行三角化。设x1,x2为同一个3D点在两帧上的归一化坐标，那么他们满足：
s2x2＝s1Rx1+t
由于之前已经算出来了旋转和平移，现在需要求解的是两帧下的深度s1,s2。先对上式两侧左乘
Figure BDA0002324884100000051
得：
Figure BDA0002324884100000052
上式左侧为0，右侧可以看成s1的方程，可以根据它求得s1，然后再带入求s2。
然后利用这些三角特征，采用PnP来估计窗口中其他帧的位姿。
S4.2对陀螺仪的偏置进行标定，将视觉SFM的结果和IMU预积分的数据进行对齐，构建目标函数：
Figure BDA0002324884100000053
Figure BDA0002324884100000054
其中B表示滑动窗口中的所有帧。q是由视觉SFM计算出的帧间的旋转四元数，γ是IMU预积分项,其中
Figure BDA0002324884100000055
是我们根据计算的陀螺仪偏置对预积分项进行更新，得到了新的预积分项
Figure BDA0002324884100000056
上述目标函数的最小值为单位四元数，由于单位四元数虚部为0，所以可以只考虑虚部，将其进一步写成：
Figure BDA0002324884100000057
其中(·)vec表示四元数的虚部，将左边转变为正定阵，这样就可以直接使用Cholesky进行分解求解最小二乘问题：
Figure BDA0002324884100000058
S4.3初始化速度，重力，尺度因子
设要初始化的状态量为：
Figure BDA0002324884100000059
其中，
Figure BDA00023248841000000510
是第k帧本体坐标系的速度，
Figure BDA00023248841000000511
是c0坐标系下的重力矢量，也就是起始点的相机坐标系下的重力矢量，s是尺度因子。
构建线性测量模型：
Figure BDA0002324884100000061
其中：
Figure BDA0002324884100000062
通过使用Cholesky分解可以求解最小二乘问题：
Figure BDA0002324884100000063
可以求得窗口中每一帧的本体坐标系速度，c0系的重力向量，以及尺度参数
进一步，所述步骤S5具体包括以下过程：
S5.1选定需要非线性优化的变量为：
Figure BDA0002324884100000064
Figure BDA0002324884100000065
Figure BDA0002324884100000066
其中，xk表示捕捉到第k帧时候的IMU状态，n是关键帧的总数，m是滑动窗子中特征总数，
Figure BDA0002324884100000067
表示IMU坐标系和相机坐标系之间的相对位姿变换，λi表示观测到的第i个特征的逆深度。
S5.2构建目标函数为视觉残差和IMU残差的Mahalanobis范数之和：
Figure BDA0002324884100000068
其中，ρ为Huber范数，
Figure BDA0002324884100000069
分别为IMU和视觉的残差，B是所有IMU测量的集合，C是窗口中至少被观测到2次的一组特征。
进一步，所述步骤S6具体包括以下过程：
S6.1在S2中计算的BRIEF描述子被用于和数据库中的数据进行匹配，使用的是DBoW2词袋模型，这些描述子描被视为用于查询的单词；当关键帧被加入数据中时。计算当前帧和词袋的相似度分数，并与关键帧数据库中的所有帧进行对比，选出回环候选帧。
S6.2为了确保回环帧的选取的准确度，添加了2个外点的剔除机制。
(1)使用用基于基础矩阵的RANSAC剔除外点，
(2)使用基于PnP的RANSAC来剔除外点。
如果剩下的匹配数大于阈值，那么确认回环。
S6.3在S5.2的目标函数中再添加一个视觉约束项，完成重定位，如下：
Figure BDA0002324884100000071
其中:ρ为Huber函数；rB(·)，rC(·)分别为IMU和视觉的残差，B是所有IMU测量的集合，C是窗口中至少被观测到2次的一组特征；
Figure BDA0002324884100000072
是指检测到的数据库中的回环帧(第v帧)的位姿，当做一个常量。L是回环帧中检索到的特征的观测集，(l,v)表示回环帧v中观察到的第l个特征。
进行重定位时，会将第v帧的位姿和相关的特征点作为视觉约束项添加到非线性优化的整体的目标函数中。
S6.4当滑窗内的关键帧被移出滑窗时，会被添加到位姿图中，这个关键帧在位姿图中作为顶点，它通过两种类型的边和其他的顶点连接：
(1)顺序边，表示局部滑动窗口中两个关键帧之间的相对转换，其值直接从VIO中获取。
(2)回环边，如果新边缘化的关键帧有一个回环连接，它将与回环帧通过一个回环边在位姿图图中连接。
通过最小化下面的代价函数，实现全局的位姿图优化：
Figure BDA0002324884100000073
其中：
Figure BDA0002324884100000074
S表示顺序边的集合，L表示回环边的集合；
Figure BDA0002324884100000075
是从VIO中获得的翻滚角和俯仰角的估计，其中的ρ为Huber函数。
附图说明
图1本发明的系统框图；
图2为本发明进行目标检测的效果图；
图3为本发明过滤掉动态特征的最终静态特征点图；
图4为本发明最终的定位效果图。
具体实施方式
以下将结合附图对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明提供的一种面向动态场景的单目视觉SLAM方法，实现原理如图1所示，其流程主要包括以下步骤：
步骤S1，使用采集的数据集对神经网络进行训练，具体包括以下过程：
S1.1采集样本数据集，一共创建3组数据集，包括训练集，测试集，验证集。三部分样本相互独立，并且互不包含。为了防止网络过拟合，对采集的数据进行增广处理，对每个图片随机使用以下三种方式来进行处理：
(1).随机拉伸图像
(2).使用原始图像
(3).采样一个patch，与物体间的jaccard overlap为0.1,0.3,0.7,0.9；随机选取上述的一个patch
S1.2手工标记样本的数据类型以及目标位置信息。
S1.3使用预训练的参数darknet53.conv.74作为网络参数的初始值
S1.4使用上述的数据集对YOLO-v3神经网络进行训练，经过约50000次迭代之后，网络的损失函数趋于收敛。
S1.5使用产生的最终的权重文件作为网络的参数，对测试集进行测试，对验证集进行验证。
步骤S2，对图像采集特征点并进行匹配，同时使用YOLO-v3神经网络筛选出静态特征点，具体包括以下过程：
S2.1对于相机获得的照片首先提取FAST角点，并计算BRIEF描述子，进而利用描述子进行特征匹配：
对于BRIEF这种二进制的描述子我们使用汉明距离作为度量-两个二进制串之间的汉明距离指的是他们不同位数的个数。此外，由于图片的特征点的数量可能很大，因此如果采用暴力匹配的效率可能较低，这里为了SLAM实时性的要求，选择快速近似最近邻(FLANN)算法来进行匹配
进行特征匹配之后，求取相对运动。具体步骤如下：
(1)根据对极几何约束：
Figure BDA0002324884100000081
其中x1,x2为匹配的两个像素在归一化平面上的坐标，t为两帧之间的平移，R为两帧之间的旋转。E为本质矩阵。
(2)使用八点法来求取本质矩阵E。
(3)对本质矩阵进行SVD分解求解出旋转t和平移R.设E的SVD分解为:
E＝UΣVT
有两种可能的旋转和平移于其对应：
Figure BDA0002324884100000091
Figure BDA0002324884100000092
根据先验知识，从中选择出正确的旋转和平移。
S2.2使用训练好的YOLO-v3神经网络对图片进行目标识别，如图2所示，可以得到识别的潜在的动态目标区域，如图3所示，可以得到识别的潜在的动态目标区域，定义生成的动态目标检测框的集合为R＝{r1,r2…},单个检测框定义为：
r(x,y,w,h)∈R
其中，(x,y)为检测框左上角在帧中的坐标，(w,h)为检测框的宽和高。对特征点的集合进行判断：
Figure BDA0002324884100000093
以此将当前的特征点划分为潜在动态特征点集合P和非潜在动态特征点集合
Figure BDA0002324884100000094
S2.3利用S2.1求得的本质矩阵，将S2.2中求得的上一帧非潜在动态特征点使用RANSAC进行外点剔除，得到最终的静态特征点集合。
步骤S3，对IMU的数据进行预积分，并求取协方差矩阵和jocobian矩阵，具体包括以下过程：
S3.1对IMU获得的测量值进行预积分：
Figure BDA0002324884100000095
Figure BDA0002324884100000096
Figure BDA0002324884100000097
其中：
Figure BDA0002324884100000098
Figure BDA0002324884100000099
Figure BDA00023248841000000910
其中
Figure BDA00023248841000000911
分别表示拍摄第k+1帧图片时体坐标系相对于世界坐标系的位置，速度和旋转，其中q表示旋转对应的四元数；α,β,γ分别表示预积分项；
Figure BDA00023248841000000912
表示IMU的测量值。ba，bω分别表示加表和陀螺仪的偏置。
对于预积分项的求解需要使用到数值积分，这里使用欧拉积分：
Figure BDA0002324884100000101
Figure BDA0002324884100000102
Figure BDA0002324884100000103
根据以上的欧拉积分可以求取每个时刻的预积分值。
S3.2构建连续时间的误差状态传递方程：
Figure BDA0002324884100000104
其中θ表示四元数的虚部部分。
S3.3误差协方差更新公式为：
Figure BDA0002324884100000105
其中Q是噪声的对角线协方差矩阵
Figure BDA0002324884100000106
S3.4一阶jocobian的迭代计算公式如下：
Jt+δt＝(I+Ftδt)Jt t∈[k,k+1]
其中初始jocobian
Figure BDA0002324884100000107
由于加表和陀螺仪的偏置也会影响预积分的值，当偏置被更新之后，需要更新预积分项，当偏置的增量很小时，我们可以使用预积分项的一阶近似来更新：
Figure BDA0002324884100000108
Figure BDA0002324884100000109
Figure BDA00023248841000001010
步骤S4，对系统进行初始化操作，得到为后端优化得到较好的初始值，具体包括以下过程：
S4.1利用S2中得到的静态特征点集合S进行纯视觉的SFM：利用八点法计算相对平移和旋转，如S2.1所示。如果八点法成功，对这两帧中的所有静态特征点进行三角化。设x1,x2为同一个3D点在两帧上的归一化坐标，那么他们满足：
s2x2＝s1Rx1+t
由于之前已经算出来了旋转和平移，现在需要求解的是两帧下的深度s1,s2。先对上式两侧左乘
Figure BDA0002324884100000111
得：
Figure BDA0002324884100000112
上式左侧为0，右侧可以看成s1的方程，可以根据它求得s1，然后再带入求s2。
然后利用这些三角特征，采用PnP来估计窗口中其他帧的位姿。
S4.2对陀螺仪的偏置进行标定，将视觉SFM的结果和IMU预积分的数据进行对齐，构建目标函数：
Figure BDA0002324884100000113
Figure BDA0002324884100000114
其中B表示滑动窗口中的所有帧。q是由视觉SFM计算出的帧间的旋转四元数，γ是IMU预积分项。
上述目标函数的最小值为单位四元数，由于单位四元数虚部为0，所以可以只考虑虚部，将其进一步写成：
Figure BDA0002324884100000115
其中(·)vec表示四元数的虚部，将左边转变为正定阵，这样就可以直接使用Cholesky进行分解求解最小二乘问题：
Figure BDA0002324884100000116
S4.3初始化速度，重力，尺度因子
设要初始化的状态量为：
Figure BDA0002324884100000117
其中，
Figure BDA0002324884100000118
是第k帧本体坐标系的速度，
Figure BDA0002324884100000119
是c0坐标系下的重力矢量，也就是起始点的相机坐标系下的重力矢量，s是尺度因子。
构建线性测量模型：
Figure BDA0002324884100000121
其中：
Figure BDA0002324884100000122
通过使用Cholesky分解可以求解最小二乘问题：
Figure BDA0002324884100000123
可以求得窗口中每一帧的本体坐标系速度，c0系的重力向量，以及尺度参数
步骤S5，进行视觉和惯性的紧耦合的非线性优化，具体包括以下过程：
S5.1选定需要非线性优化的变量为：
Figure BDA0002324884100000124
Figure BDA0002324884100000125
Figure BDA0002324884100000126
其中，xk表示捕捉到第k帧时候的IMU状态，n是关键帧的总数，m是滑动窗子中特征总数，λi表示观测到的第i个特征的逆深度。
S5.2构建目标函数为视觉残差和IMU残差的Mahalanobis范数之和：
Figure BDA0002324884100000127
其中，ρ为Huber范数，
Figure BDA0002324884100000128
分别为IMU和视觉的残差，B是所有IMU测量的集合，C是窗口中至少被观测到2次的一组特征。
步骤S6，进行回环检测，如果检测到了回环，那么则进行重定位，最后利用位姿图优化得到全局一致的位姿图，最终的效果如图4所示，其中蓝线为真实轨迹，红线为系统输出的轨迹，具体包括以下过程：
S6.1在S2中计算的BRIEF描述子被用于和数据库中的数据进行匹配，使用的是DBoW2词袋模型，这些描述子描被视为用于查询的单词；当关键帧被加入数据中时。计算当前帧和词袋的相似度分数，并与关键帧数据库中的所有帧进行对比，选出回环候选帧。
S6.2为了确保回环帧的选取的准确度，添加了2个外点的剔除机制。
(1)使用用基于基础矩阵的RANSAC剔除外点，
(2)使用基于PnP的RANSAC来剔除外点。
如果剩下的匹配数大于阈值，那么确认回环。
S6.3在S5.2的目标函数中再添加一个视觉约束项，完成重定位，如下：
Figure BDA0002324884100000131
其中
Figure BDA0002324884100000132
是指检测到的数据库中的回环帧(第v帧)的位姿，当做一个常量。L是回环帧中检索到的特征的观测集，(l,v)表示回环帧v中观察到的第l个特征。
进行重定位时，会将第v帧的位姿和相关的特征点作为视觉约束项添加到非线性优化的整体的目标函数中。
S6.4当滑窗内的关键帧被溢出滑窗时，会被添加到位姿图中，这个关键帧在位姿图中作为顶点，它通过两种类型的边和其他的顶点连接：
(1)顺序边，表示局部滑动窗口中两个关键帧之间的相对转换，其值直接从VIO中获取。
(2)回环边，如果新边缘化的关键帧有一个回环连接，它将与回环帧通过一个回环边在位姿图图中连接。
通过最小化下面的代价函数，实现全局的位姿图优化：
Figure BDA0002324884100000133
其中：
Figure BDA0002324884100000134
S表示顺序边的集合，L表示回环边的集合；
Figure BDA0002324884100000135
是从VIO中获得的翻滚角和俯仰角的估计，其中的ρ为Huber函数。

Claims (7)

1.一种面向动态场景的单目视觉惯性SLAM方法，其特征在于，该方法包括以下步骤：
S1：使用采集的数据集训练YOLO-v3神经网络，得到的网络权重参数；
S2：对相机获得的图片提取特征点并进行匹配，结合S1中训练的YOLO-v3神经网络筛选出静态特征点；
S3：将IMU获得的测量值进行预积分，并构建误差状态传递方程，计算其误差协方差矩阵和jocobian矩阵；
S4：进行视觉和惯性联合初始化；
S5：构建视觉和IMU的残差方程，进行视觉和IMU的紧耦合非线性优化模型，优化位姿，优化变量为位姿，逆深度，加表和陀螺仪偏置；
S6：使用DBoW2词袋模型，对关键帧数据库中的帧进行回环检测，当检测到回环帧后进行重定位，再利用位姿图优化得到全局一致的位姿并输出。
2.根据权利要求1所述的面向动态场景的单目视觉惯性SLAM方法，其特征在于，所述步骤S1具体包括以下过程：
首先根据需要制作自己的数据集，然后对图片数据进行标定，并在Darknet网络架构中对YOLO-v3神经网络进行训练，最终得到网络模型的权重参数。
3.根据权利要求1所述的面向动态场景的单目视觉惯性SLAM方法，其特征在于，所述步骤S2具体包括以下过程：
S2.1对于相机获得的照片首先提取FAST特征点，并计算BRIEF描述子，进而利用描述子进行特征匹配，根据对极几何约束：
Figure FDA0002324884090000011
其中x1,x2为匹配的两个像素在归一化平面上的坐标，t为两帧图像之间的平移，R为两帧图像之间的旋转，E为本质矩阵，然后利用八点法求取本质矩阵E，再对本质矩阵E进行SVD分解，求得R，t；
S2.2使用训练好的YOLO-v3神经网络对图片进行目标识别，得到识别的目标区域，以此将当前的特征点划分为潜在动态特征点集合P和非潜在动态特征点集合P；
S2.3利用S2.1求得的本质矩阵E，将S2.2中求得的上一帧非潜在动态特征点使用RANSAC进行外点剔除，得到最终的静态特征点集合S。
4.据权利要求1所述的面向动态场景的单目视觉惯性SLAM方法，其特征在于，所述步骤S3具体包括以下过程：
S3.1对IMU获得的测量值进行预积分：
Figure FDA0002324884090000021
Figure FDA0002324884090000022
Figure FDA0002324884090000023
其中：
Figure FDA0002324884090000024
Figure FDA0002324884090000025
Figure FDA0002324884090000026
其中
Figure FDA0002324884090000027
表示拍摄第k帧图片时体坐标系到世界坐标系的相对旋转；
Figure FDA0002324884090000028
分别表示拍摄第k+1帧图片时体坐标系相对于世界坐标系的位置，速度和旋转，其中q表示旋转对应的四元数；α,β,γ分别表示预积分项；
Figure FDA00023248840900000214
表示IMU的测量值，ba和bω分别表示加表和陀螺仪的偏置，na和nω分别表示加速度计和陀螺仪的噪声；
S3.2构建连续时间的误差状态传递方程：
Figure FDA0002324884090000029
其中θ表示四元数的虚部部分，t∈[k,k+1]；
S3.3误差协方差更新公式为：
Figure FDA00023248840900000210
其中初始的协方差矩阵
Figure FDA00023248840900000211
Q是噪声的对角线协方差矩阵：
Figure FDA00023248840900000212
S3.4一阶jocobian的迭代计算公式如下：
Jt+δt＝(I+Ftδt)Jt t∈[k,k+1]
其中初始
Figure FDA00023248840900000213
5.据权利要求1所述的面向动态场景的单目视觉惯性SLAM方法，其特征在于，所述步骤S4具体包括以下过程：
S4.1利用S2中得到的静态特征点集合S进行纯视觉的SFM：利用八点法计算相对平移和旋转，对这两帧图像中的所有静态特征点进行三角化，利用这些三角特征，采用PnP来估计窗口中其他帧的位姿；
S4.2对陀螺仪的偏置进行标定，将视觉SFM的结果和IMU预积分的数据进行对齐，构建目标函数：
Figure FDA0002324884090000031
Figure FDA0002324884090000032
其中B表示滑动窗口中的所有帧，q是由视觉SFM计算出的帧间的旋转四元数，γ是IMU预积分项，其中
Figure FDA0002324884090000033
是根据计算的陀螺仪偏置对预积分项进行更新，得到了新的预积分项
Figure FDA0002324884090000034
S4.3初始化速度、重力、尺度因子：
设要初始化的状态量为：
Figure FDA0002324884090000035
其中，
Figure FDA0002324884090000036
是第k帧本体坐标系的速度，
Figure FDA0002324884090000037
是c0坐标系下的重力矢量，也就是起始点的相机坐标系下的重力矢量，s是尺度因子，
构建线性测量模型：
Figure FDA0002324884090000038
其中：
Figure FDA0002324884090000039
通过求解最小二乘问题：
Figure FDA00023248840900000310
求得窗口中每一帧的本体坐标系速度，c0系的重力向量，以及尺度参数。
6.据权利要求1所述的面向动态场景的单目视觉惯性SLAM方法，其特征在于，所述步骤S5具体包括以下过程：
S5.1选定需要非线性优化的变量为：
Figure FDA0002324884090000041
Figure FDA0002324884090000042
Figure FDA0002324884090000043
其中，xk表示捕捉到第k帧时候的IMU状态，n是关键帧的总数，m是滑动窗子中特征总数，
Figure FDA0002324884090000044
表示IMU坐标系和相机坐标系之间的相对位姿变换，λi表示观测到的第i个特征的逆深度；
S5.2构建目标函数为视觉残差和IMU残差的Mahalanobis范数之和：
Figure FDA0002324884090000045
其中，ρ为Huber范数，
Figure FDA0002324884090000046
分别为IMU和视觉的残差，B是所有IMU测量的集合，C是窗口中至少被观测到2次的一组特征。
7.据权利要求1所述的面向动态场景的单目视觉惯性SLAM方法，其特征在于，所述步骤S6具体包括以下过程：
S6.1在S2中计算的描述子被用于和数据库中的数据进行匹配，进行回环检测；
S6.2为了提升回环检测的准确率，设计了两个检测机制进行筛选：
(1)使用用基于基础矩阵的RANSAC剔除外点，
(2)使用基于PnP的RANSAC来剔除外点，
如果剩下的匹配数大于阈值，那么确认回环；
S6.3在S5.2的目标函数中再添加一个视觉约束项，完成重定位，如下：
Figure FDA0002324884090000047
其中：ρ为Huber函数；rB(·)，rC(·)分别为IMU和视觉的残差，B是所有IMU测量的集合，C是窗口中至少被观测到2次的一组特征；
Figure FDA0002324884090000048
是指检测到的数据库中的回环帧(第v帧)的位姿，当做一个常量，L是回环帧中检索到的特征的观测集，(l,v)表示回环帧v中观察到的第l个特征；
S6.4通过最小化下面的代价函数，实现全局的位姿图优化：
Figure FDA0002324884090000049
其中：ρ为Huber函数；
Figure FDA0002324884090000051
S表示顺序边的集合，L表示回环边的集合；
Figure FDA0002324884090000052
是从VIO中获得的翻滚角和俯仰角的估计。
CN201911313842.1A 2019-12-18 2019-12-18 一种面向动态场景的单目视觉惯性slam方法 Active CN111156984B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911313842.1A CN111156984B (zh) 2019-12-18 2019-12-18 一种面向动态场景的单目视觉惯性slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911313842.1A CN111156984B (zh) 2019-12-18 2019-12-18 一种面向动态场景的单目视觉惯性slam方法

Publications (2)

Publication Number Publication Date
CN111156984A true CN111156984A (zh) 2020-05-15
CN111156984B CN111156984B (zh) 2022-12-09

Family

ID=70557251

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911313842.1A Active CN111156984B (zh) 2019-12-18 2019-12-18 一种面向动态场景的单目视觉惯性slam方法

Country Status (1)

Country Link
CN (1) CN111156984B (zh)

Cited By (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111780763A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN111795686A (zh) * 2020-06-08 2020-10-20 南京大学 一种移动机器人定位与建图的方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN111928857A (zh) * 2020-10-14 2020-11-13 蘑菇车联信息科技有限公司 一种动态环境中实现slam定位的方法及相关装置
CN111928842A (zh) * 2020-10-14 2020-11-13 蘑菇车联信息科技有限公司 一种基于单目视觉实现slam定位的方法及相关装置
CN112097768A (zh) * 2020-11-17 2020-12-18 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN112163502A (zh) * 2020-09-24 2021-01-01 电子科技大学 一种室内动态场景下的视觉定位方法
CN112288011A (zh) * 2020-10-30 2021-01-29 闽江学院 一种基于自注意力深度神经网络的图像匹配方法
CN112435278A (zh) * 2021-01-26 2021-03-02 华东交通大学 一种基于动态目标检测的视觉slam方法及装置
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN112747750A (zh) * 2020-12-30 2021-05-04 电子科技大学 一种基于单目视觉里程计和imu融合的定位方法
CN112837374A (zh) * 2021-03-09 2021-05-25 中国矿业大学 一种空间定位方法及系统
CN112862768A (zh) * 2021-01-28 2021-05-28 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN112884835A (zh) * 2020-09-17 2021-06-01 中国人民解放军陆军工程大学 一种基于深度学习之目标检测的视觉slam方法
CN113034571A (zh) * 2021-04-16 2021-06-25 广东工业大学 一种基于视觉—惯性的物体三维尺寸测量方法
CN113066129A (zh) * 2021-04-12 2021-07-02 北京理工大学 基于动态环境下的目标检测的视觉定位与建图系统
CN113223161A (zh) * 2021-04-07 2021-08-06 武汉大学 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法
CN113516692A (zh) * 2021-05-18 2021-10-19 上海汽车集团股份有限公司 一种多传感器融合的slam方法和装置
CN113963030A (zh) * 2021-11-09 2022-01-21 福州大学 一种提高单目视觉初始化稳定性的方法
CN114199275A (zh) * 2020-09-18 2022-03-18 阿里巴巴集团控股有限公司 传感器的参数确定方法和装置
CN114723779A (zh) * 2021-01-06 2022-07-08 广州汽车集团股份有限公司 一种车辆定位方法、装置及计算机可读存储介质
WO2023000294A1 (zh) * 2021-07-23 2023-01-26 深圳市大疆创新科技有限公司 位姿估计方法、激光雷达-惯性里程计、可移动平台及存储介质
CN116518961A (zh) * 2023-06-29 2023-08-01 煤炭科学研究总院有限公司 大规模固定视觉传感器全局位姿的确定方法和装置
WO2023116797A3 (zh) * 2021-12-22 2023-08-17 比亚迪股份有限公司 车载多传感器融合定位方法、计算机设备及存储介质
CN117671397A (zh) * 2023-12-08 2024-03-08 广东技术师范大学 基于双任务学习和位姿图优化的视觉重定位方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108717712A (zh) * 2018-05-29 2018-10-30 东北大学 一种基于地平面假设的视觉惯导slam方法
CN109029433A (zh) * 2018-06-28 2018-12-18 东南大学 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN109307508A (zh) * 2018-08-29 2019-02-05 中国科学院合肥物质科学研究院 一种基于多关键帧的全景惯导slam方法
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110455301A (zh) * 2019-08-01 2019-11-15 河北工业大学 一种基于惯性测量单元的动态场景slam方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108717712A (zh) * 2018-05-29 2018-10-30 东北大学 一种基于地平面假设的视觉惯导slam方法
CN109029433A (zh) * 2018-06-28 2018-12-18 东南大学 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN109307508A (zh) * 2018-08-29 2019-02-05 中国科学院合肥物质科学研究院 一种基于多关键帧的全景惯导slam方法
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110455301A (zh) * 2019-08-01 2019-11-15 河北工业大学 一种基于惯性测量单元的动态场景slam方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
徐晓苏等: "《室内环境下基于图优化的视觉惯性SLAM 方法》", 《中国惯性技术学报》 *

Cited By (34)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111795686B (zh) * 2020-06-08 2024-02-02 南京大学 一种移动机器人定位与建图的方法
CN111795686A (zh) * 2020-06-08 2020-10-20 南京大学 一种移动机器人定位与建图的方法
CN111780763B (zh) * 2020-06-30 2022-05-06 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN111780763A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN112884835A (zh) * 2020-09-17 2021-06-01 中国人民解放军陆军工程大学 一种基于深度学习之目标检测的视觉slam方法
CN114199275A (zh) * 2020-09-18 2022-03-18 阿里巴巴集团控股有限公司 传感器的参数确定方法和装置
CN112163502A (zh) * 2020-09-24 2021-01-01 电子科技大学 一种室内动态场景下的视觉定位方法
CN112163502B (zh) * 2020-09-24 2022-07-12 电子科技大学 一种室内动态场景下的视觉定位方法
CN111928842A (zh) * 2020-10-14 2020-11-13 蘑菇车联信息科技有限公司 一种基于单目视觉实现slam定位的方法及相关装置
CN111928857A (zh) * 2020-10-14 2020-11-13 蘑菇车联信息科技有限公司 一种动态环境中实现slam定位的方法及相关装置
CN112288011A (zh) * 2020-10-30 2021-01-29 闽江学院 一种基于自注意力深度神经网络的图像匹配方法
CN112288011B (zh) * 2020-10-30 2022-05-13 闽江学院 一种基于自注意力深度神经网络的图像匹配方法
CN112097768B (zh) * 2020-11-17 2021-03-02 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN112097768A (zh) * 2020-11-17 2020-12-18 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN112747750A (zh) * 2020-12-30 2021-05-04 电子科技大学 一种基于单目视觉里程计和imu融合的定位方法
CN114723779A (zh) * 2021-01-06 2022-07-08 广州汽车集团股份有限公司 一种车辆定位方法、装置及计算机可读存储介质
CN112435278B (zh) * 2021-01-26 2021-05-04 华东交通大学 一种基于动态目标检测的视觉slam方法及装置
CN112435278A (zh) * 2021-01-26 2021-03-02 华东交通大学 一种基于动态目标检测的视觉slam方法及装置
CN112862768A (zh) * 2021-01-28 2021-05-28 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN112837374B (zh) * 2021-03-09 2023-11-03 中国矿业大学 一种空间定位方法及系统
CN112837374A (zh) * 2021-03-09 2021-05-25 中国矿业大学 一种空间定位方法及系统
CN113223161A (zh) * 2021-04-07 2021-08-06 武汉大学 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法
CN113066129A (zh) * 2021-04-12 2021-07-02 北京理工大学 基于动态环境下的目标检测的视觉定位与建图系统
CN113034571A (zh) * 2021-04-16 2021-06-25 广东工业大学 一种基于视觉—惯性的物体三维尺寸测量方法
CN113516692A (zh) * 2021-05-18 2021-10-19 上海汽车集团股份有限公司 一种多传感器融合的slam方法和装置
WO2023000294A1 (zh) * 2021-07-23 2023-01-26 深圳市大疆创新科技有限公司 位姿估计方法、激光雷达-惯性里程计、可移动平台及存储介质
CN113963030A (zh) * 2021-11-09 2022-01-21 福州大学 一种提高单目视觉初始化稳定性的方法
WO2023116797A3 (zh) * 2021-12-22 2023-08-17 比亚迪股份有限公司 车载多传感器融合定位方法、计算机设备及存储介质
CN116518961A (zh) * 2023-06-29 2023-08-01 煤炭科学研究总院有限公司 大规模固定视觉传感器全局位姿的确定方法和装置
CN116518961B (zh) * 2023-06-29 2023-09-01 煤炭科学研究总院有限公司 大规模固定视觉传感器全局位姿的确定方法和装置
CN117671397A (zh) * 2023-12-08 2024-03-08 广东技术师范大学 基于双任务学习和位姿图优化的视觉重定位方法及系统
CN117671397B (zh) * 2023-12-08 2024-06-04 广东技术师范大学 基于双任务学习和位姿图优化的视觉重定位方法及系统

Also Published As

Publication number Publication date
CN111156984B (zh) 2022-12-09

Similar Documents

Publication Publication Date Title
CN111156984B (zh) 一种面向动态场景的单目视觉惯性slam方法
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN107869989B (zh) 一种基于视觉惯导信息融合的定位方法及系统
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN108717712B (zh) 一种基于地平面假设的视觉惯导slam方法
CN112598757B (zh) 一种多传感器时间空间标定方法及装置
Rambach et al. Learning to fuse: A deep learning approach to visual-inertial camera pose estimation
CN111595333A (zh) 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN111462135A (zh) 基于视觉slam与二维语义分割的语义建图方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN108229416B (zh) 基于语义分割技术的机器人slam方法
CN112381890B (zh) 一种基于点线特征的rgb-d视觉slam方法
CN116205947B (zh) 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
CN112233177A (zh) 一种无人机位姿估计方法及系统
CN112101160B (zh) 一种面向自动驾驶场景的双目语义slam方法
CN112767546B (zh) 移动机器人基于双目图像的视觉地图生成方法
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN113011401B (zh) 人脸图像姿态估计和校正方法、系统、介质及电子设备
WO2023050634A1 (zh) 定位方法及装置、设备、存储介质及计算机程序产品
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
Li et al. Towards end-to-end learning of visual inertial odometry with an EKF
CN106846367A (zh) 一种基于运动约束光流法的复杂动态场景的运动物体检测方法

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