无人车井下定位方法、系统、设备及介质

技术领域

本发明属于无人驾驶技术领域，特别涉及一种无人车井下定位方法、系统、设备及介质。

背景技术

目前，在复杂场景的无人驾驶车辆大多采用实时动态差分法(RTK)组合导航、超宽带技术(UWB)以及使用激光雷达配合高精地图进行定位；但针对井下复杂环境，存在RTK信号遮挡，其定位结果仅依靠轮速计和IMU的积分推算，易造成较大的累计飘移；UWB技术定位精度较低，且UWB系统占用带宽较高，极易干扰井下其他无线通信系统；同时，基于高精地图的匹配定位需要给定初始位置和姿态进行初始化，但实际中无人驾驶车辆可能处于高精地图的任意位置开始形式，在定位起始时刻无法提供点云和地图配准的初始值，定位难度较大，定位结果精度较低。

发明内容

针对现有技术中存在的技术问题，本发明提供了一种无人车井下定位方法、系统、设备及介质，以解决现有井下无人驾驶车辆进行定位时，定位误差较大，安全性低，且极易干扰井下其他无线通讯系统的技术问题。

为达到上述目的，本发明采用的技术方案为：

本发明提供了一种无人车井下定位方法，方法包括：

构建井下隧道的环境地图；

获取第一环境点云数据；其中，所述第一环境点云数据通过路侧激光雷达当前时刻采集的环境点云；所述路侧激光雷达间隔设置在井下隧道内；

根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；

根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息；

将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上；

获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果。

进一步的，构建井下隧道的环境地图的过程，具体为：利用即时定位与三维建图模块，构造所述井下隧道的环境地图。

进一步的，所述井下隧道的环境地图为井下隧道的环境三维点云信息，包括所述井下隧道的环境地图的坐标原点在世界坐标系中的位置信息和姿态信息；其中，所述姿态信息包括翻滚角、俯仰角及横滚角。

进一步的，相邻所述路侧激光雷达的间隔距离小于等于200m。

进一步的，所述预设的目标检测算法为Pointpillars目标检测算法。

进一步的，将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息之前，还包括NDT初始化步骤；

其中，所述NDT初始化步骤，具体如下：

输入所述井下隧道的环境地图为原始点云，第一环境点云数据为目标点云，进行NDT求解，若NDT求解收敛，则当前时刻的第一环境点云数据于所述井下隧道的环境地图配准成功，即完成NDT初始化；否则，重新输入当前车辆相对于路侧激光雷达的位置及姿态信息，进行NDT求解直至收敛成功，完成NDT初始化。

进一步的，获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果的过程，具体如下：

对比所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差值，若所述量化差异大于设定阈值时，输出所述当前车辆的第一定位信息，作为所述无人车井下定位结果；否则，输出所述当前车辆的第二定位信息，作为所述无人车井下定位结果。

本发明还提供了一种无人车井下定位系统，包括：

地图模块，用于构建井下隧道的环境地图；

点云数据模块，用于获取第一环境点云数据；其中，所述第一环境点云数据通过路侧激光雷达当前时刻采集的环境点云；所述路侧激光雷达间隔设置在井下隧道内；

目标识别模块，用于根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；

第一定位信息模块，用于根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息；

第二定位信息模块，用于将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上；

结果输出模块，用于获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果。

本发明还提供了一种无人车井下定位设备，包括：

存储器，用于存储计算机程序；

处理器，用于执行所述计算机程序时实现如权利要求1-7任一项所述一种无人车井下定位方法的步骤。

本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述一种无人车井下定位方法的步骤。

与现有技术相比，本发明的有益效果为：

本发明提供了一种无人车井下定位方法及系统，利用路侧激光雷达采集的第一环境点云数据，对当前车辆进行识别和定位，实现对车辆的位置信息的实时监控，提高了定位的精度和为地面监控提供了实时数据；结合第一定位信息和车侧单元采集的第二定位信息，通过NDT配准，获得第二定位信息，实现了井下车辆的双重定位，定位精度较高，安全性较低；同时，能够避免对井下其他无线通讯系统的干扰。

进一步的，结合路侧激光雷达的测距范围，将相邻所述路侧激光雷达的间隔距离设置为小于等于200m，确保了路侧激光雷达能够精确获取对应的点云特征信息，提高了车辆位置定位的精确性。

进一步的，采用Pointpillars目标检测算法对车辆进行车辆目标识别，算法过程简单，结果精确度较高。

进一步的，通过NDT初始化操作，确保了匹配在该初始的位置和姿态的基础上进行迭代，能够获得更加精确的位置和姿态。

附图说明

图1为实施例所述的无人车井下定位方法的流程图。

具体实施方式

为了使本发明所解决的技术问题，技术方案及有益效果更加清楚明白，以下具体实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如附图1所示，本发明提供了一种无人车井下定位方法，包括以下步骤：

步骤1、通过加载即时定位与三维建图模块，构造环境地图，得到井下隧道的环境地图；其中，所述井下隧道的环境地图为井下隧道的环境三维点云信息，包括所述井下隧道的环境地图的坐标原点在世界坐标系中的位置信息和姿态信息；其中，所述姿态信息包括翻滚角、俯仰角及横滚角。

步骤2、利用路侧激光雷达，获取第一环境点云数据；其中，所述路侧激光雷达间隔设置在井下隧道内，相邻所述路侧激光雷达的间隔距离小于等于200m；所述第一环境点云数据通过第一路侧激光雷达当前时刻采集的环境点云。

步骤3、根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对路侧激光雷达的位置及姿态信息；其中，所述预设的目标检测算法，采用Pointpillars目标检测算法。

步骤4、输入所述井下隧道的环境地图为原始点云，所述第一环境点云数据为目标点云，进行NDT求解，若NDT求解收敛，则当前时刻的第一环境点云数据于所述井下隧道的环境地图配准成功，即完成NDT初始化；否则，重新输入当前车辆相对于路侧激光雷达的位置及姿态信息，进行NDT求解直至收敛成功，完成NDT初始化。

步骤5、将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上。

步骤7、获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果；具体过程，包括以下步骤：

对比所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差值，若所述量化差异大于设定阈值时，输出所述当前车辆的第一定位信息，作为所述无人车井下定位结果；否则，输出所述当前车辆的第二定位信息，作为所述无人车井下定位结果。

本发明中，所述NDT：Normal Distribution Transformation，正态分布变换，一种利用激光点云的空间分布来进行求解两片点云空间变换的点云配准算法；NDT求解需要三个输入：原始点云，目标点云，点云变换的预测值，这个预测值主要作为迭代优化的初值，优化收敛后输出较为精确的点云空间变换。

本发明还提供了一种无人车井下定位系统，包括地图模块、点云数据模块、目标识别模块、第一定位信息模块、第二定位信息模块及结果输出模块；地图模块，用于构建井下隧道的环境地图；点云数据模块，用于获取第一环境点云数据；其中，所述第一环境点云数据通过路侧激光雷达当前时刻采集的环境点云；所述路侧激光雷达间隔设置在井下隧道内；目标识别模块，用于根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；第一定位信息模块，用于根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息；第二定位信息模块，用于将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上；结果输出模块，用于获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果。

本发明还提供了一种无人车井下定位设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现无人车井下定位方法的步骤。

所述处理器执行所述计算机程序时实现上述无人车井下定位方法的步骤，例如：构建井下隧道的环境地图；获取第一环境点云数据；其中，所述第一环境点云数据通过路侧激光雷达当前时刻采集的环境点云；所述路侧激光雷达间隔设置在井下隧道内；根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息；将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上；获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果。

或者，所述处理器执行所述计算机程序时实现上述系统中各模块的功能，例如：地图模块，用于构建井下隧道的环境地图；点云数据模块，用于获取第一环境点云数据；其中，所述第一环境点云数据通过路侧激光雷达当前时刻采集的环境点云；所述路侧激光雷达间隔设置在井下隧道内；目标识别模块，用于根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；第一定位信息模块，用于根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息；第二定位信息模块，用于将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上；结果输出模块，用于获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果。

示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成预设功能的一系列计算机程序指令段，所述指令段用于描述所述计算机程序在所述无人车井下定位设备中的执行过程。例如，所述计算机程序可以被分割成地图模块、点云数据模块、目标识别模块、第一定位信息模块、第二定位信息模块及结果输出模块；各模块具体功能如下：地图模块，用于构建井下隧道的环境地图；点云数据模块，用于获取第一环境点云数据；其中，所述第一环境点云数据通过路侧激光雷达当前时刻采集的环境点云；所述路侧激光雷达间隔设置在井下隧道内；目标识别模块，用于根据所述第一环境点云数据，利用预设的目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；第一定位信息模块，用于根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息；第二定位信息模块，用于将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上；结果输出模块，用于获取所述当前车辆的第一定位信息与所述当前车辆的第二定位信息的量化差异，根据所述量化差异，输出所述当前车辆的第一定位信息或所述当前车辆的第二定位信息，得到所述无人车井下定位结果。

所述无人车井下定位设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述无人车井下定位设备可包括，但不仅限于处理器、存储器。本领域技术人员可以理解，上述仅仅是无人车井下定位设备的示例，并不构成对无人车井下定位设备的限定，可以包括比上述更多的部件，或者组合某些部件，或者不同的部件，例如所述无人车井下定位设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器可以是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器是所述无人车井下定位设备的控制中心，利用各种接口和线路连接整个无人车井下定位设备的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述无人车井下定位设备的各种功能。

所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(SmartMediaCard,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(FlashCard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种无人车井下定位方法的步骤。

所述无人车井下定位系统集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。

基于这样的理解，本发明实现上述无人车井下定位方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，所述计算机程序在被处理器执行时，可实现上述无人车井下定位方法的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或预设中间形式等。

所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-OnlyMemory，ROM)、随机存取存储器(RandomAccessMemory，RAM)、电载波信号、电信信号以及软件分发介质等。

需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。

实施例

如附图1所示，本实施例提供了一种无人车井下定位方法，包括以下步骤：

步骤1、加载即时定位与建图模块构造的环境地图。

所述环境地图用于与实时激光点云进行NDT匹配定位；所述环境地图为井下隧道的环境三维点云信息，包括所述环境地图坐标系原点在世界坐标系中的位置信息和姿态信息；其中，所述位姿信息包括翻滚角、俯仰角及横滚角；并检查当前车辆的传感器接口的工作状态并获取各传感器数据以及车辆姿态信息等。

步骤2、在井下隧道内，每间隔200m范围以内的预设位置处，安装路侧激光雷达；其中，所述路侧激光雷达的测距范围为200m；所述路侧激光雷达，用于获取第一环境点云数据；和车载激光雷达获取的所述第二环境点云数据，双重探测车辆的位置进行更精确的定位。

步骤3、根据所述第一环境点云数据，利用pointpillars目标检测算法，进行目标识别，得到当前车辆相对于路侧激光雷达的位置及姿态信息；其中，所述pointpillars目标检测算法，以所述第一环境点云数据为输入，通过目标识别，即可得到所述当前车辆相对于路侧激光雷达的位置及姿态信息。

步骤4、根据根据当前车辆相对于路侧激光雷达的位置及姿态信息，以及路侧激光雷达在所述井下隧道的环境地图，对当前车辆进行位姿变换，得到当前车辆的第一定位信息。

步骤5、输入所述井下隧道的环境地图为原始点云，所述第一环境点云数据为目标点云，进行NDT求解，若NDT求解收敛，则当前时刻的第一环境点云数据于所述井下隧道的环境地图配准成功，即完成NDT初始化；否则，重新输入当前车辆相对于路侧激光雷达的位置及姿态信息，进行NDT求解直至收敛成功，完成NDT初始化。

步骤6、NDT匹配定位初始化成功后，将所述井下隧道的环境地图作为原始点云，所述第二环境点云数据作为目标点云，所述当前车辆的第一定位信息作为NDT预测值，进行NDT求解，输出得到当前车辆的第二定位信息；其中，所述第二环境点云数据为车侧激光雷达当前时刻采集的环境点云，所述车侧激光雷达安装在当前车辆上。

步骤7、将所述第二定位信息与第一定位信息进行量化差异比较，由于在较短时间内的定位较为准确，若所述量化差异大于设定阈值可以认为配准退化，将第一定位信息输出作为最终定位信息；若所述量化差异小于设定阈值，可以认为激光点云配准准确，点云配准由于将每个点都和地图中的点配准对齐，具有很高的精度，对作为初始值的第一定位信息起到了修正作用，因此将第二定位信息输出作为最终定位信息。

本实施例提供的无人车井下定位系统、设备及计算机可读存储介质中相关部分的说明可以参见本实施例所述的一种无人车井下定位方法中对应部分的详细说明，在此不再赘述。

本发明所述的无人车井下定位方法及系统，利用路测激光雷达采集的第一环境点云数据，对井下的车辆进行识别和定位，时刻监控车辆的位置信息，提高了定位的精度,为地面监控提供实时数据，为井下行驶人员提供安全保障；相比于UWB基站来说，具有成本低，工作量小等优点；利用第一定位信息和第二定位信息的量化差异输出最终的定位结果，实现了井下车辆的双重定位，如果NDT有定位匹配卡顿或者断掉的情况下，路侧激光雷达也能获取车辆的位置，防止事故的发生。

本发明中，将第一定位信息发送给NDT，让其获得到初始的位置和姿态，完成NDT定位模型的初始化，之后的匹配在这个初始的位置和姿态的基础上进行迭代，以获得更加精确的位置和姿态。

上述实施例仅仅是能够实现本发明技术方案的实施方式之一，本发明所要求保护的范围并不仅仅受本实施例的限制，还包括在本发明所公开的技术范围内，任何熟悉本技术领域的技术人员所容易想到的变化、替换及其他实施方式。