一种SLAM地图拼接方法及系统

摘要

一种SLAM地图拼接方法及系统，包括：在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化；利用车辆的定位模块获取当前车辆位姿信息，推算出车载传感器的当前位姿信息，以构建当前虚拟关键帧；将当前虚拟关键帧与历史关键帧进行关联，判断初始化是否成功；若否，利用车辆的定位模块获取当前车辆位姿信息以生成虚拟关键帧；若是，构建新的子地图，并利用当前虚拟关键帧对应的传感器位姿信息初始化新的子地图，以实现对原有子地图和新的子地图之间的拼接；同时，对所有子地图进行整体联合优化。本技术解决了SLAM制图过程中SLAM跟踪丢失导致制图失败的问题，大幅度提高构建大场景完整SLAM地图的成功率。

权利要求

1.一种SLAM地图拼接方法，其特征在于，所述方法包括：

在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化；

利用车辆的定位模块获取当前车辆位姿信息，并根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息；其中，所述车载传感器包括车载相机或者车载激光雷达；

根据所述当前传感器位姿信息，构建当前虚拟关键帧；

根据所述当前虚拟关键帧对应的传感器位姿信息以及历史关键帧对应的传感器位姿信息，将所述当前虚拟关键帧与所述历史关键帧进行关联，以利用所述当前虚拟关键帧对应的传感器位姿信息更新所述车辆的虚拟运动轨迹，并将更新后的虚拟运动轨迹与所述车辆的第一运动轨迹对接；其中，所述车辆的第一运动轨迹是基于第一子地图中所有关键帧对应的传感器位姿信息所生成的；若所述当前虚拟关键帧为所述SLAM跟踪丢失之后构建的第一帧虚拟关键帧，则所述历史关键帧为第一子地图中的任一关键帧，其中，所述第一子地图为所述SLAM跟踪丢失之前已构建的SLAM子地图；若所述当前虚拟关键帧为所述第一帧虚拟关键帧之后构建的任一虚拟关键帧，则所述历史关键帧为当前时刻之前所构建的任一关键帧；

判断SLAM建图初始化是否成功；

若SLAM建图初始化不成功，执行所述利用车辆的定位模块获取当前车辆位姿信息的步骤；

若SLAM建图初始化成功，构建第二子地图，并根据所述当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，再利用所述当前虚拟关键帧对应的传感器位姿信息与所述初始关键帧对应的传感器位姿信息将所述当前虚拟关键帧与所述第二子地图进行关联，以将所述更新后的虚拟运动轨迹与所述车辆的第二运动轨迹对接，以实现对所述第一子地图与所述第二子地图进行拼接；所述第二运动轨迹是基于所述第二子地图中所有关键帧对应的传感器位姿信息所生成的，所述初始关键帧为初始化成功后所述第二子地图中最先构建的关键帧；对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图。

2.根据权利要求1所述的方法，其特征在于，在所述根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息之后，所述方法还包括：根据所述车载传感器的当前传感器位姿信息与在当前时刻之前构建的若干关键帧对应的传感器位姿信息，构建误差函数；

优化所述误差函数中所述当前传感器位姿信息的取值，使得所述误差函数达到最小值，以获得优化后的当前传感器位姿信息；

所述根据所述当前传感器位姿信息，构建当前虚拟关键帧，包括：

根据所述优化后的当前传感器位姿信息，构建当前虚拟关键帧。

3.根据权利要求1或2所述的方法，其特征在于，在所述利用所述当前虚拟关键帧对应的传感器位姿信息与所述初始关键帧对应的传感器位姿信息将所述当前虚拟关键帧与所述第二子地图进行关联之后，所述方法还包括：根据所述车辆的实时车辆位姿信息推算出所述车载传感器的实时传感器位姿信息，若检测到所述实时传感器位姿信息与所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的传感器位姿信息相匹配，将所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的位置作为目标起点，根据所述更新后的虚拟运动轨迹指引的路线控制车辆行驶，直至检测到所述车辆进入所述第二子地图的定位范围。

4.根据权利要求1或2所述的方法，其特征在于，所述对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图，包括：对所述第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图；

对所述第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图；

将所述优化后的第一位姿图和所述优化后的第二位姿图进行整合，以生成完整的SLAM地图。

5.根据权利要求3所述的方法，其特征在于，所述对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图，包括：对所述第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图；

对所述第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图；

将所述优化后的第一位姿图和所述优化后的第二位姿图进行整合，以生成完整的SLAM地图。

6.一种SLAM地图拼接系统，其特征在于，所述系统包括：

初始化单元，用于在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化；

获取单元，用于利用车辆的定位模块获取当前车辆位姿信息，并根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息；其中，所述车载传感器包括车载相机或者车载激光雷达；

关键帧构建单元，用于根据所述当前传感器位姿信息，构建当前虚拟关键帧；

第一关联单元，用于将所述当前虚拟关键帧与历史关键帧进行关联；其中，若所述当前虚拟关键帧为所述SLAM跟踪丢失之后构建的第一帧虚拟关键帧，则所述历史关键帧为第一子地图中的任一关键帧，其中，所述第一子地图为所述SLAM跟踪丢失之前已构建的SLAM子地图；若所述当前虚拟关键帧为所述第一帧虚拟关键帧之后构建的任一虚拟关键帧，则所述历史关键帧为当前时刻之前所构建的任一关键帧；

判断单元，用于判断SLAM建图初始化是否成功；

第二关联单元，用于在所述判断单元判定出SLAM建图初始化成功时，构建第二子地图，并利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图，以实现对所述第一子地图与所述第二子地图进行拼接；

图优化单元，用于对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图；

所述获取单元，还用于在所述判断单元判定出SLAM建图初始化不成功时，利用车辆的定位模块获取当前车辆位姿信息；

其中，所述第一关联单元用于将所述当前虚拟关键帧与历史关键帧进行关联的方式具体为：所述第一关联单元，用于根据所述当前虚拟关键帧对应的传感器位姿信息以及历史关键帧对应的传感器位姿信息，将所述当前虚拟关键帧与所述历史关键帧进行关联，以利用所述当前虚拟关键帧对应的传感器位姿信息更新所述车辆的虚拟运动轨迹，并将更新后的虚拟运动轨迹与所述车辆的第一运动轨迹对接；所述车辆的第一运动轨迹是基于所述第一子地图中所有关键帧对应的传感器位姿信息所生成的；

所述第二关联单元用于利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图的方式具体为：所述第二关联单元，用于根据所述当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，以利用所述当前虚拟关键帧对应的传感器位姿信息与所述初始关键帧对应的传感器位姿信息将所述当前虚拟关键帧与所述第二子地图进行关联，并将所述更新后的虚拟运动轨迹与所述车辆的第二运动轨迹对接；所述第二运动轨迹是基于所述第二子地图中所有关键帧对应的传感器位姿信息所生成的；所述初始关键帧为初始化成功后所述第二子地图中最先构建的关键帧。

7.根据权利要求6所述的系统，其特征在于，所述系统还包括：

函数构建单元，用于在所述获取单元根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息之后，根据所述车载传感器的当前传感器位姿信息与在当前时刻之前构建的若干关键帧对应的传感器位姿信息，构建误差函数；

函数优化单元，用于优化所述误差函数中所述当前传感器位姿信息的取值，使得所述误差函数达到最小值，以获得优化后的当前传感器位姿信息；

所述关键帧构建单元用于根据所述当前传感器位姿信息，构建当前虚拟关键帧的方式具体为：所述关键帧构建单元，用于根据所述优化后的当前传感器位姿信息，构建当前虚拟关键帧。

8.根据权利要求6或7所述的系统，其特征在于，所述系统还包括：

控制单元，用于在所述第二关联单元利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图之后，根据所述车辆的实时车辆位姿信息推算出所述车载相机的实时传感器位姿信息，若检测到所述实时传感器位姿信息与所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的传感器位姿信息相匹配，将所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的位置作为目标起点，根据所述更新后的虚拟运动轨迹指引的路线控制车辆行驶，直至检测到所述车辆进入所述第二子地图的定位范围。

9.根据权利要求6或7所述的系统，其特征在于，所述图优化单元，包括：

第一图优化子单元，用于对所述第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图；

第二图优化子单元，用于对所述第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图；

整合子单元，用于将所述优化后的第一位姿图和所述优化后的第二位姿图进行整合，以生成完整的SLAM地图。

10.根据权利要求8所述的系统，其特征在于，所述图优化单元，包括：

第一图优化子单元，用于对所述第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图；

第二图优化子单元，用于对所述第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图；

整合子单元，用于将所述优化后的第一位姿图和所述优化后的第二位姿图进行整合，以生成完整的SLAM地图。

说明书

一种SLAM地图拼接方法及系统

技术领域

[0001] 本发明涉及自动驾驶技术领域，具体涉及一种SLAM地图拼接方法及系统。

背景技术

[0002] 近年来，同步定位与建图(Simultaneous Localization And Mapping，SLAM)技术越来越多地应用于自动驾驶汽车领域，主要用于解决车辆在未知环境运动时的定位与地图构建问题。其中，传统的SLAM的地图构建过程主要为：车辆通过车载传感器(比如相机、激光雷达)采集周围环境的观测数据(比如拍摄图像、点云数据)，以对采集到的观测数据进行跟踪与匹配；根据观测数据之间的匹配关系，计算出车载传感器对应的位姿运动信息，进而可以对车辆自身进行定位，并基于车辆定位信息构建增量式地图。实践中发现，当车辆处于视觉特征贫乏(比如地下白墙道路)或者环境较为苛刻(比如雨天、雨刷遮挡)的条件时，在SLAM建图过程中经常出现SLAM跟踪丢失的情况，导致无法构建完整的SLAM地图。

发明内容

[0003] 本发明实施例公开了一种SLAM地图拼接方法及系统，能够解决SLAM建图过程中SLAM跟踪丢失导致制图失败的问题，提高构建完整的SLAM地图的成功率。

[0004] 本发明实施例第一方面公开一种SLAM地图拼接方法，所述方法包括：

[0005] 在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化；

[0006] 利用车辆的定位模块获取当前车辆位姿信息，并根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息；其中，所述车载传感器包括车载相机或者车载激光雷达；

[0007] 根据所述当前传感器位姿信息，构建当前虚拟关键帧；

[0008] 将所述当前虚拟关键帧与历史关键帧进行关联；其中，若所述当前虚拟关键帧为所述SLAM跟踪丢失之后构建的第一帧虚拟关键帧，则所述历史关键帧为第一子地图中的任一关键帧，其中，所述第一子地图为所述SLAM跟踪丢失之前已构建的SLAM子地图；若所述当前虚拟关键帧为所述第一帧虚拟关键帧之后构建的任一虚拟关键帧，则所述历史关键帧为当前时刻之前所构建的任一关键帧；

[0009] 判断SLAM建图初始化是否成功；

[0010] 若SLAM建图初始化成功，执行所述利用车辆的定位模块获取当前车辆位姿信息的步骤；

[0011] 若SLAM建图初始化不成功，构建第二子地图，并利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图，以实现对所述第一子地图与所述第二子地图进行拼接；

[0012] 对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图。

[0013] 作为一种可选的实施方式，在本发明实施例第一方面中，所述将所述当前虚拟关键帧与历史关键帧进行关联，包括：

[0014] 根据所述当前虚拟关键帧对应的传感器位姿信息以及历史关键帧对应的传感器位姿信息，将所述当前虚拟关键帧与所述历史关键帧进行关联，以利用所述当前虚拟关键帧对应的传感器位姿信息更新所述车辆的虚拟运动轨迹，并将更新后的虚拟运动轨迹与所述车辆的第一运动轨迹对接；所述车辆的第一运动轨迹是基于所述第一子地图中所有关键帧对应的传感器位姿信息所生成的；

[0015] 所述利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图，包括：

[0016] 根据所述当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，以利用所述当前虚拟关键帧对应的传感器位姿信息与所述初始关键帧对应的传感器位姿信息将所述当前虚拟关键帧与所述第二子地图进行关联，并将所述更新后的虚拟运动轨迹与所述车辆的第二运动轨迹对接；所述第二运动轨迹是基于所述第二子地图中所有关键帧对应的传感器位姿信息所生成的；所述初始关键帧为初始化成功后所述第二子地图中最先构建的关键帧。

[0017] 作为一种可选的实施方式，在本发明实施例第一方面中，在所述根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息之后，所述方法还包括：

[0018] 根据所述车载传感器的当前传感器位姿信息与在当前时刻之前构建的若干关键帧对应的传感器位姿信息，构建误差函数；

[0019] 优化所述误差函数中所述当前传感器位姿信息的取值，使得所述误差函数达到最小值，以获得优化后的当前传感器位姿信息；

[0020] 所述根据所述当前传感器位姿信息，构建当前虚拟关键帧，包括：

[0021] 根据所述优化后的当前传感器位姿信息，构建当前虚拟关键帧。

[0022] 作为一种可选的实施方式，在本发明实施例第一方面中，在所述利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图之后，所述方法还包括：

[0023] 根据所述车辆的实时车辆位姿信息推算出所述车载传感器的实时传感器位姿信息，若检测到所述实时传感器位姿信息与所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的传感器位姿信息相匹配，将所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的位置作为目标起点，根据所述更新后的虚拟运动轨迹指引的路线控制车辆行驶，直至检测到所述车辆进入所述第二子地图的定位范围。

[0024] 作为一种可选的实施方式，在本发明实施例第一方面中，所述对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图，包括：

[0025] 对所述第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图；

[0026] 对所述第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图；

[0027] 将所述优化后的第一位姿图和所述优化后的第二位姿图进行整合，以生成完整的SLAM地图。

[0028] 本发明实施例第二方面公开一种SLAM地图拼接系统，所述系统包括：

[0029] 初始化单元，用于在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化；

[0030] 获取单元，用于利用车辆的定位模块获取当前车辆位姿信息，并根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息；其中，所述车载传感器包括车载相机或者车载激光雷达；

[0031] 关键帧构建单元，用于根据所述当前传感器位姿信息，构建当前虚拟关键帧；

[0032] 第一关联单元，用于将所述当前虚拟关键帧与历史关键帧进行关联；其中，若所述当前虚拟关键帧为所述SLAM跟踪丢失之后构建的第一帧虚拟关键帧，则所述历史关键帧为所述第一子地图中的任一关键帧，其中，所述第一子地图为所述SLAM跟踪丢失之前已构建的SLAM子地图；若所述当前虚拟关键帧为所述第一帧虚拟关键帧之后构建的任一虚拟关键帧，则所述历史关键帧为当前时刻之前所构建的任一关键帧；

[0033] 判断单元，用于判断SLAM建图初始化是否成功；

[0034] 第二关联单元，用于在所述判断单元判定出SLAM建图初始化成功时，构建第二子地图，并利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图，以实现对所述第一子地图与所述第二子地图进行拼接；

[0035] 图优化单元，用于对所述第一子地图和所述第二子地图进行整体联合优化，以生成完整的SLAM地图；

[0036] 所述获取单元，还用于在所述判断单元判定出SLAM建图初始化不成功时，利用车辆的定位模块获取当前车辆位姿信息。

[0037] 作为一种可选的实施方式，在本发明实施例第二方面中，所述第一关联单元用于将所述当前虚拟关键帧与历史关键帧进行关联的方式具体为：

[0038] 所述第一关联单元，用于根据所述当前虚拟关键帧对应的传感器位姿信息以及历史关键帧对应的传感器位姿信息，将所述当前虚拟关键帧与所述历史关键帧进行关联，以利用所述当前虚拟关键帧对应的传感器位姿信息更新所述车辆的虚拟运动轨迹，并将更新后的虚拟运动轨迹与所述车辆的第一运动轨迹对接；所述车辆的第一运动轨迹是基于所述第一子地图中所有关键帧对应的传感器位姿信息所生成的；

[0039] 所述第二关联单元用于利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图的方式具体为：

[0040] 所述第二关联单元，用于根据所述当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，以利用所述当前虚拟关键帧对应的传感器位姿信息与所述初始关键帧对应的传感器位姿信息将所述当前虚拟关键帧与所述第二子地图进行关联，并将所述更新后的虚拟运动轨迹与所述车辆的第二运动轨迹对接；所述第二运动轨迹是基于所述第二子地图中所有关键帧对应的传感器位姿信息所生成的；所述初始关键帧为初始化成功后所述第二子地图中最先构建的关键帧。

[0041] 作为一种可选的实施方式，在本发明实施例第二方面中，所述系统还包括：

[0042] 函数构建单元，用于在所述获取单元根据所述当前车辆位姿信息推算出车载传感器的当前传感器位姿信息之后，根据所述车载传感器的当前传感器位姿信息与在当前时刻之前构建的若干关键帧对应的传感器位姿信息，构建误差函数；

[0043] 函数优化单元，用于优化所述误差函数中所述当前传感器位姿信息的取值，使得所述误差函数达到最小值，以获得优化后的当前传感器位姿信息；

[0044] 所述关键帧构建单元用于根据所述当前传感器位姿信息，构建当前虚拟关键帧的方式具体为：

[0045] 所述关键帧构建单元，用于根据所述优化后的当前传感器位姿信息，构建当前虚拟关键帧。

[0046] 作为一种可选的实施方式，在本发明实施例第二方面中，所述系统还包括：

[0047] 控制单元，用于在所述第二关联单元利用所述当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图之后，根据所述车辆的实时车辆位姿信息推算出所述车载相机的实时传感器位姿信息，若检测到所述实时传感器位姿信息与所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的传感器位姿信息相匹配，将所述更新后的虚拟运动轨迹中任一虚拟关键帧对应的位置作为目标起点，根据所述更新后的虚拟运动轨迹指引的路线控制车辆行驶，直至检测到所述车辆进入所述第二子地图的定位范围。

[0048] 作为一种可选的实施方式，在本发明实施例第二方面中，所述图优化单元，包括：

[0049] 第一图优化子单元，用于对所述第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图；

[0050] 第二图优化子单元，用于对所述第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图；

[0051] 整合子单元，用于将所述优化后的第一位姿图和所述优化后的第二位姿图进行整合，以生成完整的SLAM地图。

[0052] 本发明实施例第三方面公开一种计算机可读存储介质，其存储计算机程序，其中，所述计算机程序使得计算机执行本发明实施例第一方面公开的一种SLAM地图拼接方法。

[0053] 本发明实施例第四方面公开一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行第一方面的任意一种方法的部分或全部步骤。

[0054] 与现有技术相比，本发明实施例具有以下有益效果：

[0055] 实施本发明实施例，基于已构建的SLAM子地图，在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化，同时，根据获取到的车辆位姿信息推算出车载传感器的传感器位姿信息，来产生虚拟关键帧，利用虚拟关键帧将前后两次构建的SLAM子地图拼接起来，能够提高构建完整的SLAM地图的成功率，其中，车载传感器包括车载相机或者车载激光雷达，可以适用于视觉SLAM建图或者激光SLAM建图；并且，通过对各子地图进行整体联合优化，提高了拼接子地图后生成的SLAM地图的统一性和完整性；此外，在SLAM跟踪丢失时，通过采集车辆的位姿信息，还能够保证记录车辆的运动轨迹的连续性，用于辅助后续的车辆导航。

附图说明

[0056] 为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

[0057] 图1是本发明实施例公开的一种SLAM地图拼接方法的流程示意图；

[0058] 图2是本发明实施例公开的另一种SLAM地图拼接方法的流程示意图；

[0059] 图3是本发明实施例公开的一种SLAM地图拼接系统的结构示意图；

[0060] 图4是本发明实施例公开的另一种SLAM地图拼接系统的结构示意图。

具体实施方式

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

[0062] 需要说明的是，本发明的说明书和权利要求书中的术语“第一”、“第二”、“第三”“第四”等是用于区别不同的对象，而不是用于描述特定顺序。本发明实施例的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

[0063] 本发明实施例公开了一种SLAM地图拼接方法及系统，能够解决SLAM建图过程中SLAM跟踪丢失导致制图失败的问题，提高构建完整的SLAM地图的成功率。以下结合附图进行详细描述。

[0064] 实施例一

[0065] 请参阅图1，图1是本发明实施例公开的一种SLAM地图拼接方法的流程示意图。如图1所示，该SLAM地图拼接方法应用于地图拼接系统，具体可以包括以下步骤。

[0066] 101、地图拼接系统在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化。

[0067] 本发明实施例适用于视觉SLAM建图和激光SLAM建图。以视觉SLAM建图为例进行说明，视觉SLAM建图主要包括以下步骤：

[0068] 前端基于车载传感器持续对周边环境采集到的图像帧，对相邻图像帧进行特征点匹配，其中，车载传感器可以为车载相机，具体可以包括单目相机、双目相机或者深度相机，对此不作限定。可选的，对于单目相机，根据相邻图像帧的特征点匹配结果，可以获得相邻图像帧之间的相机旋转量与相机平移量，再结合利用三角测量原理所确定的每个图像帧中特征点的深度信息，进而可以获得相机位姿信息；对于双目或深度相机，若已知两组相匹配的3D特征点的位置信息，使用ICP(Iterative Closest Point，迭代最近点)方法求出相机位姿信息；若已知两组相匹配的3D特征点的位置信息以及其在相机的投影位置，可使用PnP(Pespective-n-Point，透视n点定位)方法求出相机位姿信息。

[0069] 后端通过回环检测来检测车辆是否到达先前位置，并对不同图像帧对应的相机位姿信息以及回环检测的检测信息进行优化，获取车辆的运动轨迹。

[0070] 进一步地，根据车辆的运动轨迹以及采集到的图像帧，构建子地图，其中，子地图包括多个关键帧，基于每一个关键帧对应的相机位姿信息，相邻的关键帧之间对应的位移量大于或等于预设位移值；此外，根据使用的传感器类型和实际应用需求，第一子地图可以为2D栅格地图、2D拓扑地图、3D点云地图或者3D网格地图等，对此不作具体限定。

[0071] 以激光SLAM建图为例进行说明，激光SLAM建图主要是基于激光测距的原理，通过设置于车身的激光雷达对周围环境采集点云数据，其中，点云数据包括激光雷达与周边物体之间的角度信息和距离信息。并且，通过对不同时刻的点云数据进行匹配与比对，可以计算出激光雷达位姿信息，以基于激光雷达位姿信息获得车辆的运动轨迹，进而构建子地图，其中，激光雷达位姿信息可以包括激光雷达的相对运动距离和姿态变化量。

[0072] 可以理解，SLAM追踪失败可以包括以下情况：对于视觉SLAM建图，在光照不足或者纹理缺失的环境下，采集到的当前图像帧中特征点数目不足，或者，当前图像帧与前一关键帧进行特征点匹配失败，将导致SLAM跟踪丢失；对于激光SLAM建图，在动态环境(比如存在大量遮挡物)或者几何环境(比如长期形状不变的白墙)下，不同时刻的点云数据匹配失败，也会导致SLAM跟踪丢失。

[0073] 本发明实施例中，可选的，对于视觉SLAM建图，地图拼接系统重新进行SLAM建图初始化的步骤具体可以包括：

[0074] 地图拼接系统持续通过视觉传感器采集图像帧，并对采集到的图像帧进行特征点识别；若当前图像帧满足特征点数目要求，且至少存在一个满足特征点数目要求的历史关键帧，对当前图像帧以及上述至少一个历史关键帧进行特征点匹配，其中，特征点数目要求可以设定为特征点数目大于预设阈值(如100)；若根据当前图像帧以及上述至少一个历史关键帧的匹配结果、当前图像帧的特征点深度信息以及上述至少一个历史关键帧的特征点深度信息，可以计算出当前图像帧对应的传感器位姿信息，则可视为建图初始化成功；否则，继续执行持续通过视觉传感器采集图像帧的步骤。

[0075] 还可选的，对于激光SLAM建图，地图拼接系统重新进行SLAM建图初始化的方式可以为：地图拼接系统通过激光雷达持续采集周围环境的点云数据，并对不同时刻的点云数据进行匹配，直至重新匹配成功，则视为SLAM建图初始化成功。

[0076] 102、地图拼接系统利用车辆的定位模块获取当前车辆位姿信息，并根据当前车辆位姿信息推算出车载传感器的当前传感器位姿信息。

[0077] 本发明实施例中，车载传感器包括车载相机或者车载激光雷达。车辆的定位模块可以包括车身里程计、GPS、惯性组合导航等，对此不作具体限定。车辆位姿信息可以为车辆在世界坐标系下的位置信息(比如三维坐标位置)和姿态信息。举例来说，利用GPS获取车辆的位置信息；通过惯性组合导航测量车辆在特定坐标系下的角速度和加速度，以获得车辆的姿态信息。

[0078] 本发明实施例中，根据车辆坐标系与传感器坐标系之间的转换关系，将车辆位姿信息从车辆坐标系转换至传感器坐标系，获得在传感器坐标系下车载传感器的传感器位姿信息；其中，车辆坐标系与传感器坐标系之间的转换关系可以通过预先标定得到。以车载相机为例，可以根据车载相机的外部参数，获得车辆坐标系与相机坐标系之间的转换关系。

[0079] 103、根据当前传感器位姿信息，地图拼接系统构建当前虚拟关键帧。

[0080] 104、地图拼接系统将当前虚拟关键帧与历史关键帧进行关联。

[0081] 本发明实施例中，若当前虚拟关键帧为SLAM跟踪丢失之后构建的第一帧虚拟关键帧，则历史关键帧为第一子地图中的任一关键帧，其中，第一子地图为SLAM跟踪丢失之前已构建的SLAM子地图；若当前虚拟关键帧为第一帧虚拟关键帧之后构建的任一虚拟关键帧，则历史关键帧为当前时刻之前所构建的任一关键帧。需要区分的是，虚拟关键帧对应的传感器位姿信息是基于车辆在特定时刻利用定位模块获取到的车辆位姿信息推算得到的，而第一子地图中的关键帧对应的传感器位姿信息是基于车载传感器采集到的观测数据推算得到的，二者的获取方式不同。可以理解，当前虚拟关键帧为当前时刻根据当前传感器位姿信息所构建的虚拟关键帧。

[0082] 105、地图拼接系统判断SLAM建图初始化是否成功，若SLAM建图初始化成功，执行步骤102；若SLAM建图初始化不成功，执行步骤106。

[0083] 106、地图拼接系统构建第二子地图，并利用所述当前虚拟关键帧对应的传感器位姿信息初始化第二子地图，以实现对第一子地图与第二子地图进行拼接，第一子地图为SLAM跟踪丢失之前已构建的SLAM子地图。

[0084] 本发明实施例中，第二子地图为对SLAM跟踪丢失之后，重新进行SLAM建图初始化成功后生成的SLAM地图。

[0085] 作为一种可选的实施方式，步骤104具体包括：

[0086] 根据当前虚拟关键帧对应的传感器位姿信息以及历史关键帧对应的传感器位姿信息，地图拼接系统将当前虚拟关键帧与历史关键帧进行关联，以利用当前虚拟关键帧对应的传感器位姿信息更新车辆的虚拟运动轨迹，并将更新后的虚拟运动轨迹与车辆的第一运动轨迹对接；其中，车辆的第一运动轨迹是基于第一子地图中所有关键帧对应的传感器位姿信息所生成的；车辆的虚拟运动轨迹由在对第一子地图的当前图像帧进行特征点追踪失败之后构建的多个虚拟关键帧组成。一般来说，历史关键帧可以为最接近当前时刻所构建的关键帧。

[0087] 作为一种可选的实施方式，步骤106中地图拼接系统利用当前虚拟关键帧对应的传感器位姿信息初始化第二子地图的方式具体为：

[0088] 地图拼接系统根据当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，以利用当前虚拟关键帧对应的传感器位姿信息与初始关键帧对应的传感器位姿信息将当前虚拟关键帧与第二子地图进行关联，并将更新后的虚拟运动轨迹与车辆的第二运动轨迹对接；其中，第二运动轨迹是基于第二子地图中所有关键帧对应的传感器位姿信息所生成的；初始关键帧为初始化成功后第二子地图中最先构建的关键帧。

[0089] 其中，可选的，地图拼接系统根据当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，可以包括：

[0090] 地图拼接系统对当前虚拟关键帧对应的图像帧以及初始关键帧对应的图像帧进行特征点识别与匹配；

[0091] 地图拼接系统根据上述特征点匹配结果、每一个图像帧的特征点深度信息以及当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息。

[0092] 可见，实施可选的实施方式，通过将虚拟关键帧与第一子地图进行关联，以及将虚拟关键帧与第二子地图进行关联，能够实现对第一子地图和第二子地图的拼接，此外，还能基于若干包含车辆位姿信息的虚拟关键帧所构成的虚拟运动轨迹，维持整个定位与建图过程中车辆的运动轨迹的连续性。

[0093] 107、地图拼接系统对第一子地图和第二子地图进行整体联合优化，以生成完整的SLAM地图。

[0094] 可见，实施图1所描述的方法，基于已构建的SLAM子地图，在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化，同时，根据获取到的车辆位姿信息推算出车载传感器的传感器位姿信息，来产生虚拟关键帧，利用虚拟关键帧将前后两次构建的SLAM子地图拼接起来，能够提高构建完整的SLAM地图的成功率，其中，车载传感器包括车载相机或者车载激光雷达，可以适用于视觉SLAM建图或者激光SLAM建图；并且，通过对各子地图进行整体联合优化，提高了拼接子地图后生成的SLAM地图的统一性和完整性；此外，在SLAM跟踪丢失时，通过采集车辆的位姿信息，还能够保证记录车辆的运动轨迹的连续性，用于辅助后续的车辆导航。

[0095] 实施例二

[0096] 请参阅图2，图2是本发明实施例公开的另一种SLAM地图拼接方法的流程示意图。如图2所示，该SLAM地图拼接方法可以包括以下步骤。

[0097] 201、地图拼接系统在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化。

[0098] 202、地图拼接系统利用车辆的定位模块获取当前车辆位姿信息，并根据当前车辆位姿信息推算出车载相机的当前传感器位姿信息。

[0099] 203、地图拼接系统根据当前传感器位姿信息与在当前时刻之前构建的若干关键帧对应的传感器位姿信息，构建误差函数。

[0100] 本发明实施例中，当前时刻之前构建的若干关键帧可以为第一子地图中的至少两个关键帧，或者第一子地图中的关键帧与在当前时刻之前构建的虚拟关键帧的组合，或者在检测到SLAM跟踪丢失之后，以及当前时刻之前构建的至少两个虚拟关键帧。

[0101] 204、地图拼接系统优化误差函数中当前传感器位姿信息的取值，使得误差函数达到最小值，以获得优化后的当前传感器位姿信息。

[0102] 可选的，构造一个最小二乘误差函数 其中，r(ξ)为非线性函数，可以代表当前传感器位姿信息与当前时刻之前构建的若干关键帧对应的传感器位姿信息之间的相对位姿变化量，ξ代表虚拟关键帧对应的传感器位姿信息。利用迭代法对函数F(ξ)进行多次迭代，求得误差函数最小化时对应的当前传感器位姿信息的取值，以作为优化后的当前传感器位姿信息；其中，迭代法可以包括牛顿迭代、列文伯格-马克尔特迭代法等。

[0103] 205、根据优化后的当前传感器位姿信息，地图拼接系统构建当前虚拟关键帧。

[0104] 可见，实施上述步骤203～步骤205，基于第一子地图中若干关键帧的传感器位姿信息，对当前传感器位姿信息进行优化，能够提高优化后的当前传感器位姿信息的精确度，并且，根据优化后的当前传感器位姿信息所构建的虚拟关键帧进一步参与生成车辆的虚拟运动轨迹，使得车辆的虚拟运动轨迹与车辆在第一子地图中的运动轨迹之间的误差最小。

[0105] 206、地图拼接系统将当前虚拟关键帧与历史关键帧进行关联。

[0106] 207、地图拼接系统判断SLAM建图初始化是否成功，若SLAM建图初始化成功，执行步骤202；若SLAM建图初始化不成功，执行步骤208～步骤211。

[0107] 208、地图拼接系统构建第二子地图，并利用当前虚拟关键帧对应的传感器位姿信息初始化第二子地图，以实现对第一子地图与第二子地图进行拼接，第一子地图为SLAM跟踪丢失之前已构建的SLAM子地图。

[0108] 作为一种可选的实施方式，在步骤208之后，还可以包括以下步骤：

[0109] 根据车辆的实时车辆位姿信息推算出车载相机的实时传感器位姿信息，若检测到实时传感器位姿信息与更新后的虚拟运动轨迹中任一虚拟关键帧对应的传感器位姿信息相匹配，将更新后的虚拟运动轨迹中任一虚拟关键帧对应的位置作为目标起点，根据更新后的虚拟运动轨迹指引的路线控制车辆行驶，直至检测到车辆进入第二子地图的定位范围。

[0110] 可见，实施可选的实施方式，在检测到车辆回到虚拟运动轨迹中任一虚拟关键帧对应的位置时，通过虚拟运动轨迹指引的路线控制车辆行驶，能够在未覆盖SLAM地图的区域内实现车辆的自主导航与驾驶。

[0111] 209、地图拼接系统对第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图。

[0112] 210、地图拼接系统对第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图。

[0113] 211、地图拼接系统将优化后的第一位姿图和优化后的第二位姿图进行整合，以生成完整的SLAM地图。

[0114] 作为一种可选的实施方式，对于视觉SLAM建图，第一位姿图可以包括根据第一子地图中各个关键帧对应的特征点构建成的第一特征节点，以及根据各个关键帧对应的传感器位姿信息所构建的第一位姿节点。步骤209具体为：利用迭代法对第一位姿图中任意两个第一位姿节点对应的相机位姿变化量，以及相应的第一位姿节点和第一特征节点之间所对应的深度观测量进行多次迭代，使得误差值最小化，以获得图优化后的第一位姿图；其中，迭代法可以包括牛顿迭代、列文伯格-马克尔特迭代法等；图优化后的第一位姿图中提供最优相机位姿，并且测量得到最优特征点深度。

[0115] 类似地，作为一种可选的实施方式，对于视觉SLAM建图，第二位姿图可以包括根据第二子地图中各个关键帧对应的特征点构建成的第二特征节点，以及根据各个关键帧对应的传感器位姿信息所构建的第二位姿节点。步骤210具体为：利用迭代法对第二位姿图中任意两个第二位姿节点对应的相机位姿变化量，以及相应的第二位姿节点和第二特征节点之间所对应的深度观测量进行多次迭代，使得误差值最小化，以获得图优化后的第二位姿图；其中，迭代法可以包括牛顿迭代、列文伯格-马克尔特迭代法等；图优化后的第二位姿图中提供最优相机位姿，并且测量得到最优特征点深度。

[0116] 进一步地，作为一种可选的实施方式，步骤211具体为：基于图优化后的第一位姿图所提供的传感器位姿信息以及关键帧的特征点深度信息，生成图优化后的第一子地图；基于图优化后的第二位姿图所提供的传感器位姿信息以及关键帧的特征点深度信息，生成图优化后的第二子地图，以利用图优化后的第一子地图和图优化后的第二子地图更新原有的完整SLAM地图，获得图优化后的完整SLAM地图，其中，SLAM地图可以为三维地图。

[0117] 再进一步地，可选的，针对第一子地图和第二子地图整合生成的完整SLAM地图，第一子地图/第二子地图可以为地图拼接系统通过深度相机采集图像帧，并且将三维点云变换到世界坐标下进行拼接和逐步叠加后构建的稠密且特征丰富的三维地图。而针对用于拼接第一子地图和第二子地图的虚拟运动轨迹所在区域，还可以在车辆沿虚拟运动轨迹移动的过程中，根据若干虚拟关键帧对应的时刻所采集到的图像帧，在完整SLAM地图上的虚拟运动轨迹所在区域建立稀疏子地图，以丰富完整SLAM地图的内容。

[0118] 可见，实施上述步骤209～步骤211，对各子地图进行融合车身里程计和视觉特征的位姿图优化，能够减少经多个图像帧累积的误差，提高构建完整SLAM地图的准确度。

[0119] 可见，实施图2所描述的方法，基于已构建的SLAM子地图，在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化，同时，根据获取到的车辆位姿信息推算出车载传感器的传感器位姿信息，来产生虚拟关键帧，以利用虚拟关键帧将前后两次构建的SLAM子地图拼接起来，能够提高构建完整的SLAM地图的成功率，其中，车载传感器包括车载相机或者车载激光雷达，可以适用于视觉SLAM建图或者激光SLAM建图；此外，在SLAM跟踪丢失时，通过采集车辆的位姿信息，还能够保证记录车辆的运动轨迹的连续性，用于辅助后续的车辆导航；进一步地，在检测到车辆回到虚拟运动轨迹中任一虚拟关键帧对应的位置时，通过虚拟运动轨迹指引的路线控制车辆行驶，能够在未覆盖SLAM地图的区域内实现车辆的自主导航与驾驶；再进一步地，对各子地图进行融合车身里程计和视觉特征的位姿图优化，能够减少经多个图像帧累积的误差，提高构建完整SLAM地图的准确度，还能够使得车辆的虚拟运动轨迹与车辆在第一子地图中的运动轨迹之间的误差最小。

[0120] 实施例三

[0121] 请参阅3，图3是本发明实施例公开的一种SLAM地图拼接系统的结构示意图。如图3所示，该SLAM地图拼接系统可以包括初始化单元301、获取单元302、关键帧构建单元303、第一关联单元304、判断单元305、第二关联单元306以及图优化单元307，其中：

[0122] 初始化单元301，用于在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化。

[0123] 本发明实施例中，可选的，对于视觉SLAM建图，初始化单元301用于重新进行SLAM建图初始化的步骤具体可以包括：

[0124] 初始化单元301，用于持续通过视觉传感器采集图像帧，并对采集到的图像帧进行特征点识别；若当前图像帧满足特征点数目要求，且至少存在一个满足特征点数目要求的历史关键帧，对当前图像帧以及上述至少一个历史关键帧进行特征点匹配，其中，特征点数目要求可以设定为特征点数目大于预设阈值(如100)；若根据当前图像帧以及上述至少一个历史关键帧的匹配结果、当前图像帧的特征点深度信息以及上述至少一个历史关键帧的特征点深度信息，可以计算出当前图像帧对应的传感器位姿信息，则可视为建图初始化成功；否则，继续执行持续通过视觉传感器采集图像帧的步骤。

[0125] 还可选的，对于激光SLAM建图，初始化单元301用于重新进行SLAM建图初始化的方式具体为：初始化单元301，用于通过激光雷达持续采集周围环境的点云数据，并对不同时刻的点云数据进行匹配，直至重新匹配成功，则视为SLAM建图初始化成功。

[0126] 获取单元302，用于利用车辆的定位模块获取当前车辆位姿信息，并根据当前车辆位姿信息推算出车载传感器的当前传感器位姿信息，其中，车载传感器包括车载相机或者车载激光雷达。

[0127] 关键帧构建单元303，用于根据当前传感器位姿信息，构建当前虚拟关键帧。

[0128] 第一关联单元304，用于将当前虚拟关键帧与历史关键帧进行关联；其中，若当前虚拟关键帧为SLAM跟踪丢失之后构建的第一帧虚拟关键帧，则历史关键帧为第一子地图中的任一关键帧，其中，第一子地图为SLAM跟踪丢失之前已构建的SLAM子地图；若当前虚拟关键帧为第一帧虚拟关键帧之后构建的任一虚拟关键帧，则历史关键帧为当前时刻之前所构建的任一关键帧。

[0129] 作为一种可选的实施方式，第一关联单元304用于将当前虚拟关键帧与历史关键帧进行关联的方式具体为：

[0130] 第一关联单元304，用于根据当前虚拟关键帧对应的传感器位姿信息以及历史关键帧对应的传感器位姿信息，地图拼接系统将当前虚拟关键帧与历史关键帧进行关联，以利用当前虚拟关键帧对应的传感器位姿信息更新车辆的虚拟运动轨迹，并将更新后的虚拟运动轨迹与车辆的第一运动轨迹对接；其中，车辆的第一运动轨迹是基于第一子地图中所有关键帧对应的传感器位姿信息所生成的；车辆的虚拟运动轨迹由在对第一子地图的当前图像帧进行特征点追踪失败之后构建的多个虚拟关键帧组成。一般来说，历史关键帧可以为最接近当前时刻所构建的关键帧。

[0131] 判断单元305，用于判断SLAM建图初始化是否成功。

[0132] 第二关联单元306，用于判断单元305判定出SLAM建图初始化成功时，构建第二子地图，并利用当前虚拟关键帧对应的传感器位姿信息初始化第二子地图，以实现对第一子地图与第二子地图进行拼接。

[0133] 作为一种可选的实施方式，第二关联单元306用于利用当前虚拟关键帧对应的传感器位姿信息初始化所述第二子地图的方式具体为：

[0134] 第二关联单元306，用于根据当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息，以利用当前虚拟关键帧对应的传感器位姿信息与初始关键帧对应的传感器位姿信息将当前虚拟关键帧与第二子地图进行关联，并将更新后的虚拟运动轨迹与车辆的第二运动轨迹对接；其中，第二运动轨迹是基于第二子地图中所有关键帧对应的传感器位姿信息所生成的；初始关键帧为初始化成功后第二子地图中最先构建的关键帧。

[0135] 其中，可选的，第二关联单元306用于根据当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息的方式具体为：

[0136] 第二关联单元306，用于对当前虚拟关键帧对应的图像帧以及初始关键帧对应的图像帧进行特征点识别与匹配；以及，根据上述特征点匹配结果、每一个图像帧的特征点深度信息以及当前虚拟关键帧对应的传感器位姿信息，获得初始关键帧对应的传感器位姿信息。

[0137] 图优化单元307，用于对第一子地图和第二子地图进行整体联合优化，以生成完整的SLAM地图。

[0138] 获取单元302，还用于在判断单元305判定出SLAM建图初始化不成功时，利用车辆的定位模块获取当前车辆位姿信息。

[0139] 可见，实施图3所描述的系统，基于已构建的SLAM子地图，在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化，同时，根据获取到的车辆位姿信息推算出车载传感器的传感器位姿信息，来产生虚拟关键帧，利用虚拟关键帧将前后两次构建的SLAM子地图拼接起来，能够提高构建完整的SLAM地图的成功率，其中，车载传感器包括车载相机或者车载激光雷达，可以适用于视觉SLAM建图或者激光SLAM建图；并且，通过对各子地图进行整体联合优化，提高了拼接子地图后生成的SLAM地图的统一性和完整性；此外，在SLAM跟踪丢失时，通过采集车辆的位姿信息，还能够保证记录车辆的运动轨迹的连续性，用于辅助后续的车辆导航。

[0140] 实施例四

[0141] 请参阅图4，图4是本发明实施例公开的另一种SLAM地图拼接系统的结构示意图。其中，图4所示的SLAM地图拼接系统是由图3所示的SLAM地图拼接系统进行优化得到的。与图3所示的SLAM地图拼接系统相比较，图4所示的SLAM地图拼接系统还可以包括函数构建单元308和函数优化单元309，其中：

[0142] 函数构建单元308，用于在获取单元302根据当前车辆位姿信息推算出车载传感器的当前传感器位姿信息之后，根据当前传感器位姿信息与在当前时刻之前构建的若干关键帧对应的传感器位姿信息，构建误差函数。

[0143] 函数优化单元309，用于优化误差函数中当前传感器位姿信息的取值，使得误差函数达到最小值，以获得优化后的当前传感器位姿信息。

[0144] 可选的，函数构建单元308用于构造一个最小二乘误差函数 其中，r(ξ)为非线性函数，可以代表当前传感器位姿信息与当前时刻之前构建的若干关键帧对应的传感器位姿信息之间的相对位姿变化量，ξ代表虚拟关键帧对应的传感器位姿信息。函数优化单元309用于利用迭代法对函数F(ξ)进行多次迭代，求得误差函数最小化时对应的当前传感器位姿信息的取值，以作为优化后的当前传感器位姿信息；其中，迭代法可以包括牛顿迭代、列文伯格-马克尔特迭代法等。

[0145] 关键帧构建单元303用于根据当前传感器位姿信息，构建当前虚拟关键帧的方式具体为：

[0146] 关键帧构建单元303，用于根据优化后的当前传感器位姿信息，构建当前虚拟关键帧。

[0147] 作为一种可选的实施方式，该SLAM地图拼接系统还可以包括控制单元，控制单元用于在第二关联单元306利用当前虚拟关键帧对应的传感器位姿信息初始化第二子地图之后，根据车辆的实时车辆位姿信息推算出车载传感器的实时传感器位姿信息，若检测到实时传感器位姿信息与更新后的虚拟运动轨迹中任一虚拟关键帧对应的传感器位姿信息相匹配，将更新后的虚拟运动轨迹中任一虚拟关键帧对应的位置作为目标起点，根据更新后的虚拟运动轨迹指引的路线控制车辆行驶，直至检测到车辆进入第二子地图的定位范围。

[0148] 图优化单元307，包括：

[0149] 第一图优化子单元3071，用于对第一子地图对应的第一位姿图进行位姿图优化，以获得优化后的第一位姿图。

[0150] 第二图优化子单元3072，用于对第二子地图对应的第二位姿图进行位姿图优化，以获得优化后的第二位姿图。

[0151] 整合子单元3073，用于将优化后的第一位姿图和优化后的第二位姿图进行整合，以生成完整的SLAM地图。

[0152] 作为一种可选的实施方式，对于视觉SLAM建图，第一位姿图可以包括根据第一子地图中各个关键帧对应的特征点构建成的第一特征节点，以及根据各个关键帧对应的传感器位姿信息所构建的第一位姿节点。第一图优化子单元3071，具体用于利用迭代法对第一位姿图中任意两个第一位姿节点对应的相机位姿变化量，以及相应的第一位姿节点和第一特征节点之间所对应的深度观测量进行多次迭代，使得误差值最小化，以获得图优化后的第一位姿图；其中，迭代法可以包括牛顿迭代、列文伯格-马克尔特迭代法等；图优化后的第一位姿图中提供最优相机位姿，并且测量得到最优特征点深度。

[0153] 类似地，作为一种可选的实施方式，对于视觉SLAM建图，第二位姿图可以包括根据第二子地图中各个关键帧对应的特征点构建成的第二特征节点，以及根据各个关键帧对应的传感器位姿信息所构建的第二位姿节点。第二图优化子单元3072，具体用于利用迭代法对第二位姿图中任意两个第二位姿节点对应的相机位姿变化量，以及相应的第二位姿节点和第二特征节点之间所对应的深度观测量进行多次迭代，使得误差值最小化，以获得图优化后的第二位姿图；其中，迭代法可以包括牛顿迭代、列文伯格-马克尔特迭代法等；图优化后的第二位姿图中提供最优相机位姿，并且测量得到最优特征点深度。

[0154] 进一步地，作为一种可选的实施方式，整合子单元3073，具体用于基于图优化后的第一位姿图所提供的传感器位姿信息以及关键帧的特征点深度信息，生成图优化后的第一子地图；基于图优化后的第二位姿图所提供的传感器位姿信息以及关键帧的特征点深度信息，生成图优化后的第二子地图，以利用图优化后的第一子地图和图优化后的第二子地图更新原有的完整SLAM地图，获得图优化后的完整SLAM地图，其中，SLAM地图可以为三维地图。

[0155] 再进一步地，可选的，针对第一子地图和第二子地图整合生成的完整SLAM地图，第一子地图/第二子地图可以为地图拼接系统通过深度相机采集图像帧，并且将三维点云变换到世界坐标下进行拼接和逐步叠加后构建的稠密且特征丰富的三维地图。而针对用于拼接第一子地图和第二子地图的虚拟运动轨迹所在区域，还可以在车辆沿虚拟运动轨迹移动的过程中，根据若干虚拟关键帧对应的时刻所采集到的图像帧，在完整SLAM地图上的虚拟运动轨迹所在区域建立稀疏子地图，以丰富完整SLAM地图的内容。

[0156] 可见，实施图4所描述的系统，基于已构建的SLAM子地图，在检测到SLAM跟踪丢失时，重新进行SLAM建图初始化，同时，根据获取到的车辆位姿信息推算出车载传感器的传感器位姿信息，来产生虚拟关键帧，以利用虚拟关键帧将前后两次构建的SLAM子地图拼接起来，能够提高构建完整的SLAM地图的成功率，其中，车载传感器包括车载相机或者车载激光雷达，可以适用于视觉SLAM建图或者激光SLAM建图；此外，在SLAM跟踪丢失时，通过采集车辆的位姿信息，还能够保证记录车辆的运动轨迹的连续性，用于辅助后续的车辆导航；进一步地，在检测到车辆回到虚拟运动轨迹中任一虚拟关键帧对应的位置时，通过虚拟运动轨迹指引的路线控制车辆行驶，能够在未覆盖SLAM地图的区域内实现车辆的自主导航与驾驶；再进一步地，对各子地图进行融合车身里程计和视觉特征的位姿图优化，能够减少经多个图像帧累积的误差，提高构建完整SLAM地图的准确度，还能够使得车辆的虚拟运动轨迹与车辆在第一子地图中的运动轨迹之间的误差最小。

[0157] 本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行图1～图2任意一种SLAM地图拼接方法。

[0158] 本发明实施例还公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行如以上各方法实施例中的方法的部分或全部步骤。

[0159] 本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(Compact Disc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。

[0160] 以上对本发明实施例公开的一种SLAM地图拼接方法及系统进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。