一种应用于动态环境的基于多模态语义框架的SLAM方法及系统技术方案

本发明专利技术公开了一种应用于动态环境的基于多模态语义框架的SLAM方法及系统。具体过程包括相机图像获取、激光雷达数据处理、IMU预积分、实例分割、多模态融合、特征地图更新和全局语义地图构建。通过视觉、激光雷达和IMU的多模态融合，平衡了精度与计算量；通过动态语义理解，辅助多模态传感器构建3D动态地图，保证了系统的准确率和实时性。本发明专利技术解决了动态非结构化且GNSS无法作用的井下以及仓储导航定位问题，实现了非结构化环境的无人驾驶精准感知与定位技术。与定位技术。与定位技术。

【技术实现步骤摘要】
一种应用于动态环境的基于多模态语义框架的SLAM方法及系统


[0001]本专利技术属于智能定位与导航领域，具体涉及一种应用于动态环境的基于多模态语义框架的SLAM方法及系统。

技术介绍

[0002]煤矿巷道、采掘工作面、仓储物流等作业区域具有典型的非结构化环境特征，且GNSS技术无法直接应用于井下和室内，进而导致煤矿开采时矿难频发，急需机械化换人、自动化减人和提高矿山智能化水平。亟需构建适用于煤矿无人驾驶的自主定位系统方案，解决井下无人驾驶精准定位、姿态感知等问题。如何快速突破视觉、惯导和激光等多信息融合的井下无人驾驶精准感知与定位技术，是实现井下无人驾驶局部自主的关键，故融合视觉、激光雷达和惯性测量单元的SLAM方法非常重要。大多数SLAM框架目前基于静止场景的假设，然而现实世界是复杂且动态的，可能因为不能匹配到足够的正确特征导致系统失败。因为小尺度对象、遮挡、运动模糊导致语义分割结果在动态环境下不理想，误差较大。

技术实现思路

[0003]为了克服上述现有技术的不足，本专利技术提供了一种应用于动态环境的基于多模态语义框架的SLAM方法及系统。
[0004]本专利技术充分考虑了矿山井巷非结构化及动态的环境特征且GNSS技术无法直接运用的因素，由于矿山井巷非结构化环境特征，所以使用多模态融合的方式进行误差补偿，从而使整个系统保持稳定。由于矿山井巷以及仓储物流的动态环境特征，所以在一定情况下，静态地图无法监测物体的移动属性，导致误差较大，但是语义理解可以辅助多模态传感器构建3D动态地图，保证了系统的准确率和实时性。本系统突破了传统SLAM方法以静态环境假设为前提的局限，可以预知物体(人、汽车、动物等)的可移动属性。共享相似物体知识表示，可实现智能路径规划。通过维护共享知识库，提高SLAM系统的可扩展性和存储效率。
[0005]本专利技术所采用的技术方案是：一种应用于动态环境的基于多模态语义框架的SLAM方法及系统，包括以下步骤：
[0006]步骤一，通过相机获取图像，提取当前帧的信息，进行切割和分类。
[0007]步骤二，订阅激光雷达所采集到的原始点云数据。对其进行初步矫正，获取当前帧经过畸变校正之后的有效点云数据、位姿角、初始位姿等信息。进行聚类，检测出障碍物的边缘。
[0008]步骤三，进行IMU预积分，并将结果进行传播。订阅激光和IMU里程计，按照之前时刻的激光里程计，距离当前时刻的IMU里程计改变增量，进行当前时刻IMU里程计的计算；通过rviz来展示局部IMU里程计轨迹。通过激光里程计的两帧激光之间的IMU预积分构造因子图，优化当前帧的位姿、速度、偏置等状态；将优化后的状态作为基础，添加IMU预积分，得到每个时刻的IMU里程计。
[0009]步骤四，实例分割：为多模态信息融合提供实时动态物体分割的语义信息。实现形态膨胀，将动态对象及其边界作为动态信息，结合点云聚类结果和分割结果来更好地细化动态对象。
[0010]步骤五，多模态信息融合：获取激光雷达的点云信息、相机的连续关键帧信息和惯性导航模组的位姿信息进行融合作为精准的感知信息。
[0011]步骤六，定位和位姿估计：先后经过特征提取、数据关联、位姿估计等捕捉，通过最小化点到边缘和点到平面的距离及残差的总和来计算最终位姿。通过姿态估计，更新特征地图并选取其中的关键帧。
[0012]步骤七，全局优化和建图：构建全局语义地图，包括静态地图和动态地图。视觉信息通过将3D点重新投影到图像平面中来实现，同时能够反投影3D点到图像平面。动态地图用于显示动态对象。
[0013]在步骤一中，包括如下步骤：
[0014]①
订阅当前相机提取到的当前帧的信息；
[0015]②
从当前帧的信息中选取一个像素，根据当前帧的信息采用FAST算法检测当前帧的角点，并判断该像素是否为一个角特征点。
[0016]具体判断方法如下：将所选取像素p的亮度值设为I
p
，亮度阈值设为t；以像素p点为中心设定一个半径等于3像素的离散化的Bresenham圆，Bresenham圆的边界上有16个像素；在大小为16个像素的Bresenham圆上有9个连续的像素点，若9个连续像素点的像素值大于或小于I
p
+t，则判定像素p为一个角特征点；若9个连续像素点的像素值等于I
p
+t，则判定像素不是一个角特征点。
[0017]在步骤二中，包括如下步骤：
[0018]①
订阅IMU采集到的原始数据；订阅IMU增量用于位姿估算，当新的IMU数据传递进来时，通过之前IMU积分数据以及当前得到的数据，预估当前时刻的位姿；订阅激光雷达所采集到的原始点云数据。
[0019]②
默认在激光扫描一帧的时间内仅发生了旋转并没有发生平移，对齐IMU数据与时间戳，同时计算IMU队列中每帧包括RPY在内的IMU积分数据，再对激光帧中的每个扫描点进行线性插值。
[0020]③
发布当前帧经过畸变校正之后的有效点云，发布当前帧经过畸变校正之后的点云数据、位姿角、初始位姿等信息。
[0021]④
进行聚类，检测出障碍物的边缘。具体方法为：找到最近点，只要点与点之间的距离在一定半径范围内就认为这些点属于一个聚类。
[0022]在步骤三中，包括如下步骤：
[0023]①
对激光里程计数据进行接收，将数据统一为指定格式并且将当前激光里程计的时间戳进行保存。
[0024]②
移除IMU队列中时间戳早于激光里程计时间戳的IMU增量数据；计算IMU队列中起始和终止时刻对应的IMU里程计之间的相对位姿变换；将先前接收到激光里程计的数据和之前计算得出的相对位姿变换得到的数据进行结合并发布。
[0025]③
订阅IMU原始数据加入到队列中，利用之前时刻优化后得到的结果作为积分的起始值，对IMU队列中的数据进行积分计算，发布IMU增量。
[0026]④
对于IMU角速度和加速度的公式定义如下：
[0027]角速度：
[0028]加速度：
[0029]t时刻下的IMU观测源数据为和并且和都会受到白噪声n
t
和IMU零偏b
t
的影响。矩阵是世界坐标系到机器人坐标系的变换矩阵。重力常数向量g属于世界坐标系W。
[0030]⑤
对系统进行初始化判断；每当接收到100帧的激光里程计数据，优化器就进行一次重置，初始化与重置不同，进行重置时，当前的速度与位姿和之前优化得到的速度与位姿保持一致，噪声模型采用之前优化后模型的边缘分布；添加IMU因子，进行IMU数据两帧之间的积分计算；添加先验因子，以当前接收到的激光里程计作为先验因子；对变量节点进行赋予初值，执行优化并且更新之前时刻的状态；使用当前优化后的状态对预积分器进行初始化，对当前帧之后的IMU数据进行计算。
[0031]⑥
通过IMU预积分来获得不同时间本文档来自技高网...

【技术保护点】

【技术特征摘要】
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观测源数据为和并且和都会受到白噪声n
t
和IMU零偏b
t
的影响。矩阵是世界坐标系到机器人坐标系的变换矩阵。重力常数向量g属于世界坐标系W。最后对系统进行初始化判断。添加IMU因子，进行IMU数据两帧之间的积分计算；添加先验因子，以当前接收到的激光里程计作为先验因子；对变量节点进行赋予初值，执行优化并且更新之前时刻的状态；使用当前优化后的状态对预积分器进行初始化，对当前帧之后的IMU数据进行计算。通过IMU预积分来获得不同时间戳之间的相对运动。预积分得到的第m和第n时刻之间的位置变化Δp
mn
、速度变化Δv
mn
和旋转变化ΔR
mn
计算公式如下：计算公式如下：5....

【专利技术属性】
技术研发人员：王梦然，段章领，陈荪灿，常家铭，郑乔舒，邹燕黎，路然，
申请(专利权)人：合肥图灵纪元科技有限公司，
类型：发明
国别省市：