CN116352722A - 多传感器融合的矿山巡检救援机器人及其控制方法 - Google Patents

Abstract

本申请公开了一种多传感器融合的矿山巡检救援机器人及其控制方法，该机器人包括：双控制器模块分别与运动载体模块、多源传感探测模块和交互模块相连；运动载体模块采用轮式机器人，每个旋轮均为麦克纳姆轮；多源传感探测模块，包括激光雷达、高清相机、深度相机、轮式里程计、IMU和UWB探测仪；双控制器模块，包括ROS主控制器以及底层运动控制器，底层运动控制器用于传输控制指令和多源传感信息，主控制器用于基于多源传感信息融合技术实现矿山巡检救援所需的多种运算功能；人机交互模块通过无线通信技术与PC端控制系统相连。该巡检救援两用机器人基于多源传感信息融合技术提高了井下探测的精确度和适用性，具备自主探测能力。

Description

多传感器融合的矿山巡检救援机器人及其控制方法

技术领域

本申请涉及机器人探测技术领域，尤其涉及一种多传感器融合的矿山巡检救援机器人及其控制方法。

背景技术

目前，自主探测机器人在各个领域中的普及率逐渐提高，自主探测机器人最主要的任务是代替人类对环境进行自主探测，收集环境信息，从而为后期工作提供依据。其中，巡检机器人和救援机器人是自主探测机器人领域研究的热点，是解决安全生产和应急救援问题的重要技术保障。

相关技术中，巡检救援机器人的探测方法通常可以分为有源探测和无源探测。比如，在无源探测技术，仅从摄像机采集的二维图像中就可以提取大量信息；而通过激光雷达的采集的激光数据属于有源探测技术。

然而，相关技术中的井下巡检救援机器人存在以下缺陷：第一，大多数机器人采用固定式巡检轨道，只能沿着预定的轨道行走，不具备自主导航能力。第二，部分具备一定程度的自主导航能力的机器人，其环境感知传感信息来源单一，存在较大的导航盲区。第三，由于煤矿井下环境恶劣，受到光照、天气、粉尘、震动及非结构化环境等多方面影响，容易造成基于单一传感信息的机器人探测失效。第四，由于井下工作区域可能存在行人，目前的巡检机器人未考虑井下人机共融环境下的行人干扰问题，由于行人的运动具有较大的随机性和不确定性，巡检机器人不能及时对行人的运行进行提前预判，无法适用于动态避障场景，从而难以及时躲避行人，存在较大的安全隐患。

发明内容

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

为此，本申请的第一个目的在于提出一种多传感器融合的矿山巡检救援机器人。该巡检救援两用机器人基于多源传感信息融合技术提高了井下探测的精确度和适用性，具备自主探测能力。并且可以规避动态生命体，优化了机器人同步定位与建图的能力。

本申请的第二个目的在于提出一种多传感器融合的矿山巡检救援机器人的控制方法。

本申请的第三个目的在于提出一种非临时性计算机可读存储介质。

为达上述目的，本申请第一方面提出了一种多传感器融合的矿山巡检救援机器人，该机器人包括：运动载体模块、双控制器模块、多源传感探测模块和人机交互模块，其中，

所述双控制器模块分别与所述运动载体模块、所述多源传感探测模块和所述人机交互模块相连；

所述运动载体模块采用轮式机器人，每个旋轮均为麦克纳姆轮，所述运动载体模块用于实现矿山巡检救援所需的多种运动功能，所述多种运动功能包括多方向移动和转向；

所述多源传感探测模块，包括激光雷达、高清相机、深度相机、轮式里程计、惯性传感器IMU和超宽带UWB探测仪，所述多源传感探测模块用于获取实现环境感知的多源传感信息；

所述双控制器模块，包括基于机器人操作系统ROS的主控制器以及底层运动控制器，所述底层运动控制器用于传输所述主控制器的控制指令和机器人底层的所述多源传感信息，所述主控制器用于基于多源传感信息融合技术实现同步定位与建图SLAM、SLAM优化、地图转换、探测策略生成以及运动路径规划与实时避障，并根据运算结果生成所述控制指令；

所述人机交互模块通过无线通信技术与PC端控制系统相连，所述人机交互模块用于实现巡检救援机器人与用户的之间的人机交互。

另外，本申请实施例的多传感器融合的矿山巡检救援机器人还具有如下附加的技术特征：

可选地，在一些实施例中，主控制器与所述底层运动控制器之间通过UART3串口协议实现通信，所述底层运动控制器向所述主控制器发送的数据，包括：机器人使能标志位、机器人三轴速度、IMU三轴加速度和三轴角速度；所述底层运动控制器采用中断接收的方式接收所述主控制器发送的数据，接收的数据包括：机器人标识信号、使能控制标志位、机器人三轴目标速度和数据校验位。

可选地，在一些实施例中，主控制器通过Mirco-USB与所述激光雷达相连，以向所述激光雷达供电并进行数据交互，所述主控制器还通过Mirco-USB与所述UWB探测仪相连；所述主控制器通过MIPI-CSI协议分别与所述高清相机和所述深度相机相连，所述轮式里程计设置在所述运动载体模块上；所述惯性传感器IMU集成在所述底层运动控制器中，所述底层运动控制器将采集到的IMU数据发送至所述主控制器。

为实现上述目的，本发明第二方面提出了一种多传感器融合的矿山巡检救援机器人的控制方法，该多传感器融合的矿山巡检救援机器人是上述第一方面的多传感器融合的矿山巡检救援机器人，该控制方法包括：

对多源传感探测模块搭载的多个传感器进行标定，同步所述多个传感器的时间戳，并实时读取多源传感探测模块检测到的多源传感数据；

基于所述多源传感数据，通过预设的同步定位与建图SLAM算法计算机器人的实时位姿，并构建当前探测环境的三维稠密点云地图；

通过卷积神经网络和卡尔曼滤波算法对UWB探测仪采集的实时探测数据进行运算，获取生命体运动轨迹信息，并将所述生命体运动轨迹信息输入所述SLAM算法中，优化机器人的实时位姿和三维稠密点云地图；

对优化后的三维稠密点云地图进行滤波处理，将滤波处理后三维稠密点云地图的转化为八叉树地图，并通过2D投影生成点云地图对应的占据网格地图；

基于全局信息和人机交互信息，通过快速随机扩展树RRT算法生成所述矿山巡检救援机器人的探测策略，所述探测策略中包括多个目标探测点；

根据优化后的机器人的实时位姿、所述八叉树地图、所述占据网格地图和所述探测策略，生成机器人的全局路径规划和局部路径规划，并根据路径规划生成运动控制指令，控制所述矿山巡检救援机器人执行所述运动控制指令。

另外，本申请实施例的多传感器融合的矿山巡检救援机器人的控制方法还具有如下附加的技术特征：

可选地，在一些实施例中，对多源传感探测模块搭载的多个传感器进行标定，包括：通过预设的标定工具包和棋盘标定板，对激光雷达、高清相机和深度相机进行标定；确定由相机捕获的二维平面数据与激光雷达捕获的三维点云数据之间的线性方程；切换不同姿态下的棋盘标定板，获得线性方程组，并求解所述线性方程组获得标定参数，所述标定参数包括相机内参，以及相机与所述激光雷达之间的外参。

可选地，在一些实施例中，基于所述多源传感数据，通过预设的同步定位与建图SLAM算法计算机器人的实时位姿，包括：对所述多源传感数据中的视觉图像和激光点云进行特征提取，获取融合了所述视觉图像和所述激光点云的特征点，并通过稀疏光流跟踪KLT算法对所述特征点进行特征跟踪；基于视觉里程计和激光里程计的特征点解算相机的相对位姿；对IMU数据进行预积分，获得多个IMU预积分信息；将所述IMU数据与所述相对位姿进行空间对齐，以所述IMU预积分信息为运动输入构造代价函数，并采用线性最小二乘法估计机器人初始状态的参数，优化求解得到机器人的初始位姿；根据所述IMU预积分信息和位姿解算信息构建非线性最小二乘方程，并利用预设的迭代工具，通过Levenberg-Marquardt方法进行迭代搜索，求解当前约束下的最小二乘解，以获得机器人的实时位姿。

可选地，在一些实施例中，通过卷积神经网络和卡尔曼滤波算法对UWB探测仪采集的实时探测数据进行运算，包括：将所述实时探测数据输入预先训练完成的卷积神经网络，获得生命体征识别结果；通过卡尔曼滤波算法对所述生命体征识别结果进行处理，以在所述生命体征识别结果中提取出动态生命体的所述生命体运动轨迹信息。

可选地，在一些实施例中，将所述目标点云地图转化为八叉树地图，包括：对所述目标点云地图进行体素滤波；通过八叉树对滤波后的目标点云地图进行稀疏表达得到初始八叉树地图，包括：将点云地图中的每个点云对应一个节点，其中，通过节点被占据的概率表达每个所述节点；通过欧式距离分析每个点云数据对应的预设数量个近邻距离矩阵和索引，剔除所述初始八叉树地图中的离散点数据，获得目标八叉树地图；根据高度对所述目标八叉树地图进行染色。

可选地，在一些实施例中，通过快速随机扩展树RRT算法生成所述矿山巡检救援机器人的探测策略，包括：通过激光雷达采集的数据构建一个二维栅格地图；通过局部RRT探测法检测机器人周围的边界点，并采用全局RRT探测法探测所述二维栅格地图的远端边界点；对全部的边界点进行过滤和聚类，并通过机器人在自主探索过程中的信息增益、导航代价和自身定位的精确度构建边界点评价函数；根据所述边界点评价函数对每个边界点的得分选择目标边界点。

可选地，在一些实施例中，生成机器人的全局路径规划和局部路径规划，包括：根据所述八叉树地图和所述占据网格地图判断每个所述目标探测点是否能够达到；在能够到达的情况下，读取二维占据网格语义地图，根据所述优化后的机器人的实时位姿确定机器人起始点；基于关键点进行全局路径规划，并基于机器人的实时避障功能生成机器人的实时运动轨迹曲线。

为实现上述目的，本发明第三方面实施例提出了一种非临时性计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上述第二方面中任一所述的多传感器融合的矿山巡检救援机器人的控制方法。

本申请的实施例提供的技术方案至少带来以下有益效果：

本申请综合搭载激光雷达、高清摄像头、深度相机、轮式里程计和IMU等多种传感器，通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，实现了一套具有高鲁棒性、高探测精度、强适应性和具备自主探测能力的多传感器融合的矿山巡检救援两用机器人探测系统。该矿山巡检救援两用机器人具有自主探测、定位、三维环境建模、路径规划和实时避障功能，可以在复杂的井下环境下进行精准的探测且扩大了探测区域。并且，本申请还通过UWB探测仪实时探测获取生命体运动信息，并将生命体运动信息输入到SLAM算法中，对激光雷达点云数据、摄像头图像和深度相机采集到的深度信息进行优化，去除掉动态生命体数据的影响，提高机器人定位精度和三维稠密点云地图的准确度，实现同步定位与建图SLAM系统的优化，可以及时规避行人等移动对象，提高了矿山巡检救援的安全性。

本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。

附图说明

本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为本申请实施例提出的一种多传感器融合的矿山巡检救援机器人的结构示意图；

图2为本申请实施例提出的一种多传感器融合的矿山巡检救援机器人的硬件架构示意图；

图3为本申请实施例提出的一种多传感器融合的矿山巡检救援机器人的控制方法的流程图；

图4为本申请实施例提出的一种机器人实时位姿数据的计算方法的流程图；

图5为本申请实施例提出的一种基于多源传感数据融合的位姿估计算法的原理示意图；

图6为本申请实施例提出的一种卷积神经网络的结构示意图；

图7为本申请实施例提出的一种生命体征识别结果的示意图；

图8为本申请实施例提出的另一种生命体征识别结果的示意图；

图9为本申请实施例提出的一种动态目标的运动轨迹估计结果的示意图；

图10为本申请实施例提出的另一种动态目标的运动轨迹估计结果的示意图；

图11为本申请实施例提出的一种地图转换方法的流程图；

图12为本申请实施例提出的一种目标探测地图的示意图；

图13为本申请实施例提出的一种具体的基于多传感器融合的矿山巡检救援两用机器人的探测方法的流程图；

图14为本申请实施例提出的一种矿山巡检救援两用机器人实现探测的原理示意图。

具体实施方式

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。

需要说明的是，在煤矿井下，尤其是灾害发生后其环境往往是复杂的非结构化有限空间；灾后光照度极低，光线的变化更使得光照分布不均匀；潮湿、多粉尘环境、路面不平颠簸、通讯中断等状况对传感信息的获取造成了诸多阻碍。因此，难以找到一种单一有效的探测方法。

在本申请实施例中经过分析发现，目前井下巡检机器人主要采用固定式巡检轨道，只需要沿着预定的轨道行走，需要预先在采煤工作面铺设大量的轨道，巡检机器人只能以固定的方式沿着预定轨道行走，没有真正具备同步定位与建图能力，不能实现在井下复杂恶劣环境的自主导航。部分已有井下巡检机器人采用足式或者轮式的无轨结构，虽然具备一定程度的自主导航能力，但是其环境感知传感器信息来源单一，通常采用二维激光雷达和惯导模块进行组合导航。由于二维激光雷达是单线激光扫描，只能监测到单个平面内的障碍物情况，其视野范围较为，存在较大的导航盲区。在巡检机器人工作过程中，对于恶劣复杂环境的建图能力较弱，难以及时躲避障碍物。此外，在巡检机器人工作过程中，存在着诸多的静态和动态障碍物，巡检机器人需要具备实时精确避障能力。在煤矿井下的固定式障碍物主要有通风设备、排水设备、电气设备和监控设备等，相关技术中的经典路径规划算法仅适用于静态障碍物，现有的井下无轨巡检机器人导航没有考虑井下人机共融环境下的行人干扰问题，由于行人的运动具有较大的随机性和不确定性，传统巡检机器人不能及时对行人的运行进行提前预判，难以及时躲避行人，存在较大的安全隐患。

并且，目前井下巡检机器人主要采用单线激光雷达进行同步定位与建图。单线激光雷达的工作原理为飞行时间测距(Time of flight，简称TOF)，其基本原理为激光雷达在某时刻发射脉冲，经过一定时间后，在相应时刻采集由障碍物反射回来的脉冲信号，进而推算出障碍物的距离。当电机带动激光发射与接收模块进行360°转动时，返回障碍物的距离和对应的采集方向的夹角。基于激光雷达的即时定位与地图构建(SimultaneousLocalization and Mapping，简称SLAM)主要使用里程计和激光雷达实现机器人的定位，同时激光雷达还需要探测外部环境并进行地图构建。而由于机器人定位极大程度收到传感器误差的影响，定位误差会随着时间推移不断积累，这将影响地图构建的精度，使得构建的环境地图与真实环境地图的匹配程度较低。因此，传统的激光雷达SLAM存在较大的误差，需要对其进行算法和硬件优化改进。

为此，本申请先运用多传感器融合SLAM技术，再利用深度学习对SLAM技术进行优化，使得巡检救援机器人的性能得到大幅度的提高。具体的，本申请提出了一种多传感器融合的矿山巡检救援两用机器人探测系统，该系统包括硬件和软件两部分。

其中，在硬件方面，矿山巡检救援机器人载体选用轮式机器人，前后四轮均为麦克纳姆轮，可实现前后移动、左右横向移动、转向等运动功能。搭载IMU、激光雷达、RGBD相机、UWB生命探测仪和轮式里程计对环境信息进行探测，同时构建了一套基于ROS主控+STM32的双控制器多源传感系统。在软件方面，构建一套具备传感数据获取、同步定位与建图(SLAM)、多传感器融合定位优化、地图转换、探测策略生成、运动路径规划与实时避障功能模块的矿山巡检救援机器人多源传感SLAM算法框架。

本申请通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，来解决煤矿井下复杂的非结构化有限空间、光照度极低、光照分布不均匀、潮湿、多粉尘环境、路面不平颠簸、通讯中断等问题。再结合深度学习方法进行智能化特征提取、特征匹配、位姿估计、几何语义分析等，提高救援机器人SLAM技术的准确性、高效性、鲁棒性及稳定性，有利于煤矿井下巡检及应急救援工作的顺利开展。

下面参考附图详细描述本申请实施例的多传感器融合的矿山巡检救援机器人及其控制方法。

图1为本申请实施例提出的一种多传感器融合的矿山巡检救援机器人的结构示意图，如图1所示，该巡检救援机器人包括：运动载体模块10、双控制器模块20、多源传感探测模块30和人机交互模块40。

其中，双控制器模块20分别与运动载体模块10、多源传感探测模块30和人机交互模块40相连。

运动载体模块10采用轮式机器人，每个旋轮均为麦克纳姆轮，运动载体模块10用于实现矿山巡检救援所需的多种运动功能，多种运动功能包括多方向移动和转向。

具体的，本申请矿山巡检救援机器人载体选用轮式机器人，前后四轮均为麦克纳姆轮，可实现前后移动、左右横向移动和转向等运动功能。在申请一个实施例中，轮式机器人的左旋轮和右旋轮呈手性对称，每个旋轮由轮毂和固定在外周的多个辊子构成，轮轴和辊轴之间的夹角可以设置为45°

多源传感探测模块30，包括激光雷达、高清相机、深度相机、轮式里程计、惯性传感器(Inertial Measurement Unit，简称IMU)和超宽带(Ultra Wide Band，简称UWB)探测仪，多源传感探测模块用于获取实现环境感知的多源传感信息。

具体的，本申请的机器人搭载了激光雷达、高清摄像头、深度相机、轮式里程计、IMU、UWB探测仪等多种传感器，其中，惯性测量单元，可以用来检测机器人的加速度与旋转运动。本申请通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正

双控制器模块20，包括基于机器人操作系统(Robot Operating System，简称ROS)的主控制器以及底层运动控制器，底层运动控制器用于传输主控制器的控制指令和机器人底层的多源传感信息，主控制器用于基于多源传感信息融合技术实现同步定位与建图SLAM、SLAM优化、地图转换、探测策略生成以及运动路径规划与实时避障，并根据运算结果生成控制指令。

具体的，本申请构建了一套基于ROS主控+运动控制的双控制器系统。其中，底层机器人控制系统，接收来自于主控系统的命令和机器人底层传感数据，并将底层传感数据传输至主控制器进行运算，以及将控制指令发送给带有编码器的轮式机器人电机接口。主控制器基于多源传感信息融合技术实现矿山巡检救援所需的多种运算功能，包括同步定位与建图SLAM等。

人机交互模块40通过无线通信技术与PC端控制系统相连，人机交互模块用于实现巡检救援机器人与用户的之间的人机交互。

具体的，人机交互模块40可以通过蓝牙或WiFi等各种无线通信技术，与上位控制系统，比如，地面工作站中的PC端控制系统相连，用于实现本申请的巡检救援机器人与相关工作人员的之间的人机交互，比如，向工作人员展示信息，以及接收工作人员的指令等。人机交互模块40采用的无线通信技术可以根据现场的实际应用状况进行确定，此处不做限制。

在本申请一个实施例中，在实际应用中，可以采用如图2所示的方式具体构建多传感器融合的矿山巡检救援机器人。

具体而言，在本实施例中，为了实现轮式机器人底盘的稳定控制，对于底层运动控制器，构建STM32机器人控制系统。本实施例根据矿山巡检救援机器人对多源传感SLAM系统的功能需求，对控制系统的硬件方面提出了以下几方面关键指标：第一，具有较高的采样频率，以降低信息感知的延时；第二，具备较强的运算能力，支持实时的数据处理、定位、路径规划与三维建图功能；第三，具有较高的可靠性，以防止系统故障对救援工作造成的负面影响。综上，本申请实施例构建一套基于ROS主控+STM32底层运动控制的双控制器多源传感系统。

作为一种可能的实现方式，主控制器选用NVIDIAJETSON TX2工业级嵌入式芯片，NVIDIA JETSON TX2搭载有NVIDIA Pascal架构GPU，2个64位CPU以及四核A57复合处理器，配有Linux Ubuntu操作系统以及多种通信接口，可将处理信息实时发送。底层运动控制器选用STM32F103VET6 32位高性能微控制器。

继续参照图2，在本实施例中，主控制器与底层运动控制器之间通过UART3串口协议实现通信，波特率设置为115200。STM32底层运动控制器向ROS主控制器发送的数据，包括：帧头帧尾、机器人使能标志位、机器人XYZ三轴速度、IMU三轴加速度、三轴角速度、电池电压和数据校验位等。STM32底层运动控制器从ROS主控制器接收数据时，采用中断接收的方式，接收的数据包括：机器人标识信号、使能控制标志位、机器人三轴目标速度和数据校验位。ROS主控作为上位机和STM32控制器进行通信，默认选择集成了CP2102电平转换芯片的串口进行数据交互。

继续参照图2，在本实施例中，主控制器通过Mirco-USB与激光雷达相连，以向激光雷达供电并进行数据交互，主控制器还通过Mirco-USB与UWB探测仪相连。主控制器通过MIPI-CSI协议分别与高清相机和深度相机相连。轮式里程计设置在运动载体模块上。在ROS的导航系统中需要用到IMU传感器，在本申请实施例的机器人系统中，IMU传感器集成到STM32底层运动控制器上，由STM32底层运动控制器采集IMU数据后，再发送给ROS主控制器。

在本实施例中，该机器人底盘的运动控制通过改变机器人的目标速度实现的，目标速度经过运动学分析函数得出运动载体模块10中每个电机的实际输出，最后通过PID控制器中的PID速度控制函数来实现电机的速度控制。

在本申请一个实施例中，主控制器还通过相关串口读取机器人中预先安装的三轴加速度计、三轴陀螺仪、温度传感器和流量传感器的数据，并根据读取到的上述数据判断当前机器人是否处于正常工作状态。进而，ROS主控与PC端通过无线蓝牙连接进行数据传输，通过PC端软件显示机器人状态，包括上述判断出是否处于正常运行状态，以及各个模块的工作状态和巡检救援机器人在探索任务过程中的位姿信息等实时状态数据。并且，基于人机交互模块40，工作人员还可以向机器人发送调试指令，进行样机调试等各种。

在本申请一个实施例中，人机交互模块40还可以在机器人处于非正常运行状态，比如，检测出某个模块故障时向PC端控制系统发出报警信息。在双控制器模块20判断出周围区域存在行人需要规避时，通过预先安装在机器人上的声光报警设备进行告警，提醒周围的行人注意避让等，以进一步提高矿山巡检救援的安全性。

综上所述，本申请实施的多传感器融合的矿山巡检救援机器人，具有自主探测、定位、三维环境建模、路径规划和实时避障功能，可以在复杂的井下环境下进行精准的探测且扩大了探测区域。基于多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，提高了井下探测的精确度和在各种工作场景下的适用性。

为了更加清楚的说明的通过本申请的多传感器融合的矿山巡检救援机器人进行井下探测的具体实现过程，下面以本申请实施例中提出的一种多传感器融合的矿山巡检救援机器人的控制方法进行详细说明。该方法应用于上述实施例中的多传感器融合的矿山巡检救援机器人，即该多传感器融合的矿山巡检救援机器人的控制方法所针对的多传感器融合的矿山巡检救援机器人，即是上述实施中所述多传感器融合的矿山巡检救援机器人，机器人中包括的各部件、各部件的连接方式和各部件可实现的功能如上述实施例所述，此处不再赘述。

图3为本申请实施例提出的一种多传感器融合的矿山巡检救援机器人的控制方法的流程图，如图3所示，该方法包括以下步骤：

步骤S301，对多源传感探测模块搭载的多个传感器进行标定，同步多个传感器的时间戳，并实时读取多源传感探测模块检测到的多源传感数据。

具体的，首先对矿山巡检救援机器人进行探测所需的高清相机、深度相机、激光雷达、IMU传感器、里程计传感器和UWB探测仪等多个传感器，通过ROS主控器完成空间位姿标定和各个传感器之间的时间同步，并在运行过程中实时读取各个传感器采集的传感数据。

在本申请一个实施例中，先以控制系统中的时间戳为标准时间，通过控制系统中的时间戳，实现对摄像头、深度相机、激光雷达、IMU传感器、里程计传感器和UWB探测仪数据间的时间同步。其中，IMU中的三轴陀螺仪在实际应用中会存在零点漂移的问题，因此，ROS主控器在时间同步程序中设定了零点漂移消除机制，在机器人上电后，先读取稳定的初始角速度值作为漂移值，并以此作为偏置量实现数据的零点消除。

进一步的，对多个传感器进行空间位姿标定。多源传感器联合标定的作用是建立图像数据、激光雷达数据和深度数据之间的空间对应关系，在本申请一个实施例中，对多源传感探测模块搭载的多个传感器进行标定，包括以下步骤：首先，通过预设的标定工具包和棋盘标定板，对激光雷达、高清相机和深度相机进行标定；然后，确定由相机捕获的二维平面数据与激光雷达捕获的三维点云数据之间的线性方程；最后，切换不同姿态下的棋盘标定板，获得线性方程组，并求解线性方程组获得标定参数，其中，标定参数包括相机内参，以及相机与所述激光雷达之间的外参。

具体而言，在本实施例中，利用ROS主控下的camera_calibration工具包和autoware的CalibrationTookit模块等标定工具，以及标准的棋盘标定板对摄像机、深度相机和激光雷达进行标定，标定原理是控制高清摄像头、深度相机和激光雷达同时观测获取数据。

具体实施时，通过激光雷达与相机同时观察棋盘格参照物，求得由相机捕获的二维的平面数据(u,v)与激光雷达捕获的三维点云数据(x,y,z)间的转化矩阵M，数学关系式如以下公式(1)所示。然后，根据不同姿态下的标定板，可以得到一系列的线性方程，求解线性方程，得到的参数即为标定参数。

进而，得到的标定结果是高清摄像机的内参、深度相机内参、以及各相机和激光雷达之间的外参，多源传感器最终标定的结果如以下表1所示：

表1

更进一步的，在读取实时的传感数据时，通过选用的控制系统产品的安装应用获取ROS下数据读取格式，并进行配置，从而发布ROS下的摄像头图像话题/camera/rgb/image_raw、深度图话题/camera/depth/image_raw、激光雷达话题/scan、里程计话题/odom、IMU话题/imu、UWB话题/uwb、机器人速度话题/cmd_vel、机器人电压话题/PowerVoltage和关节位置及速度数据/joint_states。在机器人多源传感系统框架中，各传感器的驱动功能包读取传感信息，在安装固定的格式发布传感数据后，通过数据帧头中的时间戳实现多源传感器间的时间同步，通过frame_id获取该数据发布的数据来源，根据各帧之间已建立好的tf坐标变换关系，从而实现了多源传感数据的获取、发布、使用、转化和数据关系建立。

步骤S302，基于多源传感数据，通过预设的同步定位与建图SLAM算法计算机器人的实时位姿，并构建当前探测环境的三维稠密点云地图。

具体的，进行进行同步定位与建图(SLAM)，根据上一步骤获取的激光雷达的点云数据、IMU传感器里程计信息、摄像头图像和深度相机的环境深度信息等多源传感数据，通过SLAM算法计算得到机器人的实时位姿数据，并进行当前探测环境的三维稠密点云地图的构建。

其中，在ROS主控中获取的数据及类型包括：摄像头RGB图像/camera/rgb/image_raw(sensor_msgs::Image)、深度图像/camera/depth/image_raw(sensor_msgs::Image)、激光雷达数据/scan(sensor_msgs::LaserScan)和IMU数据/imu(sensor_msgs::Imu)。

由于IMU模块能够测量传感器本身的角速度和加速度，因此能够对短时间的快速运动提供较好的估计。本申请综合利用视觉/雷达里程计与IMU模块实现性能互补，IMU为机器人运动过程中的状态估计提供补偿与验证方法，而视觉/激光里程计则在慢速运动情况下解决IMU的运动漂移问题。

为了更加清楚的说明本申请确定机器人的实时位姿数据的具体计算过程和实现原理，在本申请一个实施例中还提出了一种机器人实时位姿数据的计算方法，图4为本申请实施例提出的一种机器人实时位姿数据的计算方法的流程图，图5为本申请实施例提出的一种基于多源传感数据融合的位姿估计算法的原理示意图。下面结合图4和图5进行详细说明。

如图4所示，该方法包括以下步骤：

步骤S401，对多源传感数据中的视觉图像和激光点云进行特征提取，获取融合了视觉图像和激光点云的特征点，并通过稀疏光流跟踪KLT算法对特征点进行特征跟踪。

具体的，首先上述基于多源传感器标定结果，得到各个相机、激光雷达与IMU坐标系之间的刚体变换方程。如图5所示，对于输入的数据，在数据预处理步骤中将融合了高清相机和深度相机采集的视觉图像和激光雷达采集的激光点云的特征点，利用稀疏光流跟踪(Kanade-Lucas-Tomasi，简称KLT)金字塔进行光流跟踪。

步骤S402，基于视觉里程计和激光里程计的特征点解算相机的相对位姿。

步骤S403，对IMU数据进行预积分，获得多个IMU预积分信息。

具体的，对于IMU模块，将IMU数据进行预积分，得到的多个IMU预积分信息包括：当前时刻的位姿、速度、旋转角，同时计算在后端优化中将要用到的相邻帧间的预积分增量，以及预积分的协方差矩阵和雅可比矩阵。

步骤S404，将IMU数据与相对位姿进行空间对齐，以IMU预积分信息为运动输入构造代价函数，并采用线性最小二乘法估计机器人初始状态的参数，优化求解得到机器人的初始位姿。

具体的，将IMU信息与视觉/激光里程计得到的相机的相对位姿进行空间对齐，以IMU预积分为运动输入构造代价函数，采用线性最小二乘来估计初始状态的线速度、重力向量和相机尺度等参数，优化求解得到机器人当前初始位姿。

步骤S405，根据IMU预积分信息和位姿解算信息构建非线性最小二乘方程，并利用预设的迭代工具，通过Levenberg-Marquardt方法进行迭代搜索，求解当前约束下的最小二乘解，以获得机器人的实时位姿。

具体的，如图5所示，在初始化完成后，在非线性优化阶段，采用基于滑动窗口的非线性优化方法来估计机器人的状态。根据输入的在多帧数据间的IMU预积分信息、激光雷达数据间位姿解算信息、图像数据间位姿解算信息，构建非线性最小二乘方程，以G2O非线性优化库为迭代工具，通过列文伯格-马夸尔特法(Levenberg-Marquardt，简称L-M方法)进行迭代搜索，求解当前约束下的最小二乘解，得到机器人的实时位姿数据。

由此，可以输出矿山巡检救援机器人的六自由度位姿信息。

进一步的，在得到机器人实时位姿后，构建三维稠密点云地图。

作为一种可能的实现方式，将每一帧帧激光雷达点云数据投影到世界坐标系下得到环境三维模型的物理结构，同时通过图像数据与激光雷达数据间的匹配关系，为三维模型增加颜色和纹理信息，从而实现环境的三维稠密点云地图构建。

由此，最终可以在ROS主控系统中发布的数据及类型包括：机器人当前位姿/slam/pose(geometry_msgs::PoseStamped)、机器人运动轨迹/slam/path(nav_msgs::Path)、机器人实时视觉里程计/slam/odom(nav_msgs::Odometry)和实时环境点云地图/slam/PointCloudOutput(sensor_msgs::PointCloud2)。

步骤S303，通过卷积神经网络和卡尔曼滤波算法对UWB探测仪采集的实时探测数据进行运算，获取生命体运动轨迹信息，并将生命体运动轨迹信息输入SLAM算法中，优化机器人的实时位姿和三维稠密点云地图。

具体的，基于机器人中搭载的UWB探测仪采集到的探测数据对步骤S302中的同步定位与建图SLAM算法进行优化。基于UWB探测仪获取实时探测数据，通过卷积神经网络CNN及卡尔曼滤波算法获取UWB探测仪探测范围内的生命体运动信息，并将生命体运动信息输入到SLAM算法中，对激光雷达点云数据、高清相机采集的图像和深度相机采集到的深度信息进行优化，去除掉动态生命体数据对视觉图像和点云数据的影响。从而能够提高机器人定位精度和三维稠密点云地图的准确度，实现SLAM系统发优化。

在本申请一个实施例中，通过卷积神经网络和卡尔曼滤波算法对UWB探测仪采集的实时探测数据进行运算，包括：先将实时探测数据输入预先训练完成的卷积神经网络，获得生命体征识别结果；然后通过卡尔曼滤波算法对生命体征识别结果进行处理，以在生命体征识别结果中提取出动态生命体的生命体运动轨迹信息。

具体而言，在本实施例中，先利用UWB探测仪获取实时探测数据，构建并训练基于CNN的动态生命体识别模型。作为一种示例，构建的CNN模型结构如图6所示，其总体架构为5层神经网络，包含一个输入层(Input layer)、4个卷积层(图6中所示的Conv1、Conv2、Conv3和Conv4)和一个全连接层(FC)，此外还设置了一个残差层(图6中所示的Residual layer)来保证训练效果的优化选择。在CNN模型中，各层包含的子层如图6所示，每个卷积层的关键参数包括：卷积核数量(Nc)、卷积核尺寸(FH*FW)、步长(SL)和填充量(Pn)，上述各个关键参数设置结果均如以下表2所示：

表2

进而，基于上面建立好的CNN模型进行算法训练，具体训练过程可以参照相关技术中的卷积神经网络的训练方式，此处不再赘述。

在CNN模型训练完成后，利用二维滑动窗口对采集到的实时探测数据，即UWB雷达回波矩阵进行遍历。将活动窗口当前获取的数据转化为u*v的矩阵形式，再输入到已经训练好的CNN模型中。作为一种示例，CNN模型最终输出的生命体征识别的效果如图7和图8所示。图7和图8中的横坐标为慢时间(Slow Time)，纵坐标为快时间(Fast Time),图中的数据点展示了不同时间下CNN模型输出的生命体识别结果。

进一步的，利用卡尔曼滤波算法对上述的生命体判识结果进行处理，得到动态生命体运动轨迹提取结果。

具体而言，在本申请中的UWB雷达探测系统中随着慢时间变化，系统的状态方程及观测方程如下所示：

X_k＝A_kX_k-1+B_kU_k+w_k

Z_k＝H_kX_k+v_k

其中，状态方程中，X_k为k时刻系统状态的估计值，U_k表示k时刻给系统施加的控制量。A_k是作用在k-1时刻的状态转移矩阵，B_k是作用在U_k上的控制矩阵，用来描述控制量对系统状态的影响，w_k为过程噪声，服从均值为0、协方差为Q_k的独立多元正态分布。观测方程中，Z_k为k时刻系统状态的测量值，H_k为观测矩阵，用于描述实际状态空间到观测空间的映射；V_k为观测噪声，服从均值为0、协方差为R_k的独立多元正态分布。

为了实现任务简化，本申请实施例采用恒速模型(constant velocity model)来描述动态目标的运动性质，再利用卡尔曼滤波算法进行运动估计时，系统的状态包含了位置和速度两个二维参量，系统的测量值可由每个慢时间上CNN模型识别位置的平均值得到，即可通过以下公式表示：

在卡尔曼滤波算法中包括预测、更新两个阶段。在预测阶段，算法通过前一时刻的系统状态值推算当前时刻的系统状态值。在更新阶段，算法综合考虑当前状态的测量值及前一时刻的预测值，给出当前系统状态的最优估计。

其中，在预测阶段，算法如下所示：

首先，根据k-1时刻的状态估计值得到k时刻的系统先验预测

其中，/>

表示k-1时刻系统状态的最优估计。然后计算k时刻系统的预测值与实际值之间的估计误差/>

其中，Q_k为过程噪声wk的协方差矩阵。

在更新阶段，算法如下所示：

即在本阶段，首先，计算卡尔曼增益K_k，其中，R_k为观测噪声v_k的协方差矩阵。参量尔曼增益K_k描述k时刻系统状态预测值与实测值间的关系，优化使得估计状态最大程度的接近真实状态。然后，综合先验预测

测量值Z_k和卡尔曼增益K_k，计算出系统状态变量的最优估计值/>

最后，对后验误差协方差矩阵P_k进行更新。

基于上述步骤，按照同样的方式，控制卡尔曼滤波算法不断更新迭代，最终得到动态目标的运动轨迹估计结果。举例而言，对于图7和图8所示的生命体征识别结果，提取出的生命体运动轨迹信息分别如图9和图10所示。由图9和图10可知，在图7和图8所示的生命体征识别结果的基础上，提取出了图中所示的生命体征范围内的曲线，该曲线表示估计出的运动轨迹。

由此，获取了当前探测环境范围内的生命体运动轨迹信息。

更进一步的，在进行SLAM优化时，根据多传感器标定的结果得到UWB探测仪与激光雷达、摄像头和深度相机之间的坐标转换关系，将得到的生命体运动信息输入到SLAM算法中，从而在每一帧实时激光雷达点云、摄像头图像和深度图像中解算出运动生命体的对应位置信息，实现运动目标生命体在多源传感数据间的关系映射和连续追踪功能。然后，对激光雷达点云数据、摄像头图像和深度相机采集到的深度信息进行优化，去除掉动态生命体数据的影响，从而可以提高机器人定位精度和三维稠密点云地图的准确度，实现同步定位与建图(SLAM)系统优化。

步骤S304，对优化后的三维稠密点云地图进行滤波处理，将滤波处理后三维稠密点云地图的转化为八叉树地图，并通过2D投影生成点云地图对应的占据网格地图。

具体的，将优化后的三维稠密点云转换为适用于路径规划与机器人导航形式的地图。先读取SLAM系统发布的三维稠密点云地图，对点云地图进行滤波处理以去除点云地图中地面和天花板信息等于机器人导航无关的信息。再通过八叉树进行稀疏表达，将点云地图转换为八叉树地图，同时通过2D投影生成占据网格地图(Occupancy Map)。

需要说明的是，机器人通过SLAM算法获得周围环境地图，可实现三维实时显示。对机器人而言，三维重建主要用于构建场景信息和各类地图，以使得机器人可以知道它在世界环境中的位姿，并且可以感知周围障碍物的位置信息，从而实现机器人的定位、导航、避障和路径规划。而稠密的点云地图在机器人进行定位过程中会消耗大量的资源，因此，本申请选用八叉树地图。八叉树地图是用一个立方体来表示一定体积的空间，该立方体也代表根节点，不断的进行八等分直至叶子结点。当空间某处的中间节点没有物体占据时，对此节点便不用展开，当某个节点被占据便不断八等分。因此使用八叉树地图会极大的节省空间。

在本申请一个实施例中，为了更加清楚的说明本申请将稠密的点云地图转换为八叉树地图的具体实现过程，下面以本申请实施例中提出的一种八叉树地图的转换方法进行示例性说明。图11为本申请实施例提出的地图转换方法的流程图，如图11所示，该方法包括以下步骤：

步骤S111，对目标点云地图进行体素滤波。

具体的，在地图转换模块中，首先对三维点云进行体素滤波，在三维点云中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示。

步骤S112，通过八叉树对滤波后的目标点云地图进行稀疏表达得到初始八叉树地图，包括：将点云地图中的每个点云对应一个节点，其中，通过节点被占据的概率表达每个节点。

具体的，对稠密点云地图通过八叉树进行稀疏表达，1个稠密点云地图中点云对应1个节点，得到八叉树地图，其中每个节点通过该节点被占据的概率来表达，1个节点被占据的概率y通过下式计算：

其中，x为该节点对应的点云从第一帧到当前帧图像样本中累积被观测到的次数，初值为0。若在一帧图像样本中被观测到一次，则次数加1，反之减1，并通过重复上面的步骤，不断地对八叉树地图进行更新，从而得到实时更新后的八叉树地图。

步骤S113，通过欧式距离分析每个点云数据对应的预设数量个近邻距离矩阵和索引，剔除初始八叉树地图中的离散点数据，获得目标八叉树地图。

具体的，结合欧式距离信息分析每个点云数据xi(i＝1,2,…,m)的k个近邻距离矩阵，以及对应的索引。其中，m表示点云的个数，k邻距离矩阵的个数，可根据实际情况确定。其中，本实施例使用的欧式距离计算方式可以通过以下公式表示：

其中，n代表数据空间维度。根据欧氏距离，在上述点云数据集{x₁,x₂,...,x_m}中找出与每个数据xi最相近的k个样本点，并将这k个样本点所表示的集合记为Nk(x)。

然后，在Nk(x)中根据预设的分类规则确定每个数据xi所属的类别yi，具体可通过以下公式确定：

其中，K为类别yi的总数目。

进而为数据xi(i＝1,2,…,m)建立k-MST结构。计算每个类别yi中的数据的最大欧式距离和平均欧式距离，根据最大欧式距离和平均欧式距离确定每个类别中的离散点，从而实现离散点数据的剔除，得到剔除后离散点的八叉树地图，即目标八叉树地图。

步骤S114，根据高度对目标八叉树地图进行染色。

具体的，对新生成的目标八叉树地图按高度进行染色，同一高度对应同一颜色，最终得到将实时三维建图转换为的八叉树地图。

步骤S305，基于全局信息和人机交互信息，通过快速随机扩展树RRT算法生成矿山巡检救援机器人的探测策略，其中，探测策略中包括多个目标探测点。

具体的，根据获取的全局信息及人机交互信息，生成矿山巡检救援机器人探测策略，并在空间环境中根据要求分发探测目标点。其中，全局信息可以包括检测到的当前探测环境下的各类信息、各个传感器采集到的信息和通过其他方式获得的当前巡检救援任务的整体信息。人机交互信息包括通过上述人机交互模块获取的工作人员发送的预期的探测区域和具体探测地点等信息。目标探测点可以是矿山巡检救援机器人需要进行探测的目标地点。

需要说明的是，对于未知区域的自主探索，是移动机器人的重要功能，在缺少全局定位参考(比如全球定位系统gps和超带宽uwb定位技术)的井下环境中，巡检救援机器人需要按照特定的算法对地图进行探测、获取环境信息并以此为基础进行后续的探测。而在本申请中，通过快速随机扩展树(Rapidly-Exploring Random Trees，简称RRT)算法构建机器人探测策略，RRT算法是一种单一查询算法，一边抽样一边建图，通过不断随机探索来实现遍历。

在本申请一个实施例中，通过快速随机扩展树RRT算法生成矿山巡检救援机器人的探测策略，包括以下步骤：先通过激光雷达采集的数据构建一个二维栅格地图；再通过局部RRT探测法检测机器人周围的边界点，并采用全局RRT探测法探测二维栅格地图的远端边界点；然后，对全部的边界点进行过滤和聚类，并通过机器人在自主探索过程中的信息增益、导航代价和自身定位的精确度构建边界点评价函数；最后，根据边界点评价函数对每个边界点的得分选择目标边界点。

具体而言，在本实施例的主探测任务中，首先用激光雷达数据构建一个初始的二维栅格地图,并使用局部RRT探测方法检测机器人周围的边界点,采用具有自适应步长的全局RRT探测在已构建地图的远端边界点。然后对获得的边界点进行过滤和聚类,以减少总数和计算负荷。最后,通过机器人在自主探索过程中的信息增益、导航代价和机器人自身定位的精确程度3个要素构建边界点评价函数,通过评价函数计算每个边界点的得分,在选择最高得分的边界点作为最佳边界点后,利用闭环控制引导机器人到最佳位置,直到地图构建完成。

举例而言，通过本申请实施例的RRT自主探测方法生成的目标探测地图如图12所示，图12中多个矩形表示生成的多个目标探测点，黑色的圆形表示本申请的矿山巡检救援机器人，图中的各个线条可表示初始确定的探测方向。

在本申请一个实施例中，在确定探测策略中的目标探测点时，可以结合上述确定的最佳边界点以及人机交互信息中用户发送的期望探测点等多种因素确定机器人待探测的目标探测点。

步骤S306，根据优化后的机器人的实时位姿、八叉树地图、占据网格地图和探测策略，生成机器人的全局路径规划和局部路径规划，并根据路径规划生成运动控制指令，控制矿山巡检救援机器人执行运动控制指令。

具体的，对机器人的运动路径进行规划，进行机器人的全局路径规划和局部路径规划，同时进行机器人实时避障，根据各个路径规划发布机器人的运动控制信息，即根据规划路径生成在不同阶段对机器人的各条运动指令，包括巡检救援机器人的运动方向、移动距离和移动速度的控制指令，从而可以控制机器人的运行和停止，以及根据当前特殊状况进行路径切换等。进而，将生成的运动控制指令发送至巡检救援机器人相关执行部件，比如，上述实施例中所述的运动载体模块10执行各控制指令，由此实现矿山巡检救援两用机器人的运动控制。

在本申请一个实施例中，生成机器人的全局路径规划和局部路径规划，包括以下步骤：先根据八叉树地图和占据网格地图判断每个目标探测点是否能够达到；在能够到达的情况下，读取二维占据网格语义地图，根据优化后的机器人的实时位姿确定机器人起始点；基于关键点进行全局路径规划，并基于机器人的实时避障功能生成机器人的实时运动轨迹曲线。

具体而言，在本实施例中，在机器人自主探测模块分发探测目标点后，根据地图转换模块生成的八叉树地图和占据网格地图判断该目标点是否可到达，如果目标不可到达，则返回探测策略模块重新分发目标点，如果目标点可达则可进行全局路径规划。

具体进行全局路径规划时，在进行路径规划之前读取保存的二维占据栅格语义地图，根据机器人的实时位置检测，判断机器人起始点。利用A*全局路径规划方法等经典的路径规划启发式算法对机器人进行基于关键点的全局路径规划，并基于TEB(Timed-Elastic-Band)算法开发机器人实时避障功能，根据避障所需的调整路径确定当前当前区域内的移动轨迹，从而生成机器人的实时运动轨迹曲线。再通过该实时运动轨迹曲线得到机器人的实时运动速度、角速度和加速度信息，由ROS主控器向STM32控制器发布运动信息，STM32再根据速度信息控制舵机转向与电机转速来使小车运动，同时通过STM32控制器向ROS主控器返回的数据来获取小车的速度与姿态，最后控制机器人根据规划路径从起始点导航至目标点。

综上所述，本申请实施的多传感器融合的矿山巡检救援机器人的控制方法，通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，实现了一套具有高鲁棒性、高探测精度、强适应性和具备自主探测能力的多传感器融合的矿山巡检救援两用机器人探测系统。并且，该方法还通过UWB探测仪实时探测获取生命体运动信息，并将生命体运动信息输入到SLAM算法中，对激光雷达点云数据、摄像头图像和深度相机采集到的深度信息进行优化，去除掉动态生命体数据的影响，提高机器人定位精度和三维稠密点云地图的准确度，实现同步定位与建图SLAM系统的优化，可以及时规避行人等移动对象，提高了矿山巡检救援的安全性。

基于上述实施例，为了更加清楚、直观的表明本申请的多传感器融合的矿山巡检救援机器人的控制方法的具体实现流程，下面结合图13所示的一种具体的基于多传感器融合的矿山巡检救援两用机器人的探测方法和图14所示的探测算法的原理进行说明。

如图13所示，该方法包括以下步骤：

步骤S1：多源传感器标定与数据获取。对本装置探测所需要用到的摄像头、深度相机、激光雷达、IMU传感器、里程计传感器、UWB探测仪，基于ROS系统完成多源传感器的空间位姿标定、传感器时间戳同步，并实时读取传感数据。

步骤S2：同步定位与建图(SLAM)。根据激光雷达的点云数据、IMU传感器\里程计信息、摄像头图像和深度相机的环境深度信息，以SLAM算法(如ORBSLAM3、FAST-LIVO、R3LIVE)为基础，进行探测环境的三维稠密点云地图构建，同时计算得到机器人的实时位姿数据。

步骤S3：基于UWB探测实现SLAM系统优化。利用UWB探测仪获取实时探测数据，基于CNN及卡尔曼滤波算法获取UWB探测仪探测范围内的生命体运动信息，将生命体运动信息输入到SLAM算法中，对激光雷达点云数据、摄像头图像和深度相机采集到的深度信息进行优化，去除掉动态生命体数据的影响，提高机器人定位精度和三维稠密点云地图的准确度，优化同步定位与建图(SLAM)系统优化。

步骤S4：地图转换。读取SLAM系统发布的点云地图，进行外点滤波和降采样滤波处理，压缩滤除地面和天花板信息，转化为八叉树地图(OctoMap)，同时通过2D投影生成占据网格地图(Occupancy Map)，用于路径规划与机器人导航。

步骤S5：机器人探测策略。获取全局信息及人机交互信息，生成矿山巡检救援机器人探测策略，在空间环境中根据要求分发探测目标点。

步骤S6：机器人路径规划。获取优化过后的机器人实时位姿、环境代价地图、机器人周围实时障碍物信息、机器人探测策略(发布探测点)，进行机器人的全局路径规划和局部路径规划，同时进行机器人实时避障，发布机器人运动控制信息，实现矿山巡检救援两用机器人的运动控制。

为了实现上述实施例，本申请还提出一种非临时性计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时实现如本申请前述第二方面实施例提出的多传感器融合的矿山巡检救援机器人的控制方法。

需要说明的是，应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

另外，在本申请的描述中，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

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

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

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

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

Claims (10)

1.一种多传感器融合的矿山巡检救援机器人，其特征在于，包括：运动载体模块、双控制器模块、多源传感探测模块和人机交互模块，其中，

所述双控制器模块分别与所述运动载体模块、所述多源传感探测模块和所述人机交互模块相连；

所述运动载体模块采用轮式机器人，每个旋轮均为麦克纳姆轮，所述运动载体模块用于实现矿山巡检救援所需的多种运动功能，所述多种运动功能包括多方向移动和转向；

所述多源传感探测模块，包括激光雷达、高清相机、深度相机、轮式里程计、惯性传感器IMU和超宽带UWB探测仪，所述多源传感探测模块用于获取实现环境感知的多源传感信息；

所述双控制器模块，包括基于机器人操作系统ROS的主控制器以及底层运动控制器，所述底层运动控制器用于传输所述主控制器的控制指令和机器人底层的所述多源传感信息，所述主控制器用于基于多源传感信息融合技术实现同步定位与建图SLAM、SLAM优化、地图转换、探测策略生成以及运动路径规划与实时避障，并根据运算结果生成所述控制指令；

所述人机交互模块通过无线通信技术与PC端控制系统相连，所述人机交互模块用于实现巡检救援机器人与用户的之间的人机交互。

2.根据权利要求1所述的矿山巡检救援机器人，其特征在于，所述主控制器与所述底层运动控制器之间通过UART3串口协议实现通信，所述底层运动控制器向所述主控制器发送的数据，包括：机器人使能标志位、机器人三轴速度、IMU三轴加速度和三轴角速度；

所述底层运动控制器采用中断接收的方式接收所述主控制器发送的数据，接收的数据包括：机器人标识信号、使能控制标志位、机器人三轴目标速度和数据校验位。

3.根据权利要求1所述的矿山巡检救援机器人，其特征在于，所述主控制器通过Mirco-USB与所述激光雷达相连，以向所述激光雷达供电并进行数据交互，所述主控制器还通过Mirco-USB与所述UWB探测仪相连；

所述主控制器通过MIPI-CSI协议分别与所述高清相机和所述深度相机相连，所述轮式里程计设置在所述运动载体模块上；

所述惯性传感器IMU集成在所述底层运动控制器中，所述底层运动控制器将采集到的IMU数据发送至所述主控制器。

4.一种多传感器融合的矿山巡检救援机器人的控制方法，其特征在于，所述多传感器融合的矿山巡检救援机器人是如权利要求1-3任一所述的多传感器融合的矿山巡检救援机器人，所述控制方法包括以下步骤：

对多源传感探测模块搭载的多个传感器进行标定，同步所述多个传感器的时间戳，并实时读取多源传感探测模块检测到的多源传感数据；

基于所述多源传感数据，通过预设的同步定位与建图SLAM算法计算机器人的实时位姿，并构建当前探测环境的三维稠密点云地图；

通过卷积神经网络和卡尔曼滤波算法对UWB探测仪采集的实时探测数据进行运算，获取生命体运动轨迹信息，并将所述生命体运动轨迹信息输入所述SLAM算法中，优化机器人的实时位姿和三维稠密点云地图；

对优化后的三维稠密点云地图进行滤波处理，将滤波处理后三维稠密点云地图的转化为八叉树地图，并通过2D投影生成点云地图对应的占据网格地图；

基于全局信息和人机交互信息，通过快速随机扩展树RRT算法生成所述矿山巡检救援机器人的探测策略，所述探测策略中包括多个目标探测点；

根据优化后的机器人的实时位姿、所述八叉树地图、所述占据网格地图和所述探测策略，生成机器人的全局路径规划和局部路径规划，并根据路径规划生成运动控制指令，控制所述矿山巡检救援机器人执行所述运动控制指令。

5.根据权利要求4所述的控制方法，其特征在于，所述对多源传感探测模块搭载的多个传感器进行标定，包括：

通过预设的标定工具包和棋盘标定板，对激光雷达、高清相机和深度相机进行标定；

确定由相机捕获的二维平面数据与激光雷达捕获的三维点云数据之间的线性方程；

切换不同姿态下的棋盘标定板，获得线性方程组，并求解所述线性方程组获得标定参数，所述标定参数包括相机内参，以及相机与所述激光雷达之间的外参。

6.根据权利要求4所述的控制方法，其特征在于，所述基于所述多源传感数据，通过预设的同步定位与建图SLAM算法计算机器人的实时位姿，包括：

对所述多源传感数据中的视觉图像和激光点云进行特征提取，获取融合了所述视觉图像和所述激光点云的特征点，并通过稀疏光流跟踪KLT算法对所述特征点进行特征跟踪；

基于视觉里程计和激光里程计的特征点解算相机的相对位姿；

对IMU数据进行预积分，获得多个IMU预积分信息；

将所述IMU数据与所述相对位姿进行空间对齐，以所述IMU预积分信息为运动输入构造代价函数，并采用线性最小二乘法估计机器人初始状态的参数，优化求解得到机器人的初始位姿；

根据所述IMU预积分信息和位姿解算信息构建非线性最小二乘方程，并利用预设的迭代工具，通过Levenberg-Marquardt方法进行迭代搜索，求解当前约束下的最小二乘解，以获得机器人的实时位姿。

7.根据权利要求4所述的控制方法，其特征在于，所述通过卷积神经网络和卡尔曼滤波算法对UWB探测仪采集的实时探测数据进行运算，包括：

将所述实时探测数据输入预先训练完成的卷积神经网络，获得生命体征识别结果；

通过卡尔曼滤波算法对所述生命体征识别结果进行处理，以在所述生命体征识别结果中提取出动态生命体的所述生命体运动轨迹信息。

8.根据权利要求4所述的控制方法，其特征在于，所述将所述目标点云地图转化为八叉树地图，包括：

对所述目标点云地图进行体素滤波；

通过八叉树对滤波后的目标点云地图进行稀疏表达得到初始八叉树地图，包括：将点云地图中的每个点云对应一个节点，其中，通过节点被占据的概率表达每个所述节点；

通过欧式距离分析每个点云数据对应的预设数量个近邻距离矩阵和索引，剔除所述初始八叉树地图中的离散点数据，获得目标八叉树地图；

根据高度对所述目标八叉树地图进行染色。

9.根据权利要求4所述的控制方法，其特征在于，所述通过快速随机扩展树RRT算法生成所述矿山巡检救援机器人的探测策略，包括：

通过激光雷达采集的数据构建一个二维栅格地图；

通过局部RRT探测法检测机器人周围的边界点，并采用全局RRT探测法探测所述二维栅格地图的远端边界点；

对全部的边界点进行过滤和聚类，并通过机器人在自主探索过程中的信息增益、导航代价和自身定位的精确度构建边界点评价函数；

根据所述边界点评价函数对每个边界点的得分选择目标边界点。

10.根据权利要求4所述的控制方法，其特征在于，所述生成机器人的全局路径规划和局部路径规划，包括：

根据所述八叉树地图和所述占据网格地图判断每个所述目标探测点是否能够达到；

在能够到达的情况下，读取二维占据网格语义地图，根据所述优化后的机器人的实时位姿确定机器人起始点；

基于关键点进行全局路径规划，并基于机器人的实时避障功能生成机器人的实时运动轨迹曲线。

Images