CN110533587A - 一种基于视觉先验信息和地图恢复的slam方法 - Google Patents

Abstract

本发明公开了一种基于视觉先验信息和地图恢复的SLAM方法，包括：以预设格式保存针对当前环境构建的SLAM地图，作为先验地图；加载所述先验地图，使用重定位初始化算法完成初始化，固定SLAM坐标系后进行跟踪和建图；当跟踪失败时，自动保存已构建的SLAM地图，作为跟踪成功地图，并采用自适应重新初始化方法完成重新初始化，继续进行跟踪与建图，并将重新初始化后构建的SLAM地图作为恢复地图；采用回环检测算法获得跟踪成功地图与恢复地图之间的转换关系；根据所述转换关系，采用地图恢复法拼接跟踪成功地图与恢复地图，得到当前环境的全局地图。本发明解决了SLAM系统初始化的坐标系不统一，以及跟踪失败到重新定位成功之前地图信息丢失的问题。

Description

一种基于视觉先验信息和地图恢复的SLAM方法

技术领域

本申请属于计算机视觉、图像处理领域，具体涉及一种基于视觉先验信息和地图恢复的SLAM方法。

背景技术

V-SLAM(同步定位与建图)以其成本廉价、小场景范围内精度较高、无需预先布置场景等优势，成为目前一个较常采用的定位方案。

在现有的研究成果中，有多种较成熟的V-SLAM方案。Klein和Murray等人提出的PTAM(Parallel Tracking and Mapping)是第一个使用BA(bundle adjustment)的实时单目SLAM应用，它将跟踪与建图拆成两个并行的线程。适用于室内与室外环境的ORB-SLAM2(Oriented Brief Simultaneous Localization And Mapping)则是目前较成熟的一个基于特征点的SLAM系统，加入了循环回路的检测和闭合机制以消除误差累积。DSO(DirectSparse Odometry)是一个使用直接法生成半稠密点云图的SLAM系统，更高效地利用了像素信息。而SVO(Semi-Direct Monocular Visual Odometry)通过结合直接法与特征点法，提升了系统处理速度。

除了上述内容以外，还有很多种V-SLAM方案，但是这些方案在实际应用中仍存在如下局限：局限一、在弱纹理场景、相机快速移动产生运动模糊等情况会出现跟踪失败的情况，且已有的V-SLAM方案均采用重定位的方法进行处理。该方法的缺点是，如果当前帧跟已有的关键帧视角不够接近，将导致重定位失败，在许多实际应用场景中造成不便。如正在前进的移动机器人在跟踪失败后被要求必须重新回到丢失位置信息的地方，系统才可以继续跟踪和建图。另一方面，系统跟踪失败之后到重定位成功之前的地图信息均无法恢复，导致大量地图信息丢失。局限二、已有的SLAM系统采用的是从初始帧而不是绝对位置开始的相对变换方案。每次启动系统时，它都会将起点设置为参考帧，因此不能在一个固定的坐标系中获得绝对位姿。

发明内容

本申请的目的在于提供一种基于视觉先验信息和地图恢复的SLAM方法，解决了SLAM系统初始化的坐标系不统一，以及跟踪失败到重新定位成功之前地图信息丢失的问题。

为实现上述目的，本申请所采取的技术方案为：

一种基于视觉先验信息和地图恢复的SLAM方法，所述的基于视觉先验信息和地图恢复的SLAM方法实施在SLAM系统，包括以下步骤：

步骤S1、以预设格式保存针对当前环境构建的SLAM地图，作为先验地图；

步骤S2、加载所述先验地图，使用重定位初始化算法完成初始化，固定SLAM坐标系后进行跟踪和建图；

步骤S3、当跟踪失败时，自动保存已构建的SLAM地图，作为跟踪成功地图，并采用自适应重新初始化方法完成重新初始化，继续进行跟踪与建图，并将重新初始化后构建的SLAM地图作为恢复地图；

步骤S4、采用回环检测算法获得跟踪成功地图与恢复地图之间的转换关系；

步骤S5、根据所述转换关系，采用地图恢复法拼接跟踪成功地图与恢复地图，得到当前环境的全局地图。

作为优选，所述预设格式为：

(MapPoints，KeyFrames)

其中，MapPoints为地图点集合，KeyFrames为关键帧集合，且每一个地图点的格式为：

(pid，x_map，y_map，z_map)

其中，pid为地图点的标识，x_map，y_map，z_map为地图点的坐标；

每一个关键帧的格式为：

(fid，time，quat，features，parentid，conid_ws)

其中，fid为关键帧的标识，time为时间戳，quat为关键帧的位姿四元数，parentid为关键帧的父节点的标识，conid_ws为集合，conid_ws中的每一个元素表示与该关键帧相连的关键帧的标识以及这两个关键帧之间的权重，features为集合，features存储了该关键帧观测到的所有特征点，其中每一个特征点的记录格式为：

(x_key，y_key，size，angle，response，octave，descriptor)

其中，x_key，y_key为特征点的坐标，size为特征点的邻域直径，angle为特征点的方向，response为特征点的响应强度，代表了该点是特征点的程度，octave表示该特征点所在的图像金字塔的层级，descriptor表示该特征点的描述子。

作为优选，所述重定位初始化算法，包括：

SLAM系统对获取到的当前图像帧提取ORB特征，采用BOW模型确定当前图像帧与先验地图中的先验图像帧之间的转换矩阵；

根据确定的转换矩阵，采用PnP算法计算当前图像帧在先验地图中的位置，从而建立与先验地图中的坐标系一致的SLAM坐标系，完成初始化。

作为优选，所述自适应重新初始化方法，包括：

若在当前环境的场景照片中检测到灭点，即采用快速初始化方法，包括：

假设找到的灭点为v_i，提取到的特征点为p_j，且特征点p_j的深度值D_j为：

其中，i∈[1，a]，a为场景照片中找到的灭点数量，j∈[1，b]，b为场景照片中找到的特征点数量，p_j.x为特征点p_j在x轴上的坐标值，p_j.y为特征点p_j在y轴上的坐标值，v_i.x为灭点v_i在x轴上的坐标值，v_i.y为灭点v_i在y轴上的坐标值；

计算所有特征点的深度值，并从所有特征点的深度值中选择最大与最小值，分别记为D_max与D_min；

对每一特征点的深度值进行归一化处理，归一化深度值区间为[0.5，1.5]，得到特征点p_j的归一化深度值p_{j_w_z}为：

p_{j_w_z}＝0.5+D_j/(D_max-D_min)

根据得到的特征点p_j的深度信息，恢复特征点p_j的三维坐标为：

其中，x_j，y_j为特征点p_j在图像上的坐标，c_x、c_y、f_x、f_y为相机的内参，则特征点p_j的三维坐标为(X，Y，p_{j_w_z})；

计算每一特征点的三维坐标，完成SLAM三维地图的构建，即完成初始化；

若在当前环境的场景照片中未检测到灭点，即采用简易初始化方法，包括：

对当前获取到的每一帧图像提取ORB特征，根据ORB特征计算两帧图像之间的单应矩阵Hcr，通过分解单应矩阵Hcr和三角化方法获得点云地图，若点云地图中点数量大于l，则将该点云地图作为初始地图，完成重新初始化。

作为优选，所述回环检测算法，包括：

使用DBoW2库寻找当前地图的关键帧与子地图的关键帧之间的匹配项，并存入匹配集H＝{h₁，...，h_n}中，所述当前地图为恢复地图，所述子地图为先验地图或跟踪成功地图，所述匹配集H中的h_q，q∈[1，n]表示DBoW2库找到的第q个匹配项，且

其中，表示第q个匹配项中当前地图的关键帧，表示第q个匹配项中子地图的关键帧；

匹配集H中的两匹配项之间存在时序关系，即两匹配项之间满足以下关系：

且

其中，q1，q2∈[1，n]，且q2＝1+q1，即两匹配项h_q1和h_q2为相邻匹配项；

若匹配集H中的匹配项|H|≥m，则认定当前地图与子地图之间出现重叠区域，停止匹配项的寻找，确定H＝{h₁，...，h_m}为最终的匹配集H；

从子地图中取一帧关键帧K_i，根据最终的匹配集H，采用Hom算法计算当前地图的坐标系到关键帧K_i的坐标系之间的转换矩阵T_curr2i，并根据转换矩阵T_curr2i得到从当前地图到子地图的坐标转换矩阵T_curr2sub为：

T_curr2sub＝T_curr2i*T_i2sub

其中，T_i2sub为已知量，T_i2sub表示关键帧K_i的坐标系到子地图的坐标系之间的转换矩阵，且T_curr2sub的具体形式为：

T_curr2sub＝(R_curr2sub，t_curr2sub，s_curr2sub)

其中，R_curr2sub和t_curr2sub代表当前地图到子地图的旋转变换矩阵与平移变换矩阵，s_curr2sub代表当前地图与子地图之间的尺度关系。

作为优选，所述地图恢复法，包括：

取当前地图到子地图的坐标转换矩阵T_curr2sub，采用去除尺度关系的坐标转换矩阵T′_curr2sub＝(R_curr2sub，t_curr2sub)，将跟踪成功地图与恢复地图的坐标系对齐，保持跟踪成功地图与恢复地图的坐标系一致；

根据最终的匹配集H关联跟踪成功地图与恢复地图之间重叠区域的关键帧，并利用坐标转换矩阵T_curr2sub合并重叠区域内的特征点，得到跟踪成功地图与恢复地图数据融合后的拼接地图；

利用求解能量方程的最小化问题恢复拼接地图的尺度，建立的能量方程包括三部分，第一部分为当前地图的位姿图E(ξ^curr)，第二部分为子地图的位姿图E(ξ^sub)，第三部分为当前地图和子地图之间的重叠区域的位姿图E(ξ^over)，能量方程具体如下：

其中，ξ^curr为关键帧在当前地图中优化前的位姿，ξ^sub为关键帧在子地图中优化前的位姿，ξ^over为关键帧在当前地图与子地图重叠区域中优化前的位姿，ξ为关键帧优化后的融合位姿，E(ξ)为拼接地图优化后的位姿图，为当前地图中的关键帧，为子地图中的关键帧，为当前地图与子地图重叠区域中的关键帧，Λ为信息矩阵；

其中，关键帧与之间的位姿误差为：

关键帧与之间的位姿误差为：

关键帧与之间的位姿误差为：

其中，log_Sim(3)函数表示将李群转换成李代数，为当前地图中的关键帧，为子地图中的关键帧，为当前地图与子地图重叠区域中的关键帧；

得到拼接地图优化后的位姿图E(ξ)，使用重投影进行全局BA优化，建立最小化能量方程为：

其中，ρ_h为Huber函数，Ω为协方差矩阵，n为关键帧数量，X^curr为当前地图的地图点；

取地图中任意前后关联的两关键帧设为关键帧1和关键帧2，且e_curr1为在当前地图中关键帧1的重投影误差，计算公式为：

e_curr1＝x_curr1，i-π(S₁₂，P_2，j)

其中，i，j为关键帧1与关键帧2之间的匹配点，x_curr1，i为匹配点i的像素坐标，S₁₂＝(R₁₂，t₁₂，s₁₂)为关键帧1与关键帧2之间的7自由度相对位姿变换矩阵，P_2，j代表在关键帧2中匹配点j的三维坐标，π为投影函数，公式如下：

其中，f_u与f_v代表焦距，c_u与c_v代表主点，且x₁、y₁、z₁满足如下关系：

[x₁ y₁ z₁]^T＝s_1，2R_1，2P_2，j+t_1，2

且e_curr2的计算公式如下：

其中，x_curr2，j为匹配点j的像素坐标，P_1，i代表在关键帧1中匹配点i的三维坐标；并同理计算得到在子地图中两关键帧的重投影误差e_sub1和e_sub2，以及在重叠区域中两关键帧的重投影误差e_over1和e_over2；

根据对最小化能量方程的求解，完成全局BA优化，从而得到当前环境的全局地图。

本申请提供的基于视觉先验信息和地图恢复的SLAM方法，先加载先验地图，通过ORB特征匹配SLAM系统当前帧与先验地图关键帧，并结合重定位方法完成SLAM系统的初始化；接着为了避免丢失地图，建立应对SLAM系统跟踪失败的地图保存机制，保存跟踪成功地图，并提出自适应快速重新初始化算法，引入灭点检测，自动选择最佳重新初始化策略，保证SLAM系统继续跟踪与建图，建立的地图称为恢复地图；最后对于跟踪成功地图与恢复地图，采用改进的回环方法获得它们之间的转换关系，并提出地图恢复法，减少跟踪成功地图与恢复地图尺度不一带来的误差，确保得到的全局地图更加准确。

附图说明

图1为本申请的基于视觉先验信息和地图恢复的SLAM方法的流程图；

图2为本申请当前地图与子地图之间的坐标系转换示意图；

图3为单目视觉SLAM尺度漂移的一种实施例示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

在一实施例中，提供一种基于视觉先验信息和地图恢复的SLAM方法，该SLAM方法能够固定SLAM坐标系，拥有获得更加完整的地图的能力。

本实施例中的基于视觉先验信息和地图恢复的SLAM方法实施在SLAM系统，如图1所示，包括以下步骤：

步骤S1、以预设格式保存针对当前环境构建的SLAM地图，作为先验地图。

在运行SLAM系统时，经常由于某些原因不得不终止SLAM系统继续建图，此时需要将当前已经构建的SLAM地图作为先验地图保存下来，以便于后续建立同坐标系的SLAM地图。

在一实施例中，设置预设格式为：

(MapPoints，KeyFrames) (1)

公式(1)中，MapPoints为地图点集合，KeyFrames为关键帧集合，且每一个地图点的格式为：

(pid，x_map，y_map，z_map) (2)

公式(2)中，pid为地图点的标识，x_map，y_map，z_map为地图点的坐标；

每一个关键帧的格式为：

(fid，time，quat，features，parentid，conid_ws) (3)

公式(3)中，fid为关键帧的标识，time为时间戳，quat为关键帧的位姿四元数，parentid为关键帧的父节点的标识，conid_ws为集合，conid_ws中的每一个元素表示与该关键帧相连的关键帧的标识以及这两个关键帧之间的权重，features为集合，features存储了该关键帧观测到的所有特征点，其中每一个特征点的记录格式为：

(x_key，y_key，size，angle，response，octave，descriptor) (4)

公式(4)中，x_key，y_key为特征点的坐标，size为特征点的邻域直径，angle为特征点的方向，response为特征点的响应强度，代表了该点是特征点的程度，octave表示该特征点所在的图像金字塔的层级，descriptor表示该特征点的描述子。

步骤S2、加载所述先验地图，使用重定位初始化算法完成初始化，固定SLAM坐标系后进行跟踪和建图。

当SLAM系统因不可抗力终止建图后一段时间，开始下一次运行时，先按照公式(1)、(2)、(3)和(4)的格式加载先验地图到SLAM系统的内存中，并重新初始化。

具体的，重定位初始化算法，包括：

SLAM系统对获取到的当前图像帧提取ORB特征，采用BOW模型(Bag of Words模型)确定当前图像帧与先验地图中的先验图像帧之间的转换矩阵；

根据确定的转换矩阵，采用PnP算法计算当前图像帧在先验地图中的位置，从而建立与先验地图中的坐标系一致的SLAM坐标系，完成初始化。

初始化后，SLAM系统即以先验地图的坐标系为基准固定SLAM坐标系，以后每一次运行时的坐标系均与先验地图一致。

步骤S3、当跟踪失败时，自动保存已构建的SLAM地图，作为跟踪成功地图，并采用自适应重新初始化方法完成重新初始化，继续进行跟踪与建图，并将重新初始化后构建的SLAM地图作为恢复地图。

由于SLAM系统容易因为弱纹理、快速运动形成的运动模糊等情况造成跟踪失败，为了保证在跟踪失败和重新初始化成功之间不丢失地图信息，本实施例在SLAM系统跟踪失败时，立即按照公式(1)、(2)、(3)和(4)的格式自动保存当前以构建的SLAM地图到内存，得到跟踪成功地图，并在保存跟踪成功地图后立即进行重新初始化操作。

在一实施例中，为了加快SLAM系统重新初始化的速度，引入灭点检测，自动选择最佳的重新初始化策略，以保证SLAM系统的继续跟踪和建图。

具体的，自适应重新初始化方法，包括：

若在当前环境的场景照片中检测到灭点，即采用《Fast initialization forfeature-based monocular slam》提出的快速初始化方法，使SLAM系统快速进入工作状态。由于人工场景大多可以找到灭点，因此快速初始化方法多数适应于SLAM系统在人工环境下工作时使用。

采用快速初始化方法包括：

假设找到的灭点为v_i，提取到的特征点为p_j，且特征点p_j的深度值D_j为：

公式(5)中，i∈[1，a]，a为场景照片中找到的灭点数量，j∈[1，b]，b为场景照片中找到的特征点数量，p_j.x为特征点p_j在x轴上的坐标值，p_j.y为特征点p_j在y轴上的坐标值，v_i.x为灭点v_i在x轴上的坐标值，v_i.y为灭点v_i在y轴上的坐标值。

计算所有特征点的深度值，并从所有特征点的深度值中选择最大与最小值，分别记为D_max与D_min；

对每一特征点的深度值进行归一化处理，归一化深度值区间为[0.5，1.5]，得到特征点p_j的归一化深度值p_{j_w_z}为：

p_{j_w_z}＝0.5+D_j/(D_max-D_min) (6)

根据公式(6)得到的特征点p_j的深度信息，恢复特征点p_j的三维坐标为：

其中，x_j，y_j为特征点p_j在图像上的坐标，c_x、c_y、f_x、f_y为相机的内参，则特征点p_j的三维坐标为(X，Y，p_{j_w_z})；

计算每一特征点的三维坐标，完成SLAM三维地图的构建，即完成初始化。

若在当前环境的场景照片中未检测到灭点，即采用简易初始化方法。目前较流行的基于特征检测的SLAM系统初始化要求较高。以ORB-SLAM2为例，对获取的每一帧图像提取ORB特征，根据匹配点对计算两帧图像之间的单应矩阵Hcr与本质矩阵Fcr，通过矩阵分解和三角化获得初始地图。这种初始化方式可以得到较准确的地图点，但耗费时间和资源。本实施例为保证得到准确的地图点，对ORB-SLAM2进行改进，让SLAM系统能够更快进入工作状态。

本实施例的简易初始化方法包括：

对当前获取到的每一帧图像提取ORB特征，根据ORB特征计算两帧图像之间的单应矩阵Hcr，通过分解单应矩阵Hcr和三角化方法获得点云地图，若点云地图中点数量大于l，则将该点云地图作为初始地图，完成重新初始化。本实施例中设置l为40。当前获取到的每一帧即为SLAM系统在当前时刻下最新获取到的图像。

简易初始化方法能够得到较准确的地图点，且初始化成功的概率高。本实施例的自适应重新初始化保留了简易初始化方法，并在对简易初始化方法改进的基础上，加入了灭点法，使得SLAM系统根据当前环境有针对性的进行重新初始化，以达到最佳的重新初始化效果。其中灭点法可参考Zhang S B，Liu S，Zhang J H，Wang Z H and Wang X Y.Fastinitialization for feature-based monocular slam[C]//2017 IEEE InternationalConference on Image Processing(ICIP).Beijing，China：IEEE.2017：2119-2123.[DOI：10.1109/ICIP.2017.8296656]的文章中提出的灭点法。

步骤S4、采用回环检测算法获得跟踪成功地图与恢复地图之间的转换关系。

所述回环检测算法，包括：

使用DBoW2库寻找当前地图的关键帧与子地图的关键帧之间的匹配项，并存入匹配集H＝{h₁，...，h_n}中，所述当前地图为恢复地图，所述子地图为先验地图或追踪成功地图，所述匹配集H中的h_q，q∈[1，n]表示DBoW2库找到的第q个匹配项，且

公式(7)中，表示第q个匹配项中当前地图的关键帧，表示第q个匹配项中子地图的关键帧；

匹配集H中的两匹配项之间存在时序关系，即两匹配项之间满足以下关系：

且

公式(8)、(9)中，q1，q2∈[1，n]，且q2＝1+q1，即两匹配项h_q1和h_q2为相邻匹配项，加入时序关系可以减少假阳性的可能性，降低回环的错误率。

若匹配集H中的匹配项|H|≥m，则认定当前地图与子地图之间出现重叠区域，停止匹配项的寻找，确定H＝{h₁，...，h_m}为最终的匹配集H，本实施例中设置m＝4。

最终的匹配集H可用于计算当前地图与子地图的相对位姿，即坐标转换矩阵，如图2所示，计算过程如下：

从子地图(Map_sub)中取一帧关键帧K_i，根据最终的匹配集H，采用Hom算法计算当前地图(Map_curr)的坐标系到关键帧K_i的坐标系之间的转换矩阵T_curr2i，并根据转换矩阵T_curr2i得到从当前地图到子地图的坐标转换矩阵T_curr2sub为：

T_curr2sub＝T_curr2i*T_i2sub (10)

公式(10)中，T_i2sub为已知量，T_i2sub表示关键帧K_i的坐标系到子地图的坐标系之间的转换矩阵，且T_curr2sub的具体形式为：

T_curr2sub＝(R_curr2sub，t_curr2sub，s_curr2sub) (11)

公式(11)中，R_curr2sub和t_curr2sub代表当前地图到子地图的旋转变换矩阵与平移变换矩阵，s_curr2sub代表当前地图与子地图之间的尺度关系。

步骤S5、根据所述转换关系，采用地图恢复法拼接跟踪成功地图与恢复地图，得到当前环境的全局地图。

如图3所示，在图像的拼接过程中基本会存在尺度漂移的问题，当场景比较大的时候，尺度上的变化也会比较明显，尤其体现在地图最开始部分与地图结束部分。图3(左)为子地图，图3(右)为当前地图，图中方框所示位置为两段地图的重叠部分，圆圈所示位置为地图的开始与结束部分，由于单目SLAM尺度漂移，两个绿色圈内的地图尺度不同，因此如果对当前地图进行统一的尺度变换显然是不合理的。

为了解决上述尺度漂移的问题，本实施例提出一种地图恢复法来减小尺度漂移引起的误差，该地图恢复法有三个步骤，分别为地图对齐，数据融合，尺度恢复与优化。

具体的，地图恢复法，包括：

地图对齐：取当前地图到子地图的坐标转换矩阵T_curr2sub，采用去除尺度关系的坐标转换矩阵T′_curr2sub＝(R_curr2sub，t_curr2sub)，将跟踪成功地图与恢复地图的坐标系对齐，保持跟踪成功地图与恢复地图的坐标系一致。地图对齐时不考虑关键帧位姿的尺度信息，在两端地图对齐之后，两段地图的坐标系保持一致，但此时两端地图仍然是独立的。

数据融合：根据最终的匹配集H关联跟踪成功地图与恢复地图之间重叠区域的关键帧，并利用坐标转换矩阵T_curr2sub合并重叠区域内的特征点，得到跟踪成功地图与恢复地图数据融合后的拼接地图。

其中，特征点合并的标准为：将当前地图中的点投影到子地图中，在投影点附近搜索和该点海明距离最小的点，若海明距离小于预先设定的阈值，则认为这两个点是同一个点，并将其合并。需要说明的是，在投影环节使用的转换矩阵都是带尺度的。

尺度恢复与优化：利用求解能量方程的最小化问题恢复拼接地图的尺度，建立的能量方程包括三部分，第一部分为当前地图的位姿图E(ξ^curr)，第二部分为子地图的位姿图E(ξ^sub)，第三部分为当前地图和子地图之间的重叠区域的位姿图E(ξ^over)，能量方程具体如下：

公式(12)中，ξ^curr为关键帧在当前地图中优化前的位姿，ξ^sub为关键帧在子地图中优化前的位姿，ξ^over为关键帧在当前地图与子地图重叠区域中优化前的位姿，ξ为关键帧优化后的融合位姿，E(ξ)为拼接地图优化后的位姿图，为当前地图中的关键帧，为子地图中的关键帧，为当前地图与子地图重叠区域中的关键帧，Λ为信息矩阵；

其中，关键帧与之间的位姿误差为：

关键帧与之间的位姿误差为：

关键帧与之间的位姿误差为：

其中，log_Sim(3)函数表示将李群转换成李代数，为当前地图中的关键帧，为子地图中的关键帧，为当前地图与子地图重叠区域中的关键帧；

根据公式(12)得到拼接地图优化后的位姿图E(ξ)，使用重投影进行全局BA优化，建立最小化能量方程为：

公式(16)中，ρ_h为Huber函数，Ω为协方差矩阵，n为关键帧数量，X^curr为当前地图的地图点；

取地图中任意前后关联的两关键帧设为关键帧1和关键帧2，且e_curr1代表在当前地图中关键帧1的重投影误差，计算公式为：

e_curr1＝x_curr1，i-π(S₁₂，P_2，j) (17)

公式(17)中，i，j为关键帧1与关键帧2之间的匹配点，x_curr1，i为匹配点i的像素坐标，S₁₂＝(R₁₂，t₁₂，s₁₂)为关键帧1与关键帧2之间的7自由度相对位姿变换矩阵，P_2，j代表在关键帧2中匹配点j的三维坐标，π为投影函数，公式如下：

公式(18)中，f_u与f_v代表焦距，c_u与c_v代表主点，且x₁、y₁、z₁满足如下关系：

[x₁ y₁ z₁]^T＝s_1，2R_1，2P_2，j+t_1，2 (19)

同理，e_curr2的计算公式如下

其中，x_curr2，j为匹配点j的像素坐标，P_1，i代表在关键帧1中匹配点i的三维坐标；并同理计算得到在子地图中两关键帧的重投影误差e_sub1和e_sub2，以及在重叠区域中两关键帧的重投影误差e_over1和e_over2。其中对关键帧1和关键帧2在各地图中求解重投影误差的方法为现有技术，可参考ORB_SLAM论文，在此不再进行赘述。

根据对最小化能量方程的求解，完成全局BA优化，从而得到当前环境的全局地图。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (6)

1.一种基于视觉先验信息和地图恢复的SLAM方法，其特征在于，所述的基于视觉先验信息和地图恢复的SLAM方法实施在SLAM系统，包括以下步骤：

步骤S1、以预设格式保存针对当前环境构建的SLAM地图，作为先验地图；

步骤S2、加载所述先验地图，使用重定位初始化算法完成初始化，固定SLAM坐标系后进行跟踪和建图；

步骤S3、当跟踪失败时，自动保存已构建的SLAM地图，作为跟踪成功地图，并采用自适应重新初始化方法完成重新初始化，继续进行跟踪与建图，并将重新初始化后构建的SLAM地图作为恢复地图；

步骤S4、采用回环检测算法获得跟踪成功地图与恢复地图之间的转换关系；

步骤S5、根据所述转换关系，采用地图恢复法拼接跟踪成功地图与恢复地图，得到当前环境的全局地图。

2.如权利要求1所述的基于视觉先验信息和地图恢复的SLAM方法，其特征在于，所述预设格式为：

(MapPoints，KeyFrames)

其中，MapPoints为地图点集合，KeyFrames为关键帧集合，且每一个地图点的格式为：

(pid，x_map，y_map，Z_map)

其中，pid为地图点的标识，x_map，y_map，Z_map为地图点的坐标；

每一个关键帧的格式为：

(fid，time，quat，features，parentid，conid_ws)

其中，fid为关键帧的标识，time为时间戳，quat为关键帧的位姿四元数，parentid为关键帧的父节点的标识，conid_ws为集合，conid_ws中的每一个元素表示与该关键帧相连的关键帧的标识以及这两个关键帧之间的权重，features为集合，features存储了该关键帧观测到的所有特征点，其中每一个特征点的记录格式为：

(x_key，y_key，size，angle，response，octave，descriptor)

其中，x_key，y_key为特征点的坐标，size为特征点的邻域直径，angle为特征点的方向，response为特征点的响应强度，代表了该点是特征点的程度，octave表示该特征点所在的图像金字塔的层级，descriptor表示该特征点的描述子。

3.如权利要求1所述的基于视觉先验信息和地图恢复的SLAM方法，其特征在于，所述重定位初始化算法，包括：

SLAM系统对获取到的当前图像帧提取ORB特征，采用BOW模型确定当前图像帧与先验地图中的先验图像帧之间的转换矩阵；

根据确定的转换矩阵，采用PnP算法计算当前图像帧在先验地图中的位置，从而建立与先验地图中的坐标系一致的SLAM坐标系，完成初始化。

4.如权利要求1所述的基于视觉先验信息和地图恢复的SLAM方法，其特征在于，所述自适应重新初始化方法，包括：

若在当前环境的场景照片中检测到灭点，即采用快速初始化方法，包括：

假设找到的灭点为v_i，提取到的特征点为p_j，且特征点p_j的深度值D_j为：

其中，i∈[1，a]，a为场景照片中找到的灭点数量，j∈[1，b]，b为场景照片中找到的特征点数量，p_j.x为特征点p_j在x轴上的坐标值，p_j.y为特征点p_j在y轴上的坐标值，v_i.x为灭点v_i在x轴上的坐标值，v_i.y为灭点v_i在y轴上的坐标值；

计算所有特征点的深度值，并从所有特征点的深度值中选择最大与最小值，分别记为D_max与D_min；

对每一特征点的深度值进行归一化处理，归一化深度值区间为[0.5，1.5]，得到特征点p_j的归一化深度值p_{j_w_z}为：

p_{j_w_z}＝0.5+D_j/(D_max-D_min)

根据得到的特征点p_j的深度信息，恢复特征点p_j的三维坐标为：

其中，x_j，y_j为特征点p_j在图像上的坐标，c_x、c_y、f_x、f_y为相机的内参，则特征点p_j的三维坐标为(X，Y，p_{j_w_z})；

计算每一特征点的三维坐标，完成SLAM三维地图的构建，即完成初始化；

若在当前环境的场景照片中未检测到灭点，即采用简易初始化方法，包括：

对当前获取到的每一帧图像提取ORB特征，根据ORB特征计算两帧图像之间的单应矩阵Hcr，通过分解单应矩阵Hcr和三角化方法获得点云地图，若点云地图中点数量大于l，则将该点云地图作为初始地图，完成重新初始化。

5.如权利要求1所述的基于视觉先验信息和地图恢复的SLAM方法，其特征在于，所述回环检测算法，包括：

使用DBoW2库寻找当前地图的关键帧与子地图的关键帧之间的匹配项，并存入匹配集H＝{h₁，...，h_n}中，所述当前地图为恢复地图，所述子地图为先验地图或跟踪成功地图，所述匹配集H中的h_q，q∈[1，n]表示DBoW2库找到的第q个匹配项，且

其中，表示第q个匹配项中当前地图的关键帧，表示第q个匹配项中子地图的关键帧；

匹配集H中的两匹配项之间存在时序关系，即两匹配项之间满足以下关系：

且

其中，q1，q2∈[1，n]，且q2＝1+q1，即两匹配项h_q1和h_q2为相邻匹配项；

若匹配集H中的匹配项|H|≥m，则认定当前地图与子地图之间出现重叠区域，停止匹配项的寻找，确定H＝{h₁，...，h_m}为最终的匹配集H；

从子地图中取一帧关键帧K_i，根据最终的匹配集H，采用Hom算法计算当前地图的坐标系到关键帧K_i的坐标系之间的转换矩阵T_curr2i，并根据转换矩阵T_curr2i得到从当前地图到子地图的坐标转换矩阵T_curr2sub为：

T_curr2sub＝T_curr2i*T_i2sub

其中，T_i2sub为已知量，T_i2sub表示关键帧K_i的坐标系到子地图的坐标系之间的转换矩阵，且T_curr2sub的具体形式为：

T_curr2sub＝(R_curr2sub，t_curr2sub，s_curr2sub)

其中，R_curr2sub和t_curr2sub代表当前地图到子地图的旋转变换矩阵与平移变换矩阵，s_curr2sub代表当前地图与子地图之间的尺度关系。

6.如权利要求5所述的基于视觉先验信息和地图恢复的SLAM方法，其特征在于，所述地图恢复法，包括：

取当前地图到子地图的坐标转换矩阵T_curr2sub，采用去除尺度关系的坐标转换矩阵T′_curr2sub＝(R_curr2sub，t_curr2sub)，将跟踪成功地图与恢复地图的坐标系对齐，保持跟踪成功地图与恢复地图的坐标系一致；

根据最终的匹配集H关联跟踪成功地图与恢复地图之间重叠区域的关键帧，并利用坐标转换矩阵T_curr2sub合并重叠区域内的特征点，得到跟踪成功地图与恢复地图数据融合后的拼接地图；

利用求解能量方程的最小化问题恢复拼接地图的尺度，建立的能量方程包括三部分，第一部分为当前地图的位姿图E(ξ^curr)，第二部分为子地图的位姿图E(ξ^sub)，第三部分为当前地图和子地图之间的重叠区域的位姿图E(ξ^over)，能量方程具体如下：

其中，ξ^curr为关键帧在当前地图中优化前的位姿，ξ^sub为关键帧在子地图中优化前的位姿，ξ^over为关键帧在当前地图与子地图重叠区域中优化前的位姿，ξ为关键帧优化后的融合位姿，E(ξ)为拼接地图优化后的位姿图，为当前地图中的关键帧，为子地图中的关键帧，为当前地图与子地图重叠区域中的关键帧，Λ为信息矩阵；

其中，关键帧与之间的位姿误差为：

关键帧与之间的位姿误差为：

关键帧与之间的位姿误差为：

其中，log_Sim(3)函数表示将李群转换成李代数，为当前地图中的关键帧，为子地图中的关键帧，为当前地图与子地图重叠区域中的关键帧；

得到拼接地图优化后的位姿图E(ξ)，使用重投影进行全局BA优化，建立最小化能量方程为：

其中，ρ_h为Huber函数，Ω为协方差矩阵，n为关键帧的数量，X^curr为当前地图的地图点；

取地图中任意前后关联的两关键帧设为关键帧1和关键帧2，且e_curr1为在当前地图中关键帧1的重投影误差，计算公式为：

e_curr1＝x_curr1，i-π(S₁₂，P_2，j)

其中，i，j为关键帧1与关键帧2之间的匹配点，x_curr1，i为匹配点i的像素坐标，S₁₂＝(R₁₂，t₁₂，s₁₂)为关键帧1与关键帧2之间的7自由度相对位姿变换矩阵，P_2，j代表在关键帧2中匹配点j的三维坐标，π为投影函数，公式如下：

其中，f_u与f_v代表焦距，c_u与c_v代表主点，且x₁、y₁、z₁满足如下关系：

[x₁ y₁ z₁]^T＝s_1，2R_1，2P_2，j+t_1，2

同理，e_curr2的计算公式如下：

其中，x_curr2，j为匹配点j的像素坐标，P_1，i代表在关键帧1中匹配点i的三维坐标；并同理计算得到在子地图中两关键帧的重投影误差e_sub1和e_sub2，以及在重叠区域中两关键帧的重投影误差e_over1和e_over2；

根据对最小化能量方程的求解，完成全局BA优化，从而得到当前环境的全局地图。