CN109460267B - 移动机器人离线地图保存与实时重定位方法 - Google Patents

Abstract

本发明公开了一种移动机器人离线地图保存与实时重定位方法，包括离线地图构建与保存，离线地图加载与重定位，其特征在于：采用所述系统离线地图构建与保存方法，当启动系统（ORB‑SLAM2）时，首先检测离线地图并执行加载，当离线地图成功加载后，系统进入跟踪丢失状态，从而触发重定位模型寻找相机位置，并能够随着相机运动继续进行全局跟踪定位。本发明具有能快速并完整地保存离线地图的数据，实现实时重定位和人机交互，且系统鲁棒性好的特点。

Description

移动机器人离线地图保存与实时重定位方法

技术领域

本发明涉及移动机器人领域，具体涉及一种移动机器人离线地图保存与实时重定位方法。

背景技术

随着科技的进步与人们生活水平的提高，智能化产品开始受到大众的关注并逐渐应用到社会的各个领域，促使人工智能技术进入高速发展的时代。机器人是人工智能领域最为重要的科研方向，如何使机器人更接近人类的思维和行为方式是当前机器人技术的核心问题。同步定位与地图构建(Simultaneous Localization and Mapping，简称SLAM)技术帮助机器人在未知环境下进行实时定位并构建环境地图，是移动机器人实现自主定位与导航的基础。视觉SLAM根据传感器的不同主要分为单目、双目和RGB-D SLAM三类。Mur-Artal等人在2015年提出ORB-SLAM算法，作者基于PTAM框架，改进了大部分组件，使用ORB特征代替FAST特征，增加闭环线程消除累计误差，实现更加精确的定位和地图构建，但ORB-SLAM没有解决单目SLAM的尺度模糊以及纯旋转问题，所构建的地图仅包含稀疏地图点，利用价值低。

ORB-SLAM2在2017由RMur-Artal等人提出，是ORB-SLAM的改进版本，在单目的基础上，增加了对双目和RGB-D相机的支持，是完整的视觉SLAM方案。ORB-SLAM2的系统框架，主要包含三个并行线程:跟踪(Tracking)、局部建图(Local Mapping)、闭环(Loop Closing)。目前，在实际应用中，我们希望通过视觉SLAM系统构建地图完成后能够完整保存，并在再次启动时加载地图直接定位。而ORB-SLAM2并没有提供地图的保存和加载重定位的功能，导致了机器人每次启动都要重新“认识”一次周围的环境，这个过程不仅耗费大量时间，还占用了移动机器人有限的计算资源，使得ORB-SLAM2的实际应用价值大幅度降低。对于视觉SLAM系统本身来说，人机交互界面不是必须的。ORB-SLAM2提供了运行过程中的可视化界面，但是当SLAM系统搭载至移动机器人时，用户不会实时关注它在建图时的状态，而希望在运行结束后可以查看地图和轨迹等信息，而ORB-SLAM2并未提供离线可视化的功能，基于机器人的人机交互性差。

发明内容

本发明的目的在于克服上述缺点而提出了一种能快速并完整地保存离线地图的数据，实现实时重定位和人机交互，且系统鲁棒性好的移动机器人离线地图保存与实时重定位方法。

本发明的一种移动机器人离线地图保存与实时重定位方法，包括离线地图构建与保存，离线地图加载与重定位，其中：采用所述系统离线地图构建与保存方法，当启动系统(ORB-SLAM2)时，首先检测离线地图并执行加载，当离线地图成功加载后，系统进入跟踪丢失状态，从而触发重定位模型寻找相机位置，并能够随着相机运动继续进行全局跟踪定位；

所述离线地图构建与保存方法的具体步骤如下：

(1)分别从地图(Map)与关键帧数据库(KeyFrameDatabase)的类对象读取地图与关键帧数据库内容；

(2)使用Boost::Serialization序列化库，在地图的各个元素与关键帧数据库类中定义序列化模板函数；

(3)创建二进制写入型文件，将读取的地图与关键帧数据库内容序列化保存至文件。

上述移动机器人离线地图保存与实时重定位方法，其中：所述重定位模型的重定位方法具体步骤如下：

(1)由公式计算获得当前帧的Bow向量；

(2)从关键帧数据库中筛选候选帧：候选关键帧是重定位模型的初始化数据，是影响重定位性能的重要因素，候选关键帧的筛选在ORB-SLAM2中的具体流程如下：

a)选取所有与当前帧具有共同单词的关键帧；

b)统计与当前帧共有单词数最多的单词数量nMaxCoWords；

c)以0.8*nMaxCoWords为阈值，剔除共有单词数量少于此阈值的候选关键帧；

d)寻找每一个候选关键帧相连的共视关系最好的10个关键帧并以此为一组，计算每一组的相似度累计得分并返回其组内得分最高的关键帧；

e)剔除得分低于0.75倍最高得分的关键帧；

(3)特征匹配：在当前帧与候选关键帧的同一个单词节点下，进行特征匹配，剔除匹配点数小于15的候选帧；

(4)估计当前帧位姿：利用匹配的特征点，构建3D-2D的PnP问题，然后利用RANSAC迭代求解，当有足够的内点时，判断重定位成功。

上述移动机器人离线地图保存与实时重定位方法，其中：所述包括离线地图构建与保存，离线地图加载与重定位外，还包括离线地图以及轨迹可视化模块，以方便用户查看建图效果和机器人运行轨迹，具体步骤如下：

(1)利用所述离线地图构建与保存方法加载地图；

(2)地图点和关键帧是最能够直观反映地图效果和建图轨迹的数据，因此调用Pangolin库创建可视化窗口，显示地图点和关键帧；

(3)使用Pangolin库单独可视化机器人轨迹，输入数据可以是ORB-SLAM2运行结束后保存的相机位姿文件或者关键帧文件。

本发明与现有技术的相比，具有明显的有益效果，由以上方案可知，所述离线地图构建与保存方法，使用了序列化的方法执行保存操作，序列化具有对第三方类的非侵入性特点，并能够保证数据以相同的顺序结构完全恢复，这保证了系统正确恢复地图与关键帧数据库的结构；使用二进制格式的文件存储离线地图，能够快速并且完整地保存离线地图的数据。即该离线地图构建与保存方法具有以下特点：当系统再次启动时，加载离线地图后能够恢复到前一次运行结束前的数据状态并根据地图进行全局定位跟踪。

所述离线地图加载与重定位的方法，基于已保存的离线地图，离线地图保存了完整的有效数据，且使用序列化方法保证了数据结构的正确恢复，所以当离线地图的数据加载完成，并通过关键帧中的特征点描述子计算了Bow向量后，系统的数据就已经恢复到了前一次运行结束时的状态，这时使用重定位模型恢复相机位置，相当于系统在运行时跟踪丢失后进行重定位的过程。所以，基于已保存的离线地图，该离线地图加载与重定位的方法将系统相邻两次的运行通过二次拟合机制连接起，且重定位模块能够让系统重新定位相机位置并继续跟踪，对系统的鲁棒性有非常大的提升。

总之，本发明的移动机器人离线地图保存与实时重定位方法，具有能快速并完整地保存离线地图的数据，实现实时重定位和人机交互，且系统鲁棒性好的特点。

以下通过具体实施方式，进一步说明本发明的有益效果。

附图说明

图1为本发明的离线地图构建与保存方法流程图；

图2为本发明的地图重用方法示意图；

图3为实施例中的实验测试环境概况图；

图4为实施例中的实际环境的点云地图和机器人移动轨迹图；

图5为实施例中的地图加载与重定位效果图；

图6为现有技术中的P3P问题示意图；

图7为现有技术中的局部BA示意图。

具体实施方式

以下结合附图及较佳实施例，对依据本发明提出的一种移动机器人离线地图保存与实时重定位方法的具体实施方式、特征及其功效，详细说明如后。

本发明的一种移动机器人离线地图保存与实时重定位方法，包括离线地图构建与保存，离线地图加载与重定位，其中：采用所述系统离线地图构建与保存方法，当启动系统(ORB-SLAM2)时，首先检测离线地图并执行加载，当离线地图成功加载后，系统进入跟踪丢失状态，从而触发重定位模型寻找相机位置，并能够随着相机运动继续进行全局跟踪定位；

所述离线地图构建与保存方法的具体步骤如下(如图1)：

(1)分别从地图(Map)与关键帧数据库(KeyFrameDatabase)的类对象读取地图与关键帧数据库内容；

(2)使用Boost::Serialization序列化库，在地图的各个元素与关键帧数据库类中定义序列化模板函数；

(3)创建二进制写入型文件，将读取的地图与关键帧数据库内容序列化保存至文件。

所述重定位模型的重定位方法具体步骤如下：

(1)由公式计算获得当前帧的Bow向量；

(2)从关键帧数据库中筛选候选帧：候选关键帧是重定位模型的初始化数据，是影响重定位性能的重要因素，候选关键帧的筛选在ORB-SLAM2中的具体流程如下：

a)选取所有与当前帧具有共同单词的关键帧；

b)统计与当前帧共有单词数最多的单词数量nMaxCoWords；

c)以0.8*nMaxCoWords为阈值，剔除共有单词数量少于此阈值的候选关键帧；

d)寻找每一个候选关键帧相连的共视关系最好的10个关键帧并以此为一组，计算每一组的相似度累计得分并返回其组内得分最高的关键帧；

e)剔除得分低于0.75倍最高得分的关键帧；

(3)特征匹配：在当前帧与候选关键帧的同一个单词节点下，进行特征匹配，剔除匹配点数小于15的候选帧；

(4)估计当前帧位姿：利用匹配的特征点，构建3D-2D的PnP问题，然后利用RANSAC迭代求解，当有足够的内点时，判断重定位成功。

所述包括离线地图构建与保存，离线地图加载与重定位外，还包括离线地图以及轨迹可视化模块，以方便用户查看建图效果和机器人运行轨迹，具体步骤如下：

(1)利用所述离线地图构建与保存方法加载地图；

(2)地图点和关键帧是最能够直观反映地图效果和建图轨迹的数据，因此调用Pangolin库创建可视化窗口，显示地图点和关键帧；

(3)使用Pangolin库单独可视化机器人轨迹，输入数据可以是ORB-SLAM2运行结束后保存的相机位姿文件或者关键帧文件。

实施例如下：

1.ORB-SLAM2

ORB-SLAM2在2017由RMur-Artal等人提出，是ORB-SLAM的改进版本，在单目的基础上，增加了对双目和RGB-D相机的支持，是完整的视觉SLAM方案。ORB-SLAM2的系统框架主要包含三个并行线程:跟踪(Tracking)、局部建图(Local Mapping)、闭环(Loop Closing)。

1.1跟踪

跟踪线程的主要任务是对输入的每一帧图像提取ORB特征并估计相机位姿。由于受到环境变化或相机剧烈运动等因素的影响，跟踪状态会随时变化，为保证系统的鲁棒性，跟踪线程会根据不同的情况切换三种跟踪模型:运动模型、参考帧模型、重定位模型，每一种跟踪模型的输入数据不尽相同，但目标都是求解初始相机位姿。PnP(Perspective-n-Point)是ORB-SLAM2中主要的位姿估计算法，其存在多种求解方法，例如P3P，直接线性变换(DLT)，EPnP(Efficient PnP)，非线性优化等。

由于P3P方法的思想被其他多种改进后的方法所沿用，所以我们以P3P为例介绍其求解过程。P3P需要输入3对3D-2D匹配点，如图6所示，A,B,C为世界坐标系下的三个空间点，a,b,c为三个空间点对应的在图像平面的投影点，相机光心为O，此外，还需要一组点对用来确定P3P的最优解。

根据三角形的相似性质，利用余弦定理，有方程组：

OA²+OB²-2OA·OB·cos<OA,OB>＝AB²

OB²+OC²-2OB·OC·cos<OB,OC>＝BC² (1)

OA²+OC²-2OA·OC·cos<OA,OC>＝AC²

令x＝OA/OC，y＝OB/OC，可得：

x²+y²-2xycos<OA,OB>＝AB²/OC²

y²+1-2ycos<OB,OC>＝BC²/OC² (2)

x²+1-2xcos<OA,OC>＝AC²/OC²

由公式(2)可求解出A,B,C三点在相机坐标系下的坐标，然后根据3D-3D的点对，通过ICP计算相机位姿R,t。经过PnP得到的位姿带有一定的误差，线程利用局部地图跟踪模块将更新后的局部地图点投影到当前帧，运行过滤函数把不符合匹配标准的点删除，最后通过非线性优化得到更为精确的位姿结果。此外，跟踪线程将根据制定的策略决定是否生成并插入新的关键帧。

1.2局部建图

局部建图线程主要负责接收处理新的关键帧，增加新的地图点，维护局部地图的精度和关键帧集合的质量与规模。具体步骤如下：

(1).处理新的关键帧。计算当前关键帧的Bow向量；更新当前关键帧的地图点观测值，并将这些地图点添加到mlpRecentAddedMapPoints()列表中；更新共视图和本质图，将当前关键帧加入到地图中。

(2).地图点的检查筛选。对最近添加地图点列表进行检查，把不符合标准的冗余点剔除。剔除地图点的规则主要有：a).该地图点被标记为坏点；b).能够观测到该地图点的关键帧不超过25％；c).观察到该点的关键帧不超过3个(单目情况下为2个)。当地图点通过筛选规则被正式创建后，只有当少于3个关键帧能够观察到该点时它才会被剔除。

(3).根据当前关键帧恢复新的地图点。从共视图中选取当前关键帧附近的关键帧，对当前关键帧和选出的关键帧进行特征匹配并构建对极约束，通过对极约束求解R,t；然后通过三角化恢复特征点点深度，对于已知匹配点和当前关键帧的位姿，有：

s₁p₁＝s₂Rp₂+t (3)

其中，s₁,s₂为待求深度值，p₁,p₂为两个匹配特征点的归一化坐标，经过公式(4)，(3)依次可以解出s₂,s₁。计算恢复出的新地图点的重投影误差，大于阈值的点被剔除；最后检查尺度一致性。

(4).局部BA。在局部建图线程中进行局部BA优化，优化框架如图7所示，假设当前帧为图中Pose1，则优化对象为Pose1，参与优化的有：a).所有在共视图中与Pose1相连的关键帧Pose2；b).被这些局部关键帧观察到的地图点X1,X2；c).观察到局部地图点但不与当前关键帧相连的关键帧Pose3，需要注意的是，此类关键帧作为优化约束，但不作为变量，为固定关键帧。局部优化不同于全局优化，它首先进行5次迭代优化，然后检查并删除外点，再继续进行10次迭代优化并剔除外点。

(5).局部关键帧筛选。为了控制重建密度和BA优化的复杂度，局部建图线程会检测冗余关键帧并剔除。ORB-SLAM2中对冗余关键帧的判定标准为：该关键帧能狗看到的90％的地图点能够被其他至少3个关键帧观察到，则该关键帧被认为是冗余关键帧，需要剔除。

1.3闭环

闭环的工作分为闭环检测和后端优化两部分完成。闭环检测部分负责筛选并确认闭环，首先通过计算当前关键帧与相连关键帧的Bow分值，并以最低分为阈值选取闭环候选帧，然后统计共有单词数量和聚类得分，剔除质量不高或鲁棒性差的独立关键帧，并对留存的候选关键帧进行连续性检测；检测到闭环后通过RANSAC框架求解相似变换Sim3，然后进行再匹配和g2o优化Sim3，校正当前关键帧的位姿。

后端优化部分负责消除全局的累计误差，首先利用传播法调整与当前关键帧相连的关键帧的位姿，并更新对应的地图点，将更新后的地图点投影到相应的关键帧，融合匹配的地图点，最后根据调整的地图点更新关键帧的链接关系；在完成地图融合之后，通过本质图进行位姿图优化。位姿图优化以相机位姿为顶点，以相对运动为边，目标函数为：

其中，x为优化变量相机位姿，ΔT_i,j为位姿变换，Ω为边的信息矩阵。这是一个最小二乘优化问题，利用g2o进行求解。

1.4应用缺陷

ORB-SLAM2是当前性能最为出色的实时视觉SLAM系统之一，但在实用性方面仍有诸多不足，尤其是当运行平台为具有多任务、计算能力有限、人机互动要求高以及成本控制严格等特点的移动机器人时，ORB-SLAM2的缺点显得尤为突出，主要总结为以下几点：

(1)无法保存地图并重用。在实际应用中，我们希望通过视觉SLAM系统构建地图完成后能够完整保存，并在再次启动时加载地图直接定位。而ORB-SLAM2并没有提供地图的保存和加载重定位的功能，导致了机器人每次启动都要重新“认识”一次周围的环境，这个过程不仅耗费大量时间，还占用了移动机器人有限的计算资源，使得ORB-SLAM2的实际应用价值大幅度降低。

(2)基于机器人的人机交互性差。对于视觉SLAM系统本身来说，人机交互界面不是必须的。ORB-SLAM2提供了运行过程中的可视化界面，但是当SLAM系统搭载至移动机器人时，用户不会实时关注它在建图时的状态，而希望在运行结束后可以查看地图和轨迹等信息，而ORB-SLAM2并未提供离线可视化的功能。

2.离线地图构建与保存方法

离线地图的构建与保存的目标是：当系统再次启动时，加载离线地图后能够恢复到前一次运行结束前的数据状态并根据地图进行全局定位跟踪，通过第1节对ORB-SLAM2的三个主要线程的分析，可以得知系统运行过程中的有效累积数据主要包括地图和关键帧数据库两部分内容，本发明提出将它们共同组成新的离线地图。

ORB-SLAM2的地图主要包括3D地图点、关键帧、共视图、关键帧生长树、Bow向量等，其中，关键帧是一个复杂概念，它具体包含了当前帧的相机位姿、ORB特征点、三角化的地图点以及对应的ID编号等数据，Bow向量是由关键帧特征描述子计算获得的，不是原始数据，因此不需要进行保存。关键帧数据库是区别于地图中关键帧的独立的数据，主要用于位置识别，帮助系统进行相机重定位和闭环检测，保证对环境变化或相机快速运动的鲁棒性。ORB-SLAM2的代码结构清晰，地图与关键帧数据库分别定义于Map与KeyFrameDatabase，且地图点、关键帧等数据成员接口完善，综上所述，我们的离线地图构建与保存步骤如下：

(1).分别从地图(Map)与关键帧数据库(KeyFrameDatabase)的类对象读取地图与关键帧数据库内容。

(2).使用Boost::Serialization序列化库，在地图的各个元素与关键帧数据库类中定义序列化模板函数。

(3).创建二进制写入型文件，将读取的地图与关键帧数据库内容序列化保存至文件。

上述步骤与图1展示了我们的离线地图构建和保存方法流程图，在实现过程中，我们主要关注以下问题：首先，我们使用了序列化的方法执行保存操作，序列化具有对第三方类的非侵入性特点，并能够保证数据以相同的顺序结构完全恢复，这保证了系统正确恢复地图与关键帧数据库的结构。此外，由于本发明所提出的离线地图是一个抽象概念，不是指单纯的仅包含地图点的可视化地图，它的部分数据是不可见的，并且地图元素的部分关联关系是无法被可视化文件保存的，为了快速并且完整地保存离线地图的数据，我们使用二进制格式的文件存储离线地图。

3.基于二次拟合机制的离线地图加载与重定位方法

基于已保存的离线地图，本发明提出一种离线地图加载与重定位的方法，将系统相邻两次的运行通过二次拟合机制连接起来。方法设计如图2所示，当ORB-SLAM2启动时，系统首先检测离线地图并执行加载，基于2节的叙述，加载离线地图使用反序列化方法，如地图离线不存在或者加载未成功，则初始化所有线程并重新跟踪建图；我们设计一种新的重定位触发机制，当离线地图成功加载后，系统进入跟踪丢失状态，从而触发重定位模型寻找相机位置，并能够随着相机运动继续进行全局跟踪定位。

由于我们的离线地图保存了完整的有效数据，且使用序列化方法保证了数据结构的正确恢复，所以当离线地图的数据加载完成，并通过关键帧中的特征点描述子计算了Bow向量后，系统的数据就已经恢复到了前一次运行结束时的状态，这时使用重定位模型恢复相机位置，相当于系统在运行时跟踪丢失后进行重定位的过程。

由于特征点缺失或者剧烈运动等因素，跟踪丢失在无论哪种视觉SLAM方法中都是常见的情形，重定位模块能够让系统重新定位相机位置并继续跟踪，对系统的鲁棒性有非常大的提升，重定位方法的步骤如下：

(1).由公式(7)，(8)计算获得当前帧的Bow向量。

(2).从关键帧数据库中筛选候选帧。候选关键帧是重定位模型的初始化数据，是影响重定位性能的重要因素，所以候选关键帧的筛选在ORB-SLAM2中是十分严格的，流程如下：

a).选取所有与当前帧具有共同单词的关键帧；

b).统计与当前帧共有单词数最多的单词数量nMaxCoWords；

c).以0.8*nMaxCoWords为阈值，剔除共有单词数量少于此阈值的候选关键帧；

d).寻找每一个候选关键帧相连的共视关系最好的10个关键帧并以此为一组，计算每一组的相似度累计得分并返回其组内得分最高的关键帧；

e).剔除得分低于0.75倍最高得分的关键帧。

(3).特征匹配。在当前帧与候选关键帧的同一个单词节点下，进行特征匹配，剔除匹配点数小于15的候选帧。

(4).估计当前帧位姿。利用匹配的特征点，构建3D-2D的PnP问题，然后利用RANSAC迭代求解，当有足够的内点时，判断重定位成功。

ORB-SLAM2提供的字典是通过数据库训练得来，在训练字典开始之前，我们需要传入构建字典的必要参数，包括聚类中心数量K字典树聚类层数L，权重类型，相似度计算方式为保证一致性，采用与ORB-SLAM2所使用的字典一致的参数，其中Weighting为TF-IDF(TermFrequency-Inverse Document Frequency)，TF代表某个特征在一幅图像中的区分度，IDF代表某个单词在字典中的区分度，所以对于一幅图像A中的单词w_i，其权重可由公式(6)计算获得：

其中

是在图像A中单词w_i出现的次数，n_w是图像A中所有单词出现的总次数，nfeatures代表字典中所有特征的数量，

代表字典中单词w_i中的特征数量。

计算出每个单词权重之后，图像A的BoW向量可以表示为：

对任意两幅图像A和B的相似度计算设置为L₁范数形式，利用公式(8)的词袋模型，由公式(8)计算获得相似度得分：

4.离线地图与轨迹可视化方法

人机交互性是机器人必须具备的能力，为了达到这一标准，本发明提出为ORB-SLAM2增加离线地图以及轨迹可视化模块，方便用户可以查看建图效果和机器人运行轨迹，具体步骤如下：

(1).利用本发明在2节中提出的方法加载地图；

(2).地图点和关键帧是最能够直观反映地图效果和建图轨迹的数据，因此我们调用Pangolin库创建可视化窗口，显示地图点和关键帧；

(3).我们同样使用Pangolin库单独可视化机器人轨迹，输入数据可以是ORB-SLAM2运行结束后保存的相机位姿文件或者关键帧文件。

5.实验测试与结果分析

5.1实验平台

实验基于EAI-B1型移动机器人底盘搭建，上位机为华硕N551JM型号笔记本电脑，其中CPU为Intel Core i5-4210M，主频为2.8GHz，内存为8Gb，显卡型号为Nvidia GTX860M；上位机搭载的系统为Ubuntu 16.04，完全支持ORB-SLAM2的运行；上位机连接的视觉传感器为ORBBEC Astra深度相机，RGB图像分辨率为640*480，最大有效测量深度为5米，满足实验环境的测量需求。在进行实验之前，我们已经将根据文中方法改动后的ORB-SLAM2系统集成于上位机的Ubuntu系统，并已经调试完成；同时，深度相机已经进行了标定，系统运行时的相机参数文件已经根据标定参数进行了改动。

5.2实验设计

本发明中涉及的工作主要为地图保存与加载，主要目的分别为：保存地图使用户方便查看建图效果，并重载已有地图，使系统能够启动后立即自定位，增加实用性。根据以上分析，我们设计如下实验用于验证本发明提出的方法：

(1).地图保存与加载实验。本发明提出在系统运行结束时保存地图，并在再次启动时加载地图进行重定位，以避免系统每次启动都需要重新定位建图，增加了其实用性。我们将使用实验平台在实际环境中测试这一功能，并在离线状态下使用本发明提出的可视化模块查看建图效果。

5.3实验及结果分析

5.3.1地图保存与加载

为了能够验证本发明提出的方法的可行性，我们使用搭载了带有地图保存与加载模块的移动机器人平台在实际环境中进行测试。测试环境为典型的教学办公室，如图3所示，面积约50平方米，室内包括办公桌椅、电脑、黑板等物体，一面墙包含大面积玻璃窗，所以带有一定的特征点数量差异和光照变化，对视觉SLAM的效果会产生一定影响，但实际情况中移动机器人的运行环境通常是带有一定干扰的，所以我们的测试环境更具实际意义，也更加能够验证本发明方法的鲁棒性。

我们首先运行机器人沿室内空地部分的边缘行走一圈进行建图并保存，然后使用本发明提出的地图可视化模块查看建图的效果。如图4-a所示，可视化模块显示了地图点云和关键帧轨迹，点云图比较清晰地表达了测试环境的整体结构，将图中关键帧轨迹视为机器人运行轨迹，可以看到路线最终完成闭环，与实际运行结果基本相同。此外，为了验证存储数据的真实性，我们利用ORB-SLAM2自带的关键帧轨迹保存功能，读取关键帧文件并显示以作对比，如图4-b所示，可以发现与图4-a中的关键帧轨迹一致。综和上述结果和分析，可以判断本发明提出的地图保存与可视化方法是真实可靠的。

在环境构建和地图保存之后，我们进行地图加载与重定位的实验，首先运行机器人脱离在构建地图时的行进轨迹，并停留在比较容易在点云图中分辨位置的地方，这样做的目的是为了能够清晰地观察是否正确地重定位。然后我们启动SLAM系统，在可视化窗口中查看地图加载是否完整，并辨别是否成功定位。最后我们移动机器人到下一个位置，在此期间保持SLAM系统运行，观察运动路线和最终位置是否定位准确，进一步验证本发明提出方法的实用性。实验结果如图5所示，5-a为系统启动时刻的地图加载效果、重定位定位结果、当前帧的RGB图像；5-b为移动到下一个位置的相应图像。

当系统启动后，观察图5-a的地图加载效果，根据与图4中的地图对比以及右图的图像下方显示的关键帧和地图点数量等数据，可以得知地图加载成功且数据已经恢复，系统当前的运行连接了上一次运行结束时的状态；根据右侧RGB图像可以判断当前的机器人处于建图路线的拐角旁，在其左前方和正前方有办公桌椅，左侧地图中的淡灰色图像块即为机器人位置，根据地图的布局并与左侧图像对比，可以判断出移动机器人重定位成功，定位效果比较精确。图5-b为机器人直线向后移动一段距离后的地图与当前帧的图像，根据与5-a两幅图像的对比，5-b地图上代表机器人位置的淡灰色图像块正确地向后方移动了一段距离，与机器人实际移动保持一致。

上述实验表明本发明提出的地图保存与加载模块，以及地图加载并重定位的机制是有效的，在系统重新启动之后重定位成功，我们将机器人移动一段距离，机器人在地图上的位置时跟随移动的，定位始终保持实时跟随，这验证了本发明方法的鲁棒性和实用性。此外，当运行环境发生变化时，从ORB-SLAM2的实时可视化窗口点击按钮关闭重定位模式，系统将继续在原有地图基础上进行建图，更改地图的变化部分，在运行结束后会保存最新的地图，这进一步增强了本发明方法的鲁棒性和实用性。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (3)

1.一种移动机器人离线地图保存与实时重定位方法，包括离线地图构建与保存，离线地图加载与重定位，其特征在于：当启动系统ORB-SLAM2时，首先检测离线地图并执行加载，当离线地图成功加载后，系统进入跟踪丢失状态，从而触发重定位模型寻找相机位置，并能够随着相机运动继续进行全局跟踪定位；

所述离线地图构建与保存方法的具体步骤如下：

(1)分别从地图Map与关键帧数据库KeyFrameDatabase的类对象读取地图与关键帧数据库内容；

(2)使用Boost::Serialization序列化库，在地图的各个元素与关键帧数据库类中定义序列化模板函数；

(3)创建二进制写入型文件，将读取的地图与关键帧数据库内容序列化保存至文件。

2.如权利要求1所述的移动机器人离线地图保存与实时重定位方法，其特征在于：所述重定位模型的重定位方法具体步骤如下：

(1)由公式计算获得当前帧的Bow向量；

所述公式计算为：对应对于一幅图像A中的单词w_i，其权重η_i可由以下公式计算获得：

其中

是在图像A中单词w_i出现的次数，n_w是图像A中所有单词出现的总次数，nfeatures代表字典中所有特征的数量，

代表字典中单词w_i中的特征数量；

计算出每个单词权重之后，图像A的BoW向量v_A可以表示为：

(2)从关键帧数据库中筛选候选帧：候选关键帧是重定位模型的初始化数据，是影响重定位性能的重要因素，候选关键帧的筛选在ORB-SLAM2中的具体流程如下：

a)选取所有与当前帧具有共同单词的关键帧；

b)统计与当前帧共有单词数最多的单词数量nMaxCoWords；

c)以0.8*nMaxCoWords为阈值，剔除共有单词数量少于此阈值的候选关键帧；

d)寻找每一个候选关键帧相连的共视关系最好的10个关键帧并以此为一组，计算每一组的相似度累计得分并返回其组内得分最高的关键帧；

e)剔除得分低于0.75倍最高得分的关键帧；

(3)特征匹配：在当前帧与候选关键帧的同一个单词节点下，进行特征匹配，剔除匹配点数小于15的候选帧；

(4)估计当前帧位姿：利用匹配的特征点，构建3D-2D的PnP问题，然后利用RANSAC迭代求解，当有足够的内点时，判断重定位成功。

3.如权利要求1或2所述的移动机器人离线地图保存与实时重定位方法，其特征在于：所述包括离线地图构建与保存，离线地图加载与重定位外，还包括离线地图以及轨迹可视化模块，以方便用户查看建图效果和机器人运行轨迹，具体步骤如下：

(1)利用所述离线地图构建与保存方法加载地图；

(2)地图点和关键帧是最能够直观反映地图效果和建图轨迹的数据，因此调用Pangolin库创建可视化窗口，显示地图点和关键帧；

(3)使用Pangolin库单独可视化机器人轨迹，输入数据可以是ORB-SLAM2运行结束后保存的相机位姿文件或者关键帧文件。

Images