一种多传感器融合的煤矿井下数字化定位及地图构建系统

摘要

本申请公开了一种多传感器融合的煤矿井下数字化定位及地图构建系统，该系统包括：处理组件和隔爆组件；其中，所述处理组件，包括空间数据采集单元、设置于所述空间数据采集单元下方的视觉图像采集单元和设置于所述视觉图像采集单元下方的数据处理终端；所述隔爆组件，包括隔爆罩和隔爆壳体，其中，所述空间数据采集单元处于所述隔爆罩覆盖范围内；所述隔爆壳体包括透光区域和不透光区域，所述透光区域与所述空间数据采集单元和视觉图像采集单元的视角所对应。由此，本申请通过多传感器进行感知能力互补，提高系统的稳定性，保证系统的鲁棒性。

说明书

技术领域

本申请涉及煤矿开采技术领域，尤其涉及一种煤矿井下数字化定位及地图构建系统。

背景技术

煤炭是我国的主体能源，在推动全国工业发展和国民经济进步等方面发挥了重要作用。近年来，随着采掘设备智能化水平的快速提高，我国煤矿煤炭产量大幅度提高。然而在煤矿采掘过程中，由于煤矿井下场景环境恶劣，且作业设备与相关联目标对象错综复杂，作业人员长时间处于危险系数较高的空间环境中进行作业。由此，为了保障井下人员的生命安全，实现煤矿智能化无人、绿色、高效开采，煤矿井下空间数字化定位与场景重建已经成为重要的技术发展发展方向之一。

相关技术中，通常采用基于信号的通信方式进行定位，然而由于极度依赖信号强度，通常需要在井下设置大量基站，存在操作复杂、成本高的技术问题；同时，基于煤矿井下作业的特点，由于相关信号受井下特殊场景环境的作用问题(例如煤壁吸收)，定位精度也较低。因此，如何生成高精度的煤矿井下全局空间地图，从而实现更加准确地定位，消除安全隐患，确保人员安全，已成为了亟待解决的问题。

发明内容

本申请旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本申请的第一个目的在于提出一种多传感器信息融合的煤矿井下空间定位及建图方法，用于解决现有技术中存在的煤矿井下全局空间建图精度低，导致定位准确性差、存在安全隐患的技术问题。

为了实现上述目的，本申请第一方面实施例提供了一种多传感器信息融合的煤矿井下空间定位及建图方法，该方法包括：获取传感器依次采集的第1至第N帧视觉图像以及所述视觉图像对应的点云数据，其中，所述N为正整数；根据所述视觉图像，获取视觉位姿信息；获取惯性测量单元IMU的IMU数据，以根据所述IMU数据获取IMU位姿信息；根据所述视觉位姿信息和所述IMU位姿信息，获取目标融合位姿信息；根据所述点云数据，对所述目标融合位姿信息进行修正，以得到最终目标位姿信息；根据所述最终目标位姿信息，生成全局空间语义地图。

另外，根据本申请上述实施例的一种多传感器信息融合的煤矿井下空间定位及建图方法，还可以具有如下附加的技术特征：

根据本申请的一个实施例，所述根据所述视觉图像，获取视觉位姿信息，包括：根据所述视觉图像，提取所述视觉图像的视觉特征；在提取所述视觉图像的所述视觉特征之后，进行相机初始化，并判断相机初始化是否成功；响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息。

根据本申请的一个实施例，所述根据所述视觉位姿信息和所述IMU位姿信息，获取目标融合位姿信息，包括：进行IMU初始化，并判断IMU初始化是否成功；响应于IMU初始化成功，则根据所述视觉位姿信息和所述IMU位姿信息，获取融合位姿信息；基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到所述目标融合位姿信息。

根据本申请的一个实施例，所述响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息，包括：分别确定所述第1至第N帧视觉图像中的匹配特征点以及所述匹配特征点的数量，并判断视觉特征匹配与跟踪是否成功；响应于所述匹配特征点的数量达到第一预设数量阈值，则确认视觉特征匹配与跟踪成功，以得到视觉特征匹配与跟踪信息；根据所述视觉特征匹配与跟踪信息，获取所述视觉位姿信息。

根据本申请的一个实施例，所述响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息之后，还包括：获取相邻帧所述视觉位姿信息；根据所述相邻帧所述视觉位姿信息，选取视觉关键帧图像。

根据本申请的一个实施例，所述基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到所述目标融合位姿信息，包括：基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到优化后的所述融合位姿信息；根据所述视觉关键帧图像，获取当前视觉关键帧图像与历史视觉关键帧图像的特征相似度；根据所述特征相似度，判断是否存在闭环状态；响应于存在所述闭环状态，则获取闭环视觉关键帧图像，并对所有所述视觉关键帧图像对应的所述融合位姿信息进行全局闭环非线性优化，以得到所述目标融合位姿信息。

根据本申请的一个实施例，所述根据所述点云数据，对所述目标融合位姿信息进行修正，以得到最终目标位姿信息，包括：根据所述目标融合位姿信息，对所述点云数据进行畸变去除操作，以得到精准点云数据；根据所述精准点云数据，提取目标点云特征；以所述目标融合位姿信息为初始值，通过非线性优化方式对所述目标点云特征进行匹配与跟踪，以得到所述最终目标位姿信息。

根据本申请的一个实施例，所述根据所述最终目标位姿信息，生成全局空间语义地图，包括：根据所述最终目标位姿信息，生成全局空间地图；对所述全局空间地图进行点云着色处理，以生成所述全局空间语义地图。

本申请第一方面实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图方法，能够基于对多种传感器信息进行融合处理，实现了具有场景丰富语义信息的全局空间地图的生成，实现了煤矿井下场景的实时数字化定位与三维空间构建，提升了井下空间定位与三维构建的精度。

为了实现上述目的，本申请第二方面实施例提供了一种多传感器信息融合的煤矿井下空间定位及建图装置，该装置包括：第一获取模块，用于获取传感器依次采集的第1至第N帧视觉图像以及所述视觉图像对应的点云数据，其中，所述N为正整数；第二获取模块，用于根据所述视觉图像，获取视觉位姿信息；第三获取模块，用于获取惯性测量单元IMU的IMU数据，以根据所述IMU数据获取IMU位姿信息；第四获取模块，用于根据所述视觉位姿信息和所述IMU位姿信息，获取目标融合位姿信息；确定模块，用于根据所述点云数据，对所述目标融合位姿信息进行修正，以得到最终目标位姿信息；生成模块，用于根据所述最终目标位姿信息，生成全局空间语义地图。

另外，根据本申请上述实施例的一种多传感器信息融合的煤矿井下空间定位及建图装置，还可以具有如下附加的技术特征：

根据本申请的一个实施例，所述第二获取模块，还用于：根据所述视觉图像，提取所述视觉图像的视觉特征；在提取所述视觉图像的所述视觉特征之后，进行相机初始化，并判断相机初始化是否成功；响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息。

根据本申请的一个实施例，所述第四获取模块，还用于：进行IMU初始化，并判断IMU初始化是否成功；响应于IMU初始化成功，则根据所述视觉位姿信息和所述IMU位姿信息，获取融合位姿信息；基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到所述目标融合位姿信息。

根据本申请的一个实施例，所述第四获取模块，还用于：分别确定所述第1至第N帧视觉图像中的匹配特征点以及所述匹配特征点的数量，并判断视觉特征匹配与跟踪是否成功；响应于所述匹配特征点的数量达到第一预设数量阈值，则确认视觉特征匹配与跟踪成功，以得到视觉特征匹配与跟踪信息；根据所述视觉特征匹配与跟踪信息，获取所述视觉位姿信息。

根据本申请的一个实施例，所述第四获取模块，还用于：获取相邻帧所述视觉位姿信息；根据所述相邻帧所述视觉位姿信息，选取视觉关键帧图像。

根据本申请的一个实施例，所述第四获取模块，还用于：基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到优化后的所述融合位姿信息；根据所述视觉关键帧图像，获取当前视觉关键帧图像与历史视觉关键帧图像的特征相似度；根据所述特征相似度，判断是否存在闭环状态；响应于存在所述闭环状态，则获取闭环视觉关键帧图像，并对所有所述视觉关键帧图像对应的所述融合位姿信息进行全局闭环非线性优化，以得到所述目标融合位姿信息。

根据本申请的一个实施例，所述确定模块，还用于：根据所述目标融合位姿信息，对所述点云数据进行畸变去除操作，以得到精准点云数据；根据所述精准点云数据，提取目标点云特征；以所述目标融合位姿信息为初始值，通过非线性优化方式对所述目标点云特征进行匹配与跟踪，以得到所述最终目标位姿信息。

根据本申请的一个实施例，所述生成模块，还用于：根据所述最终目标位姿信息，生成全局空间地图；对所述全局空间地图进行点云着色处理，以生成所述全局空间语义地图。

本申请第二方面实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图装置，能够基于对多种传感器信息进行融合处理，实现了具有场景丰富语义信息的全局空间地图的生成，实现了煤矿井下场景的实时数字化定位与三维空间构建，提升了井下空间定位与三维构建的精度。

为了实现上述目的，本申请第三方面实施例提供了一种多传感器融合的煤矿井下数字化定位及地图构建系统，该系统包括：处理组件和隔爆组件；其中，所述处理组件，包括空间数据采集单元、设置于所述空间数据采集单元下方的视觉图像采集单元和设置于所述视觉图像采集单元下方的数据处理终端；所述隔爆组件，包括隔爆罩和隔爆壳体，其中，所述空间数据采集单元处于所述隔爆罩覆盖范围内；所述隔爆壳体包括透光区域和不透光区域，所述透光区域与所述空间数据采集单元和视觉图像采集单元的视角所对应。

另外，根据本申请上述实施例的一种多传感器融合的煤矿井下数字化定位及地图构建系统，还可以具有如下附加的技术特征：

根据本申请的一个实施例，所述处理组件，还包括：惯性测量单元IMU，所述IMU布置于所述空间数据采集单元下方。

根据本申请的一个实施例，所述数据处理终端，包括：参数标定单元，所述参数标定单元用于对所述视觉图像采集单元进行参数标定、对所述视觉图像采集单元和所述IMU进行联合参数标定、对所述视觉图像采集单元和所述空间数据采集单元进行联合参数标定，对所述视觉图像采集单元、所述空间数据采集单元、所述IMU三者数据的时间戳进行配准，以及对三者数据进行相关智能分析和处理。

根据本申请的一个实施例，所述视觉图像采集单元为单目或双目视觉相机，用于获取煤矿井下视觉图像。

根据本申请的一个实施例，所述空间数据采集单元为激光雷达传感器或毫米波雷达传感器，用于获取煤矿井下视觉图像对应的点云数据。

根据本申请的一个实施例，所述不透光区域为金属壳体。

根据本申请的一个实施例，所述处理组件，还包括：防抖云台，所述防抖云台布置于所述数据处理终端上方。

根据本申请的一个实施例，还包括：移动组件，所述移动组件布置于所述隔爆组件底部，用于承载所述隔爆组件进行移动。

根据本申请的一个实施例，还包括：存储器，所述存储器用于存储煤矿井下视觉图像、煤矿井下视觉图像对应的点云数据以及数据处理终端输出的煤矿井下全局空间地图。

根据本申请的一个实施例，还包括：无线传输单元，所述无线传输单元用于将煤矿井下视觉图像、煤矿井下图像对应的点云数据以及数据处理终端输出的煤矿井下全局空间地图发送至终端设备。

本申请第三方面实施例提供的一种多传感器融合的煤矿井下数字化定位及地图构建系统，能够通过多传感器进行感知能力互补，提高系统的稳定性，保证系统的鲁棒性。

为了实现上述目的，本申请第四方面实施例提供了一种电子设备，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时，实现如本申请第一方面实施例中任一项所述的多传感器信息融合的煤矿井下空间定位及建图方法。

为了实现上述目的，本申请第五方面实施例提供了一种计算机可读存储介质，该程序被处理器执行时实现如本申请第一方面实施例中任一项所述的多传感器信息融合的煤矿井下空间定位及建图方法。

附图说明

图1为本申请一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图2为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图3为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图4为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图5为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图6为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图7为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图8为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图9为本申请另一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图方法的方法示意图。

图10为本申请一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统的结构示意图。

图11为本申请一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统的处理组件的结构示意图。

图12为本申请另一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统的处理组件的结构示意图。

图13为本申请一个实施例公开的数据流时间戳配准流程的示意图。

图14为本申请另一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统的处理组件的结构示意图。

图15为本申请一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统整体框架的示意图。

图16为本申请一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统上、中层主要元件的示意图。

图17为本申请一个实施例公开的多传感器融合的煤矿井下数字化定位及地图构建系统下层主要元件的示意图。

图18为本申请一个实施例公开的多传感器信息融合的煤矿井下空间定位及建图装置的结构示意图。

具体实施方式

为了更好的理解上述技术方案，下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

需要说明的是，煤矿井下空间数字化定位与场景重建是实现智能化无人化开采的关键支撑技术之一，对于煤矿的智能开采，数字化监控管理，危险事先预警，设备环境状态的实时定位与感知等方面都有着极大的应用价值与意义。

相关技术中，主要通过以下两种方式生成煤矿井下全局空间地图，从而实现煤矿井下空间的定位：基于信号的通信方式以及依赖单一传感器进行感知的方式。

针对基于信号的通信方式，典型的技术有UWB(Ultra-Wide Band，超宽带)、GPS(Global Positioning System，全球定位系统)、AGPS(Assisted Global PositioningSystem，辅助全球卫星定位系统)、ZigBEE(紫蜂协议)等。这类方法的实现方式通常有以下不足之处。一方面是此类项技术方法十分依赖于信号强度与状况，通常需要在井下设置大量基站，操作复杂、成本高、存在潜在风险；另一方面是此类技术方法在井下环境空间中由于信号受井下特殊场景环境的作用(例如煤壁吸收)，精准度较低，难以实现高精度的定位。

针对依赖单一传感器进行感知的方式，相关的传感器有各种类型的可视化相机、轮速计、IMU(Inertial Measurement Unit，惯性测量单元)、激光雷达等。然而当前此类技术方案大多数因仅仅使用单一传感器，常常导致系统定位精度不足，同时存在成本过高的问题。

由此，本申请提出一种多传感器信息融合的煤矿井下空间定位及建图方法，能够基于对多种传感器信息进行融合处理，实现了具有场景丰富语义信息的全局空间地图的生成，实现了煤矿井下场景的实时数字化定位与三维空间构建，提升了井下空间定位与三维构建的精度。

下面参考附图描述本申请实施例的一种多传感器信息融合的煤矿井下空间定位及建图方法与装置。

图1是本申请公开的一个实施例的一种多传感器信息融合的煤矿井下空间定位及建图方法的流程示意图。

如图1所示，本申请实施例提出的多传感器信息融合的煤矿井下空间定位及建图方法，具体包括以下步骤：

S101、获取传感器依次采集的第1至第N帧视觉图像以及视觉图像对应的点云数据，其中，N为正整数。

在本申请的实施例中，可以接收视觉相机在预先设置的任一路段行驶过程中采集到的场景图片，即视觉图像。

其中，试图获取的视觉相机上传的视觉图像的数量可以根据实际情况进行设定。例如，可以获取相机在路段行驶过程中依次采集到的60张视觉图像，即言，可以设定N为60。

需要说明的是，视觉相机可以按照预设间隔采集视觉图像，并上传依次采集到的视觉图像。其中，预设间隔可以为预设距离间隔，也可以为预设时间间隔。例如，可以设定预设间隔为17米、20米等；也可以设定预设间隔为10秒、15秒等。

S102、根据视觉图像，获取视觉位姿信息。

需要说明的是，本申请中对于根据视觉图像，获取视觉位姿信息的具体方式不作限定，可以根据实际情况进行选取。

可选地，可以将视觉图像输入至预先训练好的解码-编码器中，通过多层卷积、反卷积等处理，输出视觉图像的视觉特征。进一步地，根据视觉特征获取视觉位姿信息。

S103、获取惯性测量单元IMU的IMU数据，以根据IMU数据获取IMU位姿信息。

其中，IMU为测量物体三轴姿态角(或角速率)以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计分别测量刚体在载体坐标系统独立三个轴向的加速度数据，而陀螺仪则测量载体相对于导航坐标系的角速度数据，由此可测量得到刚体的IMU姿态测量数据，包括刚体在三维空间中三个轴向上的角速度和加速度。进一步地，可以根据IMU数据获取IMU位姿信息。

其中，IMU具有不同的型号，每种型号采用的采集频率可能不同，在本申请中不对IMU的型号进行限定。

需要说明的是，当刚体携带IMU运动时，IMU可以通过测量得到运动时的加速度以及角速度，并根据IMU原始设置的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，计算得到IMU的待融合姿态数据。

标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声均是已知的，一般情况下，IMU硬件制造厂均会对出厂的IMU设备进行测试，并通过大量的测试数据确定IMU的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，因此，在本申请中，并不对标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声的数值进行限定，不同的IMU设有不同的数值。

S104、根据视觉位姿信息和IMU位姿信息，获取目标融合位姿信息。

本申请实施例中，可以根据视觉位姿信息和IMU位姿信息获取融合位姿信息。进一步地，可以对融合位姿信息进行优化，以得到优化后的目标融合位姿信息。

需要说明的是，本申请中对于对融合位姿信息进行优化的具体方式不作限定，可以根据实际情况进行选取。可选地，基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到优化后的目标融合位姿信息。

S105、根据点云数据，对目标融合位姿信息进行修正，以得到最终目标位姿信息。

需要说明的是，本申请中对于对目标融合位姿信息进行修正的具体方式不作限定，可以根据实际情况进行选取。可选地，可以采用非线性优化方式对目标融合位姿信息进行修正，以得到最终的目标位姿信息。

S106、根据最终目标位姿信息，生成全局空间语义地图。

本申请实施例中，可以根据最终目标位姿信息，生成全局空间语义地图。进一步地，可以对全局空间地图进行点云着色处理，以生成全局空间语义地图。

本申请第一方面实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图方法，能够基于对多种传感器信息进行融合处理，实现了具有场景丰富语义信息的全局空间地图的生成，实现了煤矿井下场景的实时数字化定位与三维空间构建，提升了井下空间定位与三维构建的精度。

需要说明的是，本申请中，在试图根据视觉图像，获取视觉位姿信息时，可以首先进行视觉前端的初始化。

作为一种可能的实现方式，如图2所示，具体包括以下步骤：

S201、根据视觉图像，提取视觉图像的视觉特征。

S202、在提取视觉图像的视觉特征之后，进行相机初始化，并判断相机初始化是否成功。

需要说明的是，本申请中，首先输入视觉相机的序列帧图像(即第1至第N帧视觉图像)进行灰度化操作，然后进行多尺度下金字塔图像构建。在多尺度下金字塔图像构建的基础上进行特征点检测、特征点描述与特征点畸变矫正。

为了更好的适应煤矿井下场景视觉图像的关键特征点检测，使用GPU(GraphicsProcessing Unit，图形处理器)针对关键特征点的提取和筛选操作进行加速，保证序列帧图像中关键特征点的稳定鲁棒检测。

进一步地，由于本申请中的相机优选为双目摄像头，因此在实现过程中还需要基于相机外参进行左右摄像头的视觉图像特征匹配以及场景深度估计。

可选地，可以在相机初始化后，对视觉图像帧进行特征点的稳定检测，以得到关键特征点。进一步地，可以将检测到的关键特征点的数量与第二预设数量阈值进行比较，若数量达到第二预设数量阈值，则识别相机初始化成功；若数量未达到第二预设数量阈值，则识别相机初始化未成功。

其中，第二预设数量阈值可以根据实际情况进行设定。

S203、响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息。

作为一种可能的实现方式，如图3所示，上述步骤S203中进行视觉特征匹配与跟踪，以得到视觉位姿信息的具体方式，包括以下步骤：

S301、分别确定第1至第N帧视觉图像中的匹配特征点以及匹配特征点的数量，并判断视觉特征匹配与跟踪是否成功。

作为一种可能的实现方式，如图4所示，上述步骤S301中确定第1至第N帧视觉图像中的匹配特征点以及匹配特征点的数量的具体方式，包括以下步骤：

S401、获取第1至第N帧视觉图像的特征点信息。

S402、基于预先设定的筛选策略对特征点信息进行筛选，以确定匹配特征点。

可选地，当视觉前端初始化完成后进行视觉前端帧图像的特征点匹配与跟踪，实现基于视觉信息的位姿初估计。首先针对视觉前端连续帧图像中特征点的特征匹配，实现特征点在连续帧图像中的稳定跟踪。

S302、响应于匹配特征点的数量达到第一预设数量阈值，则确认视觉特征匹配与跟踪成功，以得到视觉特征匹配与跟踪信息。

可选地，可以将稳定匹配的特征点的数量与第一预设数量阈值进行比较，若数量达到第一预设数量阈值，则识别稳定跟踪成功；若数量未达到第一预设数量阈值，则识别稳定跟踪未成功。

其中，第一预设数量阈值可以根据实际情况进行设定。

S303、根据视觉特征匹配与跟踪信息，获取视觉位姿信息。

可选地，可以利用上一帧的位姿根据特定模型进行当前帧位姿的初始推断，以及当前帧与上一帧匹配特征点信息，完成当前帧的视觉位姿信息求解。在实现过程中，具体根据公式(1)实现视觉前端的位姿初估计。

其中，R∈SO(3)和

需要说明的是，本申请中对于得到视觉位姿信息的具体方式不作限定，可以根据实际情况进行选取。可选地，可以通过预先训练好的优化模型得到视觉位姿信息；可选地，可以采用运动恢复结构算法(Structure from Motion，简称SFM)，从获取到的依次采集到的第1至第N帧视觉图像(二维图像序列)中估计出三维结构，以得到视觉位姿信息。

进一步地，在响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息之后，还可以选取视觉关键帧图像。

作为一种可能的实现方式，如图5所示，具体包括以下步骤：

S501、获取相邻帧视觉位姿信息。

S502、根据相邻帧视觉位姿信息，选取视觉关键帧图像。

需要说明的是，本申请中，在试图根据视觉位姿信息和IMU位姿信息，获取目标融合位姿信息时，可以首先进行IMU的初始化。

作为一种可能的实现方式，如图6所示，具体包括以下步骤：

S601、进行IMU初始化，并判断IMU初始化是否成功。

需要说明的是，在视觉前端初始化过程中，主要是实现视觉前端帧图像的关键特征点稳定检测与特征描述。在具体实现过程中，当视觉图像帧中可以稳点检测到关键特征点，且检测数量达到一定预设数量后，就将当前视觉图像帧构造为视觉前端的初始关键帧图像。其中，构造的信息可以包括关键帧ID、关键帧位姿以及与之共视的普通帧图像信息。同时，可以将视觉前端的初始位姿，即视觉位姿信息设置为单位矩阵，即T＝[I 0]。当关键帧图像完成初始后计算获取关键帧图像中关键特征点的深度信息，同时进行各关键特征点的3D地图点构建。

在视觉位姿初估计的同时，基于IMU数据进行IMU的位姿估计。针对IMU数据进行预积分操作计算获取与IMU数据配准的相邻视频帧的预积分增量，以及相应的Jacobian矩阵与协方差值，从而实现IMU位姿与速度的估计。

其中，IMU初始化过程主要是实现IMU传感器偏置量，尺度信息与重力加速度的参数估计。首先基于IMU数据序列计算获得的位姿与速度，根据与之配准的相邻视频帧之间的IMU旋转增量，以及视觉前端位姿估计的旋转增量，实现IMU偏置量初估计。然后基于视觉相机和IMU的联合标定外参，根据视觉相机与IMU分别在世界坐标系中的平移量利用SVD(奇异值分解)计算获得尺度与重力加速度的初估计。最后基于非线性优化进行IMU传感器偏置量，尺度信息与重力加速度参数的进一步估计从而完成IMU的初始化过程。

S602、响应于IMU初始化成功，则根据视觉位姿信息和IMU位姿信息获取融合位姿信息。

可选地，当IMU初始化过程完成后，将基于IMU位姿信息进行视觉前端的位姿估计。首先根据视觉前端中序列帧图像中前一帧的位姿信息，以及与当前帧匹配的IMU位姿信息，进行当前帧的关键特征点匹配与跟踪。然后在视觉前端连续帧图像中关键特征点成功跟踪的基础上进行当前帧的位姿求解，获得IMU信息与视觉信息融合的位姿初估计，即得到融合位姿信息。

S603、基于滑动窗口的非线性优化算法对融合位姿信息进行优化，以得到目标融合位姿信息。

需要说明的是，本申请中，为了充分利用视觉语义信息针对全局位姿进行约束优化，提高空间定位的精确度。在进行基于IMU与视觉信息融合的高频位姿估计的同时，需要利用闭环检测的方式不断针对全局位姿进行优化。

作为一种可能的实现方式，如图7所示，上述步骤S603中基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到目标融合位姿信息的具体过程，包括以下步骤：

S701、基于滑动窗口的非线性优化算法对融合位姿信息进行优化，以得到优化后的融合位姿信息。

S702、根据视觉关键帧图像，获取当前视觉关键帧图像与历史视觉关键帧图像的特征相似度。

S703、根据特征相似度，判断是否存在闭环状态。

本申请实施例中，在视觉关键帧图像集合中根据词袋特征进行相似关键帧检索，并按照一定标准进行闭环视觉关键帧的选取。

本申请实施例中，仅当候选闭环关键帧与当前视觉关键帧的词袋特征相似度达到一定标准并通过闭环关键帧的连续性检测后，才认为可能出现闭环状态。

可选地，可以将特征相似度与预先设定的特征相似度阈值进行比较，若特征相似度达到特征相似度阈值，则识别存在闭环状态；若特征相似度未达到特征相似度阈值，则识别不存在闭环状态。

可选地，为了进一步提高是否存在闭环状态的判定结果的准确性，可以通过Sim3相似变换方式，获得当前视觉关键帧与候选闭环关键帧之间的特征点匹配，以及之间的相对位姿。当匹配的特征点数目大于一定数量时，则识别闭环检测成功，否则识别闭环检测失败。

S704、响应于存在闭环状态，则获取闭环视觉关键帧图像，并对所有视觉关键帧图像对应的融合位姿信息进行全局闭环非线性优化，以得到目标融合位姿信息。

在完成IMU信息与视觉信息融合位姿初估计后，将通过非线性优化进行融合位姿的进一步优化校准，完成基于IMU与视觉信息融合的高频位姿估计。由于煤矿井下场景多为长直甬道的特点，考虑到基于共视关系的优化计算复杂度较高，因此本申请技术方案实施过程中，为了减轻优化规模保障系统实时性，使用滑动窗口的方式进行融合位姿的优化校准。在融合位姿的非线性优化过程中，利用特定关键帧对应的IMU位姿约束、视觉观测约束、先验位姿信息约束进行位姿估计优化，优化目标函数如公式(2)所示。

其中，X为优化变量的集合，r

在闭环检测成功的基础上首先进行当前帧与其他视觉关键帧共视关系的搜索与更新，然后通过位姿传播得到修正后的其他关键帧位姿与相应特征点的空间坐标信息，并通过当前帧与闭环关键帧及其相邻关键帧所对应特征点的冲突检查进行相应帧的共视关系的更新。最后根据修正后的特征点空间坐标信息，以及帧之间更新后的共视关系进行全局闭环非线性优化实现所有关键帧位姿的修订，从而得到优化后的目标融合位姿信息。

进一步地，本申请中，在试图根据点云数据，对目标融合位姿信息进行修正，以得到最终目标位姿信息时，可以首先对点云数据进行畸变去除操作。

作为一种可能的实现方式，如图8所示，具体包括以下步骤：

S801、根据目标融合位姿信息，对点云数据进行畸变去除操作，以得到精准点云数据。

S802、根据精准点云数据，提取目标点云特征。

S803、以目标融合位姿信息为初始值，通过非线性优化方式对目标点云特征进行匹配与跟踪，以得到最终目标位姿信息。

本申请实施例中，可以根据视频序列帧与点云数据序列之间的时钟配准关系，对每一帧点云数据利用平面光化度方式实现特征提取。进一步地，在IMU与视觉信息融合高频位姿估计实现的基础上进行点云数据畸变去除操作。以IMU与视觉信息融合得到的高频位姿信息为初始值，通过非线性优化方式进行连续点云数据帧的特征匹配，实现低频位姿优化矫正，以得到最终目标位姿信息。

进一步地，本申请中，在试图根据最终目标位姿信息，生成全局空间语义地图时，可以通过对全局空间地图进行点云着色处理，以生成具有丰富场景语义信息的全局空间语义地图。

作为一种可能的实现方式，如图9所示，具体包括以下步骤：

S901、根据最终目标位姿信息，生成全局空间地图。

S902、对全局空间地图进行点云着色处理，以生成全局空间语义地图。

本申请实施例中，可以基于激光雷达与视觉相机之间的外参标定结果，将与视觉帧图像配准的点云数据投影到视觉平面内，获取相应投影像素点的视觉信息，然后利用获取的视觉信息针对点云数据进行着色处理，生成全局空间的数字化语义地图。

下面参考附图描述本申请实施例的一种多传感器融合的煤矿井下数字化定位及地图构建系统。

图10是本申请公开的一个实施例的一种多传感器融合的煤矿井下数字化定位及地图构建系统的结构示意图。

如图10所示，本申请实施例提出的多传感器融合的煤矿井下数字化定位及地图构建系统1000，包括：处理组件110和隔爆组件120。

其中，如图11所示，处理组件110，包括空间数据采集单元111、设置于空间数据采集单元111下方的视觉图像采集单元112和设置于视觉图像采集单元112下方的数据处理终端113。

空间数据采集单元111可以为激光雷达传感器或微波传感器，用于获取煤矿井下视觉图像对应的空间深度信息，用于采集传感器依次采集的第1至第N帧视觉图像对应的点云信息，其中，N为正整数。

视觉图像采集单元112可以为双目红外视觉相机，用于传感器依次采集煤矿井下图像第1至第N帧视觉图像；视觉图像采集单元112也可以为全景摄像头等摄像装置，此种情况下，在获取到煤矿井下视频后，可以通过抽帧等处理，获取依次采集的第1至第N帧视觉图像。

数据处理终端113，用于根据视觉图像，提取视觉图像的视觉特征；根据视觉特征和IMU位姿信息，获取目标融合位姿信息；根据点云信息，对目标融合位姿信息进行修正，以得到最终目标位姿信息；以及根据最终目标位姿信息，生成全局空间语义地图。

其中，隔爆组件120，包括隔爆罩121和隔爆壳体122，其中，空间数据采集单元111处于隔爆罩121覆盖范围内；隔爆壳体122包括透光区域1221和不透光区域1222。

透光区域1221与视觉图像采集单元112和空间数据采集单元111的视角所对应，以确保视觉图像采集单元112能够顺利采集视觉图像，以及确保空间数据采集单元111能够顺利采集空间数据；不透光区域1222为金属壳体。

由此，本申请实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图系统，通过设置空间数据采集单元、视觉图像采集单元和数据处理终端，能够实现煤矿井下空间定位与实时三维场景构建。该系统本体结构的具备一体化本质安全设计，以通过多传感器进行感知能力互补，提高系统的稳定性，保证系统的鲁棒性。同时，一定程度上解决在井下空间中空间数据采集单元存在的定位漂移问题，充分利用了视觉图像采集单元的视觉感知特长，获取空间场景的语义信息，使得空间数据采集单元获取的空间点云数据具有场景丰富的语义信息，提升了井下空间定位与三维构建的精度。

在一些实施例中，如图12所示，处理组件110，还包括：惯性测量单元IMU114。

其中，IMU114布置于空间数据采集单元111下方，并与视觉图像采集单元112连接，用于获取IMU数据，以根据IMU数据数据获取IMU位姿信息。

在一些实施例中，数据处理终端113，包括：参数标定单元1131。

其中，参数标定单元1131，用于对视觉图像采集单元进行参数标定、对视觉图像采集单元和IMU进行联合参数标定、对视觉图像采集单元和空间数据采集单元进行联合参数标定，对视觉图像采集单元、空间数据采集单元和IMU三者数据的时间戳进行配准，以及对三者数据进行相关智能分析和处理。

需要说明的是，在实际应用中，在本申请提出的一种多传感器融合的煤矿井下数字化定位及地图构建系统1000出厂前，可以进行参数标定。

下面分别针对对视觉图像采集单元进行参数标定、对视觉图像采集单元和IMU进行联合参数标定、对视觉图像采集单元和空间数据采集单元进行联合参数标定，以及对对视觉图像采集单元、空间数据采集单元和IMU三者数据的时间戳配准过程进行解释说明。

针对对视觉图像采集单元进行参数标定，需要说明的是，有关视觉相机的选型，可以根据实际需求选择单目视觉相机、双目视觉相机、双目红外相机，以及深度相机等。在本发明的技术方案实施过程中，根据煤矿井下场景特点，采用双目红外视觉相机，可以充分获取井下场景的纹理视觉特征，有效保证跟踪特征点的鲁棒检测。

以双目红外视觉相机为例，对于视觉相机的标定分为内参标定和外参标定两部分进行。在标定过程中使用kalibr等开源标定工具进行标定。同时，为保证视觉相机标定的精准度，采用带有背光板的氧化铝材料(或陶瓷)制作的专有标定板进行标定视觉图像采集。在标定视觉图像采集过程中，需要针对各个拍摄角度(相机相对于标定板的拍摄角度)进行充分激励，同时保证采集的视觉图像没有因运动而产生的模糊现象，从而保证视觉相机的标定精准度。

针对对视觉图像采集单元和IMU进行联合参数标定，在完成视觉相机标定后需要进行视觉相机与IMU的联合标定。视觉相机-IMU的联合标同样使用kalibr等开源标定工具(类似的开源工具还有opencv标定等标定工具)以及上述专有标定板进行标定。在标定前需要固定相机与IMU的相对位置，同时需要对IMU中的加速度计与陀螺仪进行充分激励，并保证视觉相机的标定图像画面没有因运动而产生的模糊现象。

针对对视觉图像采集单元和空间数据采集单元进行联合参数标定，激光雷达-视觉相机的联合标定使用公共autoware工具中的calibrtToolKit模块进行标定。在标定前需要固定好激光雷达与视觉相机的相对位置，并制作大规格棋盘格形式的专有标定板。举例而言，可以制作并使用120cm*80cm规格的专有标定板。标定过程中需要改变标定板的角度进行特定角度标定数据的获取，以保证激光雷达-视觉相机联合标定的精准度。

需要说明的是，在实际应用中，可以采用两种方式进行标定数据的获取，一种方式是固定标定板，通过充分移动传感器系统装置进行不同角度标定数据的获取。一种方式是固定传感器系统装置，通过移动标定板来改变视角从而获取不同视角的标定数据。

针对对视觉图像采集单元、空间数据采集单元和IMU三者数据的时间戳进行配准，由于IMU提供高频数据流，其频率通常为200Hz到400Hz之间，而视觉相机和激光雷达传感器的数据流频率为低频。一般情况下，视觉相机的数据流频率为15Hz到60Hz之间，而激光雷达数据流的频率低至5Hz到15Hz。因此需要针对传感器系统数据的时间戳进行配准，实现传感器系统数据流在时间序列上的一致性。

对于IMU的高频数据流与视觉相机的低频数据流时间戳配准，在本申请技术方案中首先将IMU与视觉相机序列数据帧之间的时间差设置为优化变量，然后通过非线性优化针对时间差进行求解实现时钟的配准。非线性优化的残差方程是基于重投影进行构建。对于视觉相机与激光雷达，使用软触发与硬触发相结合的方式进行视觉相机与激光雷达数据流时钟配准，数据流时间戳配准流程如图13所示。软触发过程的触发信号由系统软件启动，例如使用视觉相机SDK(Software Development Kit，软件开发工具包)提供的API(Application Programming Interface，应用程序编程接口)接口进行软触发。硬触发的实现首先需要在传感器外部设置特定设备(如MCU(Micro-Controller Unit，微控制单元))，然后将特定设备与系统内各传感器的I/O接口(I/O Interface)进行连接，基于Strobe(选通脉冲)信号实现硬触发。

根据图13可知，可以首先利用视觉相机(此处为双目视觉相机，有左相机和右相机)的API对左相机进行软触发，然后使用左相机返回的Strobe信号对右相机和激光雷达进行硬触发。其中对右相机的触发频率为15Hz到60Hz之间的一个量(即每收到1个Strobe信号触发一次)，对激光雷达的触发频率为5Hz到15Hz之间的一个量(即每收到3个Strobe信号触发一次)。其中，Strobe信号为视觉相机曝光发生时所产生的信号，且高电平为有效位。在实施过程中，由于考虑到系统软触发所产生的Strobe信号在被系统接收时会有一定延迟，因此系统需要针对软触发产生的Strobe信号进行预输出处理。通常情况下对左相机进行曝光延迟处理，使得曝光产生时间与系统接收到的Strobe信号的时间达到同步。

在一些实施例中，如图14所示，处理组件110，还包括：防抖云台115。

其中，防抖云台115布置于数据处理终端113上方，用于平衡煤矿井下空间定位及建图系统1000运动过程中的抖动。

在一些实施例中，多传感器融合的煤矿井下数字化定位及地图构建系统1000，还包括：移动组件130、存储器140以及无线传输单元150。

其中，移动组件130布置于隔爆组件120底部，用于承载隔爆组件120进行移动。

其中，存储器140用于存储煤矿井下视觉图像、煤矿井下视觉图像对应的点云数据信息以及数据处理终端输出的煤矿井下全局空间地图。

其中，无线传输单元140用于将煤矿井下视觉图像、煤矿井下图像对应的点云数据信息以及数据处理终端输出的煤矿井下全局空间地图发送至智能分析处理终端设备。

综上所述，如图15(a)所示，多传感器融合的煤矿井下数字化定位及地图构建系统1000，整体框架可以分为内外两层，如图15(b)所示，外部为隔爆组件120，内部为由各传感器元件构成的处理组件110。

其中，处理组件110内部整体分为三部分，如图16所示，最上层布置空间数据采集单元111，例如激光雷达传感器，中层布置视觉图像采集单元112和IMU114，例如视觉相机与IMU器件，如图17所示，下层布置数据处理终端113以及防抖云台115。

其中，外部整体分为两部分，顶部为隔爆罩121，例如激光雷达传感器防爆罩，其余部分为隔爆壳体122，隔爆壳体122可以分为透光区域与不透光区域；其中，透光区域有视觉相机视角专有可透光面和激光雷达视角专有可透光面，不透光区域为不可透光金属防爆面。

由此，本申请实施例提供的一种多传感器融合的煤矿井下数字化定位及地图构建系统，能够实现传感器系统的快速标定，以及基于软硬结合方式实现的各传感器数据的时间戳一致性配准功能。

与上述几种实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图方法相对应，本申请的一个实施例还提供一种多传感器信息融合的煤矿井下空间定位及建图装置，由于本申请实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图装置与上述几种实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图方法相对应，因此在一种多传感器信息融合的煤矿井下空间定位及建图方法的实施方式也适用于本实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图装置，在本实施例中不再详细描述。

图18是根据本申请一个实施例的一种多传感器信息融合的煤矿井下空间定位及建图装置的结构示意图。

如图18所示，该煤矿井下空间定位及建图装置2000，包括：第一获取模块210、第二获取模块220、第三获取模块230、第四获取模块240、确定模块250和生成模块260。其中：

第一获取模块210，用于获取传感器依次采集的第1至第N帧视觉图像以及所述视觉图像对应的点云数据，其中，所述N为正整数；

第二获取模块220，用于根据所述视觉图像，获取视觉位姿信息；

第三获取模块230，用于获取惯性测量单元IMU的IMU数据，以根据所述IMU数据获取IMU位姿信息；

第四获取模块240，用于根据所述视觉位姿信息和所述IMU位姿信息，获取目标融合位姿信息；

确定模块250，用于根据所述点云数据，对所述目标融合位姿信息进行修正，以得到最终目标位姿信息；

生成模块260，用于根据所述最终目标位姿信息，生成全局空间语义地图。

根据本申请的一个实施例，第二获取模块220，还用于：根据所述视觉图像，提取所述视觉图像的视觉特征；在提取所述视觉图像的所述视觉特征之后，进行相机初始化，并判断相机初始化是否成功；响应于相机初始化成功，则进行视觉特征匹配与跟踪，以得到视觉位姿信息。

根据本申请的一个实施例，第四获取模块240，还用于：进行IMU初始化，并判断IMU初始化是否成功；响应于IMU初始化成功，则根据所述视觉位姿信息和所述IMU位姿信息，获取融合位姿信息；基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到所述目标融合位姿信息。

根据本申请的一个实施例，第四获取模块240，还用于：分别确定所述第1至第N帧视觉图像中的匹配特征点以及所述匹配特征点的数量，并判断视觉特征匹配与跟踪是否成功；响应于所述匹配特征点的数量达到第一预设数量阈值，则确认视觉特征匹配与跟踪成功，以得到视觉特征匹配与跟踪信息；根据所述视觉特征匹配与跟踪信息，获取所述视觉位姿信息。

根据本申请的一个实施例，第四获取模块240，还用于：获取相邻帧所述视觉位姿信息；根据所述相邻帧所述视觉位姿信息，选取视觉关键帧图像。

根据本申请的一个实施例，第四获取模块240，还用于：基于滑动窗口的非线性优化算法对所述融合位姿信息进行优化，以得到优化后的所述融合位姿信息；根据所述视觉关键帧图像，获取当前视觉关键帧图像与历史视觉关键帧图像的特征相似度；根据所述特征相似度，判断是否存在闭环状态；响应于存在所述闭环状态，则获取闭环视觉关键帧图像，并对所有所述视觉关键帧图像对应的所述融合位姿信息进行全局闭环非线性优化，以得到所述目标融合位姿信息。

根据本申请的一个实施例，确定模块250，还用于：根据所述目标融合位姿信息，对所述点云数据进行畸变去除操作，以得到精准点云数据；根据所述精准点云数据，提取目标点云特征；以所述目标融合位姿信息为初始值，通过非线性优化方式对所述目标点云特征进行匹配与跟踪，以得到所述最终目标位姿信息。

根据本申请的一个实施例，生成模块260，还用于：根据所述最终目标位姿信息，生成全局空间地图；对所述全局空间地图进行点云着色处理，以生成所述全局空间语义地图。

根据本申请实施例提供的一种多传感器信息融合的煤矿井下空间定位及建图装置，能够基于对多种传感器信息进行融合处理，实现了具有丰富场景语义信息的全局空间地图生成，实现了煤矿井下场景的实时数字化定位与三维空间构建，提升了井下空间定位与三维构建的精度。

在本申请的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本申请中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。

在本申请中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。