(12)发明专利申请
(10)申请公布号 CN 111561923 A(43)申请公布日 2020.08.21
(21)申请号 202010422626.7(22)申请日 2020.05.19
(71)申请人 北京数字绿土科技有限公司
地址 100094 北京市海淀区东北旺北京中
关村软件园孵化器2号楼三层2301-2308室(72)发明人 刘继廷
其他发明人请求不公开姓名 (74)专利代理机构 北京绘聚高科知识产权代理
事务所(普通合伙) 11832
代理人 罗硕(51)Int.Cl.
G01C 21/00(2006.01)G01C 21/16(2006.01)G01S 19/45(2010.01)
权利要求书2页 说明书8页 附图5页
G01S 19/47(2010.01)
(54)发明名称
系统基于多传感器融合的SLAM制图方法、
(57)摘要
本发明提供了一种基于多传感器融合的SLAM制图方法和系统，应用于服务器，包括：获取移动平台关于周围环境的多个传感器数据；多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；对多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；基于多个定位信息，得到移动平台的目标定位信息；基于目标定位信息，生成局部高精度地图；对局部高精度地图进行闭环检测操作，得到移动平台的高精度全局地图。本发明缓解了现有技术中存在的容易受周围环境限制而导致的精度低的技术问题。CN 111561923 ACN 111561923 A
权 利 要 求 书
1/2页
1.一种基于多传感器融合的SLAM制图方法，其特征在于，应用于服务器，包括：获取移动平台关于周围环境的多个传感器数据；所述多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；
对所述多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；
基于所述多个定位信息，得到所述移动平台的目标定位信息；基于所述目标定位信息，生成局部高精度地图；对所述局部高精度地图进行闭环检测操作，得到所述移动平台的高精度全局地图。2.根据权利要求1所述的方法，其特征在于，获取移动平台关于周围环境的多个传感器数据，包括：
以激光器为基准，标定相机、IMU、GNSS和所述激光器之间的位置关系，得到标定信息；其中，所述激光器、所述相机、IMU和所述GNSS均为所述移动平台上的传感器；
以所述GNSS的时间为基准，同步所述激光器、所述相机、所述IMU的时间到当前GNSS的时间系统；
同步采集所述激光器、所述相机、所述IMU和所述GNSS的数据，得到所述移动平台关于周围环境的多个传感器数据；其中，所述点云数据为所述激光器采集到的数据，所述图像数据为所述相机采集到的数据，所述IMU数据为所述IMU采集到的数据，所述GNSS数据为所述GNSS采集到的数据。
3.根据权利要求2所述的方法，所述多个定位信息包括：初始定位信息，第一定位信息和第二定位信息，其特征在于，对所述多个传感器数据进行层级处理，生成多个定位信息，包括：
基于所述IMU数据、所述GNSS数据和所述标定信息，生成初始定位信息；基于所述初始定位信息和所述图像数据，利用视觉SLAM生成第一定位信息；基于所述第一定位信息和所述点云数据，利用激光SLAM生成第二定位信息。4.根据权利要求3所述的方法，其特征在于，基于所述多个定位信息，得到所述移动平台的目标定位信息，包括：
提取所述图像数据的关键帧匹配点集和所述点云数据匹配点集；基于所述第二定位信息、所述IMU数据、所述GNSS数据、所述关键帧匹配点集和所述点云数据匹配点集，生成综合定位信息数据库；
对所述综合定位信息数据库中的数据集进行联合优化，得到所述移动平台的高精度轨迹；
将所述高精度轨迹作为所述目标定位信息。5.根据权利要求4所述的方法，所述局部高精度地图包括：影像局部地图和点云三维场景局部地图，其特征在于，基于所述目标定位信息，生成局部高精度地图，包括：
基于所述高精度轨迹解算所述图像数据的关键帧的位置和姿态信息，生成影像局部地图；
基于所述高精度轨迹解算所述点云数据的位置和姿态信息，生成点云三维场景局部地图。
6.根据权利要求5所述的方法，其特征在于，对所述局部高精度地图进行闭环检测操
2
CN 111561923 A
权 利 要 求 书
2/2页
作，得到所述移动平台的高精度全局地图，包括：
对所述局部高精度地图进行闭环检测操作，得到局部地图旋转平移矩阵；基于所述局部地图旋转平移矩阵构造图优化中位姿约束；利用所述图优化中位姿约束对所述高精度轨迹进行修正，得到修正后的高精度轨迹；基于所述修正后的高精度轨迹，得到所述移动平台的高精度全局地图。7.一种基于多传感器融合的SLAM制图系统，其特征在于，应用于服务器，包括：获取模块，层级处理模块，定位模块，第一生成模块和第二生成模块，其中，
所述获取模块，用于获取移动平台关于周围环境的多个传感器数据；所述多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；
所述层级处理模块，用于对所述多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；
所述定位模块，用于基于所述多个定位信息，得到所述移动平台的目标定位信息；所述第一生成模块，用于基于所述目标定位信息，生成局部高精度地图；所述第二生成模块，用于对所述局部高精度地图进行闭环检测操作，得到所述移动平台的高精度全局地图。
8.根据权利要求7所述的系统，其特征在于，所述获取模块包括：标定单元，同步单元和采集单元，其中，
所述标定单元，用于以激光器为基准，标定相机、IMU、GNSS和所述激光器之间的位置关系，得到标定信息；其中，所述激光器、所述相机、IMU和所述GNSS均为所述移动平台上的传感器；
所述同步单元，用于以所述GNSS的时间为基准，同步所述激光器、所述相机、所述IMU的时间到当前GNSS的时间系统；
所述采集单元，用于同步采集所述激光器、所述相机、所述IMU和所述GNSS的数据，得到所述移动平台关于周围环境的多个传感器数据；其中，所述点云数据为所述激光器采集到的数据，所述图像数据为所述相机采集到的数据，所述IMU数据为所述IMU采集到的数据，所述GNSS数据为所述GNSS采集到的数据。
9.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至6任一项所述的方法的步骤。
10.一种具有处理器可执行的非易失的程序代码的计算机可读介质，其特征在于，所述程序代码使所述处理器执行所述权利要求1-6任一项所述方法。
3
CN 111561923 A
说 明 书
基于多传感器融合的SLAM制图方法、系统
1/8页
技术领域
[0001]本发明涉及导航多传感器融合技术领域，尤其是涉及一种基于多传感器融合的SLAM制图方法、系统。背景技术[0002]SLAM (SIMUltaneous Localization And Mapping)技术是指即时定位与地图构建，即通过处理传感器采集的周围环境数据，实时反馈当前运动系统在未知环境的位置并同时绘制运动系统的周围环境地图，此地图可以是2D平面地图，也可以是三维周围环境地图。在机器人、自动驾驶、虚拟现实、测绘、农业、林业、电力、建筑等行业已经有了很广泛的应用。目前常用的传感器单元包括激光器、惯性导航系统(Inertial Measurement Unit，IMU)、视觉相机、全球导航卫星系统（Global Navigation Satellite System，GNSS）。[0003]当前比较成熟的SLAM算法大致可以分为激光SLAM和视觉SLAM两类。激光SLAM指主要利用激光传感器获取数据，进行同步定位和制图。激光器不依赖周围环境的光照，且能够扫描周围环境的高精度三维信息，其算法较为稳健，目前随着激光器成本的降低，逐渐成为SLAM领域较为热门的研究领域之一。 但是激光器在无明显结构的环境中，如平面墙、草原、狭窄走廊等环境，其无法探测到有效的环境特征，导致定位和制图容易失败。视觉SLAM主要是利用相机传感器来获取周围环境的影像数据，通过拍摄的影像信息来进行定位和制图。其价格低廉、可视化性强，一直是SLAM研究领域最热门的方向。但是视觉相机很依赖周围环境的光照信息和纹理信息，一但光照变化过大，或者纹理重复性单一，容易导致制图失败。[0004]无论是激光SLAM还是视觉SLAM两着还存在随着时间的增长，累计误差会逐渐增加，造成定位和制图效果精度下降，目前比较流行的方式是通过闭环等方式来进行修正，但是在大规模制图中，受限周围环境，精度往往都达不到目前地图生产所要求的精度。发明内容
[0005]有鉴于此，本发明的目的在于提供一基于多传感器融合的SLAM制图方法和系统，以缓解了现有技术中存在的容易受周围环境限制而导致的精度低、误差大的技术问题。[0006]第一方面，本发明实施例提供了一种基于多传感器融合的SLAM制图方法，应用于服务器，包括：获取移动平台关于周围环境的多个传感器数据；所述多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；对所述多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；基于所述多个定位信息，得到所述移动平台的目标定位信息；基于所述目标定位信息，生成局部高精度地图；对所述局部高精度地图进行闭环检测操作，得到所述移动平台的高精度全局地图。[0007]进一步地，获取移动平台关于周围环境的多个传感器数据，包括：以激光器为基准，标定相机、IMU、GNSS和所述激光器之间的位置关系，得到标定信息；其中，所述激光器、所述相机、IMU和所述GNSS均为所述移动平台上的传感器；以所述GNSS的时间为基准，同步所述激光器、所述相机、所述IMU的时间到当前GNSS的时间系统；同步采集所述激光器、所述
4
CN 111561923 A
说 明 书
2/8页
相机、所述IMU和所述GNSS的数据，得到所述移动平台关于周围环境的多个传感器数据；其中，所述点云数据为所述激光器采集到的数据，所述图像数据为所述相机采集到的数据，所述IMU数据为所述IMU采集到的数据，所述GNSS数据为所述GNSS采集到的数据。[0008]进一步地，所述多个定位信息包括：初始定位信息，第一定位信息和第二定位信息，对所述多个传感器数据进行层级处理，生成多个定位信息，包括：基于所述IMU数据、所述GNSS数据和所述标定信息，生成初始定位信息；基于所述初始定位信息和所述图像数据，利用视觉SLAM生成第一定位信息；基于所述第一定位信息和所述点云数据，利用激光SLAM生成第二定位信息。[0009]进一步地，基于所述多个定位信息，得到所述移动平台的目标定位信息，包括：提取所述图像数据的关键帧匹配点集和所述点云数据匹配点集；基于所述第二定位信息、所述IMU数据、所述GNSS数据、所述关键帧匹配点集和所述点云数据匹配点集，生成综合定位信息数据库；对所述综合定位信息数据库中的数据集进行联合优化，得到所述移动平台的高精度轨迹；将所述高精度轨迹作为所述目标定位信息。[0010]进一步地，所述局部高精度地图包括：影像局部地图和点云三维场景局部地图，基于所述目标定位信息，生成局部高精度地图，包括：基于所述高精度轨迹解算所述图像数据的关键帧的位置和姿态信息，生成影像局部地图；基于所述高精度轨迹解算所述点云数据的位置和姿态信息，生成点云三维场景局部地图。[0011]进一步地，对所述局部高精度地图进行闭环检测操作，得到所述移动平台的高精度全局地图，包括：对所述局部高精度地图进行闭环检测操作，得到局部地图旋转平移矩阵；基于所述局部地图旋转平移矩阵构造图优化中位姿约束；利用所述图优化中位姿约束对所述高精度轨迹进行修正，得到修正后的高精度轨迹；基于所述修正后的高精度轨迹，得到所述移动平台的高精度全局地图。[0012]第二方面，本发明实施例还提供了一种基于多传感器融合的SLAM制图系统，应用于服务器，包括：获取模块，层级处理模块，定位模块，第一生成模块和第二生成模块，其中，所述获取模块，用于获取移动平台关于周围环境的多个传感器数据；所述多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；所述层级处理模块，用于对所述多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；所述定位模块，用于基于所述多个定位信息，得到所述移动平台的目标定位信息；所述第一生成模块，用于基于所述目标定位信息，生成局部高精度地图；所述第二生成模块，用于对所述局部高精度地图进行闭环检测操作，得到所述移动平台的高精度全局地图。[0013]进一步地，所述获取模块包括：标定单元，同步单元和采集单元，其中，所述标定单元，用于以激光器为基准，标定相机、IMU、GNSS和所述激光器之间的位置关系，得到标定信息；其中，所述激光器、所述相机、IMU和所述GNSS均为所述移动平台上的传感器；所述同步单元，用于以所述GNSS的时间为基准，同步所述激光器、所述相机、所述IMU的时间到当前GNSS的时间系统；所述采集单元，用于同步采集所述激光器、所述相机、所述IMU和所述GNSS的数据，得到所述移动平台关于周围环境的多个传感器数据；其中，所述点云数据为所述激光器采集到的数据，所述图像数据为所述相机采集到的数据，所述IMU数据为所述IMU采集到的数据，所述GNSS数据为所述GNSS采集到的数据。[0014]第三方面，本发明实施例还提供了一种电子设备，包括存储器、处理器及存储在所
5
CN 111561923 A
说 明 书
3/8页
述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一方面所述的方法的步骤。[0015]第四方面，本发明实施例还提供了一种具有处理器可执行的非易失的程序代码的计算机可读介质，所述程序代码使所述处理器执行上述第一方面所述方法。[0016]本发明提供了一种基于多传感器融合的SLAM制图方法和系统，应用于服务器，包括：获取移动平台关于周围环境的多个传感器数据；多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；对多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；基于多个定位信息，得到移动平台的目标定位信息；基于目标定位信息，生成局部高精度地图；对局部高精度地图进行闭环检测操作，得到移动平台的高精度全局地图。本发明缓解了现有技术中存在的容易受周围环境限制而导致的精度低、误差大的技术问题。附图说明
[0017]为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0018]图1为本发明实施例提供的一种基于多传感器融合的SLAM制图方法的流程图；
图2为本发明实施例提供的一种获取移动平台关于周围环境的多个传感器数据的方法流程图；
图3为本发明实施例提供的一种对多个传感器数据进行层级处理的方法流程图；图4为本发明实施例提供的一种对多个定位信息进行联合优化的方法流程图；图5为本发明实施例提供的一种得到移动平台的高精度全局地图的方法流程图；图6为本发明实施例提供的一种基于多传感器融合的SLAM制图系统的示意图；图7为本发明实施例提供的另一种基于多传感器融合的SLAM制图系统的示意图。具体实施方式
[0019]下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。[0020]实施例一：
图1是根据本发明实施例提供的一种基于多传感器融合的SLAM制图方法的流程图，该方法应用于服务器。如图1所示，该方法具体包括如下步骤：
步骤S102，获取移动平台关于周围环境的多个传感器数据；多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据。[0021]具体地，通过激光器采集周围环境的点云信息，得到点云数据；通过相机采集图像信息，得到图像数据；通过IMU获取移动平台的角速度和加速度，得到IMU数据；通过GNSS获取每一时刻经纬度绝对坐标，得到GNSS数据。[0022]步骤S104，对多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感
6
CN 111561923 A
说 明 书
4/8页
器数据对应于一个定位信息。[0023]步骤S106，基于多个定位信息，得到移动平台的目标定位信息。[0024]步骤S108，基于目标定位信息，生成局部高精度地图。[0025]步骤S110，对局部高精度地图进行闭环检测操作，得到移动平台的高精度全局地图。
[0026]本发明提供了一种基于多传感器融合的SLAM制图方法，获取移动平台关于周围环境的多个传感器数据；多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据；对多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息；基于多个定位信息，得到移动平台的目标定位信息；基于目标定位信息，生成局部高精度地图；对局部高精度地图进行闭环检测操作，得到移动平台的高精度全局地图。本发明缓解了现有技术中存在的容易受周围环境限制而导致的精度低、误差大的技术问题。[0027]可选地，如图2所示，步骤S102包括如下步骤：
步骤S1021，以激光器为基准，标定相机、IMU、GNSS和激光器之间的位置关系，得到标定信息；其中，激光器、相机、IMU和GNSS均为移动平台上的传感器。[0028]具体地，激光器和相机的标定，使用在同一刚体上的激光器和相机对着标定板采集数据，拟合点云数据里面的面特征，以及图像中的角点等信息，通过点到面的距离去优化求解出相机和激光器的外参，激光器和IMU之间的标定，通过激光器采集的点云数据可以获取移动设备的轨迹，通过IMU也可以获取移动设备的轨迹，对轨迹进行对齐，即可以获得外参，由于IMU的加速度计容易漂，只能大致的估计两者之间的外参（或者通过尺子进行量测）。
[0029]步骤S1022，以GNSS的时间为基准，同步激光器、相机、IMU的时间到当前GNSS的时间系统。
[0030]步骤S1023，同步采集激光器、相机、IMU和GNSS的数据，得到移动平台关于周围环境的多个传感器数据；其中，点云数据为激光器采集到的数据，图像数据为相机采集到的数据，IMU数据为IMU采集到的数据，GNSS数据为GNSS采集到的数据。[0031]在本发明实施例中，多个定位信息包括：初始定位信息，第一定位信息和第二定位信息。
[0032]可选地，如图3所示，步骤S104包括如下步骤：
步骤S1041，基于IMU数据、GNSS数据和标定信息，生成初始定位信息。[0033]具体地，首先在移动平台上完成导航系统的初始化及对准，对卡尔曼滤波参数的初始参数进行设定，然后利用GNSS卫星定位信息，以及惯性导航系统（INS），以卡尔曼滤波原理，求解得到每一时刻t的状态后验估计信息P0t。[0034]步骤S1042，基于初始定位信息和图像数据，利用视觉SLAM生成第一定位信息。[0035]具体地，a)，针对每一关键帧图像计算其特征点，具体特征点包含ORB特征点及Harris角点等。[0036]b).对相邻两个图像关键帧，依据初始定位信息P0，得到位置修正后的 特征顶F1和F2如下所示，其中，Pt-1表示图像帧F1的所有特征点集合，Pt表示图像帧F2的所有特征点集合。
7
CN 111561923 A
说 明 书
5/8页
[0037]通过RANSAC去除异常点，并进一步优化计算得到相机归一化平面下的特征点,
。
[0038]
由于处在同一特征环境下，这些特征匹配点对的变换关系可表示为如下式所示：
；
其中，R为机器人位姿旋转变换矩阵，T为机器人位移矩阵，与为t时刻到t+1时
刻的特征点匹配点对。采用最小化重投影误差的方法来求解位姿R与T，如下式：
其中，表示图像帧F1的所有特征点集合，表示图像帧载体的旋转矩阵，T为载体的平移向量，表示特征点对的数目。
[0039]
的所有特征点集合；为
计算得到相邻关键帧的旋转平移矩阵，并依次得到所有关键帧的第一定位信息
P1，将当前的最优特征匹配对（即关于第一定位信息P1的最优特征匹配对）加入到匹配数据库中。
[0040]步骤S1043，基于第一定位信息和点云数据，利用激光SLAM生成第二定位信息。
[0041]
具体的，对于当前帧激光点云数据，面特征
。
，依据以下公式可计算得到其点特征，线
特征
[0042]其中，i为中的一个点，Xi为点i的坐标，p为点i的邻域点集合，j为p中的点，Xi为
，大于M4的点属于
。
点i的坐标，f为特征值；预先给定阈值M1、M2、M3和M4，针对当前点的特征值f小于M1的点属于特征
[0043]
，大于M2小于M3的点属于
根据第一定位信息P1，将每帧的特征数据转换到P1定位信息对应的坐标系。获取
和
，对所有匹配对
在
中进行领域搜索，确定所有候选特征匹
相邻两帧点云数据
配对。根据匹配对并通过最小二乘法求解得到相邻两帧点云数据的旋转平移参数R和T。具体的，可通过如下公式求解参数：
其中，Y表示从相邻的后一数据帧中提取的特征，X表示从相邻的前一数据帧中提取的特征，为载体的旋转矩阵，为载体的平移向量。
[0044]
然后根据得到的结果对匹配对进行优选，并重新计算特征点
中重新查找特征点对，重新计算得到新的旋转平移矩阵
和
。对点云和
中的
特征点在，并更新之。最
终得到相邻两帧的旋转平移位置信息，并将当前的最优特征匹配对加入到匹配
8
CN 111561923 A
说 明 书
6/8页
数据库K中。[0045]最后，通过依据相邻帧的转换矩阵，得到依据激光点云数据的第二定位信息P2（即关于第二定位信息P2的最优特征匹配对）。上述图3涉及了如下操作，步骤S1041，基于IMU数据、GNSS数据和标定信息，生成初始定位信息。步骤S1042，基于初始定位信息和图像数据，利用视觉SLAM生成第一定位信息。步骤S1043，基于第一定位信息和点云数据，利用激光SLAM生成第二定位信息。在上述技术方案中，基于IMU数据、GNSS数据和标定信息，生成初始定位信息采用了全新的算法设计（最小化重投影误差的方法结合最小二乘法），应用上述计算算法，得到初始定位信息。[0046]可选地，图4是根据本发明实施例提供的一种对多个定位信息进行联合优化的方法流程图。如图4所示，该方法包括如下步骤：
步骤S1061，提取图像数据的关键帧匹配点集和点云数据匹配点集。[0047]步骤S1062，基于第二定位信息、IMU数据、GNSS数据、关键帧匹配点集和点云数据匹配点集，生成综合定位信息数据库。[0048]步骤S1063，对综合定位信息数据库中的数据集进行联合优化，得到移动平台的高精度轨迹。
[0049]步骤S1064，将高精度轨迹作为目标定位信息。[0050]具体地，首先构建一个容量为n的滑动窗口，滑动窗口每个单元包含原始相机关键帧匹配对信息，或激光点云匹配对信息以及IMU预积分信息，其中，IMU预积分信息为连续两帧数据的所有IMU数据通过IMU预积分模型构成的一个观测值。然后，依次对滑动窗口内的数据构造因子图模型，包括构建IMU预积分约束键帧匹配约束
以及GNSS位置约束
，激光点云特征匹配约束
，图像关
等，通过求解联合概率分布的最大后验概率，得
到每一个时间点的各个状态变量。其中，需要估计的状态变量为
。
[0051]
其中，分别表示世界坐标系的三维坐标；
表示姿态角；
分别表示东向、北向、
分
天向速度；
别表示加速度计偏差量。
[0052]
分别表示陀螺仪偏差量；
对每一时刻的状态集合
，求解联合概率分布；
,根据上述构造的测量值集合，
的最大后验概率：
其中轨迹T。
[0053]
,表示的最优估计值。求解得到最优状态量，从而得到高精度
在本发明实施例中，局部高精度地图包括：影像局部地图和点云三维场景局部地图。可选地，步骤S108还包括如下步骤：
步骤S1081，基于高精度轨迹解算图像数据的关键帧的位置和姿态信息，生成影像局部地图。
9
CN 111561923 A[0054]
说 明 书
7/8页
步骤S1082，基于高精度轨迹解算点云数据的位置和姿态信息，生成点云三维场景
局部地图。
[0055]可选地，如图5所示，步骤S110包括如下步骤：
步骤S1101，对局部高精度地图进行闭环检测操作，得到局部地图旋转平移矩阵。[0056]具体地，首先根据GNSS数据初步判断当前地图和之前扫描过的地图是否有重复。若经纬度信息相差在一定的阈值之内，则认为所处在同一地方，那么者两帧就会形成闭环。[0057]然后根据影像的特征点信息判断当前影像局部地图和之前形成的影像地图有无重复，进行影像闭环检测。具体地，将每帧图片的特征在字典里面进行搜索，计算相似度，相似度过高，则认为移动平台回到了之前的某一位置，形成闭环。
[0058]之后根据激光点云信息判断当前点云局部地图和之前形成的点云地图有无重复，判断进行点云闭环检测。
[0059]
具体的，针对获取的两帧候选判断的点云和，计算其配准误差，求解最小误差
下面公式为计算配准误差及最小误差函数公式：函数。
其中为所述当前地图中的待配准数据帧中的点，据帧中的点，数。
[0060]
为的坐标，为全局坐标系数
为配准误差，为预设范
为的坐标，为j到i的旋转平移矩阵，
判断依据为基于点云的重叠度，即当配准误差最小时，其同名点占点云重叠区域
即为其旋转平移矩
的点的比例。如果点云重叠度高于一定百分比则判断其为闭环。此时阵。
[0061][0062]
步骤S1102，基于局部地图旋转平移矩阵构造图优化中位姿约束。步骤S1103，利用图优化中位姿约束对高精度轨迹进行修正，得到修正后的高精度
轨迹。
步骤S1104，基于修正后的高精度轨迹，得到移动平台的高精度全局地图。
[0064]在本发明实施例中，随着时间的推移，累计误差会越来越大，导致精度降低，而闭环检测可以去判断当前帧所采集的场景和之前的某一帧是否有很大的相似，若是，则形成了环，优化的时候就可以降低误差的累积，生成高精度的全局地图。[0065]与现有技术的方式相比，本发明实施例具有以下至少一种优点：
（1）解决了单一传感器在特征环境下SLAM制图失败的情况，系统采集处理稳定性、鲁棒性较强；
（2）利用多传感器的融合，解决了常规SLAM算法累计误差大的问题，提高了全局地图的成图精度。
[0066]实施例二：
图6是根据本发明实施例提供的一种基于多传感器融合的SLAM制图系统的示意图，该
10
[0063]
CN 111561923 A
说 明 书
8/8页
系统应用于服务器。如图6所示，该系统包括：获取模块10，层级处理模块20，定位模块30，第一生成模块40和第二生成模块50。[0067]具体地，获取模块10，用于获取移动平台关于周围环境的多个传感器数据；多个传感器数据包括：点云数据，图像数据，IMU数据和GNSS数据。[0068]层级处理模块20，用于对多个传感器数据进行层级处理，生成多个定位信息；其中，一个传感器数据对应于一个定位信息。[0069]定位模块30，用于基于多个定位信息，得到移动平台的目标定位信息。[0070]第一生成模块40，用于基于目标定位信息，生成局部高精度地图。[0071]第二生成模块50，用于对局部高精度地图进行闭环检测操作，得到移动平台的高精度全局地图。[0072]可选地，图7是根据本发明实施例提供的另一种基于多传感器融合的SLAM制图系统的示意图。如图7所示，获取模块10包括：标定单元11，同步单元12和采集单元13。[0073]具体地，标定单元11，用于以激光器为基准，标定相机、IMU、GNSS和激光器之间的位置关系，得到标定信息；其中，激光器、相机、IMU和GNSS均为移动平台上的传感器。[0074]同步单元12，用于以GNSS的时间为基准，同步激光器、相机、IMU的时间到当前GNSS的时间系统。
[0075]采集单元13，用于同步采集激光器、相机、IMU和GNSS的数据，得到移动平台关于周围环境的多个传感器数据；其中，点云数据为激光器采集到的数据，图像数据为相机采集到的数据，IMU数据为IMU采集到的数据，GNSS数据为GNSS采集到的数据。[0076]本发明实施例还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述实施例一中的方法的步骤。
[0077]本发明实施例还提供了一种具有处理器可执行的非易失的程序代码的计算机可读介质，程序代码使处理器执行上述实施例一中的方法。[0078]最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
11
CN 111561923 A
说 明 书 附 图
1/5页
图1
12
CN 111561923 A
说 明 书 附 图
2/5页
图2
图3
13
CN 111561923 A
说 明 书 附 图
3/5页
图4
14
CN 111561923 A
说 明 书 附 图
4/5页
图5
15
CN 111561923 A
说 明 书 附 图
5/5页
图6
图7
16
因篇幅问题不能全部显示，请点此查看更多更全内容