多模态数据联合优化的同步定位及建图方法及系统与流程

1.本发明涉及多模态传感器融合的同步定位技术领域，具体地说是多模态数据联合优化的同步定位及建图方法及系统。


背景技术：

2.同步定位与建图(slam)技术旨在利用传感器数据估计环境采集设备在未知场景中的运动状态并建立场景地图，在数字孪生、虚拟现实、增强现实、混合现实、自动驾驶、无人机、移动机器人定位导航及场景图构建等领域有着广泛的应用。相机与激光雷达在环境信息采集中扮演着重要的角色，基于视觉的slam技术主要利用单目、双目及rgb-d相机获取环境信息，其价格及计算成本较低，但对光线变化敏感，另外，大尺度场景中深度信息的获取比较困难。另一方面，基于激光雷达的slam技术主要利用2d与3d雷达采集环境的几何结构信息，对光线变化不敏感，但当环境结构化信息不明显的场景中，定位精度及鲁棒性较差。
3.为提高slam在不同场景中的定位精度和鲁棒性，多模态传感数据融合定位与建图的方式吸引了研究者广泛关注。另外，随着科技的发展，slam技术涉及的传感器，比如相机、雷达、惯性测量单元、gps等往小型化发展，价格也有所下降，这使得在环境采集设备上部署多种传感器，利用多模态数据进行同步定位与建图成为可能，但仍没有通用的框架将多种传感模式的信息高效地进行融合。
4.如何将多种传感模式的信息高效融合，实现基于多模态数据的同步定位和建图，是需要解决的技术问题。


技术实现要素：

5.本发明的技术任务是针对以上不足，提供多模态数据联合优化的同步定位及建图方法，来解决如何将多种传感模式的信息高效融合，实现基于多模态数据的同步定位和建图的技术问题。
6.第一方面，本发明一种多模态数据联合优化的同步定位及建图方法，应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，基于所述装置提供包括视觉-惯性里程计模式、激光-惯性里程计模式以及视觉-惯性-激光里程计模式的多模态模式；
7.基于所述多模态模式进行位姿估计以及场景图构建，包括如下步骤：
8.在环境纹理信息满足预定条件时，通过相机单元和惯性测量单元提供视觉-惯性里程计模式，在视觉-惯性里程计模式下，基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建；
9.在环境结构化信息满足预定条件时，通过惯性测量单元和激光雷达单元提供激光-惯性里程计模式，在激光-惯性里程计模式下，基于采集的点云数据进行位姿估计以及场景地图构建；
10.在环境纹理信息和环境结构信息均不满足预定条件时，通过相机单元、惯性测量
单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，将相机、雷达与惯性测量单元标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建。
11.作为优选，在视觉-惯性里程计模式下，基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建，包括如下步骤：
12.对采集的rgb图像进行特征提取，得到图像特征，并对惯性测量单元的原始数据进行预积分处理，得到惯性测量单元预防积分结果；
13.基于图像特征和惯性测量单元预积分结果初始化相机位姿，得到相机以及惯性测量单元在世界坐标系中的状态；
14.通过与局部地图的比较，判断当前采集的rgb图像是否为关键帧，如果是，将所述关键帧加入局部优化任务中，进行局部地图的构建；
15.基于词袋模型提取关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
16.基于回环侯选帧与当前帧的相对运动，验证回环是否成立；
17.基于惯性测量单元约束以及回环检测约束构建任务约束，基于任务约束进行全局优化，得到优化后的位姿。
18.作为优选，在激光-惯性里程计模式下，基于采集的点云数据进行位姿估计以及场景地图构建，包括如下步骤：
19.对当前帧点云数据进行畸变矫正；
20.对于矫正后的点云数据，提取所述点云数据的线特征和面特征；
21.基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，通过正态分布变换方法获取当前帧点云数据与前一帧点云数据之间的相对运动信息，得到当前帧点云数据与前一帧点云数据之间的位姿变换；
22.如果位姿变化大于等于设定的阈值，当前帧点云数据作为关键帧，将所述关键帧加入局部优化任务中，进行局部地图的构建；
23.基于当前帧点云线、面特征及局部地图信息，通过正态分布变换方法，检测回环并构建回环约束。
24.作为优选，在视觉-惯性-激光里程计模式下，将相机、雷达与惯性测量单元标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建，包括如下步骤：
25.基于相机与激光雷达的标定，将rgb图像与激光雷达提供的距离信息相融合，得到环境的rgb-d信息；
26.循环执行如下操作直至完成全局地图的构建：
27.基于所述rgb-d信息，在激光-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，将通过视觉-惯性里程计模式获得的优化后位姿作为初始位姿，基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，并以在视觉-惯性里程计下得到的回环侯选帧为初始回环检测范围；
28.通过基于所述rgb-d信息，在视觉-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，以激光-惯性里程计模式下获得的优化后位姿
为初始位姿。
29.作为优选，在视觉-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程计因子和视觉回环因子；
30.在激光-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及激光回环因子；
31.在视觉-惯性-激光里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及视觉-激光回环因子；
32.其中，构建惯性测量单元预积分因子包括如下步骤：
33.以距离当前时刻最近的传感器位姿为基础位姿，计算所述基础位姿与当前时刻惯性测量单元里程计增量位姿变换，通过进行相乘计算得到当前时刻惯性测量单元里程计位姿，其中，对于视觉-惯性里程计模式，所述传感器为相机，对于激光-惯性里程计模式，所述传感器为激光雷达；
34.计算前一帧与当前帧之间的惯性测量单元预积分量，基于前一帧状态施加惯性测量单元预积分量得到当前帧初始状态估计，对于相邻时刻i和j，惯性测量单元预积分增量δv
ij
、位置预积分增量δp
ij
以及旋转预积分增量δr
ij
计算公式如下：
[0035][0036][0037][0038]
其中，vi表示第i时刻的速度，vj表示第j时刻的速度，g表示加速度，δt
ij
表示第i时刻和第j时刻的时间间隔，pi表示第i时刻的位置，pj表示第j时刻的位置，ri表示第i时刻的旋转矩阵，rj表示第j时刻的旋转矩阵；
[0039]
表示ri的转置；
[0040]
对于激光关键帧，基于提取的线特征和面特征，构建激光里程计因子，包括如下步骤：
[0041]
采用滑窗法创建区域地图
[0042]
其中，线特征集合
[0043]
面特征集合
[0044]
为第i-1刻关键帧的线特征在世界坐标系中的表示，
[0045]
为第i-1刻关键帧的面特征在世界坐标系中的表示，
[0046]
第i时刻激光关键帧的线特征及面特征距离计算公式如下：
[0047]
[0048][0049]
其中，k,u,v,w表示特征点的索引，与为对应的线特征上的两个点，与为对应的面特征上的三个点，第i时刻激光关键帧相当于世界坐标系的位姿变换ti表示为：
[0050][0051]
激光里程计因子
[0052]
在视觉-惯性里程计模式下，基于词袋模型构建视觉回环因子；
[0053]
在激光-惯性里程计模式下，基于正态分布变换方法构建激光回环因子；
[0054]
在视觉-惯性-激光里程计模式下，结合词袋模型和正态分布变换方法构建视觉-激光回环因子，包括如下步骤：
[0055]
基于词袋模型提取rgb图像对应关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0056]
通过点云数据确定所述回环侯选帧，得到确认的回环帧；
[0057]
在回环帧附近选取局部地图，通过局部地图进行扫描匹配，得到相对变换位姿，基于相对变换位姿构建回环因子；
[0058]
其中，在回环帧附近选取局部地图时，选取与回环帧位置和时间距离均大于预设阈值的关键帧，基于选取的关键帧构建局部地图。
[0059]
作为优选，所述装置还包括gps传感器，对应的，在用户选择使用gps传感器时，构建任务约束应包括构建gps因子；
[0060]
通过如下步骤构建gps因子：
[0061]
将gps数据转换到局部坐标系中，将gps和激光雷法标定，实现gps与激光雷达坐标系的对齐；
[0062]
基于估计位姿的波动，添加gps因子，如果估计位姿的波动大于gps位置波动，添加gps因子，否则，关闭gps因子。
[0063]
第二方面，本发明一种多模态数据联合优化的同步定位及建图系统，应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，所述系统用于基于如第一方面任一项所述的多模态数据联合优化的同步定位及建图方法进行位姿估计以及场景图构建，所述系统包括：
[0064]
视觉惯性里程计模块，在环境纹理信息满足预定条件时，所述视觉惯性里程计模块用于通过相机单元和惯性测量单元提供视觉-惯性里程计模式，在视觉-惯性里程计模式下，用于基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建；
[0065]
激光惯性里程计模块，在环境结构化信息满足预定条件时，所述激光惯性里程计
模块用于通过惯性测量单元和激光雷达单元提供激光-惯性里程计模式，在激光-惯性里程计模式下，用于基于采集的点云数据进行位姿估计以及场景地图构建；
[0066]
视觉-惯性-激光里程计模块，在环境纹理信息和环境结构信息均不满足预定条件时，所述视觉-惯性-激光里程计模块用于通过相机单元、惯性测量单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，用于将相机、雷达与惯性测量单元标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建；
[0067]
多模态数据联合优化模块，所述多模态数据联合优化模块用于构建任务约束，包括构建惯性测量单元预积分因子、视觉里程因子、激光里程因子、视觉回环因子、激光回环因子以及视觉-激光回环因子，所述惯性测量单元预积分因子、视觉里程因子、以及视觉回环因子配合作用于视觉-惯性里程计模式下的位姿优化，所述惯性测量单元预积分因子、激光里程因子以及激光回环因子配合作用于激光-惯性里程计模式下位姿优化，所述惯性测量单元预积分因子、激光里程因子、视觉里程因子以及视觉-激光回环因子配合作用于视觉-惯性-激光里程计模式下的位姿优化。8、根据权利要求7所述的多模态数据联合优化的同步定位及建图系统，其特征在于，所述视觉惯性里程计模块用于执行如下实现位姿估计和场景图构建：
[0068]
对采集的rgb图像进行特征提取，得到图像特征，并对惯性测量单元的原始数据进行预积分处理，得到惯性测量单元预防积分结果；
[0069]
基于图像特征和惯性测量单元预积分结果初始化相机位姿，得到相机以及惯性测量单元在世界坐标系中的状态；
[0070]
通过与局部地图的比较，判断当前采集的rgb图像是否为关键帧，如果是，将所述关键帧加入局部优化任务中，进行局部地图的构建；
[0071]
基于词袋模型提取关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0072]
基于回环侯选帧与当前帧的相对运动，验证回环是否成立；
[0073]
基于惯性测量单元约束以及回环检测约束构建任务约束，基于任务约束进行全局优化，得到优化后的位姿；
[0074]
所述激光惯性里程计模块用于执行如下进行位姿估计和场景图构建：
[0075]
对当前帧点云数据进行畸变矫正；
[0076]
对于矫正后的点云数据，提取所述点云数据的线特征和面特征；
[0077]
基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，通过正态分布变换方法获取当前帧点云数据与前一帧点云数据之间的相对运动信息，得到当前帧点云数据与前一帧点云数据之间的位姿变换；
[0078]
如果位姿变化大于等于设定的阈值，当前帧点云数据作为关键帧，将所述关键帧加入局部优化任务中，进行局部地图的构建；
[0079]
基于当前帧点云线、面特征及局部地图信息，通过正态分布变换方法，检测回环并构建回环约束；
[0080]
所述视觉-惯性-激光里程计用于执行如下进行位姿估计以及场景图构建：
[0081]
基于相机、雷达与惯性测量单元的标定，将rgb图像与激光雷达提供的距离信息相
融合，得到环境的rgb-d信息；
[0082]
循环执行如下操作直至完成全局地图的构建：
[0083]
基于所述rgb-d信息，在激光-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，将通过视觉-惯性里程计模式获得的优化后位姿作为初始位姿，基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，并以在视觉-惯性里程计下得到的回环侯选帧为初始回环检测范围；
[0084]
通过基于所述rgb-d信息，在视觉-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，以激光-惯性里程计模式下获得的优化后位姿为初始位姿。
[0085]
作为优选，在视觉-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程计因子和视觉回环因子；
[0086]
在激光-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及激光回环因子；
[0087]
在视觉-惯性-激光里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及视觉-激光回环因子；
[0088]
其中，构建惯性测量单元预积分因子包括如下步骤：
[0089]
以距离当前时刻最近的传感器位姿为基础位姿，计算所述基础位姿与当前时刻惯性测量单元里程计增量位姿变换，通过进行相乘计算得到当前时刻惯性测量单元里程计位姿，其中，对于视觉-惯性里程计模式，所述传感器为相机，对于激光-惯性里程计模式，所述传感器为激光雷达；
[0090]
计算前一帧与当前帧之间的惯性测量单元预积分量，基于前一帧状态施加惯性测量单元预积分量得到当前帧初始状态估计，对于相邻时刻i和j，惯性测量单元预积分增量δv
ij
、位置预积分增量δp
ij
以及旋转预积分增量δr
ij
计算公式如下：
[0091][0092][0093][0094]
其中，vi表示第i时刻的速度，vj表示第j时刻的速度，g表示加速度，δt
ij
表示第i时刻和第j时刻的时间间隔，pi表示第i时刻的位置，pj表示第j时刻的位置，ri表示第i时刻的旋转矩阵，rj表示第j时刻的旋转矩阵；
[0095]
表示ri的转置；
[0096]
对于激光关键帧，基于提取的线特征和面特征，构建激光里程计因子，包括如下步骤：
[0097]
采用滑窗法创建区域地图
[0098]
其中，线特征集合
[0099]
面特征集合
[0100]
为第i-1刻关键帧的线特征在世界坐标系中的表示，
[0101]
为第i-1刻关键帧的面特征在世界坐标系中的表示，
[0102]
第i时刻激光关键帧的线特征及面特征距离计算公式如下：
[0103][0104][0105]
其中，k,u,v,w表示特征点的索引，与为对应的线特征上的两个点，与为对应的面特征上的三个点，第i时刻激光关键帧相当于世界坐标系的位姿变换ti表示为：
[0106][0107]
激光里程计因子
[0108]
在视觉-惯性里程计模式下，基于词袋模型构建视觉回环因子；
[0109]
在激光-惯性里程计模式下，基于正态分布变换方法构建激光回环因子；
[0110]
在视觉-惯性-激光里程计模式下，结合词袋模型和正态分布变换方法构建视觉-激光回环因子，包括如下步骤：
[0111]
基于词袋模型提取rgb图像对应关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0112]
通过点云数据确定所述回环侯选帧，得到确认的回环帧；
[0113]
在回环帧附近选取局部地图，通过局部地图进行扫描匹配，得到相对变换位姿，基于相对变换位姿构建回环因子；
[0114]
其中，在回环帧附近选取局部地图时，选取与回环帧位置和时间距离均大于预设阈值的关键帧，基于选取的关键帧构建局部地图
[0115]
作为优选，所述装置还包括gps传感器，对应的，在用户选择使用gps传感器时，构建任务约束应包括构建gps因子；
[0116]
所述多模态数据联合优化模块用于执行如下构建gps因子：
[0117]
将gps数据转换到局部坐标系中，将gps和激光雷法标定，实现gps与激光雷达坐标系的对齐；
[0118]
基于估计位姿的波动，添加gps因子，如果估计位姿的波动大于gps位置波动，添加gps因子，否则，关闭gps因子。为为第刻关键帧的面特征在世界坐标系中的表示，为为第刻关键帧的面特征在世界坐标系中的表示，本发明的多模态数据联合优化的同步定位及建图方法及系统具有以下优点：
[0119]
1、基于视觉、惯性测量单元、激光雷达、gps等传感数据及回环约束进行联合优化，提高同步定位与建图技术在复杂环境中的精度和鲁棒性，可应用于数字孪生相关产品与平
台，实现高精视觉定位与场景图构建，解决虚拟世界与真实世界的精准映射问题；
[0120]
2、支持多种传感模式的融合，为虚拟孪生世界多传感器融合感知、高精定位与导航、动态目标实时追踪提供多模态融合架构支撑。
附图说明
[0121]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0122]
下面结合附图对本发明进一步说明。
[0123]
图1为实施例1一种多模态数据联合优化的同步定位及建图方法的流程框图。
具体实施方式
[0124]
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定，在不冲突的情况下，本发明实施例以及实施例中的技术特征可以相互结合。
[0125]
本发明实施例提供多模态数据联合优化的同步定位及建图方法及系统，用于解决的技术问题。
[0126]
实施例1：
[0127]
本发明一种多模态数据联合优化的同步定位及建图方法，应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，基于所述装置提供包括视觉-惯性里程计模式、激光-惯性里程计模式以及视觉-惯性-激光里程计模式的多模态模式。基于所述多模态模式进行位姿估计以及场景图构建，包括如下步骤：
[0128]
模式一、在环境纹理信息满足预定条件时，通过相机单元和惯性测量单元提供视觉-惯性里程计模式，在视觉-惯性里程计模式下，基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建；
[0129]
模式二、在环境结构化信息满足预定条件时，通过惯性测量单元和激光雷达单元提供激光-惯性里程计模式，在激光-惯性里程计模式下，基于采集的点云数据进行位姿估计以及场景地图构建；
[0130]
模式三、在环境纹理信息和环境结构信息均不满足预定条件时，通过相机单元、惯性测量单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，将相机与雷达标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建。
[0131]
其中，在视觉-惯性里程计模式下，基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建，包括如下步骤：
[0132]
(1)对采集的rgb图像进行特征提取，得到图像特征，并对惯性测量单元的原始数据进行预积分处理，得到惯性测量单元预防积分结果；
[0133]
(2)基于图像特征和惯性测量单元预积分结果初始化相机位姿，得到相机以及惯性测量单元在世界坐标系中的状态；
[0134]
(3)通过与局部地图的比较，判断当前采集的rgb图像是否为关键帧，如果是，将所述关键帧加入局部优化任务中，进行局部地图的构建；
[0135]
(4)基于词袋模型提取关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0136]
基于回环侯选帧与当前帧的相对运动，验证回环是否成立；
[0137]
(5)基于惯性测量单元约束以及回环检测约束构建任务约束，基于任务约束进行全局优化，得到优化后的位姿。
[0138]
在激光-惯性里程计模式下，基于采集的点云数据进行位姿估计以及场景地图构建，包括如下步骤：
[0139]
(1)对当前帧点云数据进行畸变矫正；
[0140]
(2)对于矫正后的点云数据，提取所述点云数据的线特征和面特征；
[0141]
(3)基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，通过正态分布变换方法获取当前帧点云数据与前一帧点云数据之间的相对运动信息，得到当前帧点云数据与前一帧点云数据之间的位姿变换；
[0142]
(4)如果位姿变化大于等于设定的阈值，当前帧点云数据作为关键帧，将所述关键帧加入局部优化任务中，进行局部地图的构建；
[0143]
(5)基于当前帧点云线、面特征及局部地图信息，通过正态分布变换方法，检测回环并构建回环约束。
[0144]
在视觉-惯性-激光里程计模式下，将相机与雷达标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建，包括如下步骤：
[0145]
(1)基于相机、激光雷达以及惯性测量单元的标定，将rgb图像与激光雷达提供的距离信息相融合，得到环境的rgb-d信息；
[0146]
(2)循环执行如下操作直至完成全局地图的构建：
[0147]
(2-1)基于所述rgb-d信息，在激光-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，将通过视觉-惯性里程计模式获得的优化后位姿作为初始位姿，基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，并以在视觉-惯性里程计下得到的回环侯选帧为初始回环检测范围；
[0148]
(2-2)通过基于所述rgb-d信息，在视觉-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，以激光-惯性里程计模式下获得的优化后位姿为初始位姿。
[0149]
当场景结构化信息不明显时，难以提取lidar点云线、面等几何特征，扫描匹配困难，设备位姿估计及地图构建精度低。场景纹理信息较弱时，图像特征提取困难，设备快速运动或抖动时，图像易模糊，视觉-惯性里程计难以获得设备的精确位姿。本实施例可依据场景特点，柔性化编排视觉、惯性及激光模块以实现复杂场景中的定位与建图。当视觉-惯性模组不能正常感知环境信息时，激光-惯性模块将依据视觉-惯性模组发送的信号，自动启动工作，形成视觉-惯性-激光联合工作模式。
[0150]
本实施例中，在视觉-惯性里程计模式下，构建任务约束包括构建惯性测量单元约束、视觉约束和回环检测约束，具体的涉及构建惯性测量单元预积分因子、视觉里程因子和视觉回环因子。
[0151]
在激光-惯性里程计模式下，构建任务约束包括构建惯性测量单元约束、激光雷达约束以及回环检测约束，具体的涉及构建惯性测量单元预积分因子、激光里程计因子以及激光回环因子。
[0152]
在视觉-惯性-激光里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程因子、激光里程计因子以及视觉-激光回环因子。
[0153]
其中，构建惯性测量单元预积分因子包括如下步骤：
[0154]
以距离当前时刻最近的传感器位姿为基础位姿，计算所述基础位姿与当前时刻惯性测量单元里程计增量位姿变换，通过进行相乘计算得到当前时刻惯性测量单元里程计位姿，其中，对于视觉-惯性里程计模式，所述传感器为相机，对于激光-惯性里程计模式，所述传感器为激光雷达；
[0155]
计算前一帧与当前帧之间的惯性测量单元预积分量，基于前一帧状态施加惯性测量单元预积分量得到当前帧初始状态估计，对于相邻时刻i和j，惯性测量单元预积分增量δv
ij
、位置预积分增量δp
ij
以及旋转预积分增量δr
ij
计算公式如下：
[0156][0157][0158][0159]
其中，vi表示第i时刻的速度，vj表示第j时刻的速度，g表示加速度，δt
ij
表示第i时刻和第j时刻的时间间隔，pi表示第i时刻的位置，pj表示第j时刻的位置，ri表示第i时刻的旋转矩阵，rj表示第j时刻的旋转矩阵；
[0160]
表示ri的转置。
[0161]
对于激光关键帧，基于提取的线特征和面特征，构建激光里程计因子，包括如下步骤：
[0162]
采用滑窗法创建区域地图
[0163]
其中，线特征集合
[0164]
面特征集合
[0165]
为第i-1刻关键帧的线特征在世界坐标系中的表示，
[0166]
为第i-1刻关键帧的面特征在世界坐标系中的表示，
[0167]
第i时刻激光关键帧的线特征及面特征距离计算公式如下：
[0168][0169][0170]
其中，k,u,v,w表示特征点的索引，与为对应的线特征上的两个点，
与为对应的面特征上的三个点，第i时刻激光关键帧相当于世界坐标系的位姿变换ti表示为：
[0171][0172]
激光里程计因子
[0173]
在视觉-惯性里程计模式下，基于词袋模型构建视觉回环因子。
[0174]
在激光-惯性里程计模式下，基于正态分布变换方法构建激光回环因子。
[0175]
在视觉-惯性-激光里程计模式下，结合词袋模型和正态分布变换方法构建视觉-激光回环因子，包括如下步骤：
[0176]
(1)基于词袋模型提取rgb图像对应关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0177]
(2)通过点云数据确定所述回环侯选帧，得到确认的回环帧；
[0178]
(3)在回环帧附近选取局部地图，通过局部地图进行扫描匹配，得到相对变换位姿，基于相对变换位姿构建回环因子；
[0179]
其中，在回环帧附近选取局部地图时，选取与回环帧位置和时间距离均大于预设阈值的关键帧，基于选取的关键帧构建局部地图。
[0180]
本实施例中，视觉-惯性里程计模式以及激光惯性理里程计模式下回环因子的构建采用现有的视觉-惯性里程计模式以及激光惯性理里程计模式中常用的方法，本实施例不做限定。
[0181]
作为改进，本实施例的装置中还包括gps传感器，对应的，构建任务约束应包括构建gps因子。
[0182]
本实施例中，通过如下步骤构建gps因子：
[0183]
(1)将gps数据转换到局部坐标系中，将gps和激光雷法标定，实现gps与激光雷达坐标系的对齐；
[0184]
(2)基于估计位姿的波动，添加gps因子，如果估计位姿的波动大于gps位置波动，添加gps因子，否则，关闭gps因子。
[0185]
本实施例中，依据任务及场景的不同，相机单元与惯性测量单元配合形成视觉惯性里程计，惯性测量单元和激光雷达配合可形成激光惯性里程计，相机单元、惯性测量单元以及激光激光雷达配合，通过柔性编排自动形成视觉-惯性-激光紧耦合的协同工作模式，以应对视觉或激光退化的场景，提高系统的精度及鲁棒性。
[0186]
实施例2：
[0187]
本发明一种多模态数据联合优化的同步定位及建图系统，应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，所述系统用于基于如权利要求1-6任一项所述的多模态数据联合优化的同步定位及建图方法进行位姿估计以及场景图构建。该系统包括多模态数据联合优化模块、视觉-惯性里程计模块、激光-惯性里程计模块以及视觉-惯性-激光里程计模块，该系统可基于实施例1公开的进行位姿估计以及场景图构建。
[0188]
环境纹理信息满足预定条件时，所述视觉-惯性里程计模块用于通过相机单元和
惯性测量单元提供视觉-惯性里程计模式，在视觉-惯性里程计模式下，用于基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建。
[0189]
本实施例中，视觉-惯性里程计模块用于执行如下实现位姿估计和场景图构建：
[0190]
(1)对采集的rgb图像进行特征提取，得到图像特征，并对惯性测量单元的原始数据进行预积分处理，得到惯性测量单元预防积分结果；
[0191]
(2)基于图像特征和惯性测量单元预积分结果初始化相机位姿，得到相机以及惯性测量单元在世界坐标系中的状态；
[0192]
(3)通过与局部地图的比较，判断当前采集的rgb图像是否为关键帧，如果是，将所述关键帧加入局部优化任务中，进行局部地图的构建；
[0193]
(4)基于词袋模型提取关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0194]
基于回环侯选帧与当前帧的相对运动，验证回环是否成立；
[0195]
(5)基于惯性测量单元约束以及回环检测约束构建任务约束，基于任务约束进行全局优化，得到优化后的位姿。
[0196]
在环境结构化信息满足预定条件时，所述激光惯性里程计模块用于通过惯性测量单元和激光雷达单元提供激光-惯性里程计模式，在激光-惯性里程计模式下，用于基于采集的点云数据进行位姿估计以及场景地图构建。
[0197]
本实施例中，激光-惯性里程计模块用于执行如下进行位姿估计和场景图构建：
[0198]
(1)对当前帧点云数据进行畸变矫正；
[0199]
(2)对于矫正后的点云数据，提取所述点云数据的线特征和面特征；
[0200]
(3)基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，通过正态分布变换方法获取当前帧点云数据与前一帧点云数据之间的相对运动信息，得到当前帧点云数据与前一帧点云数据之间的位姿变换；
[0201]
(4)如果位姿变化大于等于设定的阈值，当前帧点云数据作为关键帧，将所述关键帧加入局部优化任务中，进行局部地图的构建；
[0202]
(5)基于当前帧点云线、面特征及局部地图信息，通过正态分布变换方法，检测回环并构建回环约束。
[0203]
在环境纹理信息和环境结构信息均不满足预定条件时，所述觉-惯性-激光里程计模块用于通过相机单元、惯性测量单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，用于将相机与雷达标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建。
[0204]
本实施例中，视觉-惯性-激光里程计用于执行如下进行位姿估计以及场景图构建：
[0205]
(1)基于相机、激光雷达以及惯性测量单元的标定，将rgb图像与激光雷达提供的距离信息相融合，得到环境的rgb-d信息；
[0206]
(2)循环执行如下操作直至完成全局地图的构建：
[0207]
(2-1)基于所述rgb-d信息，在激光-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，将通过视觉-惯性里程计模式获得的优化后位姿作为初始位姿，基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，
并以在视觉-惯性里程计下得到的回环侯选帧为初始回环检测范围；
[0208]
(2-2)通过基于所述rgb-d信息，在视觉-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，以激光-惯性里程计模式下获得的优化后位姿为初始位姿。
[0209]
在视觉-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程计因子和视觉回环因子；在激光-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及激光回环因子；在视觉-惯性-激光里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及视觉-激光回环因子。
[0210]
多模态数据联合优化模块用于构建任务约束，包括构建惯性测量单元预积分因子、视觉里程因子、激光里程因子、视觉回环因子、激光回环因子以及视觉-激光回环因子，所述惯性测量单元预积分因子、视觉里程因子、以及视觉回环因子配合作用于视觉-惯性里程计模式下的位姿优化，所述惯性测量单元预积分因子、激光里程因子以及激光回环因子配合作用于激光-惯性里程计模式下位姿优化，所述惯性测量单元预积分因子、激光里程因子、视觉里程因子以及视觉-激光回环因子配合作用于视觉-惯性-激光里程计模式下的位姿优化。
[0211]
其中，所述多模态数据联合优化模块用于执行如下构建惯性测量单元预积分因子：
[0212]
(1)以距离当前时刻最近的传感器位姿为基础位姿，计算所述基础位姿与当前时刻惯性测量单元里程计增量位姿变换，通过进行相乘计算得到当前时刻惯性测量单元里程计位姿，其中，对于视觉-惯性里程计模式，所述传感器为相机，对于激光-惯性里程计模式，所述传感器为激光雷达；
[0213]
(2)计算前一帧与当前帧之间的惯性测量单元预积分量，基于前一帧状态施加惯性测量单元预积分量得到当前帧初始状态估计，对于相邻时刻i和j，惯性测量单元预积分增量δv
ij
、位置预积分增量δp
ij
以及旋转预积分增量δr
ij
计算公式如下：
[0214][0215][0216][0217]
其中，vi表示第i时刻的速度，vj表示第j时刻的速度，g表示加速度，δt
ij
表示第i时刻和第j时刻的时间间隔，pi表示第i时刻的位置，pj表示第j时刻的位置，ri表示第i时刻的旋转矩阵，rj表示第j时刻的旋转矩阵；
[0218]
表示ri的转置。
[0219]
对于激光关键帧，所述多模态数据联合优化模块用于基于提取的线特征和面特征，构建激光里程计因子，包括如下步骤：
[0220]
采用滑窗法创建区域地图
[0221]
其中，线特征集合
[0222]
面特征集合
[0223]
为第i-1刻关键帧的线特征在世界坐标系中的表示，
[0224]
为第i-1刻关键帧的面特征在世界坐标系中的表示，
[0225]
第i时刻激光关键帧的线特征及面特征距离计算公式如下：
[0226][0227][0228]
其中，k,u,v,w表示特征点的索引，与为对应的线特征上的两个点，与为对应的面特征上的三个点，第i时刻激光关键帧相当于世界坐标系的位姿变换ti表示为：
[0229][0230]
激光里程计因子
[0231]
在视觉-惯性里程计模式下，多模态数据联合优化模块用于基于词袋模型构建视觉回环因子。
[0232]
在激光-惯性里程计模式下，多模态数据联合优化模块用于基于正态分布变换方法构建激光回环因子。
[0233]
在视觉-惯性-激光里程计模式下，多模态数据联合优化模块用于结合词袋模型和正态分布变换方法执行如下构建视觉-激光回环因子，具体如下：
[0234]
(1)基于词袋模型提取rgb图像对应关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；
[0235]
(2)通过点云数据确定所述回环侯选帧，得到确认的回环帧；
[0236]
(3)在回环帧附近选取局部地图，通过局部地图进行扫描匹配，得到相对变换位姿，基于相对变换位姿构建回环因子；
[0237]
其中，在回环帧附近选取局部地图时，选取与回环帧位置和时间距离均大于预设阈值的关键帧，基于选取的关键帧构建局部地图。
[0238]
作为改进，本实施例所应用的装置还包括gps传感器，对应的，构建任务约束应包括构建gps因子。
[0239]
多模态数据联合优化模块用于执行如下构建gps因子：
[0240]
(1)将gps数据转换到局部坐标系中，将gps和激光雷法标定，实现gps与激光雷达坐标系的对齐；
[0241]
(2)基于估计位姿的波动，添加gps因子，如果估计位姿的波动大于gps位置波动，
添加gps因子，否则，关闭gps因子。
[0242]
上文通过附图和优选实施例对本发明进行了详细展示和说明，然而本发明不限于这些已揭示的实施例，基与上述多个实施例本领域技术人员可以知晓，可以组合上述不同实施例中的代码审核手段得到本发明更多的实施例，这些实施例也在本发明的保护范围之内。