CN109307508B - 一种基于多关键帧的全景惯导slam方法 - Google Patents

Abstract

本发明公开了一种基于多关键帧的全景惯导SLAM方法，该方法包括：构建多目全景视觉模型，结合惯导单元紧耦合融合视觉特征，通过多关键帧视觉惯导非线性优化，实现高精度定位与场景地图，用于解决现有技术中环境感知方法获取信息的不充分、快速旋转的不确定性，能够解决现有视觉SLAM方法视野受限、光照影响和视觉退化等问题，为强光线/弱纹理/运动模糊等复杂环境中的机器人环境建模和定位导航提供有效的自动化方法，提高方法的精确性和鲁棒性，从而保证机器人自主导航的充分性，同时兼顾计算效率，可以实现视觉SLAM方法验证***实时性。

Description

一种基于多关键帧的全景惯导SLAM方法

技术领域

本发明属于计算机视觉检测领域，涉及一种视觉感知的SLAM方法，尤其涉及一种基于多关键帧的全景惯导SLAM方法。

背景技术

SLAM是实现移动平台自主性的关键，近年来在多领域呈现出较大的应用价值，尤其是导航机器人、虚拟现实等。同步定位与地图构建方法(Simultaneous LocalizationandMapping,SLAM)指移动平台在位姿未知情况下，利用本体或外置传感器，感知并构建一致的环境地图，同时确定自身相对位姿的方法。较于激光SLAM，视觉SLAM能够获取非结构化空间信息，提供语义标注场景，在复杂环境应用意义更大。为此，实现视觉SLAM的鲁棒性和精确性，需要一种通用、实时的视觉SLAM方法。

SLAM方法的数学建模过程影响着***性能，也决定了不同的处理方法。早期研究中，基于滤波器的求解方式成为主流，但线性化假设增加了资源消耗，也不适应于动态环境。而非线性优化方法，计算复杂程度较高，长期制约着SLAM研究。之后，稀疏性概念的提出，基于图优化的视觉SLAM优化方法加速了该领域的发展，也是现今视觉SLAM的主要方向。

目前，传统视觉方法基于RGBD相机获取深度数据，然后转换成二维激光数据，并没有充分利用视觉信息，也不是真正意义上的视觉SLAM方法框架，如申请号为201710835285.4的专利《一种基于Kinect的室内移动机器人视觉SLAM方法》；大多数学者也在研究单目SLAM，而此方法仅采用单相机，视野有限，并对场景特征不能直接进行距离估算，如申请号为201710658521.X的专利《基于半直接法和滑动窗口优化的单目视觉SLAM算法》；文献《ORB-SLAM:a versatile and accurate monocular SLAM system》提出一种基于关键帧的多线程视觉SLAM方法，采用ORB特征稳定较高并兼顾实时性，但光照敏感，在快速旋转等极端情况易丢失。上述专利和文献均未解决视野受限问题，不能适用于弱纹理环境，同时在视觉退化(运动模糊、大场景)时，尺度估计影响较大，导致视觉SLAM方法无法正常使用。

因此，如何提供一种稳定可靠的视觉SLAM方法，以解决现有技术中环境感知方法获取信息的不充分、快速旋转的不确定性，保证视觉SLAM方法的精度和稳定性，实已成为本领域亟待解决的技术问题。

发明内容

鉴于以上所述现有技术的特点，本发明的目的是提供一种基于多关键帧的全景惯导SLAM方法，构建多目全景视觉模型，结合惯导单元紧耦合融合视觉特征，通过多关键帧视觉惯导非线性优化，实现高精度定位与场景地图，用于解决现有技术中环境感知方法获取信息的不充分、快速旋转的不确定性，为强光线/弱纹理/运动模糊等复杂环境中的机器人环境建模和定位导航提供有效的自动化方法。

为实现上述目的及其他相关目的，本发明提供一种基于多关键帧的全景惯导SLAM方法，包括如下步骤：

步骤S1、构建多目全景模型，融合惯性单元运动信息，进行全景惯导信息采集与特征提取；针对各帧全景惯导视觉信息，采用AGAST角点改进ORB特征提取，并结合基于任务的BRIEF在线学习算法表述描述子，适应于畸变图像，尤其是全景图像；

步骤S2、利用多关键帧全景惯导信息SLAM初始化，提供视觉补偿参数；所述视觉补偿参数包括尺度、重力方向、速度、陀螺仪偏差、加速度计偏差等；

步骤S3、结合IMU流型预积分、多关键帧运动模型与多关键帧参考帧模型，进行视觉惯导追踪，估计位姿及重定位监测；

步骤S4、根据视觉惯导因子图采用全景非线性后端优化，分别完成局部集束调整和全局回环检测，更新地图点及多关键帧状态，最终获取完整的多关键帧位姿与环境空间重构地图。

可选地，所述多目全景模型通过N个相机以环视、环视+上视、环视+下视、或环视+上视+下视组合实现；N为大于或等于2的正数。

可选地，所述全景惯导信息采集与特征提取包括AGAST-BRIEF特征提取、多目全景视觉投影模型和IMU信息采集。AGAST-BRIEF特征提取采用AGAST角点改进ORB特征提取，避免FAST角点新环境重训练的耗时问题，以一种在线学***面的投影关系；IMU信息采集通过软件或硬件实现IMU与多相机帧的时间戳精准对齐，由外参标定方法获取Multi-body与IMU实体的旋转平移矩阵，并进行流型预积分处理。

可选地，所述多关键帧全景惯导SLAM初始化包括多关键帧视觉初始化和融合IMU信息的视觉惯导初始化。多关键帧视觉初始化由各相机分别找到初始点，由视场阈值选择用于平面场景的单应性矩阵模型或用于非平面场景的基础矩阵，进行运动恢复，然后根据相机间外参关系合成Multi-body多关键帧，完成地图初始化并获取多关键帧的初始位姿；融合IMU信息的视觉惯导初始化结合IMU流型预积分数据，通过连续多关键帧间相对方位估计陀螺仪偏差，然后通过奇异值分解多相机和IMU关系式获取重力向量和尺度因子，根据重力大小G建立加速度计偏差估计关系，获取加速度计偏差，并重新修正重力向量和尺度因子，由已知量计算多关键帧速度，并进行全局集束调整，优化视觉补偿参数。

可选地，所述视觉惯导追踪包括多关键帧IMU流型预积分、基于运动模型多关键帧状态估计、基于参考帧模型多关键帧状态估计和重定位。多关键帧IMU流型预积分指利用流型预积分方法将相邻多关键帧间的离散IMU数据整合成单独观测值，从而用于表示运动模型及其它；基于运动模型多关键帧状态估计利用运动模型预测多目相机整体姿态及上帧地图点，根据多目全景视觉投影模型在一定范围内进行投影匹配，并更新多关键帧状态；若匹配点过少，启动基于参考帧模型多关键帧状态估计，与最近关键帧进行匹配；如上述两种情况匹配不成功，重定位将与所有多关键帧进行匹配以寻找合适位置，在多关键帧数据库中选取若干关键帧候选，寻找足够特征点的关键帧，利用RANSAC算法迭代，并用PNP算法求解位姿。

可选地，所述流型预积分得到的相邻多关键帧间的运动模型的一种计算方式为：

其中，R，v和p分别表示旋转，速度和位置，a，ω和g分别表示加速度，角速率和重力加速度，W表示世界坐标系，B表示IMU，

和

分别表示k和k+1时刻IMU在世界坐标系中的旋转，R_△k，k+1，v_△k,k+1，p_△k,k+1为k和k+1两个多关键帧之间的IMU预积分结果，v_B、a_B和ω_B分别表示IMU的速度、加速度和角速率，△t表示时间变化，b_g和b_a分别代表角速率和加速度的偏差，

和

分别表示k+1和k时刻IMU在世界坐标系中的位置，雅可比矩阵

和

表示IMU预积分观测值随偏差变化方程的一阶近似。

可选地，所述全景非线性后端优化包括当前多关键帧状态优化、局部BA地图点和多关键帧状态优化、全局BA回环检测和全局关键帧位姿优化。当前多关键帧状态优化将当前关键帧和IMU状态作为优化变量，最小化全景视觉重投影误差和IMU误差，优化当前Multi-body多关键帧位姿；局部BA地图点和多关键帧状态优化处理新多关键帧，计算BOW后***地图，剔除冗余的地图点，然后三角化计算新的地图点，并进行局部集束调整，剔除冗余多关键帧；全局BA回环检测和全局关键帧位姿优化首先基于BOW进行闭环探测，通过Sim3算法计算相似变换，发现闭环后进行融合以及本质图优化校正闭环。

可选地，最小化全景视觉重投影误差和IMU误差的一种表述方式为：

argmin{E_proj(i)+E_IMU(i,j)}

其中，E_proj(i)为全景视觉重投影误差，E_IMU(i,j)为IMU误差。

如上所述，本发明所述的基于多关键帧的全景惯导SLAM方法，具有以下有益效果：

本发明所述的基于多关键帧的全景惯导SLAM方法能够解决现有视觉SLAM方法视场角小、视野受限的问题，同时能够补偿现有SLAM方法在光照变化、纹理特征少、剧烈晃动导致视觉模糊等视觉退化问题，极大提高了视觉SLAM方法的精确性和鲁棒性，为机器人的自主性提供可靠保障，优化了机器人的自主避障等自主能力，保证机器人自主导航的充分性，同时兼顾计算效率，可以实现视觉SLAM方法验证***实时性。

附图说明

图1显示为本发明的全景惯导SLAM方法的流程示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅附图。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

下面结合实施例和附图对本发明进行详细说明。

本发明所述的基于多关键帧的全景惯导SLAM方法的发明原理如下：

本发明的基于多关键帧的全景惯导SLAM方法，构建多目全景视觉模型，结合惯导单元紧耦合融合视觉特征，采用AGAST角点改进ORB特征提取，并结合基于任务的BRIEF在线学习算法表述描述子，进行全景惯导信息采集与特征提取，然后利用多关键帧全景惯导SLAM初始化，获取视觉补偿参数，之后结合IMU流型预积分、多关键帧运动模型与多关键帧参考帧模型，进行视觉惯导追踪和位姿估计，最后根据视觉惯导因子图采用全景非线性后端优化，分别完成局部集束调整和全局回环检测，更新地图点及多关键帧状态，从而获取最终的多关键帧位姿与环境空间重构地图，实现高精度定位与场景地图。

实施例一

本实施例提供一种基于多关键帧的全景惯导SLAM方法，请参阅图1，显示为基于多关键帧的全景惯导SLAM方法的流程示意图。如图1所述，所述全景惯导SLAM方法包括以下步骤：

S1，全景惯导信息采集与特征提取。本实施例中，通过硬件同步触发，以100HZ频率实时运行惯性单元，20HZ频率实时运行三相机，分别对各相机图像采用AGAST角点进行特征提取并以基于任务的BRIEF在线学习算法表述描述子。本实施例中，多目全景视觉投影由三相机的外参关系确定Multi-body帧，通过向量存储提取特征点与该帧的投影关系。同时，在相邻两多关键帧内包含5帧IMU信息，进行流型预积分，可与视觉建立因子图。本领域人员技术人员应该理解，上述全景惯导信息采集与特征提取仅仅只是列示，而非对本发明的限制。

S2，多关键帧全景惯导SLAM初始化。本实施例中，多关键帧视觉初始化通过评分规则分别选择三相机的响应模型，恢复旋转矩阵和平移向量，之后启动融合IMU信息的视觉惯导初始化，通过连续多关键帧间相对方位估计陀螺仪偏差，然后利用该偏差旋转加速度计至正确的测量值，在加速度计偏差未知的情况下通过奇异值分解多相机和IMU关系式获取重力向量和尺度因子，根据重力大小G建立加速度计偏差估计关系，获取加速度计偏差，并重新修正重力向量和尺度因子，最后根据已知量计算多关键帧速度。若重定位开启，需利用纯视觉的连续邻近帧重新估计各偏差。初始化结束后，实现一次全局集束调整，优化视觉惯导(结构、多关键帧位姿、尺度、速度、重力)。

S3，视觉惯导追踪。本实施例中，采用全景视觉惯导因子图方式，表述视觉惯导中的当前多关键帧位姿、速度和IMU偏差，一旦三相机位姿预测，局部地图中的场景点投影至各相机帧并进行匹配，通过最小化所有特征匹配点的重投影误差和IMU误差，优化当前多关键帧，并利用G2O中的列文伯格-马夸尔特算法实现。根据地图是否更新，优化方式不同。若集束调整或回环检测引发地图更新，优化Multi-body帧重投影误差以及与最近多关键帧的IMU误差，同时估计值和海森矩阵作为下次优化的先验值；若地图没有更新，则当前Multi-body帧在优化重投影误差和相邻帧间IMU误差的同时，考虑前一Multi-body帧先验值，此后边缘化前一Multi-body帧并生成当前多关键帧的先验值，直至地图重新更新。

S4，全景非线性后端优化。对三相机组成的当前多关键帧检测是否在序列中，若是则处理新帧并计算其BOW，将新多关键帧***地图，删除冗余地图点并三角化计算新的地图点，同时通过邻近关键帧优化地图点，进行局部集束调整优化当前多关键帧、有共视关系的多关键帧和地图点。发现闭环时，对所有结果进行优化并校正闭环。

本发明所述的基于多关键帧的全景惯导SLAM方法能够解决现有视觉SLAM方法视场角小、视野受限的问题，同时能够补偿现有SLAM方法在光照变化、纹理特征少、剧烈晃动导致视觉模糊等视觉退化问题，极大提高了视觉SLAM方法的精确性和鲁棒性，为机器人的自主性提供可靠保障，优化了机器人的自主避障等自主能力，保证机器人自主导航的充分性，同时兼顾计算效率，可以实现视觉SLAM方法验证***实时性。

综上所述，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (7)

1.一种基于多关键帧的全景惯导SLAM方法，其特征在于：包括如下步骤：

步骤S1、构建多目全景模型，融合惯性单元运动信息，进行全景惯导信息采集与特征提取；针对各帧全景惯导视觉信息，采用AGAST角点改进ORB特征提取，并结合基于任务的BRIEF在线学***面的投影关系；IMU信息采集通过软件或硬件实现IMU与多相机帧的时间戳精准对齐，由外参标定方法获取Multi-body与IMU实体的旋转平移矩阵，并进行流型预积分处理；

步骤S2、利用多关键帧全景惯导信息SLAM初始化，提供视觉补偿参数；所述视觉补偿参数包括尺度、重力方向、速度、陀螺仪偏差、加速度计偏差；

步骤S3、结合IMU流型预积分、多关键帧运动模型与多关键帧参考帧模型，进行视觉惯导追踪，估计位姿及重定位监测；

步骤S4、根据视觉惯导因子图采用全景非线性后端优化，分别完成局部集束调整和全局回环检测，更新地图点及多关键帧状态，最终获取完整的多关键帧位姿与环境空间重构地图。

2.根据权利要求1所述的一种基于多关键帧的全景惯导SLAM方法，其特征在于：所述多目全景模型通过N个相机以环视、环视+上视、环视+下视、或环视+上视+下视组合实现；N为大于或等于2的正数。

3.根据权利要求1所述的一种基于多关键帧的全景惯导SLAM方法，其特征在于：所述多关键帧全景惯导SLAM初始化包括多关键帧视觉初始化和融合IMU信息的视觉惯导初始化；多关键帧视觉初始化由各相机分别找到初始点，由视场阈值选择用于平面场景的单应性矩阵模型或用于非平面场景的基础矩阵，进行运动恢复，然后根据相机间外参关系合成Multi-body多关键帧，完成地图初始化并获取多关键帧的初始位姿；融合IMU信息的视觉惯导初始化结合IMU流型预积分数据，通过连续多关键帧间相对方位估计陀螺仪偏差，然后通过奇异值分解多相机和IMU关系式获取重力向量和尺度因子，根据重力大小G建立加速度计偏差估计关系，获取加速度计偏差，并重新修正重力向量和尺度因子，由已知量计算多关键帧速度，并进行全局集束调整，优化视觉补偿参数。

4.根据权利要求1所述的一种基于多关键帧的全景惯导SLAM方法，其特征在于：所述视觉惯导追踪包括多关键帧IMU流型预积分、基于运动模型多关键帧状态估计、基于参考帧模型多关键帧状态估计和重定位；多关键帧IMU流型预积分指利用流型预积分方法将相邻多关键帧间的离散IMU数据整合成单独观测值，从而用于表示运动模型及其它；基于运动模型多关键帧状态估计利用运动模型预测多目相机整体姿态及上帧地图点，根据多目全景视觉投影模型在一定范围内进行投影匹配，并更新多关键帧状态；若匹配点过少，启动基于参考帧模型多关键帧状态估计，与最近关键帧进行匹配；如投影匹配以及与最近关键帧进行匹配不成功，重定位将与所有多关键帧进行匹配以寻找合适位置，在多关键帧数据库中选取若干关键帧候选，寻找足够特征点的关键帧，利用RANSAC算法迭代，并用PNP算法求解位姿。

5.根据权利要求1所述的一种基于多关键帧的全景惯导SLAM方法，其特征在于：所述流型预积分得到的相邻多关键帧间的运动模型的一种计算方式为：

其中，R，v和p分别表示旋转，速度和位置，a和 g分别表示加速度和重力加速度，W表示世界坐标系，B表示IMU，

和

分别表示k和k+1时刻IMU在世界坐标系中的旋转，R_Δk，k+1，v_Δk,k+1，p_Δk,k+1为k和k+1两个多关键帧之间的IMU预积分结果，v_B、a_B分别表示IMU的速度、加速度，Δt表示时间变化，b_g和b_a分别代表重力加速度和加速度的偏差，

和

分别表示k+1和k时刻IMU在世界坐标系中的位置，雅可比矩阵

和

表示IMU预积分观测值随偏差变化方程的一阶近似。

6.根据权利要求1所述的一种基于多关键帧的全景惯导SLAM方法，其特征在于：所述全景非线性后端优化包括当前多关键帧状态优化、局部BA地图点和多关键帧状态优化、全局BA回环检测和全局关键帧位姿优化；当前多关键帧状态优化将当前关键帧和IMU状态作为优化变量，最小化全景视觉重投影误差和IMU误差，优化当前Multi-body多关键帧位姿；局部BA地图点和多关键帧状态优化处理新多关键帧，计算BOW后***地图，剔除冗余的地图点，然后三角化计算新的地图点，并进行局部集束调整，剔除冗余多关键帧；全局BA回环检测和全局关键帧位姿优化首先基于BOW进行闭环探测，通过Sim3算法计算相似变换，发现闭环后进行融合以及本质图优化校正闭环。

7.根据权利要求1所述的一种基于多关键帧的全景惯导SLAM方法，其特征在于：最小化全景视觉重投影误差和IMU误差的一种表述方式为：

argmin{E_proj(i)+E_IMU(i,j)}

其中，E_proj(i)为全景视觉重投影误差，E_IMU(i,j)为IMU误差。

Images