CN113485325A - 煤矿井下水泵房巡检机器人slam建图、自主导航方法 - Google Patents

Abstract

本发明属于矿井下导航技术领域，公开了一种煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，包括建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划最短路径；采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；通过SLAM建图和导航算法利用煤矿井下巡检机器人对水泵房设备进行巡检。本发明能够实现煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检工作。

Description

煤矿井下水泵房巡检机器人SLAM建图、自主导航方法

技术领域

本发明属于矿井下导航技术领域，尤其涉及一种煤矿井下水泵房巡检机器人SLAM建图、自主导航方法。

背景技术

目前，目前，我国煤炭资源丰富，煤炭仍是我国主要能源。近年来，煤炭行业先后提出机械化换人、智能化无人等理念。2019年1月，国家煤矿安监局发布《煤矿机器人重点研发目录》，公布了煤炭企业、科研机构、制造企业重点研发应用的5类38种煤矿机器人，其中巡检机器人是安控类机器人的一种。

近年来，国家加大力度整治和规范煤矿的本质安全型管理，同时极力推进煤矿企业向“数字化、无人化”方向发展。任何行业，设备的巡检工作一般都依靠人工完成，不仅消耗大量的人力资源，工作流程也较为繁琐，特别是高危行业，巡检过程中可能存在风险，人员工作环境也会比较恶劣。煤矿井下主排水系统是煤炭生产中不可缺少的重要设备，其运行状况的好坏直接影响着矿山的正常生产和工人的生命安全，为保证水泵的正常运行，巡检人员要定期查看设备的运行情况，例如水泵盘根、水仓状况、电机以及水泵是否存在异常等情况。一般一个水泵房需要几个人来看管、操作，工作效率低。

随着科学技术的进步，在电力和石化行业中，轮式的巡检机器人、履带式巡检机器人或是轨道式的吊挂巡检机器人已经有了众多应用案例，移动机器人在巡检过程中对被检设备有很强的针对性巡检，工作质量甚至比人还要专业。

煤矿灾害频发，危害严重，风险较大，下井人员密集，危险岗位多，研发应用煤矿水泵房巡检机器人可代替巡检工人进行设备及环境巡检，能够有效的减轻工作人员的劳动强度，降低巡检过程中存在的安全隐患，提升巡检质量的同时，最大限度的提升煤矿企业的本质安全水平。对推动煤炭开采技术革命，实现煤炭工业高质量发展，推动煤矿智能化进程，国家能源安全供应具有重要意义。

国内外专家从机器人的应用环境出发，将机器人分为两大类，制造环境下的工业机器人和非制造环境下的特种机器人。国内外特种机器人的研发主要集中在探险救灾、军事防爆、侦察探测等领域。煤矿特殊工作环境以及高标准的移动、通讯、防爆性能要求，实现矿井内部特定功能的特种移动机器人的研发进程较缓慢。

(1)煤矿探测型机器人

上世纪90年代，澳大利亚联邦科工组织(CSIRO)就设计了煤矿探测型机器人Numbat，测井下气体环境是这个机器人的主要工作。它通过可见光与红外图像传感器，传输井下信息回地面，将井下最新状态告知救援队员，以便救援方案的制定。这个机器人的动力来源是一个140Ah的镍镉电池，行走依赖于两侧分别装配的750W电机组成的差速八轮结构，其尺寸为2.5×1.65m，速度最快可达每小时两千米，且有着8小时的续航能力。

Groundhog煤矿探测型机器人是美国卡耐基梅隆大学机器人研究所研制，这个机器人主要用于井下环境探测和井下三维地图绘制，搭载激光传感器、夜视摄像头、体感传感器和陀螺仪等。由液压驱动，四轮差速运转，带有原地转向功能。Thrun等人在2003年5月30日用其探测了宾夕法尼亚考特尼县的马蒂斯矿的主巷道，并完成三维构图。

2005年，中国矿业大学救援与装备研究所成功研发出我国第一台煤矿探测型机器人CUMT-I，该机器人可以探测灾后现场的环境，搭配危险气体传感器，低照明度摄像机和双向语音对讲系统，以遥控方式受控，且能随机装载各类救灾物资(药物、实物或救援工具)，是救援队灾后搜救好帮手。

2007年，哈尔滨工业大学机器人研究所与唐山开诚电器有限公司共同研发了煤矿探测型机器人，这个机器人能探测灾后现场的环境，搭配危险气体传感器、低照明度摄像机和双向语音对讲系统。其行走方式为履带式，履带分为驱动、摆臂、摆腿三个部分。支持遥控操作，通过遥感和控制按键来实现。

(2)煤矿救援机器人

Ratler煤矿救援机器人由美国劳工部矿山安全与卫生管理局(MSHA)和桑迪亚智能系统与机器人中心(ISRC)共同研发，主要用于灾后勘察，搭载了红外摄像机、陀螺仪和危险气体传感器，且可通过无线射频装置进行最大距离76 米的遥控。在1998年12月发生的Willow Greek煤矿火灾救援中对这个机器人做适用性实验表明，其远不能达到煤矿业对救援机器人的标准。

第二代移动煤矿救灾机器人平台Cave Crawler由卡耐基梅隆大学机器人研究所设计，该机器人内部使用了与“勇气号”火星探测车相似的齿轮差动机构，摇杆式左右轮移动系统，且左右摇杆与机器人主体通过差动机构连接，从而线性平均了两摇杆的摆角，进一步转化为机器人车身的摆角，从而为机器人主体保持平衡。某边车轮抬起时，整车的摆动幅度仅为车轮抬起幅度的一半，从而削弱了地形起伏对机器人运动的阻碍。此外，这样的设计均匀地将车体重量分配到机器人的各个车轮上，且车轮可根据地形调整其相对位置，使机器人运动更平稳、不易颠覆，有更强的越障能力。

(3)煤矿巡检机器人

现有技术1主要研究了井下巷道悬线巡检机器人的机械本体，在对悬线巡检机器人的本体结构、自主运动和巡检过程控制要求进行细致分析的基础上深入研究了巡检机器人的控制系统。

现有技术2设计了一种能够自主延长通信距离并且自动回撤的煤矿探测机器人，以打破煤矿井下探测机器人收到工作行程的限制。在检测到无线控制信号削弱时，机器人搭载的中继器弹射系统能够运作以延长无线通信距离。若无线信号终止，机器人变回启动回撤程序，按照运行记录的传感器数据，通过原路径回撤，从而回到通信顺畅的安全位置。机器人运用编码器和激光传感器数据集成的方法来实现回撤程序，且依赖ICP算法来双重匹配数据，修正机器人的坐标和朝向，达到足够的回撤精度。从实验结果看，这个机器人的鲜明特点是有着较长的工作行程和回撤能力，这意味着它能够在煤矿灾难现场安全性未知时，替代救援人员深入矿井，完成危险探测的初期工作。

现有技术3设计了一种能摆脱井下巷道复杂环境和人员设备分布影响的煤矿巷道悬线巡检机器人，以解决现有煤矿巷道轮式或履带式机器人所具有的结构和控制复杂、可靠性低、成本高、占用空间多的问题。他们着重描述了这个机器人的机构设计，并在Adams软件里建立虚拟样机的模型进行仿真分析，以评估其水平行走、上下坡途中三种运动状态下越过障碍物的能力。其结果显示该机器人在如上三种运动状态下都相对平稳，特别是在水平方向上机器人始终保持匀速直线运动，而在竖直和水平侧摆方向上会产生允许范围以内的波动。

现有技术4为解决井下皮带运输机的巡检、监控问题，设计了轨道式无人巡检装置，描述了该机器人的整体设计方案，能力特性以及软件。硬件开发途径。对煤矿井下皮带运输机、刮板运输机等大型设备的检查都可以通过这个巡检机器人来完成，真正从根本上做到无人值守，为巡检人员减负，在最大程度上提升了煤矿的安全管理。

现有技术5在对煤矿井下水泵房工人巡检出现的问题研究的基础上，设计出了一种能替代工人巡检的自动巡检机器人装置，着重描述了该机器人体系设计依据、构成及具备的作用，描述了该机器人的软硬件的实施、数据采集等方式，该体系能够实时的发现井下水泵房可能出现的问题，并保障其稳定、安全的运行。

现有技术6对煤矿井下水泵房关键实施巡检、监督产生问题研究下，设计了一种可靠性高、能够代替人工巡检、信息采集、视频巡检及障碍报告等多种功能于一体的智能机器人，其还描述了该机器人设备的构成及作用特征，还在研究结论中对该机器人在不同条件下的自动巡检功能进行了详细描述，从而为煤矿井下达成无人值班、自动化管理方式开启了新方式。

通过上述分析，现有技术存在的问题及缺陷为：现有矿井下导航技术井下定位不准确，无法自主规划最优路径，且不能自主避障。

解决以上问题及缺陷的难度为：矿井下导航是巡检机器人在水泵房内稳定安全行进的必要条件，没有可靠的导航，机器人将无法完成巡检任务。目前常用的导航方式有：电磁导航、视觉导航、GPS导航等。电磁导航虽然能做到车体较精确定位，但种导航方式需要在机器人运行路线上铺设磁条和设定RFID标签，施工的工作量较大，不能预测前方道路情况，严重制约了移动机器人的行走速度；视觉导航在黑夜或光线不足的情况下效果较差，并且地面导航标志一旦破损或丢失会极大地影响机器人的导航定位性能；GPS导航有很好的灵活性，但是在煤矿井下水泵房内很难接收到GPS信号，即使接收到信号，也无法解决信号失真的问题。目前如何在煤矿井下水泵房内进行精准导航是实现水泵房巡检机器人自主巡检所面临的重要问题。

解决以上问题及缺陷的意义为：通过机器人对水泵房设备进行巡检可减少一部分人工的工作量，同时实现远程巡检、远程控制的功能，可实现值班人员远程监控其设备运行状态，实现由原来的人工巡视转化为无人值班。同时使用机器人自动巡检避免了人工巡检所出现的一些漏检、误检，也避免了突发事故导致的人员伤亡。

发明内容

针对现有技术存在的问题，本发明提供了一种煤矿井下水泵房巡检机器人 SLAM建图、自主导航方法。

本发明是这样实现的，一种煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法包括：

步骤一，建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物，可避免机器人在巡检过程中遇到地面上的碎煤、水池和杂物等发生事故；

步骤二，建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径，使得机器人在满足安全巡检的前提下提高巡检效率；

步骤三，采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图，大大提高了机器人在水泵房内巡检时的定位精度；

步骤四，通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

进一步，所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法还包括：

进行水泵房巡检机器人平台运动学和动力学分析，进行地形感知，并对激光雷达数据的预处理，进行SLAM建图和导航算法的优化。

进一步，所述进行水泵房巡检机器人平台运动学和动力学分析包括：

根据水泵房巡检机器人的主要技术指标和参数，设计轮式煤矿水泵房巡检机器人模拟平台，对机器人平台模型建立运动学模型以及动力学模型并进行运动学和动力学分析。

进一步，所述进行地形感知包括：

利用Kinect深度相机得到有关地形参数的数据信息，进行路面危险等级的自动识别；将计算得到的地形信息作为机器人的路径规划的主要指标，进行路径规划。

进一步，所述进行激光雷达数据预处理包括：

通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变。

进一步，所述进行SLAM建图和导航算法的优化包括：

首先，用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向，减少全局路径规划的时间；

其次，制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略方面进行实验结果的对比分析；

最后，优化ROS系统和单片机程序相关程序。

本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：

建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；

建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径；

采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；

通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：

建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；

建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径；

采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；

通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终端用于实现所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法

本发明的另一目的在于提供一种执行所述煤矿井下水泵房巡检机器人 SLAM建图、自主导航方法的系统，所述系统包括：

障碍物识别模块，用于建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；

最短路径规划模块，用于建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径；

全局一致性地图获取模块，用于采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；

设备巡检模块，用于通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明能够实现煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检工作。

本发明可实现巡检机器人在水泵房内准确定位，机器人全局定位在X轴和 Y轴上距离误差小于10cm，角度误差小于0.2rad；本发明可实现在建立好二维栅格地图的水泵房内规划出一条经过所有水泵房设备的路径，栅格地图与真实环境尺寸误差小于1cm；机器人的真实导航路径与算法估计路径的偏离程度小于10cm；移动机器人在结构相似的环境中建图时可进行回环；预处理后激光雷达采集的点云数据的方差小于0.1。

本发明可实现控制机器人按照规划好的路径运动，同时在规划的路径上突然出现障碍物时能及时避障。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的煤矿井下水泵房巡检机器人SLAM建图、自主导航方法流程图。

图2是本发明实施例提供的水泵房巡检机器人地面感知方案示意图。

图3是本发明实施例提供的无地面感知时ROS算法的路径规划指标示意图。

图4是本发明实施例提供的存在地面感知时ROS算法的路径规划指标示意图。

图5是本发明实施例提供的地形信息示意图。

图6是本发明实施例提供的SLAM建图中机器人位姿的约束示意图。

图7是本发明实施例提供的A*最短路径搜索算法原理示意图。

图8是本发明实施例提供的煤矿井下水泵房巡检机器人SLAM建图、自主导航方法原理图。

图9是本发明实施例提供的水泵房巡检机器人运动底盘三维模型示意图。

图10是本发明实施例提供的结合地形因素进行路径规划的效果示意图。

图11是本发明实施例提供的真实地面示意图。

图12是本发明实施例提供的激光雷达运动畸变去除前示意图。

图13是本发明实施例提供的激光雷达运动畸变去除后示意图。

图14是本发明实施例提供的图优化前后建图效果对比示意图。

图15是本发明实施例提供的PRM路径规划算法仿真结果示意图。

图16是本发明实施例提供的PRM路径规划算法生成的路径示意图。

图17是本发明实施例提供的Pure Pursuit路径跟踪算法仿真结果示意图。

图18是本发明实施例提供的Pure Pursuit算法跟踪路径过程分析示意图。

图19是本发明实施例提供的避障算法仿真结果示意图。

图20是本发明实施例提供的避障算法过程分析示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种煤矿井下水泵房巡检机器人 SLAM建图、自主导航方法，下面结合附图对本发明作详细的描述。

如图1所示，本发明实施例提供的煤矿井下水泵房巡检机器人SLAM建图、自主导航方法包括以下步骤：

S101，建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；

S102，建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径；

S103，采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；

S104，通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

本发明实施例提供的煤矿井下水泵房巡检机器人SLAM建图、自主导航方法还包括：

进行水泵房巡检机器人平台运动学和动力学分析，进行地形感知，并对激光雷达数据的预处理，进行SLAM建图和导航算法的优化。

本发明实施例提供的进行水泵房巡检机器人平台运动学和动力学分析包括：

根据水泵房巡检机器人的主要技术指标和参数，设计轮式煤矿水泵房巡检机器人模拟平台，对机器人平台模型建立运动学模型以及动力学模型并进行运动学和动力学分析。

本发明实施例提供的进行地形感知包括：

利用Kinect深度相机得到有关地形参数的数据信息，进行路面危险等级的自动识别；将计算得到的地形信息作为机器人的路径规划的主要指标，进行路径规划。

本发明实施例提供的进行激光雷达数据预处理包括：

通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变。

本发明实施例提供的进行SLAM建图和导航算法的优化包括：

首先，用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向，减少全局路径规划的时间；

其次，制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略方面进行实验结果的对比分析；

最后，优化ROS系统和单片机程序相关程序。

下面结合具体实施例对本发明的技术效果作进一步描述。

实施例1：

1、内容

(1)移动机器人平台运动学和动力学分析

根据已有的矿用水泵房巡检机器人主要技术指标和参数的研究基础，首先对巡检机器人进行以下相关研究：

1)设计轮式矿用水泵房巡检机器人模拟平台

根据水泵房巡检机器人上搭载的传感器尺寸与巡检机器人各零件的尺寸设计水泵房巡检机器人三维模型，并对巡检机器人整体进行静力学有限元分析，最终得到每个零件的应力和应变图，以此确定各个零件的选型是否合理。

2)机器人平台模型运动学和动力学分析

将三维模型导入ADAMS软件，通过对机器人平台模型进行运动学分析，从而确定机器人各零件在运动过程中有无冲突，进而确定零件的选型是否合理；通过对机器人平台进行动力学分析可得出机器人在行走、越障和爬楼梯过程中驱动力的变化，从而为机器人驱动轮电机功率的选择提供参考。

(2)地形感知方法研究

为了使机器人更好地路径规划，拟进行以下相关研究：

1)收集有关地形参数数据信息

水泵房地面与实验室内的地面状况有明显的区别，前者地面上存在水池、深井、线缆、落石、碎煤等各种阻碍，岩层运动会造成地面鼓起、塌陷、撕裂等。若地面凸起高度小于车体高度，该地面凸起不会被激光雷达检测到，巡检机器人会直接经过该路段，有可能发生撞车或翻车事故；若地面凹陷，巡检机器人在导航时经过该路段，有可能会卡在凹陷处，无法前进。因此，在复杂的地面环境下机器人要想自主行走并完成设备巡检工作，必须考虑机器人所处环境的地面状况信息，这是保证其运动安全性的前提。需要收集有关地形参数数据信息，具体分析凸起地面以及凹陷地面的形态特征。

2)研究实现路面危险等级自动识别方法和流程

①无人驾驶汽车地面感知

用于前方避障的激光雷达传感器向下倾斜15°安装在车辆前方保险杠，离地面1.2m，能够对前方4.5m范围内的巷道地面的状态。从图中可知激光整个扫射平面与路面的夹角为15°用于对前方物体识别的CCD摄像头传感器安装在车辆的顶部中央位置，离地距离1.6m，由于摄像头的视角为30°，为和激光雷达传感器进行联合识别，所以需要让摄像头向下倾斜17°安装，保证摄像头的识别点能够和激光雷达传感器感知点同步，用以对前方物体更好识别，与激光雷达识别路面点对应，能够更好针对车辆前4.5m内的地面障碍物识别。

②水泵房巡检机器人地面感知

参照无人驾驶汽车的地面感知方案，设计ROS巡检机器人的地面感知方案。 ROS巡检机器人上装有二维激光，该雷达无法像三维激光雷达一样对地面状况进行检测，因此可把二维激光雷达装载车体上部，对周围高于车体的障碍物建图，在巡检机器人车体前部装一个Kinect深度相机，用其探知地面的状况。

③复杂地面状况建图

一般的ROS中的算法没有考虑地面状况因素，所建立的地图只包含有高度大于ROS车体高度的障碍物信息，在进行路径规划时所参考指标只有路径最短、无障碍物。考虑到巡检机器人在执行路径的过程中可能会经过地面凸起和地面凹陷路段，造成翻车和卡车事故，因此，需要对复杂地面状况进行建图。

因此，在机器人建图的过程中，二维激光雷达负责建立周围的二维障碍物地图，安装在ROS巡检机器人前端的Kinect负责建立地面地图，通过平坦地面、路径最短、无障碍物这三个指标使机器人对行走区域的通过性进行评估，对可通过和不可通过区域进行划分，确保机器人路径规划得到最佳路径。

3)研究计算得到的地形信息感知的计算方法和实现

衡量地貌形态要从高度、地表倾斜方位和程度、地平面形状和面积三个方面进行考虑。机器人技术研究过程中，地形不仅是高低起伏的状态，还要考虑地面材质等内容。地形特征的表现包括地形起伏度、地形纹理、地形坡度等内容，这三方面的内容可以有效表现地形信息特征。地形信息主要采用数字高程模型DEM来表示，该模型主要以规则的栅格为主，各个栅格都具有栅格区域对应的平均高程。以DEM作为基础划分栅格，由此可知，D取值受到地形坡度 S(打滑约束)、起伏度H(步高及障碍边缘约束)以及粗糙度R(机体稳定约束)影响较为显著，通常情况下，数值计算使用3×3DEM栅格，具体情况如图5所示，其中е₀至е₈为这一区域栅格的高度值。

(3)激光雷达数据预处理研究

为了去除激光雷达运动畸变，拟进行以下相关研究：

1)高精度和高适应性的滤波算法研究

激光雷达在采集二维点云数据的过程中，因受到干扰影响，所以在获取数据时，会出现一些噪声。因此在实际工作中除了自身测量的误差外，还会受到外界环境的影响如被测目标被遮挡，障碍物与被测目标表面材质等影响因素，另外，一些局部大尺度噪声由于距离目标点云较远，无法使用同一种方法对其进行滤波，因此必须选择一种高精度和高适应性的滤波算法对二维点云数据中进行滤波。

2)里程计辅助方法与卡尔曼滤波结合算法研究

用单片机读取激光雷达数据，每次读取激光点数据时都可以获取当时机器人的位姿，根据机器人的位姿消除运动畸变，得到一帧完整数据后，上传至处理器。在单片机层消除运动畸变，无需考虑时间同步问题，需要对数据进行压缩，否则会产生较大延迟。

求解当前帧激光数据中每一个激光点对应的机器人位姿，即求解 {t_s，t_s+Δt，…t_e}时刻的机器人位姿，根据求解的位姿把所有激光点转换到同一坐标系下重新封装成一帧激光数据，发布出去。设p_s和p_s+1之间有N个位姿 {p_s，p_s1，…，p_s(n-2)，p_sn，p_s+1}则：一帧激光数据n个激光点，每个激光点对应的位姿{p₁，p₂，…，p_n}，通过上述介绍的方法插值得到xi为转化之前的坐标，x′_i为转化之后的坐标，则

把转换之后的坐标转换为激光数据发布出去：

(4)ROS系统中SLAM建图和导航算法优化设计

1)研究机器人的位姿、自然特征以及人工特征之间约束与全局优化，以期获得具有全局一致性的地图

激光雷达可以提取角点、线段、圆弧等特征作为自然信标用于机器人的定位导航。在各种几何元素构成的特征中，线段特征是较为简单且易识别的一类特征，它能有效的描述结构化环境。提取激光雷达扫描数据中的自然特征之后，对相邻时刻扫描数据中的特征进行匹配求解特征之间的坐标变换关系，就可以增量式地确定机器人的位置，从而实现机器人的定位目的。除了可以提取环境中存在的点、线段等自然特征进行数据关联外，激光SLAM的前端算法也可以提取人工特征进行数据关联。人工特征是人为布置在环境中的一些标志物，也被称为人工信标。相比较自然特征，人工特征具有易提取和易匹配的特点，可以适用于复杂的非结构化环境中。当机器人的工作环境中自然特征不够丰富时，可以在环境中布置一些人工信标，SLAM算法通过提取人工特征进行定位导航，可以有效提高SLAM算法的鲁棒性。

2)研究融入启发信息的路径规划算法，以期获得指导搜索方向，减少全局路径规划的时间；通过启发式搜索A*算法，评估代价值以搜索最近的路径。对于任意一个点的代价值，有F＝G+H；F表示通往目标点的代价，G表示从起始点移动到该点的距离，H则表示从该点到目标点的距离，距离的算法是采用曼哈顿距离，即从当前格子到目的格子之间水平和垂直的方格的数量总和。

2、目标

(1)通过研究建图算法(SLAM)，可实现巡检机器人在水泵房内准确定位，在此基础上建立出水泵房的二维栅格地图。

(2)通过研究路径规划算法(PRM)，可实现在建立好二维栅格地图的水泵房内规划出一条经过所有水泵房设备的路径。

(3)通过研究路径跟踪算法(Pure Persui)，可实现控制机器人按照规划好的路径运动，同时在规划的路径上突然出现障碍物时能及时避障。

3、拟解决的关键问题

(1)建图问题

①矿用水泵房巡检机器人遇到恶劣的自然条件和复杂的地质环境，例如巷道经常有积水、线缆、落石、碎煤等各种阻拦物；巷道表面坡度大等，因此巡检机器人如何实现对周围的障碍物建立二维地图，同时还要建立崎岖地形的二维地图，是本发明拟解决的关键问题之一。

②机器人在行走过程中难免会遇到斜坡路段，由于二维激光雷达是针对空间中的一个水平面上的障碍物建图，因此这个斜坡视为障碍物，造成机器人无法通过该斜坡路段，因此巡检机器人如何把激光雷达数据(二维)和深度摄像头数据(三维)结合建立斜坡的二维环境地图，是本发明拟解决的关键问题之二。

(2)自主导航问题

①巡检机器人上装有摄像头或红外热成像仪对设备进行观测，观测视野内电气设备的大小决定了数据采集精度的好坏，因此巡检机器人在进行全局路径规划时不仅只体现出躲避障碍物，行走路径最短，用时最少等这些特点，还需要选取适当的观测距离和观测角来确定每个电气设备合适的观测范围，是本发明拟解决的关键问题之三。

②巡检机器人在保证巡检安全的前提下，从巡检的起点开始，躲避地图中的静态障碍物，安全到达目标点，由于厂房内一般会有多个设备，则会有多个目标点，因此巡检机器人如何解决阶段性路径规划问题，即把刚到达的目标点作为新的起点，重新规划出新的路径到达下一个目标点，是本发明拟解决的关键问题之四。

③机器人在执行全局路径规划时，经过危险地段的过程中难免会发生侧翻情况，此时机器人的巡检工作便会因为侧翻而终止，因此机器人如何在无人干预的情况下自动翻转到正常状态，继续进行导航完成巡检工作是本发明拟解决的关键问题之五。

四、方法、技术路线、实验方案及可行性分析

1、方法

本发明拟将理论分析、算法设计、试验研究与仿真模拟相结合的研究方法进行，以解决将目前ROS中的定位和导航算法应用于煤矿井下设备巡检时所遇到的关键问题。根据ROS巡检机器人的组成结构，根据工作流程及实时环境，完善主要研究内容，并建立ROS机器人的三维模型，对模型的物理参数进行设置，并结合Matlab和Gazebo实现算法的仿真，找出适用于煤矿井下水泵房巡检机器人巡检的最优算法。

2、实验方案

(1)水泵房巡检机器人平台运动学和动力学分析

根据水泵房巡检机器人的主要技术指标和参数，设计轮式煤矿水泵房巡检机器人模拟平台，对机器人平台模型进行运动学和动力学分析。包括三维模型设计、运动学模型建立、动力学模型建立。水泵房巡检机器人运动底盘三维模型如图8所示。

地形感知方法的研究，利用Kinect深度相机得到有关地形参数的数据信息，实现路面危险等级的自动识别。将计算得到的地形信息作为机器人的路径规划的主要指标。结合地形因素进行路径规划的效果如图9所示。

激光雷达数据的预处理，通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变。激光雷达运动畸变去除前后效果对比如图10所示。

ROS中SLAM建图和导航算法的优化，用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向，减少全局路径规划的时间；然后制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略等方面进行实验结果的对比分析，最后优化ROS 系统和单片机程序相关程序。图优化前后建图效果对比如图11所示。

4、可行性分析

4.1理论研究的可行性

在机器人本体方面通过对建立的水泵房巡检机器人三维模型模型进行静力学、运动学和动力学仿真，可以不断优化巡检机器人的运动特性，使之能够达到水泵房巡检机器人要求的性能指标，在算法方面本发明采用ROS中开源的 SLAM建图和导航算法，该算法已经被广泛应用到了室内扫地机器人上，若要将该算法用于煤矿井下水泵房设备巡检机器人，由于工作环境的改变必须对原始的SLAM建图和导航算法做适当调整。

同时，本发明设计科学合理，水泵房巡检机器人建图和导航算法的研究可以把水泵房巡检人员从日常简单、机械、重复性的工作中解放出来，便于人们集中精力处理更加重要和复杂的工作，对强化煤矿生产安全管理，真正实现减员增效、节能降耗具有重要意义。

本发明针对煤矿井下水泵房巡检机器人的SLAM建图和自主导航算法进行研究，主要创新点如下：

(1)建立基于Kinect机器视觉的地形感知系统，能够识别巡检机器人行进方向上的障碍物、坡度、坡向、粗糙度以及危险等级。

(2)基于巡检机器人的地形感知信息，建立以优化崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，同时保证巡检机器人行走路线安全的情况下，规划起点与终点的最短路径。

(3)采用线性最小二乘方法对里程计进行标定减小机器人里程计的累积误差，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后对地图进行全局优化，最终可获得全局一致性的地图。

(4)运用激光雷达通过SLAM算法建立水泵房的二维栅格地图，然后在栅格地图中通过路径规划算法得到一条经过所有水泵房设备的路径，最后通过路径跟踪算法控制机器人跟踪这条路径，到达每一个水泵房设备的位置。实现了煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

Claims (7)

1.一种煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，其特征在于，所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法包括：

建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；

建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径；

采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；

通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

2.如权利要求1所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，其特征在于，所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法还包括：进行水泵房巡检机器人平台运动学和动力学分析，进行地形感知，并对激光雷达数据的预处理，进行SLAM建图和导航算法的优化。

3.如权利要求2所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，其特征在于，所述进行水泵房巡检机器人平台运动学和动力学分析包括：根据水泵房巡检机器人的主要技术指标和参数，设计轮式煤矿水泵房巡检机器人平台，对机器人平台模型建立运动学模型以及动力学模型并进行运动学和动力学分析。

4.如权利要求2所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，其特征在于，所述进行地形感知包括：利用Kinect深度相机得到有关地形参数的数据信息，进行路面危险等级的自动识别；将计算得到的地形信息作为机器人的路径规划的主要指标，进行路径规划。

5.如权利要求2所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，其特征在于，所述进行激光雷达数据预处理包括：通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变。

6.如权利要求2所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法，其特征在于，所述进行SLAM建图和导航算法的优化包括：

首先，用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向，减少全局路径规划的时间；

其次，制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略方面进行实验结果的对比分析；

最后，优化ROS系统和单片机程序相关程序。

7.一种执行权利要求1～6任意一项所述煤矿井下水泵房巡检机器人SLAM建图、自主导航方法的系统，其特征在于，所述系统包括：

障碍物识别模块，用于建立基于Kinect机器视觉的地形感知系统，识别巡检机器人行进方向上的障碍物；

最短路径规划模块，用于建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，规划起点与终点的最短路径；

全局一致性地图获取模块，用于采用迭代最小二乘方法对里程计进行标定，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，对地图进行全局优化，得到全局一致性的地图；

设备巡检模块，用于通过SLAM建图和导航算法利用煤矿井下巡检机器人在无人控制，无轨道、线缆和GPS辅助的情况下对水泵房设备进行巡检。

Images