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

技术特征：
1.一种多模态数据联合优化的同步定位及建图方法，其特征在于，应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，基于所述装置提供包括视觉-惯性里程计模式、激光-惯性里程计模式以及视觉-惯性-激光里程计模式的多模态模式；基于所述多模态模式进行位姿估计以及场景图构建，包括如下步骤：在环境纹理信息满足预定条件时，通过相机单元和惯性测量单元提供视觉-惯性里程计模式，在视觉-惯性里程计模式下，基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建；在环境结构化信息满足预定条件时，通过惯性测量单元和激光雷达单元提供激光-惯性里程计模式，在激光-惯性里程计模式下，基于采集的点云数据进行位姿估计以及场景地图构建；在环境纹理信息和环境结构信息均不满足预定条件时，通过相机单元、惯性测量单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，将相机、雷达与惯性测量单元标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建。2.根据权利要求1所述的多模态数据联合优化的同步定位及建图方法，其特征在于，在视觉-惯性里程计模式下，基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建，包括如下步骤：对采集的rgb图像进行特征提取，得到图像特征，并对惯性测量单元的原始数据进行预积分处理，得到惯性测量单元预防积分结果；基于图像特征和惯性测量单元预积分结果初始化相机位姿，得到相机以及惯性测量单元在世界坐标系中的状态；通过与局部地图的比较，判断当前采集的rgb图像是否为关键帧，如果是，将所述关键帧加入局部优化任务中，进行局部地图的构建；基于词袋模型提取关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；基于回环侯选帧与当前帧的相对运动，验证回环是否成立；基于惯性测量单元约束以及回环检测约束构建任务约束，基于任务约束进行全局优化，得到优化后的位姿。3.根据权利要求2所述的多模态数据联合优化的同步定位及建图方法，其特征在于，在激光-惯性里程计模式下，基于采集的点云数据进行位姿估计以及场景地图构建，包括如下步骤：对当前帧点云数据进行畸变矫正；对于矫正后的点云数据，提取所述点云数据的线特征和面特征；基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，通过正态分布变换方法获取当前帧点云数据与前一帧点云数据之间的相对运动信息，得到当前帧点云数据与前一帧点云数据之间的位姿变换；如果位姿变化大于等于设定的阈值，当前帧点云数据作为关键帧，将所述关键帧加入局部优化任务中，进行局部地图的构建；基于当前帧点云线、面特征及局部地图信息，通过正态分布变换方法，检测回环并构建
回环约束。4.根据权利要求3所述的多模态数据联合优化的同步定位及建图方法，其特征在于，在视觉-惯性-激光里程计模式下，将相机、雷达与惯性测量单元标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建，包括如下步骤：基于相机与激光雷达的标定，将rgb图像与激光雷达提供的距离信息相融合，得到环境的rgb-d信息；循环执行如下操作直至完成全局地图的构建：基于所述rgb-d信息，在激光-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，将通过视觉-惯性里程计模式获得的优化后位姿作为初始位姿，基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，并以在视觉-惯性里程计下得到的回环侯选帧为初始回环检测范围；通过基于所述rgb-d信息，在视觉-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，以激光-惯性里程计模式下获得的优化后位姿为初始位姿。5.根据权利要求4所述的多模态数据联合优化的同步定位及建图方法，其特征在于，在视觉-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程计因子和视觉回环因子；在激光-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及激光回环因子；在视觉-惯性-激光里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程因子、激光里程计因子以及视觉-激光回环因子；其中，构建惯性测量单元预积分因子包括如下步骤：以距离当前时刻最近的传感器位姿为基础位姿，计算所述基础位姿与当前时刻惯性测量单元里程计增量位姿变换，通过进行相乘计算得到当前时刻惯性测量单元里程计位姿，其中，对于视觉-惯性里程计模式，所述传感器为相机，对于激光-惯性里程计模式，所述传感器为激光雷达；计算前一帧与当前帧之间的惯性测量单元预积分量，基于前一帧状态施加惯性测量单元预积分量得到当前帧初始状态估计，对于相邻时刻i和j，惯性测量单元预积分增量δv
ij
、位置预积分增量δp
ij
以及旋转预积分增量δr
ij
计算公式如下：计算公式如下：计算公式如下：其中，v
i
表示第i时刻的速度，v
j
表示第j时刻的速度，g表示加速度，δt
ij
表示第i时刻和第j时刻的时间间隔，p
i
表示第i时刻的位置，p
j
表示第j时刻的位置，r
i
表示第i时刻的旋转矩阵，r
j
表示第j时刻的旋转矩阵；表示r
i
的转置。对于激光关键帧，基于提取的线特征和面特征，构建激光里程计因子，包括如下步骤：
采用滑窗法创建区域地图其中，线特征集合面特征集合面特征集合为第i-1刻关键帧的线特征在世界坐标系中的表示，为第i-1刻关键帧的面特征在世界坐标系中的表示，第i时刻激光关键帧的线特征及面特征距离计算公式如下：第i时刻激光关键帧的线特征及面特征距离计算公式如下：其中，k,u,v,w表示特征点的索引，与为对应的线特征上的两个点，与为对应的面特征上的三个点，第i时刻激光关键帧相当于世界坐标系的位姿变换t
i
表示为：激光里程计因子在视觉-惯性里程计模式下，基于词袋模型构建视觉回环因子；在激光-惯性里程计模式下，基于正态分布变换方法构建激光回环因子；在视觉-惯性-激光里程计模式下，结合词袋模型和正态分布变换方法构建视觉-激光回环因子，包括如下步骤：基于词袋模型提取rgb图像对应关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；通过点云数据确定所述回环侯选帧，得到确认的回环帧；在回环帧附近选取局部地图，通过局部地图进行扫描匹配，得到相对变换位姿，基于相对变换位姿构建回环因子；其中，在回环帧附近选取局部地图时，选取与回环帧位置和时间距离均大于预设阈值的关键帧，基于选取的关键帧构建局部地图。6.根据权利要求5任一项所述的多模态数据联合优化的同步定位及建图方法，其特征在于，所述装置还包括gps传感器，对应的，在用户选择使用gps传感器时，构建任务约束应包括构建gps因子；通过如下步骤构建gps因子：将gps数据转换到局部坐标系中，将gps和激光雷法标定，实现gps与激光雷达坐标系的对齐；
基于估计位姿的波动，添加gps因子，如果估计位姿的波动大于gps位置波动，添加gps因子，否则，关闭gps因子。7.一种多模态数据联合优化的同步定位及建图系统，其特征在于，应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，所述系统用于基于如权利要求1-6任一项所述的多模态数据联合优化的同步定位及建图方法进行位姿估计以及场景图构建，所述系统包括：视觉惯性里程计模块，在环境纹理信息满足预定条件时，所述视觉惯性里程计模块用于通过相机单元和惯性测量单元提供视觉-惯性里程计模式，在视觉-惯性里程计模式下，用于基于采集的rgb图像以及惯性测量单元的预积分结果进行位姿估计以及场景地图构建；激光惯性里程计模块，在环境结构化信息满足预定条件时，所述激光惯性里程计模块用于通过惯性测量单元和激光雷达单元提供激光-惯性里程计模式，在激光-惯性里程计模式下，用于基于采集的点云数据进行位姿估计以及场景地图构建；视觉-惯性-激光里程计模块，在环境纹理信息和环境结构信息均不满足预定条件时，所述视觉-惯性-激光里程计模块用于通过相机单元、惯性测量单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，将相机、雷达与惯性测量单元标定，基于视觉提供的rgb图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建；多模态数据联合优化模块，所述多模态数据联合优化模块用于构建任务约束，包括构建惯性测量单元预积分因子、视觉里程因子、激光里程因子、视觉回环因子、激光回环因子以及视觉-激光回环因子，所述惯性测量单元预积分因子、视觉里程因子、以及视觉回环因子配合作用于视觉-惯性里程计模式下的位姿优化，所述惯性测量单元预积分因子、激光里程因子以及激光回环因子配合作用于激光-惯性里程计模式下位姿优化，所述惯性测量单元预积分因子、激光里程因子、视觉里程因子以及视觉-激光回环因子配合作用于视觉-惯性-激光里程计模式下的位姿优化。8.根据权利要求7所述的多模态数据联合优化的同步定位及建图系统，其特征在于，所述视觉-惯性里程计模块用于执行如下实现位姿估计和场景图构建：对采集的rgb图像进行特征提取，得到图像特征，并对惯性测量单元的原始数据进行预积分处理，得到惯性测量单元预防积分结果；基于图像特征和惯性测量单元预积分结果初始化相机位姿，得到相机以及惯性测量单元在世界坐标系中的状态；通过与局部地图的比较，判断当前采集的rgb图像是否为关键帧，如果是，将所述关键帧加入局部优化任务中，进行局部地图的构建；基于词袋模型提取关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；基于回环侯选帧与当前帧的相对运动，验证回环是否成立；基于惯性测量单元约束以及回环检测约束构建任务约束，基于任务约束进行全局优化，得到优化后的位姿；所述激光-惯性里程计模块用于执行如下进行位姿估计和场景图构建：
对当前帧点云数据进行畸变矫正；对于矫正后的点云数据，提取所述点云数据的线特征和面特征；基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，通过正态分布变换方法获取当前帧点云数据与前一帧点云数据之间的相对运动信息，得到当前帧点云数据与前一帧点云数据之间的位姿变换；如果位姿变化大于等于设定的阈值，当前帧点云数据作为关键帧，将所述关键帧加入局部优化任务中，进行局部地图的构建；基于当前帧点云线、面特征及局部地图信息，通过正态分布变换方法，检测回环并构建回环约束；所述视觉-惯性-激光里程计用于执行如下进行位姿估计以及场景图构建：基于相机、雷达与惯性测量单元的标定，将rgb图像与激光雷达提供的距离信息相融合，得到环境的rgb-d信息；循环执行如下操作直至完成全局地图的构建：基于所述rgb-d信息，在激光-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，将通过视觉-惯性里程计模式获得的优化后位姿作为初始位姿，基于线特征和面特征，将当前帧点云数据与前一帧点云数据进行匹配，并以在视觉-惯性里程计下得到的回环侯选帧为初始回环检测范围；通过基于所述rgb-d信息，在视觉-惯性里程计模式下进行位姿估计以及场景地图构建，在位姿估计以及场景地图构建过程中，以激光-惯性里程计模式下获得的优化后位姿为初始位姿。9.根据权利要求8所述的多模态数据联合优化的同步定位及建图系统，其特征在于，在视觉-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、视觉里程计因子和视觉回环因子；在激光-惯性里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及激光回环因子；在视觉-惯性-激光里程计模式下，构建任务约束包括构建惯性测量单元预积分因子、激光里程计因子以及视觉-激光回环因子；其中，构建惯性测量单元预积分因子包括如下步骤：以距离当前时刻最近的传感器位姿为基础位姿，计算所述基础位姿与当前时刻惯性测量单元里程计增量位姿变换，通过进行相乘计算得到当前时刻惯性测量单元里程计位姿，其中，对于视觉-惯性里程计模式，所述传感器为相机，对于激光-惯性里程计模式，所述传感器为激光雷达；计算前一帧与当前帧之间的惯性测量单元预积分量，基于前一帧状态施加惯性测量单元预积分量得到当前帧初始状态估计，对于相邻时刻i和j，惯性测量单元预积分增量δv
ij
、位置预积分增量δp
ij
以及旋转预积分增量δr
ij
计算公式如下：计算公式如下：计算公式如下：
其中，v
i
表示第i时刻的速度，v
j
表示第j时刻的速度，g表示加速度，δt
ij
表示第i时刻和第j时刻的时间间隔，p
i
表示第i时刻的位置，p
j
表示第j时刻的位置，r
i
表示第i时刻的旋转矩阵，r
j
表示第j时刻的旋转矩阵；表示r
i
的转置；对于激光关键帧，基于提取的线特征和面特征，构建激光里程计因子，包括如下步骤：采用滑窗法创建区域地图其中，线特征集合面特征集合面特征集合为第i-1刻关键帧的线特征在世界坐标系中的表示，为第i-1刻关键帧的面特征在世界坐标系中的表示，第i时刻激光关键帧的线特征及面特征距离计算公式如下：第i时刻激光关键帧的线特征及面特征距离计算公式如下：其中，k,u,v,w表示特征点的索引，与为对应的线特征上的两个点，与为对应的面特征上的三个点，第i时刻激光关键帧相当于世界坐标系的位姿变换t
i
表示为：激光里程计因子在视觉-惯性里程计模式下，基于词袋模型构建视觉回环因子；在激光-惯性里程计模式下，基于正态分布变换方法构建激光回环因子；在视觉-惯性-激光里程计模式下，结合词袋模型和正态分布变换方法构建视觉-激光回环因子，包括如下步骤：基于词袋模型提取rgb图像对应关键帧的词袋信息，基于关键帧的词袋信息进行回环检测，得到回环候选帧；通过点云数据确定所述回环侯选帧，得到确认的回环帧；在回环帧附近选取局部地图，通过局部地图进行扫描匹配，得到相对变换位姿，基于相对变换位姿构建回环因子；其中，在回环帧附近选取局部地图时，选取与回环帧位置和时间距离均大于预设阈值的关键帧，基于选取的关键帧构建局部地图为为第刻关键帧的面特征在世界坐标系中的表
示。10.根据权利要求9所述的多模态数据联合优化的同步定位及建图系统，其特征在于，所述装置还包括gps传感器，对应的，在用户选择使用gps传感器时，构建任务约束应包括构建gps因子；所述多模态数据联合优化模块用于执行如下构建gps因子：将gps数据转换到局部坐标系中，将gps和激光雷法标定，实现gps与激光雷达坐标系的对齐；基于估计位姿的波动，添加gps因子，如果估计位姿的波动大于gps位置波动，添加gps因子，否则，关闭gps因子。

技术总结
本发明公开了多模态数据联合优化的同步定位及建图方法及系统，属于多模态传感器融合的同步定位技术领域，要解决的技术问题为如何将多种传感模式的信息高效融合，实现基于多模态数据的同步定位和建图。应用于配置有相机单元、惯性测量单元、激光雷达单元的装置，基于所述装置提供包括视觉-惯性里程计模式、激光-惯性里程计模式以及视觉-惯性-激光里程计模式的多模态模式。在环境纹理信息和环境结构信息均不满足预定条件时，通过相机单元、惯性测量单元和激光雷达单元提供视觉-惯性-激光里程计模式，在视觉-惯性-激光里程计模式下，将相机、雷达与惯性测量单元标定，基于视觉提供的RGB图像以及激光雷达提供的距离信息进行位姿估计以及场景图构建。估计以及场景图构建。估计以及场景图构建。


技术研发人员：李从健 王建华 仝其胜
受保护的技术使用者：山东浪潮科学研究院有限公司
技术研发日：2023.03.24
技术公布日：2023/10/20