地下作业设备定位方法、装置、系统及存储介质与流程

本申请涉及地下作业领域，具体涉及一种地下作业设备定位方法、装置、系统及存储介质。

背景技术：

随着国民经济的发展，对于各种矿产资源的需求及利用越来越高。传统采矿模式很难满足矿山企业在成本、效率、效益、安全和环保等各方面的要求，尤其对资源量多、开采强度高、落矿规模大的地下金属矿山，无人采矿技术的应用极为紧迫。地下作业设备(如地下采矿设备)的无人作业相比于人工作业，具有提高采矿生产效率、降低采矿成本、改善采矿作业环境及保证现场工人的作业安全等优势。

然而，为了实现地下作业设备的自主行走、自主作业等控制，获取该设备的位姿(即位置和姿态)是关键。虽然，随着地面无人驾驶技术发展迅速，基于gps的支持并结合高精度地图技术的支撑，车辆的位姿获取已不成问题，但由于地下受限空间环境的特殊性，无法接收到gps信号对其定位，且设备的精确控制对姿态获取的高要求，设备位姿信息的实时、准确获取依然是难题，也是制约未来无人开采发展的关键技术。

基于wi-fi定位、蓝牙定位、rfid(射频识别)定位、uwb(超宽带)定位、红外技术、超声波等定位技术虽然在相关技术中有所应用，但以上方法大多需要在巷道中安装相关的定位装置，架设成本及维护费用较高；此外，基于安装在设备上的三维激光雷达进行点云定位亦有所应用，但相关方法由于点云数据量大或三维栅格多而导致匹配速度慢，很难满足控制的实时性要求。

技术实现要素：

有鉴于此，本申请实施例提供了一种地下作业设备定位方法、装置、系统及存储介质，旨在提高地下作业设备定位的定位精度及响应速度，以满足控制的实时性要求。

本申请实施例的技术方案是这样实现的：

第一方面，本申请实施例提供一种地下作业设备定位方法，包括：

获取所述作业设备当前对应的巷道腰线点云、巷道底板点云；

根据所述巷道腰线点云和地下二维概率定位地图进行第一点云匹配确定初始位姿及其概率，并基于所述初始位姿的概率确定用于第二点云匹配的位姿搜索范围；

根据所述巷道底板点云、所述地下二维概率定位地图及所述初始位姿，基于直方图滤波方法在所述位姿搜索范围内进行所述第二点云匹配，根据符合设定条件的配准位姿确定所述作业设备的当前位姿。

第二方面，本申请实施例提供一种地下作业设备定位装置，包括：

获取模块，用于获取所述作业设备当前对应的巷道腰线点云、巷道底板点云；

第一匹配确定模块，用于根据所述巷道腰线点云和地下二维概率定位地图进行第一点云匹配确定初始位姿及其概率，并基于所述初始位姿的概率确定用于第二点云匹配的位姿搜索范围；

第二匹配确定模块，用于根据所述巷道底板点云、所述地下二维概率定位地图及所述初始位姿，基于直方图滤波方法在所述位姿搜索范围内进行所述第二点云匹配，根据符合设定条件的配准位姿确定所述作业设备的当前位姿。

第三方面，本申请实施例提供一种地下作业设备定位系统，包括：

存储器，用于存储可执行程序；

处理器，用于执行所述存储器中存储的可执行程序时，实现前述实施例的地下作业设备定位方法。

第四方面，本申请实施例一种计算机存储介质，存储有可执行程序，所述可执行程序被处理器执行时，实现如前述实施例的地下作业设备定位方法。

本申请实施例的技术方案中，根据所述巷道腰线点云和地下二维概率定位地图进行第一点云匹配确定初始位姿及其概率，并基于所述初始位姿的概率确定用于第二点云匹配的位姿搜索范围；根据所述巷道底板点云、所述地下二维概率定位地图及所述初始位姿，基于直方图滤波方法在所述位姿搜索范围内进行所述第二点云匹配，根据符合设定条件的配准位姿确定所述作业设备的当前位姿，其能充分利用地下巷道的特点进行三维点云的高效匹配定位，通过利用巷道腰线点云和地下二维概率定位地图的第一点云匹配及巷道底板点云和地下二维概率定位地图的第二点云匹配，提高了匹配运算的效率，且充分利用了三维点云的信息，有效改善了地下作业设备定位的定位精度及响应速度，利于满足控制的实时性要求。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本申请一实施例中地下作业设备定位方法的流程示意图；

图2为本申请一实施例中地下作业设备定位方法的应用场景示意图；

图3为本申请一实施例中地下矿采矿装备实时采集点云数据示意图；

图4为本申请一实施例中地下二维概率定位地图的数据存储示意图；

图5为本申请一实施例中地下巷道腰线点云的示意图；

图6为本申请一实施例中地下巷道底板点云的示意图；

图7为本申请一实施例中地下作业设备定位装置的结构示意图；

图8为本申请一实施例中地下作业设备定位系统的结构示意图。

具体实施方式

以下结合说明书附图及具体实施例对本申请技术方案做进一步的详细阐述。应当理解，此处所提供的实施例仅仅用以解释本申请，并不用于限定本申请。另外，以下所提供的实施例是用于实施本申请的部分实施例，而非提供实施本申请的全部实施例，在不冲突的情况下，本申请实施例记载的技术方案可以任意组合的方式实施。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

本申请实施例提供一种地下作业设备定位方法，用于满足对地下作业设备的实时定位。该地下作业设备可以为用于采矿的地下采矿设备或者其他用于地下开采的挖掘设备。需要说明的是，本实施例地下作业设备上设置用于采集信息的传感器，如三维激光扫描设备、imu(inertialmeasurementunit，惯性测量单元)，该地下作业设备可以基于三维激光扫描设备和imu运用slam(simultaneouslocalizationandmapping，即时定位与地图构建)方法构建地下巷道内的三维点云地图。

请参阅图1，在本申请一实施例中，该地下作业设备为地下采矿设备，地下作业设备定位方法可以包括：

步骤101，获取所述作业设备当前对应的巷道腰线点云、巷道底板点云。

如图2所示，地下采矿设备可以在地下巷道内行走，地下采矿设备上安装imu和三维激光雷达。地下采矿设备在作业过程中，可以实时采集周边环境的三维点云信息(如图3所示)。示例性地，三维激光雷达采集生成三维点云信息，该三维点云信息包括各点对应的三维坐标和反射强度。三维激光雷达生成的三维点云信息，并基于三维点云信息提取地下采矿设备当前对应的巷道腰线点云、巷道底板点云。这里，巷道腰线点云包括三维激光雷达扫描的巷道两帮对应的点云，巷道底板点云包括三维激光雷达扫描的巷道底板对应的点云。可选地，可以把三维点云信息划分为以三维激光雷达中线扫描的二维的巷道腰线点云和运用随机抽样一致算法(ransac)提取的三维的巷道底板点云。

步骤102，根据所述巷道腰线点云和地下二维概率定位地图进行第一点云匹配确定初始位姿及其概率，并基于所述初始位姿的概率确定用于第二点云匹配的位姿搜索范围。

本实施例中，地下二维概率定位地图可以为预先存储的数据，即地下采矿设备预先已采集生成或者加载了该地下二维概率定位地图。

在一些实施例中，在执行步骤101之前，还包括：

基于即时定位与地图构建slam方法构建地下二维概率定位地图。

在一些实施例中，基于slam方法构建地下二维概率定位地图，包括：

获取所述作业设备上基于传感器和slam方法生成的三维点云信息；

基于所述三维点云信息提取对应的巷道腰线点云和巷道底板点云；

基于所述三维点云信息确定二维栅格，并将所述巷道腰线点云、巷道底板点云分别投影到所述二维栅格生成地下二维概率定位地图；其中，所述地图的各所述二维栅格存储巷道腰线点云的位置参数、巷道底板点云的位置参数和反射强度参数。

示例性地，地下采矿设备基于16线激光雷达和imu运用slam方法构建地下矿山三维点云地图，同时把三维点云划分为以激光雷达中线扫描的二维的巷道腰线点云和运用ransac提取的三维的巷道底板点云。再以此三维点云地图范围划分xy平面的二维栅格，如图4所示，把划分的巷道腰线点云和巷道底板点云分别投影到对应二维栅格，生成地下二维概率定位地图。其中每个二维栅格中分别存储巷道腰线点云的x,y坐标的均值、协方差矩阵，以及巷道底板点云的高度对应的均值、方差，反射强度对应的均值、方差。由于一个栅格内会有许多个点，该地下二维概率定位地图各栅格中存储巷道腰线点云的x,y坐标的均值、协方差矩阵，该协方差矩阵可以说明x，y的相关性。且各栅格存储巷道底板点云的高度、反射强度对应的均值、方差，利于后续直方图滤波。

在一些实施例中，所述根据所述巷道腰线点云和地下二维概率定位地图进行第一点云匹配确定初始位姿及其概率，并基于所述初始位姿的概率确定用于第二点云匹配的位姿搜索范围，包括：

基于正态分布变换(normaldistributionstransform，ndt)的迭代算法对所述巷道腰线点云和所述地下二维概率定位地图进行第一点云匹配的匹配计算；

获取所述匹配计算符合预设终止条件的输出结果，基于所述输出结果确定所述初始位姿及所述初始位姿的概率；

基于所述初始位姿的概率选取置信区间符合设定阈值的位姿作为所述位姿搜索范围。

请参阅图5，基于正态分布变换的迭代算法对巷道腰线点云进行第一点云匹配，可以基于第一点云匹配确定初始位姿。这里，匹配计算对应的预设终止条件可以为迭代次数的阈值或者迭代结果的阈值。若符合预设终止条件，则可以根据输出结果确定初始位姿及该初始位姿的概率。可选地，选取置信区间为95％的位姿作为巷道底板点云配准(即第二点云匹配)的位姿搜索范围。该位姿搜索范围可以表示为x,y,θ的搜索范围，其中，x、y表示地图中的横坐标、纵坐标，θ表示航向角。

在一些实施例中，可选地，地下作业设备定位方法还包括：

未获取到所述匹配计算的输出结果时，基于所述作业设备上的传感器对应的预测位姿确定位姿搜索范围。可以基于imu获取当前对应的预测位姿，imu可以从两帧数据中确定预测位姿并作为所述初始位姿，进而确定本实施例的位姿搜索范围。

步骤103，根据所述巷道底板点云、所述地下二维概率定位地图及所述初始位姿，基于直方图滤波方法在所述位姿搜索范围内进行所述第二点云匹配，根据符合设定条件的配准位姿确定所述作业设备的当前位姿。

这里，基于当前的三维点云信息经ransac提取巷道底板点云后，可以结合imu提供的预测位姿，将巷道底板点云投影到地下二维概率定位地图并计算各栅格内高度(即z坐标)对应的均值、方差，反射强度对应的均值、方差，以用于后续的直方图滤波匹配。

在一些实施例中，根据所述巷道底板点云、所述地下二维概率定位地图及所述初始位姿，基于直方图滤波方法在所述位姿搜索范围内进行所述第二点云匹配，根据符合设定条件的配准位姿确定所述作业设备的当前位姿，包括：

根据所述巷道底板点云按照所述初始位姿及所述地下二维概率定位地图，分别计算点云的高度、反射强度的直方图滤波，得到高度、反射强度分别对应的匹配概率，并基于所述高度、反射强度分别对应的匹配概率确定当前对应的概率；

对所述巷道底板点云遍历所述位姿搜索范围，并进行直方图滤波计算，得到所述位姿搜索范围内的概率分布，选取概率最大对应的点作为所述配准位姿；

根据所述匹准位姿确定所述作业设备的当前位姿。

请参阅图6，在一些实施例中，包括：

根据巷道底板点云、地下二维概率定位地图、及初始位姿，分别计算点云高度和强度的直方图滤波，得到高度、强度各自对应的匹配概率，并根据样品相似度(ssd)计算其高度和强度融合后的位姿概率；把点云高度和强度的融合后的位姿概率同imu预测的概率分布融合，得到此时刻当前位姿对应的概率；根据所述位姿搜索范围，遍历所有的位姿搜索范围，进行直方图滤波计算，得到所有遍历的位姿下的概率分布，该概率分布包含当前位姿对应的概率，以该概率分布中概率最大处的点作为点云配准位姿，最终通过坐标变换得到此时刻采矿装备的位姿。

本实施例地下作业设备定位方法，充分利用地下巷道的特点进行三维激光点云的高效匹配定位，通过两次点云匹配，既避免了二维激光点云匹配没有充分利用点云信息的缺陷，同时也避免了三维激光点云匹配计算量大的问题，为地下作业设备的定位提供了了一种有效实现方法，有效改善了地下作业设备定位的定位精度及响应速度，利于满足控制的实时性要求。

本申请实施例还提供一种地下作业设备定位装置，请参阅图7，该地下作业设备定位装置包括：

获取模块701，用于获取所述作业设备当前对应的巷道腰线点云、巷道底板点云；

第一匹配确定模块702，用于根据所述巷道腰线点云和地下二维概率定位地图进行第一点云匹配确定初始位姿及其概率，并基于所述初始位姿的概率确定用于第二点云匹配的位姿搜索范围；

第二匹配确定模块703，用于根据所述巷道底板点云、所述地下二维概率定位地图及所述初始位姿，基于直方图滤波方法在所述位姿搜索范围内进行所述第二点云匹配，根据符合设定条件的配准位姿确定所述作业设备的当前位姿。

在一些实施例中，地下作业设备定位装置还包括地图构建模块(图中未示出)，用于基于即时定位与地图构建slam方法构建地下二维概率定位地图。

该地图构建模块具体用于：

获取所述作业设备上基于传感器和slam方法生成的三维点云信息；

基于所述三维点云信息提取对应的巷道腰线点云和巷道底板点云；

基于所述三维点云信息确定二维栅格，并将所述巷道腰线点云、巷道底板点云分别投影到所述二维栅格生成地下二维概率定位地图；其中，所述地图的各所述二维栅格存储巷道腰线点云的位置参数、巷道底板点云的位置参数和反射强度参数。

在一些实施例中，第一匹配确定模块702具体用于：

基于正态分布变换(normaldistributionstransform，ndt)的迭代算法对所述巷道腰线点云和所述地下二维概率定位地图进行第一点云匹配的匹配计算；

获取所述匹配计算符合预设终止条件的输出结果，基于所述输出结果确定所述初始位姿及所述初始位姿的概率；

基于所述初始位姿的概率选取置信区间符合设定阈值的位姿作为所述位姿搜索范围。

在一些实施例中，第一匹配确定模块702还用于：

未获取到所述匹配计算的输出结果时，基于所述作业设备上的传感器对应的预测位姿确定位姿搜索范围。可以基于imu获取当前对应的预测位姿，imu可以从两帧数据中确定预测位姿并作为所述初始位姿，进而确定本实施例的位姿搜索范围。

在一些实施例中，第二匹配确定模块703具体用于：

根据所述巷道底板点云按照所述初始位姿及所述地下二维概率定位地图，分别计算点云的高度、反射强度的直方图滤波，得到高度、反射强度分别对应的匹配概率，并基于所述高度、反射强度分别对应的匹配概率确定当前对应的概率；

对所述巷道底板点云遍历所述位姿搜索范围，并进行直方图滤波计算，得到所述位姿搜索范围内的概率分布，选取概率最大对应的点作为所述配准位姿；

根据所述匹准位姿确定所述作业设备的当前位姿。

需要说明的是：上述实施例提供的地下作业设备定位装置在进行地下作业设备定位时，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述处理分配由不同的程序模块完成，即将地下作业设备定位装置的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分处理。另外，上述实施例提供的地下作业设备定位装置与地下作业设备定位方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。

在实际应用中，上述各程序模块均可由中央处理器(cpu，centralprocessingunit)、微处理器(mpu，microprocessorunit)、数字信号处理器(dsp，digitalsignalprocessor)、或现场可编程门阵列(fpga，fieldprogrammablegatearray)等实现。

请参阅图8，本申请实施例还提供一种地下作业设备定位系统800，该系统800包括：至少一个处理器801、存储器802和至少一个网络接口803。系统800中的各个组件通过总线系统804耦合在一起。可以理解，总线系统804用于实现这些组件之间的连接通信。总线系统804除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图8中将各种总线都标为总线系统804。

可以理解，存储器802可以是易失性存储器或非易失性存储器，也可包括易失性和非易失性存储器两者。

本申请实施例中的存储器802用于存储各种类型的数据以支持地下作业设备定位方法的执行。这些数据的示例包括：用于在系统800上运行的任何可执行程序，如可执行程序8021，实现本申请实施例的地下作业设备定位方法的程序可以包含在可执行程序8021中。

本申请实施例揭示的地下作业设备定位方法可以应用于处理器801中，或者由处理器801实现。处理器801可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，地下作业设备定位方法的各步骤可以通过处理器801中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器801可以是通用处理器、数字信号处理器(dsp，digitalsignalprocessor)，或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。处理器801可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者任何常规的处理器等。结合本申请实施例所公开的方法的步骤，可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于存储介质中，该存储介质位于存储器802，处理器801读取存储器802中的信息，结合其硬件完成本申请实施例提供的地下作业设备定位方法的步骤。

本申请实施例还提供了一种可读存储介质，存储介质可以包括：移动存储设备、随机存取存储器(ram，randomaccessmemory)、只读存储器(rom，read-onlymemory)、磁碟或者光盘等各种可以存储程序代码的介质。所述可读存储介质存储有可执行程序；所述可执行程序用于被处理器执行时实现本申请任一实施例所述的地下作业设备定位方法。

本领域内的技术人员应明白，本申请实施例可提供为方法、系统、或计算机程序产品。因此，本申请实施例可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本申请实施例是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理系统的处理器以产生一个机器，使得通过计算机或其他可编程数据处理系统的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理系统以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理系统上，使得在计算机或其他可编程系统上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程系统上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。