CN116977628A - 一种应用于动态环境的基于多模态语义框架的slam方法及系统 - Google Patents
一种应用于动态环境的基于多模态语义框架的slam方法及系统 Download PDFInfo
- Publication number
- CN116977628A CN116977628A CN202310357078.8A CN202310357078A CN116977628A CN 116977628 A CN116977628 A CN 116977628A CN 202310357078 A CN202310357078 A CN 202310357078A CN 116977628 A CN116977628 A CN 116977628A
- Authority
- CN
- China
- Prior art keywords
- imu
- dynamic
- data
- point
- information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000011218 segmentation Effects 0.000 claims abstract description 34
- 230000004927 fusion Effects 0.000 claims abstract description 20
- 230000010354 integration Effects 0.000 claims abstract description 14
- 238000010276 construction Methods 0.000 claims abstract 2
- 230000003068 static effect Effects 0.000 claims description 31
- 238000005457 optimization Methods 0.000 claims description 20
- 230000000007 visual effect Effects 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000004458 analytical method Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 5
- 230000000877 morphologic effect Effects 0.000 claims description 5
- 230000006870 function Effects 0.000 claims description 4
- 230000008447 perception Effects 0.000 claims description 4
- 238000005520 cutting process Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000007480 spreading Effects 0.000 claims description 3
- 238000003892 spreading Methods 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims 1
- 238000007670 refining Methods 0.000 claims 1
- 230000008569 process Effects 0.000 abstract description 6
- 238000005516 engineering process Methods 0.000 abstract description 4
- 238000003860 storage Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 12
- 239000003245 coal Substances 0.000 description 5
- 230000010339 dilation Effects 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000002372 labelling Methods 0.000 description 2
- 238000005065 mining Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/08—Learning methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/764—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using classification, e.g. of video objects
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
- G06V10/80—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20081—Training; Learning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20084—Artificial neural networks [ANN]
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Software Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computing Systems (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Databases & Information Systems (AREA)
- Geometry (AREA)
- Biophysics (AREA)
- Computer Graphics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Biomedical Technology (AREA)
- Remote Sensing (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Molecular Biology (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种应用于动态环境的基于多模态语义框架的SLAM方法及系统。具体过程包括相机图像获取、激光雷达数据处理、IMU预积分、实例分割、多模态融合、特征地图更新和全局语义地图构建。通过视觉、激光雷达和IMU的多模态融合，平衡了精度与计算量；通过动态语义理解，辅助多模态传感器构建3D动态地图，保证了系统的准确率和实时性。本发明解决了动态非结构化且GNSS无法作用的井下以及仓储导航定位问题，实现了非结构化环境的无人驾驶精准感知与定位技术。
Description
技术领域
本发明属于智能定位与导航领域，具体涉及一种应用于动态环境的基于多模态语义框架的SLAM方法及系统。
背景技术
煤矿巷道、采掘工作面、仓储物流等作业区域具有典型的非结构化环境特征，且GNSS技术无法直接应用于井下和室内，进而导致煤矿开采时矿难频发，急需机械化换人、自动化减人和提高矿山智能化水平。亟需构建适用于煤矿无人驾驶的自主定位系统方案，解决井下无人驾驶精准定位、姿态感知等问题。如何快速突破视觉、惯导和激光等多信息融合的井下无人驾驶精准感知与定位技术，是实现井下无人驾驶局部自主的关键，故融合视觉、激光雷达和惯性测量单元的SLAM方法非常重要。大多数SLAM框架目前基于静止场景的假设，然而现实世界是复杂且动态的，可能因为不能匹配到足够的正确特征导致系统失败。因为小尺度对象、遮挡、运动模糊导致语义分割结果在动态环境下不理想，误差较大。
发明内容
为了克服上述现有技术的不足，本发明提供了一种应用于动态环境的基于多模态语义框架的SLAM方法及系统。
本发明充分考虑了矿山井巷非结构化及动态的环境特征且GNSS技术无法直接运用的因素，由于矿山井巷非结构化环境特征，所以使用多模态融合的方式进行误差补偿，从而使整个系统保持稳定。由于矿山井巷以及仓储物流的动态环境特征，所以在一定情况下，静态地图无法监测物体的移动属性，导致误差较大，但是语义理解可以辅助多模态传感器构建3D动态地图，保证了系统的准确率和实时性。本系统突破了传统SLAM方法以静态环境假设为前提的局限，可以预知物体(人、汽车、动物等)的可移动属性。共享相似物体知识表示，可实现智能路径规划。通过维护共享知识库，提高SLAM系统的可扩展性和存储效率。
本发明所采用的技术方案是：一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，包括以下步骤：
步骤一，通过相机获取图像，提取当前帧的信息，进行切割和分类。
步骤二，订阅激光雷达所采集到的原始点云数据。对其进行初步矫正，获取当前帧经过畸变校正之后的有效点云数据、位姿角、初始位姿等信息。进行聚类，检测出障碍物的边缘。
步骤三，进行IMU预积分，并将结果进行传播。订阅激光和IMU里程计，按照之前时刻的激光里程计，距离当前时刻的IMU里程计改变增量，进行当前时刻IMU里程计的计算；通过rviz来展示局部IMU里程计轨迹。通过激光里程计的两帧激光之间的IMU预积分构造因子图，优化当前帧的位姿、速度、偏置等状态；将优化后的状态作为基础，添加IMU预积分，得到每个时刻的IMU里程计。
步骤四，实例分割：为多模态信息融合提供实时动态物体分割的语义信息。实现形态膨胀，将动态对象及其边界作为动态信息，结合点云聚类结果和分割结果来更好地细化动态对象。
步骤五，多模态信息融合：获取激光雷达的点云信息、相机的连续关键帧信息和惯性导航模组的位姿信息进行融合作为精准的感知信息。
步骤六，定位和位姿估计：先后经过特征提取、数据关联、位姿估计等捕捉，通过最小化点到边缘和点到平面的距离及残差的总和来计算最终位姿。通过姿态估计，更新特征地图并选取其中的关键帧。
步骤七，全局优化和建图：构建全局语义地图，包括静态地图和动态地图。视觉信息通过将3D点重新投影到图像平面中来实现，同时能够反投影3D点到图像平面。动态地图用于显示动态对象。
在步骤一中，包括如下步骤：
①订阅当前相机提取到的当前帧的信息；
②从当前帧的信息中选取一个像素，根据当前帧的信息采用FAST算法检测当前帧的角点，并判断该像素是否为一个角特征点。
具体判断方法如下：将所选取像素p的亮度值设为Ip，亮度阈值设为t；以像素p点为中心设定一个半径等于3像素的离散化的Bresenham圆，Bresenham圆的边界上有16个像素；在大小为16个像素的Bresenham圆上有9个连续的像素点，若9个连续像素点的像素值大于或小于Ip+t，则判定像素p为一个角特征点；若9个连续像素点的像素值等于Ip+t，则判定像素不是一个角特征点。
在步骤二中，包括如下步骤：
①订阅IMU采集到的原始数据；订阅IMU增量用于位姿估算，当新的IMU数据传递进来时，通过之前IMU积分数据以及当前得到的数据，预估当前时刻的位姿；订阅激光雷达所采集到的原始点云数据。
②默认在激光扫描一帧的时间内仅发生了旋转并没有发生平移，对齐IMU数据与时间戳，同时计算IMU队列中每帧包括RPY在内的IMU积分数据，再对激光帧中的每个扫描点进行线性插值。
③发布当前帧经过畸变校正之后的有效点云，发布当前帧经过畸变校正之后的点云数据、位姿角、初始位姿等信息。
④进行聚类，检测出障碍物的边缘。具体方法为：找到最近点，只要点与点之间的距离在一定半径范围内就认为这些点属于一个聚类。
在步骤三中，包括如下步骤：
①对激光里程计数据进行接收，将数据统一为指定格式并且将当前激光里程计的时间戳进行保存。
②移除IMU队列中时间戳早于激光里程计时间戳的IMU增量数据；计算IMU队列中起始和终止时刻对应的IMU里程计之间的相对位姿变换；将先前接收到激光里程计的数据和之前计算得出的相对位姿变换得到的数据进行结合并发布。
③订阅IMU原始数据加入到队列中，利用之前时刻优化后得到的结果作为积分的起始值，对IMU队列中的数据进行积分计算，发布IMU增量。
④对于IMU角速度和加速度的公式定义如下：
角速度：
加速度：
t时刻下的IMU观测源数据为和/>并且/>和/>都会受到白噪声nt和IMU零偏bt的影响。矩阵/>是世界坐标系到机器人坐标系的变换矩阵。重力常数向量g属于世界坐标系W。
⑤对系统进行初始化判断；每当接收到100帧的激光里程计数据，优化器就进行一次重置，初始化与重置不同，进行重置时，当前的速度与位姿和之前优化得到的速度与位姿保持一致，噪声模型采用之前优化后模型的边缘分布；添加IMU因子，进行IMU数据两帧之间的积分计算；添加先验因子，以当前接收到的激光里程计作为先验因子；对变量节点进行赋予初值，执行优化并且更新之前时刻的状态；使用当前优化后的状态对预积分器进行初始化，对当前帧之后的IMU数据进行计算。
⑥通过IMU预积分来获得不同时间戳之间的相对运动。预积分得到的第m和第n时刻之间的位置变化Δpmn、速度变化Δvmn和旋转变化ΔRmn计算公式如下：
在步骤四中，包括如下步骤：
①使用2D实例分割网络，一张图像的实例分割结果为：
C代表类别，M是物体的掩码信息，n代表当前图像中存在物体数量。图像在空间上被分成N×N个网格单元。如果一个对象的中心落入一个网格单元，该网格单元负责分别预测类别分支Bc和掩码分支Pm中对象的语义类别Cij和语义掩码Mij：
Bc(I,θc):I→{Cij∈Rλ|i,j＝0,1,...,N},
Pm(I,θm):I→{Mij∈Rφ|i,j＝0,1,...,N},
λ是类的数量。φ是网格单元的总数。
②采用SOLOv2的轻量级版本，实现实时实例分割。在骨干网络中构建更有效和更健壮的特征表示鉴别器：输出每个动态对象的像素级实例掩码，以及它们对应的边界框和类类型。输出二进制掩码转换为包含场景中所有像素级实例掩码的单个图像。蒙版落在其上的像素被认为是“动态状态”，否则被认为是“静态”。然后将二进制掩码应用于语义融合模块以生成3D动态掩码。
在步骤五中，包括如下步骤：
①首先实现形态膨胀，将2D像素级掩模图像与结构元素进行卷积，以逐渐扩展动态对象的区域边界。形态膨胀结果标志着动态对象周围的模糊边界。将动态对象及其边界作为动态信息，将在多模态融合部分进一步细化。
②通过欧几里得空间的连通性分析进行补偿。结合点云聚类结果和分割结果来更好地细化动态对象。对几何信息进行连通性分析，并与基于视觉的分割结果合并。
③首先将3D点云缩小以减少数据规模，并将其用作点云聚类的输入。然后将实例分割结果投影到点云坐标上，对每个点进行标注。当大多数点(90％)是动态标记点时，点云簇将被视为动态簇。当静态点靠近动态点簇时，它会被重新标记为动态标签。并且当附近没有动态点聚类时，动态点将被重新标记。
在步骤六中，包括如下步骤：
①特征提取：多模态动态分割后，点云分为动态点云PD和静态点云PS。静态点云用于定位和建图模块，对于每个静态点Pk∈PS，在欧几里得空间中通过半径搜索来搜索其附近的静态点集Sk。让|S|是集合S的基数，因此局部平滑度定义为：
边缘特征由σk大的点定义，平面特征由σk小的点定义。
②数据关联：
通过最小化点到边缘和点到平面的距离来计算最终位姿。对于边缘特征点pε∈Pε，通过将其转换为局部地图坐标，其中T∈SE是当前位姿。从局部边缘特征图中搜索2个最近的边缘特征/>和/>点到边缘残差定义：
给定一个平面特征点pι∈Pι及其变换点从局部平面图中搜索3个最近点。点到平面残差定义为：
③位姿估计：通过最小化点到平面和点到边缘残差的总和来计算最终的位姿。
④特征地图更新和关键帧选择：位姿优化完毕，将特征点更新到局部地图和平面地图中。这些点将被用于下一帧的数据关联。当平移或者旋转的值大于阈值时候，该帧将被选作关键帧。
在步骤七中，包括如下步骤：
全局语义地图由静态地图和动态地图构成。
①静态地图：视觉信息用于构建彩色密集静态地图。通过将3D点重新投影到图像平面中来实现视觉信息。视觉信息能够反投影3D点到图像平面。
②每次更新后，使用3D体素化网格方法(3D voxelized grid approach)对地图进行下采样，以防止内存溢出。具体方式为：PCL实现的VoxelGrid类通过输入的点云数据创建一系列三维的体素栅格，即微小的空间三维立方体的集合，然后在每一个体素中用这个体素栅格内所有点的重心来近似的显示体素中的其他点。
③由PD构建动态地图，用于显示动态对象。动态信息可用于运动规划等高级任务。
与现有技术相比，本发明的有益效果是：
1.本发明采用激光雷达、视觉相机和IMU对煤矿井下和智能仓储系统实现高精准定位，并提高对非结构化环境的姿态感知，由此实现对煤矿井下和智能仓储系统进行高精度定位，同时平衡了精度与计算量，提高矿山井巷的和仓库的定位与建图准确率与实时性，有效的解决了非结构化环境特征下且GNSS无法作用的井下定位与建图难的问题。
2.本发明提出了一个强大且快速的多模态语义SLAM框架，旨在解决复杂和动态环境中的SLAM问题。将仅几何聚类和视觉语义信息相结合，以减少由于小尺度对象、遮挡和运动模糊导致的分割误差的影响。学习更强大的对象特征表示，并将三思机制部署到主干网络，从而为实例分割模型带来更好的识别结果，能够提供可靠的定位和语义密集的地图。
附图说明
图1为本发明实施例的应用于动态环境的基于多模态语义框架的SLAM方法及系统的步骤流程图；
图2为IMU预积分数据传递示意图；
图3为多模态语义融合示意图；
图4为实例分割流程示意图；
图5为定位与位姿估计流程示意图；
图6为全局地图优化流程示意图；
图7为应用于动态环境的基于多模态语义框架的SLAM方法及系统实验结果图。
具体实施方式
下面结合附图对本发明进一步说明。
参照图1所示应用于动态环境的基于多模态语义框架的SLAM方法及系统的步骤流程图。实施过程可以分为七大步骤：
步骤一，通过相机获取图像，提取当前帧的信息，进行切割和分类。
步骤二，订阅激光雷达所采集到的原始点云数据。对其进行初步矫正，获取当前帧经过畸变校正之后的有效点云数据、位姿角、初始位姿等信息。进行聚类，检测出障碍物的边缘。
步骤三，进行IMU预积分，并将结果进行传播。
步骤四，实例分割：为多模态信息融合提供实时动态物体分割的语义信息。实现形态膨胀，将动态对象及其边界作为动态信息，结合点云聚类结果和分割结果来更好地细化动态对象。
步骤五，多模态信息融合：获取激光雷达的点云信息、相机的连续关键帧信息和惯性导航模组的位姿信息进行融合作为精准的感知信息。
步骤六，定位和位姿估计：先后经过特征提取、数据关联、位姿估计等捕捉，通过最小化点到边缘和点到平面的距离及残差的总和来计算最终位姿。通过姿态估计，更新特征地图并选取其中的关键帧。
步骤七，全局优化和建图：构建全局语义地图，包括静态地图和动态地图。视觉信息通过将3D点重新投影到图像平面中来实现，同时能够反投影3D点到图像平面。动态地图用于显示动态对象。
在步骤一中，包括如下步骤：
请参照图3多模态语义融合示意图。
①订阅当前相机提取到的当前帧的信息；
②从当前帧的信息中选取一个像素，根据当前帧的信息采用FAST算法检测当前帧的角点，并判断该像素是否为一个角特征点。
具体判断方法如下：将所选取像素p的亮度值设为Ip，亮度阈值设为t；以像素p点为中心设定一个半径等于3像素的离散化的Bresenham圆，Bresenham圆的边界上有16个像素；在大小为16个像素的Bresenham圆上有9个连续的像素点，若9个连续像素点的像素值大于或小于Ip+t，则判定像素p为一个角特征点；若9个连续像素点的像素值等于Ip+t，则判定像素不是一个角特征点。
在步骤二中，包括如下步骤：
请参照图3多模态语义融合示意图。
①订阅IMU采集到的原始数据；订阅IMU增量用于位姿估算，当新的IMU数据传递进来时，通过之前IMU积分数据以及当前得到的数据，预估当前时刻的位姿；订阅激光雷达所采集到的原始点云数据。
②默认在激光扫描一帧的时间内仅发生了旋转并没有发生平移，对齐IMU数据与时间戳，同时计算IMU队列中每帧包括RPY在内的IMU积分数据，再对激光帧中的每个扫描点进行线性插值。
③发布当前帧经过畸变校正之后的有效点云，发布当前帧经过畸变校正之后的点云数据、位姿角、初始位姿等信息。
④进行聚类，检测出障碍物的边缘。具体方法为：找到最近点，只要点与点之间的距离在一定半径范围内就认为这些点属于一个聚类。
在步骤三中，包括如下步骤：
请参照图2IMU预积分数据传递示意图。
①对激光里程计数据进行接收，将数据统一为指定格式并且将当前激光里程计的时间戳进行保存。
②移除IMU队列中时间戳早于激光里程计时间戳的IMU增量数据；计算IMU队列中起始和终止时刻对应的IMU里程计之间的相对位姿变换；将先前接收到激光里程计的数据和之前计算得出的相对位姿变换得到的数据进行结合并发布。
③订阅IMU原始数据加入到队列中，利用之前时刻优化后得到的结果作为积分的起始值，对IMU队列中的数据进行积分计算，发布IMU增量。
④对于IMU角速度和加速度的公式定义如下：
角速度：
加速度：
t时刻下的IMU观测源数据为和/>并且/>和/>都会受到白噪声nt和IMU零偏bt的影响。矩阵/>是世界坐标系到机器人坐标系的变换矩阵。重力常数向量g属于世界坐标系W。
⑤对系统进行初始化判断；每当接收到100帧的激光里程计数据，优化器就进行一次重置，初始化与重置不同，进行重置时，当前的速度与位姿和之前优化得到的速度与位姿保持一致，噪声模型采用之前优化后模型的边缘分布；添加IMU因子，进行IMU数据两帧之间的积分计算；添加先验因子，以当前接收到的激光里程计作为先验因子；对变量节点进行赋予初值，执行优化并且更新之前时刻的状态；使用当前优化后的状态对预积分器进行初始化，对当前帧之后的IMU数据进行计算。
⑥通过IMU预积分来获得不同时间戳之间的相对运动。预积分得到的第m和第n时刻之间的位置变化Δpmn、速度变化Δvmn和旋转变化ΔRmn计算公式如下：
在步骤四中，包括如下步骤：
请参照图4实例分割流程示意图。
①使用2D实例分割网络，一张图像的实例分割结果为：
C代表类别，M是物体的掩码信息，n代表当前图像中存在物体数量。图像在空间上被分成N×N个网格单元。如果一个对象的中心落入一个网格单元，该网格单元负责分别预测类别分支Bc和掩码分支Pm中对象的语义类别Cij和语义掩码Mij：
Bc(I,θc):I→{Cij∈Rλ|i,j＝0,1,...,N},
Pm(I,θm):I→{Mij∈Rφ|i,j＝0,1,...,N},
λ是类的数量。φ是网格单元的总数。
②采用SOLOv2的轻量级版本，实现实时实例分割。在骨干网络中构建更有效和更健壮的特征表示鉴别器：输出每个动态对象的像素级实例掩码，以及它们对应的边界框和类类型。输出二进制掩码转换为包含场景中所有像素级实例掩码的单个图像。蒙版落在其上的像素被认为是“动态状态”，否则被认为是“静态”。然后将二进制掩码应用于语义融合模块以生成3D动态掩码。
在步骤五中，包括如下步骤：
请参照图3多模态语义融合示意图。
①首先实现形态膨胀，将2D像素级掩模图像与结构元素进行卷积，以逐渐扩展动态对象的区域边界。形态膨胀结果标志着动态对象周围的模糊边界。将动态对象及其边界作为动态信息，将在多模态融合部分进一步细化。
②通过欧几里得空间的连通性分析进行补偿。结合点云聚类结果和分割结果来更好地细化动态对象。对几何信息进行连通性分析，并与基于视觉的分割结果合并。
③首先将3D点云缩小以减少数据规模，并将其用作点云聚类的输入。然后将实例分割结果投影到点云坐标上，对每个点进行标注。当大多数点(90％)是动态标记点时，点云簇将被视为动态簇。当静态点靠近动态点簇时，它会被重新标记为动态标签。并且当附近没有动态点聚类时，动态点将被重新标记。
在步骤六中，包括如下步骤：
请参照图5定位与位姿估计流程示意图。
①特征提取：多模态动态分割后，点云分为动态点云PD和静态点云PS。静态点云用于定位和建图模块，对于每个静态点Pk∈PS，在欧几里得空间中通过半径搜索来搜索其附近的静态点集Sk。让|S|是集合S的基数，因此局部平滑度定义为：
边缘特征由σk大的点定义，平面特征由σk小的点定义。
②数据关联：
通过最小化点到边缘和点到平面的距离来计算最终位姿。对于边缘特征点pε∈Pε，通过将其转换为局部地图坐标，其中T∈SE是当前位姿。从局部边缘特征图中搜索2个最近的边缘特征/>和/>点到边缘残差定义：
给定一个平面特征点pι∈Pι及其变换点从局部平面图中搜索3个最近点。点到平面残差定义为：
③位姿估计：通过最小化点到平面和点到边缘残差的总和来计算最终的位姿。
④特征地图更新和关键帧选择：位姿优化完毕，将特征点更新到局部地图和平面地图中。这些点将被用于下一帧的数据关联。当平移或者旋转的值大于阈值时候，该帧将被选作关键帧。
在步骤七中，包括如下步骤：
请参照图6全局地图优化流程示意图。
全局语义地图由静态地图和动态地图构成。
①静态地图：视觉信息用于构建彩色密集静态地图。通过将3D点重新投影到图像平面中来实现视觉信息。视觉信息能够反投影3D点到图像平面。
②每次更新后，使用3D体素化网格方法(3D voxelized grid approach)对地图进行下采样，以防止内存溢出。具体方式为：PCL实现的VoxelGrid类通过输入的点云数据创建一系列三维的体素栅格，即微小的空间三维立方体的集合，然后在每一个体素中用这个体素栅格内所有点的重心来近似的显示体素中的其他点。
③由PD构建动态地图，用于显示动态对象。动态信息可用于运动规划等高级任务。
最后实验效果请参照图7应用于动态环境的基于多模态语义框架的SLAM方法及系统实验结果图。
本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。如果本领域技术人员，在不脱离本发明的精神所做的非实质性改变或改进，都应该属于本发明权利要求保护的范围。
Claims (8)
1.一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，用于GNSS无法直接定位与导航的矿山井巷和仓储物流动态等环境中。其特征在于：
步骤一，通过相机获取图像，提取当前帧的信息，进行切割和分类。
步骤二，订阅激光雷达所采集到的原始点云数据。对其进行初步矫正，获取当前帧经过畸变校正之后的有效点云数据、位姿角、初始位姿等信息。进行聚类，检测出障碍物的边缘。
步骤三，进行IMU预积分，并将结果进行传播。
步骤四，实例分割：为多模态信息融合提供实时动态物体分割的语义信息。实现形态膨胀，将动态对象及其边界作为动态信息，结合点云聚类结果和分割结果来更好地细化动态对象。
步骤五，多模态信息融合：获取激光雷达的点云信息、相机的连续关键帧信息和惯性导航模组的位姿信息进行融合作为精准的感知信息。
步骤六，定位和位姿估计：先后经过特征提取、数据关联、位姿估计等捕捉，通过最小化点到边缘和点到平面的距离及残差的总和来计算最终位姿。通过姿态估计，更新特征地图并选取其中的关键帧。
步骤七，全局优化和建图：构建全局语义地图，包括静态地图和动态地图。视觉信息通过将3D点重新投影到图像平面中来实现，同时能够反投影3D点到图像平面。动态地图用于显示动态对象。
2.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤一中的包括：
首先通过相机提取当前帧的信息，根据当前帧的信息采用FAST算法检测当前帧的角点，并判断是否为一个角特征点。包括以下过程：订阅当前相机提取到的当前帧的信息；从当前帧的信息中选取一个像素，并判断该像素是否为一个角特征点。
3.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤二中的包括：
首先订阅IMU采集到的原始数据；订阅IMU增量用于位姿估算，当新的IMU数据传递进来时，通过之前IMU积分数据以及当前得到的数据，预估当前时刻的位姿；订阅激光雷达所采集到的原始点云数据。
然后，默认在激光扫描一帧的时间内仅发生了旋转并没有发生平移，对齐IMU数据与时间戳，同时计算IMU队列中每帧包括RPY在内的IMU积分数据，再对激光帧中的每个扫描点进行线性插值。
最后，发布当前帧经过畸变校正之后的有效点云，发布当前帧经过畸变校正之后的点云数据、位姿角、初始位姿等信息。同时进行聚类，检测出障碍物的边缘。
4.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤三中的包括：
首先对激光里程计数据进行接收，将数据统一为指定格式并且将当前激光里程计的时间戳进行保存。
然后移除IMU队列中时间戳早于激光里程计时间戳的IMU增量数据；计算IMU队列中起始和终止时刻对应的IMU里程计之间的相对位姿变换；将先前接收到激光里程计的数据和之前计算得出的相对位姿变换得到的数据进行结合并发布。
其次订阅IMU原始数据加入到队列中，利用之前时刻优化后得到的结果作为积分的起始值，对IMU队列中的数据进行积分计算，发布IMU增量。
对于IMU角速度和加速度的公式定义如下：
角速度：
加速度：
t时刻下的IMU观测源数据为和/>并且/>和/>都会受到白噪声nt和IMU零偏bt的影响。矩阵/>是世界坐标系到机器人坐标系的变换矩阵。重力常数向量g属于世界坐标系W。
最后对系统进行初始化判断。添加IMU因子，进行IMU数据两帧之间的积分计算；添加先验因子，以当前接收到的激光里程计作为先验因子；对变量节点进行赋予初值，执行优化并且更新之前时刻的状态；使用当前优化后的状态对预积分器进行初始化，对当前帧之后的IMU数据进行计算。通过IMU预积分来获得不同时间戳之间的相对运动。预积分得到的第m和第n时刻之间的位置变化Δpmn、速度变化Δvmn和旋转变化ΔRmn计算公式如下：
5.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤四中的包括：
首先使用2D实例分割网络，一张图像的实例分割结果：
C代表类别，M是物体的掩码信息，n代表当前图像中存在物体数量。图像在空间上被分成N×N个网格单元。如果一个对象的中心落入一个网格单元，该网格单元负责分别预测类别分支Bc和掩码分支Pm中对象的语义类别Cij和语义掩码Mij：
Bc(I,θc):I→{Cij∈Rλ|i,j＝0,1,...,N},
Pm(I,θm):I→{Mij∈Rφ|i,j＝0,1,...,N},
λ是类的数量。φ是网格单元的总数。
采用SOLOv2的轻量级版本，实现实时实例分割。输出是每个动态对象的像素级实例掩码，以及它们对应的边界框和类类型。输出二进制掩码被转换为包含场景中所有像素级实例掩码的单个图像。蒙版落在其上的像素被认为是“动态状态”，否则被认为是“静态”。然后将二进制掩码应用于语义融合模块以生成3D动态掩码。
6.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤五中的包括：
首先实现形态膨胀，将2D像素级掩模图像与结构元素进行卷积，以逐渐扩展动态对象的区域边界。将动态对象及其边界作为动态信息，将在多模态融合部分进一步细化。
接下来，通过欧几里得空间的连通性分析进行补偿。结合点云聚类结果和分割结果细化动态对象。
7.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤六中的包括：
首先进行特征提取：多模态动态分割后，点云分为动态点云PD和静态点云PS。静态点云用于定位和建图模块。对于每个静态点Pk∈PS，可以在欧几里得空间中通过半径搜索来搜索其附近的静态点集Sk。让|S|是集合S的基数，因此局部平滑度定义为：
边缘特征由σk大的点定义，平面特征由σk小的点定义。
其次进行数据关联：通过最小化点到边缘和点到平面的距离来计算最终位姿。对于边缘特征点pε∈Pε，可以通过将其转换为局部地图坐标，其中T∈SE是当前位姿。从局部边缘特征图中搜索2个最近的边缘特征/>和/>点到边缘残差定义：
给定一个平面特征点pι∈Pι及其变换点从局部平面图中搜索3个最近点。点到平面残差定义为：
然后进行位姿估计：通过最小化点到平面和点到边缘残差的总和来计算最终的位姿。
最后进行特征地图更新和关键帧选择：特征点被更新到局部地图和平面地图中，将被用于下一帧的数据关联。当平移或者旋转的值大于阈值时候，该帧将被选作关键帧。
8.根据权利要求1所述的一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，其特征在于，上述步骤七中的包括：
全局语义地图由静态地图和动态地图构成。视觉信息用于构建彩色密集静态地图。通过将3D点重新投影到图像平面中来实现视觉信息。视觉信息能够反投影3D点到图像平面。每次更新后，使用3D体素化网格方法(3D voxelized grid approach)对地图进行下采样，以防止内存溢出。动态地图由PD构建。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310357078.8A CN116977628A (zh) | 2023-04-01 | 2023-04-01 | 一种应用于动态环境的基于多模态语义框架的slam方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310357078.8A CN116977628A (zh) | 2023-04-01 | 2023-04-01 | 一种应用于动态环境的基于多模态语义框架的slam方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116977628A true CN116977628A (zh) | 2023-10-31 |
Family
ID=88475569
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310357078.8A Pending CN116977628A (zh) | 2023-04-01 | 2023-04-01 | 一种应用于动态环境的基于多模态语义框架的slam方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116977628A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117685954A (zh) * | 2024-02-01 | 2024-03-12 | 中国科学院自动化研究所 | 一种用于矿区的多模态语义地图构建系统和方法 |
| CN117928519A (zh) * | 2024-03-19 | 2024-04-26 | 北京理工大学 | 服务机器人的多传感器融合定位与建图方法及系统 |
-
2023
- 2023-04-01 CN CN202310357078.8A patent/CN116977628A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117685954A (zh) * | 2024-02-01 | 2024-03-12 | 中国科学院自动化研究所 | 一种用于矿区的多模态语义地图构建系统和方法 |
| CN117685954B (zh) * | 2024-02-01 | 2024-05-24 | 中国科学院自动化研究所 | 一种用于矿区的多模态语义地图构建系统和方法 |
| CN117928519A (zh) * | 2024-03-19 | 2024-04-26 | 北京理工大学 | 服务机器人的多传感器融合定位与建图方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
| CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
| EP3779358B1 (en) | Map element extraction method and apparatus | |
| CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN113168717B (zh) | 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达 | |
| CN113865580B (zh) | 构建地图的方法、装置、电子设备及计算机可读存储介质 | |
| CN102313547B (zh) | 基于手绘轮廓语义地图的移动机器人视觉导航方法 | |
| CN112070770B (zh) | 一种高精度三维地图与二维栅格地图同步构建方法 | |
| CN111462207A (zh) | 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 | |
| JP7204823B2 (ja) | 車両制御方法、車両制御装置及び車両 | |
| CN116977628A (zh) | 一种应用于动态环境的基于多模态语义框架的slam方法及系统 | |
| CN111664843A (zh) | 一种基于slam的智能仓储盘点方法 | |
| Jeong et al. | Multimodal sensor-based semantic 3D mapping for a large-scale environment | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| US20190311209A1 (en) | Feature Recognition Assisted Super-resolution Method | |
| CN112734765A (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 | |
| CN114004869A (zh) | 一种基于3d点云配准的定位方法 | |
| WO2022062480A1 (zh) | 移动设备的定位方法和定位装置 | |
| CN116518984B (zh) | 一种煤矿井下辅助运输机器人车路协同定位系统及方法 | |
| Ram et al. | Rp-vio: Robust plane-based visual-inertial odometry for dynamic environments | |
| CN111681172A (zh) | 协同构建点云地图的方法、设备和系统 | |
| CN114565726A (zh) | 一种非结构化动态环境下的同时定位与建图方法 | |
| CN116643291A (zh) | 一种视觉与激光雷达联合剔除动态目标的slam方法 | |
| CN113838129B (zh) | 一种获得位姿信息的方法、装置以及系统 | |
| Wei et al. | Novel robust simultaneous localization and mapping for long-term autonomous robots |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |