一种地形感知方法、系统、存储介质、计算机设备

1.本发明属于地形感知技术领域，尤其涉及一种地形感知方法、系统、存储介质、计算机设备。


背景技术：

2.目前，我国煤炭资源丰富，煤炭仍是我国主要能源。近年来，煤炭行业先后提出机械化换人、智能化无人等理念。2019年1月，国家煤矿安监局发布《煤矿机器人重点研发目录》，公布了煤炭企业、科研机构、制造企业重点研发应用的5类38种煤矿机器人，其中巡检机器人是安控类机器人的一种。
3.近年来，国家加大力度整治和规范煤矿的本质安全型管理，同时极力推进煤矿企业向“数字化、无人化”方向发展。任何行业，设备的巡检工作一般都依靠人工完成，不仅消耗大量的人力资源，工作流程也较为繁琐，特别是高危行业，巡检过程中可能存在风险，人员工作环境也会比较恶劣。煤矿井下主排水系统是煤炭生产中不可缺少的重要设备，其运行状况的好坏直接影响着矿山的正常生产和工人的生命安全，为保证水泵的正常运行，巡检人员要定期查看设备的运行情况，例如水泵盘根、水仓状况、电机以及水泵是否存在异常等情况。一般一个水泵房需要几个人来看管、操作，工作效率低。
4.随着科学技术的进步，在电力和石化行业中，轮式的巡检机器人、履带式巡检机器人或是轨道式的吊挂巡检机器人已经有了众多应用案例，移动机器人在巡检过程中对被检设备有很强的针对性巡检，工作质量甚至比人还要专业。
5.煤矿灾害频发，危害严重，风险较大，下井人员密集，危险岗位多，研发应用煤矿水泵房巡检机器人可代替巡检工人进行设备及环境巡检，能够有效的减轻工作人员的劳动强度，降低巡检过程中存在的安全隐患，提升巡检质量的同时，最大限度的提升煤矿企业的本质安全水平。对推动煤炭开采技术革命，实现煤炭工业高质量发展，推动煤矿智能化进程，国家能源安全供应具有重要意义。
6.国内外专家从机器人的应用环境出发，将机器人分为两大类，制造环境下的工业机器人和非制造环境下的特种机器人。国内外特种机器人的研发主要集中在探险救灾、军事防爆、侦察探测等领域。煤矿特殊工作环境以及高标准的移动、通讯、防爆性能要求，实现矿井内部特定功能的特种移动机器人的研发进程较缓慢。
7.(1)煤矿探测型机器人，上世纪90年代，澳大利亚联邦科工组织(csiro)就设计了煤矿探测型机器人numbat，测井下气体环境是这个机器人的主要工作。它通过可见光与红外图像传感器，传输井下信息回地面，将井下最新状态告知救援队员，以便救援方案的制定。这个机器人的动力来源是一个140ah的镍镉电池，行走依赖于两侧分别装配的750w电机组成的差速八轮结构，其尺寸为2.5
×
1.65m，速度最快可达每小时两千米，且有着8小时的续航能力。groundhog煤矿探测型机器人是美国卡耐基梅隆大学机器人研究所研制，这个机器人主要用于井下环境探测和井下三维地图绘制，搭载激光传感器、夜视摄像头、体感传感器和陀螺仪等。由液压驱动，四轮差速运转，带有原地转向功能。thrun等人在2003年5月30
日用其探测了宾夕法尼亚考特尼县的马蒂斯矿的主巷道，并完成三维构图。2005年，中国矿业大学救援与装备研究所成功研发出我国第一台煤矿探测型机器人cumt
‑
i，该机器人可以探测灾后现场的环境，搭配危险气体传感器，低照明度摄像机和双向语音对讲系统，以遥控方式受控，且能随机装载各类救灾物资(药物、实物或救援工具)，是救援队灾后搜救好帮手。2007年，哈尔滨工业大学机器人研究所与唐山开诚电器有限公司共同研发了煤矿探测型机器人，这个机器人能探测灾后现场的环境，搭配危险气体传感器、低照明度摄像机和双向语音对讲系统。其行走方式为履带式，履带分为驱动、摆臂、摆腿三个部分。支持遥控操作，通过遥感和控制按键来实现。
8.(2)煤矿救援机器人，ratler煤矿救援机器人由美国劳工部矿山安全与卫生管理局(msha)和桑迪亚智能系统与机器人中心(isrc)共同研发，主要用于灾后勘察，搭载了红外摄像机、陀螺仪和危险气体传感器，且可通过无线射频装置进行最大距离76米的遥控。在1998年12月发生的willow greek煤矿火灾救援中对这个机器人做适用性实验表明，其远不能达到煤矿业对救援机器人的标准。第二代移动煤矿救灾机器人平台cave crawler由卡耐基梅隆大学机器人研究所设计，该机器人内部使用了与“勇气号”火星探测车相似的齿轮差动机构，摇杆式左右轮移动系统，且左右摇杆与机器人主体通过差动机构连接，从而线性平均了两摇杆的摆角，进一步转化为机器人车身的摆角，从而为机器人主体保持平衡。某边车轮抬起时，整车的摆动幅度仅为车轮抬起幅度的一半，从而削弱了地形起伏对机器人运动的阻碍。此外，这样的设计均匀地将车体重量分配到机器人的各个车轮上，且车轮可根据地形调整其相对位置，使机器人运动更平稳、不易颠覆，有更强的越障能力。
9.(3)煤矿巡检机器人，周展主要研究了井下巷道悬线巡检机器人的机械本体，在对悬线巡检机器人的本体结构、自主运动和巡检过程控制要求进行细致分析的基础上深入研究了巡检机器人的控制系统。王志同等设计了一种能够自主延长通信距离并且自动回撤的煤矿探测机器人，以打破煤矿井下探测机器人收到工作行程的限制。在检测到无线控制信号削弱时，机器人搭载的中继器弹射系统能够运作以延长无线通信距离。若无线信号终止，机器人变回启动回撤程序，按照运行记录的传感器数据，通过原路径回撤，从而回到通信顺畅的安全位置。机器人运用编码器和激光传感器数据集成的方法来实现回撤程序，且依赖icp算法来双重匹配数据，修正机器人的坐标和朝向，达到足够的回撤精度。从实验结果看，这个机器人的鲜明特点是有着较长的工作行程和回撤能力，这意味着它能够在煤矿灾难现场安全性未知时，替代救援人员深入矿井，完成危险探测的初期工作。
10.姜俊英等设计了一种能摆脱井下巷道复杂环境和人员设备分布影响的煤矿巷道悬线巡检机器人，以解决现有煤矿巷道轮式或履带式机器人所具有的结构和控制复杂、可靠性低、成本高、占用空间多的问题。他们着重描述了这个机器人的机构设计，并在adams软件里建立虚拟样机的模型进行仿真分析，以评估其水平行走、上下坡途中三种运动状态下越过障碍物的能力。其结果显示该机器人在如上三种运动状态下都相对平稳，特别是在水平方向上机器人始终保持匀速直线运动，而在竖直和水平侧摆方向上会产生允许范围以内的波动。周明静等为解决井下皮带运输机的巡检、监控问题，设计了轨道式无人巡检装置，描述了该机器人的整体设计方案，能力特性以及软件。硬件开发途径。对煤矿井下皮带运输机、刮板运输机等大型设备的检查都可以通过这个巡检机器人来完成，真正从根本上做到无人值守，为巡检人员减负，在最大程度上提升了煤矿的安全管理。
11.崔鑫在对煤矿井下水泵房工人巡检出现的问题研究的基础上，设计出了一种能替代工人巡检的自动巡检机器人装置，着重描述了该机器人体系设计依据、构成及具备的作用，描述了该机器人的软硬件的实施、数据采集等方式，该体系能够实时的发现井下水泵房可能出现的问题，并保障其稳定、安全的运行。裴文等对煤矿井下水泵房关键实施巡检、监督产生问题研究下，设计了一种可靠性高、能够代替人工巡检、信息采集、视频巡检及障碍报告等多种功能于一体的智能机器人，其还描述了该机器人设备的构成及作用特征，还在研究结论中对该机器人在不同条件下的自动巡检功能进行了详细描述，从而为煤矿井下达成无人值班、自动化管理方式开启了新方式。
12.但是现有技术中存在建图问题和自主导航问题；矿用水泵房巡检机器人遇到恶劣的自然条件和复杂的地质环境，例如巷道经常有积水、线缆、落石、碎煤等各种阻拦物；巷道表面坡度大等，因此巡检机器人如何实现对周围的障碍物建立二维地图，同时还要建立崎岖地形的二维地图。
13.机器人在行走过程中难免会遇到斜坡路段，由于二维激光雷达是针对空间中的一个水平面上的障碍物建图，因此这个斜坡视为障碍物，造成机器人无法通过该斜坡路段，因此巡检机器人如何把激光雷达数据(二维)和深度摄像头数据(三维)结合建立斜坡的二维环境地图。
14.自主导航问题：巡检机器人上装有摄像头或红外热成像仪对设备进行观测，观测视野内电气设备的大小决定了数据采集精度的好坏，因此巡检机器人在进行全局路径规划时不仅只体现出躲避障碍物，行走路径最短，用时最少等这些特点，还需要选取适当的观测距离和观测角来确定每个电气设备合适的观测范围。
15.巡检机器人在保证巡检安全的前提下，从巡检的起点开始，躲避地图中的静态障碍物，安全到达目标点，由于厂房内一般会有多个设备，则会有多个目标点，因此巡检机器人如何解决阶段性路径规划问题，即把刚到达的目标点作为新的起点，重新规划出新的路径到达下一个目标点。
16.机器人在执行全局路径规划时，经过危险地段的过程中难免会发生侧翻情况，此时机器人的巡检工作便会因为侧翻而终止，因此机器人如何在无人干预的情况下自动翻转到正常状态，继续进行导航完成巡检工作。
17.通过上述分析，现有技术存在的问题及缺陷为：
18.(1)巡检机器人如何实现对周围的障碍物建立二维地图，同时还要建立崎岖地形的二维地图。
19.(2)巡检机器人如何把激光雷达数据(二维)和深度摄像头数据(三维)结合建立斜坡的二维环境地图。
20.(3)巡检机器人在进行全局路径规划时不仅只体现出躲避障碍物，行走路径最短，用时最少等这些特点，还需要选取适当的观测距离和观测角来确定每个电气设备合适的观测范围。
21.(4)巡检机器人如何解决阶段性路径规划问题，即把刚到达的目标点作为新的起点，重新规划出新的路径到达下一个目标点。
22.(5)机器人如何在无人干预的情况下自动翻转到正常状态，继续进行导航完成巡检工作。
23.解决以上问题及缺陷的难度为：当下，大多数巡检机器人采用雷达技术勘测周围情况，但这种探测手段只能适用于较大，较高，且静止或移动速度缓慢的障碍，在遇到障碍物多且移动快的障碍(行人)和较低较小的障碍(一个小坑，小洼)就无能为力了。但如果将图像技术应用到路况识别中，这些问题无疑都会迎刃而解，因为视觉相对激光测距来说更全面，更直观。
24.水泵房环境地面上会有蓄水池、深井、线缆、落石、碎煤等各种阻碍，岩层运动会造成地面鼓起、塌陷、撕裂等。该地面状况不会被激光雷达检测到，巡检机器人直接通过该路段，有可能发生撞车或翻车事故。在复杂的地面环境下机器人要想自主行走并完成设备的巡检工作，必须检测机器人前进路况信息，这是保证其运动安全性的前提。


技术实现要素：

25.针对现有技术存在的问题，本发明提供了一种地形感知方法、系统、存储介质、计算机设备。
26.本发明是这样实现的，一种地形感知方法，所述地形感知方法，包括：
27.步骤一，利用kinect深度相机获得路面状况的深度信息，建立数字高程模型(dem)，以此为基础划分栅格，将地形信息危险等级值进行划分，实现路面危险等级的自动识别；
28.步骤二，通过里程计辅助方法与卡尔曼滤波算法结合，首先用里程计数据对机器人的位姿进行传播，根据激光雷达数据与环境地图的匹配实现位姿误差修正；
29.步骤三，ros中slam建图和导航算法的优化。
30.进一步，所述步骤一中，将计算得到的地形信息作为机器人的路径规划的主要指标。
31.进一步，所述步骤三中，ros中slam建图和导航算法的优化，具体过程为：
32.用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向；然后制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略等方面进行实验结果的对比分析，最后优化ros系统和单片机程序相关程序。
33.本发明的另一目的在于提供一种实施所述地形感知方法的地形感知系统，所述地形感知系统设置有：
34.主体；
35.主体两侧设置有滚轮，滚轮上套接有履带；主体前侧安装有激光雷达和kinect相机。
36.本发明的另一目的在于提供一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行所述地形感知方法包括下列步骤：
37.步骤一，利用kinect深度相机得到有关地形参数的数据信息，实现路面危险等级的自动识别；
38.步骤二，通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变；
39.步骤三，ros中slam建图和导航算法的优化。
40.本发明的另一目的在于提供一种存储在计算机可读介质上的计算机程序产品，包
括计算机可读程序，供于电子装置上执行时，提供用户输入接口以实施所述的地形感知方法。
41.结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明中机器人全局定位在x轴和y轴上距离误差小于10cm，角度误差小于0.2rad；栅格地图与真实环境尺寸误差小于1cm；机器人的真实导航路径与算法估计路径的偏离程度小于10cm；移动机器人在结构相似的环境中建图时可进行回环；预处理后激光雷达采集的点云数据的方差小于0.1。
42.本发明建立地形感知系统，能够识别巡检机器人行进方向上的障碍物；建立崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，同时保证巡检机器人行走路线安全的情况下，规划起点与终点的最短路径；采用迭代最小二乘方法对里程计进行标定减小机器人里程计的累积误差，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后对地图进行全局优化，最终可获得全局一致性的地图；通过slam建图和导航算法实现煤矿井下巡检机器人在无人控制，无轨道、线缆和gps辅助的情况下对水泵房设备进行巡检工作。
附图说明
43.图1是本发明实施例提供的地形感知方法流程图。
44.图2是本发明实施例提供的地形感知系统结构示意图。
45.图2中：1、主体；2、滚轮；3、履带；4、激光雷达；5、kinect相机。
46.图3是本发明实施例提供的无人驾驶汽车地面感知示意图。
47.图4是本发明实施例提供的水泵房巡检机器人地面感知示意图。
48.图5是本发明实施例提供的无地面感知时ros算法的路径规划指标示意图。
49.图6是本发明实施例提供的存在地面感知时ros算法的路径规划指标示意图。
50.图7是本发明实施例提供的地形信息示意图。
51.图8是本发明实施例提供的slam建图中机器人位姿的约束示意图。
52.图9是本发明实施例提供的a*最短路径搜索算法原理示意图。
53.图10是本发明实施例提供的技术路线框图。
54.图11是本发明实施例提供的结合地形因素进行路径规划的效果示意图。
55.图12是本发明实施例提供的激光雷达运动畸变去除前后效果对比示意图。
56.图13是本发明实施例提供的优化前后建图效果对比示意图。
57.图14是本发明实施例提供的prm路径规划算法仿真结果示意图。
58.图15是本发明实施例提供的prm路径规划算法生成的路径示意图。
59.图16是本发明实施例提供的pure persuit路径跟踪算法仿真结果示意图。
60.图17是本发明实施例提供的pure persuit算法跟踪路径过程分析示意图。
61.图18是本发明实施例提供的避障算法仿真结果示意图。
62.图19是本发明实施例提供的避障算法过程分析示意图。
具体实施方式
63.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明
进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
64.针对现有技术存在的问题，本发明提供了一种地形感知方法、系统、存储介质、计算机设备，下面结合附图对本发明作详细的描述。
65.本发明提供的地形感知方法业内的普通技术人员还可以采用其他的步骤实施，图1的本发明提供的地形感知方法仅仅是一个具体实施例而已。
66.如图1所示，本发明实施例提供的地形感知方法，包括：
67.s101：利用kinect深度相机得到有关地形参数的数据信息，实现路面危险等级的自动识别。
68.s102：通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变。
69.s103：ros中slam建图和导航算法的优化。
70.本发明实施例提供的s101中，将计算得到的地形信息作为机器人的路径规划的主要指标。
71.本发明实施例提供的s103中，ros中slam建图和导航算法的优化，具体过程为：
72.用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向，减少全局路径规划的时间；然后制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略等方面进行实验结果的对比分析，最后优化ros系统和单片机程序相关程序。
73.如图2所示，本发明实施例提供的地形感知系统中主体1两侧设置有滚轮2，滚轮2上套接有履带3；主体1前侧安装有激光雷达4和kinect相机5。
74.下面结合具体实施例对本发明的技术方案作进一步的描述。
75.1、机器人的导航问题，就是将移动机器人从起点引导到目标点的技术和方法。近年来，国内外学者把导航问题总结为三个问题：(1)定位问题；(2)目标的识别问题；(3)路径规划问题。
76.定位问题作为移动机器人实现自主能力的最基本问题，是机器人导航问题的核心。按照定位方式不同一般分为以下三类：
77.(1)相对定位。又叫航迹推算(dead
‑
reckoning，dr)，主要依靠编码器、陀螺仪、加速度计等本体传感器，根据初始状态信息，通过测量相对距离、角速度、线加速度等信息推算移动机器人当前时刻位置和姿态等信息；
78.(2)绝对定位。绝对定位主要常用rfid、gps等定位传感器，原理是利用导航信标、地图匹配或卫星定位等技术方法确定机器人的绝对位置信息；
79.(3)组合定位。相对定位是根据机器人运动学模型和内部传感器进行自主定位，信号不受外界干扰，但误差随时间而累计。
80.移动机器人导航的发展趋势主要有以下几方面：
81.(1)实时、精确以及稳定的导航方式。常用的导航方式有很多，但是各自都有着一定的局限性。因此在改善传感器精度的同时，添加一些智能算法应用于导航任务中。
82.(2)多种导航方式配合使用。单一的导航技术往往存在其固有的局限性，既要完善单一导航方式，同时也要综合的运用多种导航方式相结合，采用多种传感器，相互弥补各自
的缺点，是导航系统具有更加稳定，可以在更加复杂多变的环境中完成导航任务。
83.(3)随着网络技术和无线技术的成熟，可以利用网络和无线技术，实现对机器人的远程控制，这也是移动机器人导航技术的一个发展方向。
84.(4)系统的模块化问题。由于导航系统可以分成不同层次的若干模块，为了达到一定程度的统一性和通用性，研究导航的模块化及其接口统一的问题，也是今后研究的重点。
85.2、内容
86.(1)移动机器人平台运动学和动力学分析
87.根据已有的矿用水泵房巡检机器人主要技术指标和参数的研究基础，首先对巡检机器人进行以下相关研究。
88.1)设计轮式矿用水泵房巡检机器人模拟平台
89.根据水泵房巡检机器人上搭载的传感器尺寸与巡检机器人各零件的尺寸设计水泵房巡检机器人ug三维模型，并对巡检机器人整体进行静力学有限元分析，最终得到每个零件的应力和应变图，以此确定各个零件的选型是否合理。
90.2)机器人平台模型运动学和动力学分析
91.将ug三维模型导入adams软件，通过对机器人平台模型进行运动学分析，从而确定机器人各零件在运动过程中有无冲突，进而确定零件的选型是否合理；通过对机器人平台进行动力学分析可得出机器人在行走、越障和爬楼梯过程中驱动力的变化，从而为机器人驱动轮电机功率的选择提供参考。
92.(2)地形感知方法
93.为了使机器人更好地路径规划，拟进行以下相关研究：
94.1)收集有关地形参数数据信息
95.水泵房地面与实验室内的地面状况有明显的区别，前者地面上存在蓄水池、深井、线缆、落石、碎煤等各种阻碍，岩层运动会造成地面鼓起、塌陷、撕裂等。若地面凸起高度小于车体高度，该地面凸起不会被激光雷达检测到，巡检机器人会直接经过该路段，有可能发生撞车或翻车事故；若地面凹陷，巡检机器人在导航时经过该路段，有可能会卡在凹陷处，无法前进。因此，在复杂的地面环境下机器人要想自主行走并完成设备巡检工作，必须考虑机器人所处环境的地面状况信息，这是保证其运动安全性的前提。需要收集有关地形参数数据信息，具体分析凸起地面以及凹陷地面的形态特征。
96.2)研究实现路面危险等级自动识别方法和流程
97.①
无人驾驶汽车地面感知
98.用于前方避障的激光雷达传感器向下倾斜15
°
安装在车辆前方保险杠，离地面1.2m，能够对前方4.5m范围内的巷道地面的状态。从图中可知激光整个扫射平面与路面的夹角为15
°
用于对前方物体识别的ccd摄像头传感器安装在车辆的顶部中央位置，离地距离1.6m，由于摄像头的视角为30
°
，为和激光雷达传感器进行联合识别，所以需要让摄像头向下倾斜17
°
安装，保证摄像头的识别点能够和激光雷达传感器感知点同步，用以对前方物体更好识别，与激光雷达识别路面点对应，能够更好针对车辆前4.5m内的地面障碍物识别，如图3所示。
99.②
水泵房巡检机器人地面感知
100.参照无人驾驶汽车的地面感知方案，设计ros巡检机器人的地面感知方案。ros巡
检机器人上装有二维激光，该雷达无法像三维激光雷达一样对地面状况进行检测，因此可把二维激光雷达装载车体上部，对周围高于车体的障碍物建图，在巡检机器人车体前部装一个kinect深度相机，用其探知地面的状况，如图4所示。
101.③
复杂地面状况建图
102.如图5所示，一般的ros中的算法没有考虑地面状况因素，所建立的地图只包含有高度大于ros车体高度的障碍物信息，在进行路径规划时所参考指标只有路径最短、无障碍物。考虑到巡检机器人在执行路径的过程中可能会经过地面凸起和地面凹陷路段，造成翻车和卡车事故，因此，需要对复杂地面状况进行建图。
103.因此，在机器人建图的过程中，二维激光雷达负责建立周围的二维障碍物地图，安装在ros巡检机器人前端的kinect负责建立地面地图，通过平坦地面、路径最短、无障碍物这三个指标使机器人对行走区域的通过性进行评估，对可通过和不可通过区域进行划分，确保机器人路径规划得到最佳路径，如图5所示。
104.3)研究计算得到的地形信息感知的计算方法和实现
105.衡量地貌形态要从高度、地表倾斜方位和程度、地平面形状和面积三个方面进行考虑。机器人技术研究过程中，地形不仅是高低起伏的状态，还要考虑地面材质等内容。由此会引发机器人避障、路径规划等问题研究。地形特征的表现包括地形起伏度、地形纹理、地形坡度等内容，这三方面的内容可以有效表现地形信息特征。地形信息主要采用数字高程模型dem来表示，该模型主要以规则的栅格为主，各个栅格都具有栅格区域对应的平均高程。以dem作为基础划分栅格，由此可知，d取值受到地形坡度s(打滑约束)、起伏度h(步高及障碍边缘约束)以及粗糙度r(机体稳定约束)影响较为显著，通常情况下，数值计算使用3
×
3dem栅格，具体情况如图7所示，其中е0至е8为这一区域栅格的高度值。
106.(3)激光雷达数据预处理研究
107.为了去除激光雷达运动畸变，拟进行以下相关研究：
108.1)高精度和高适应性的滤波算法研究
109.激光雷达在采集二维点云数据的过程中，因受到干扰影响，所以在获取数据时，会出现一些噪声。因此在实际工作中除了自身测量的误差外，还会受到外界环境的影响如被测目标被遮挡，障碍物与被测目标表面材质等影响因素，另外，一些局部大尺度噪声由于距离目标点云较远，无法使用同一种方法对其进行滤波，因此必须选择一种高精度和高适应性的滤波算法对二维点云数据中进行滤波。
110.2)里程计辅助方法与卡尔曼滤波结合算法研究
111.用单片机读取激光雷达数据，每次读取激光点数据时都可以获取当时机器人的位姿，根据机器人的位姿消除运动畸变，得到一帧完整数据后，上传至处理器。在单片机层消除运动畸变，无需考虑时间同步问题，需要对数据进行压缩，否则会产生较大延迟。
112.求解当前帧激光数据中每一个激光点对应的机器人位姿，即求解{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个激光点，每个激光点对应的位姿{p1，p2，
…
，p
n
}，通过上述介绍的方法插值得到xi为转化之前的坐标，x
′
i
为转化之后的坐标，则把转换之后的坐标转换为激光数据发布出去：
[0113][0114]
(4)ros系统中slam建图和导航算法优化设计
[0115]
1)研究机器人的位姿、自然特征以及人工特征之间约束与全局优化，以期获得具有全局一致性的地图
[0116]
如图8所示，激光雷达可以提取角点、线段、圆弧等特征作为自然信标用于机器人的定位导航。在各种几何元素构成的特征中，线段特征是较为简单且易识别的一类特征，它能有效的描述结构化环境。提取激光雷达扫描数据中的自然特征之后，对相邻时刻扫描数据中的特征进行匹配求解特征之间的坐标变换关系，就可以增量式地确定机器人的位置，从而实现机器人的定位目的。除了可以提取环境中存在的点、线段等自然特征进行数据关联外，激光slam的前端算法也可以提取人工特征进行数据关联。人工特征是人为布置在环境中的一些标志物，也被称为人工信标。相比较自然特征，人工特征具有易提取和易匹配的特点，可以适用于复杂的非结构化环境中。当机器人的工作环境中自然特征不够丰富时，可以在环境中布置一些人工信标，slam算法通过提取人工特征进行定位导航，可以有效提高slam算法的鲁棒性。
[0117]
2)研究融入启发信息的路径规划算法，以期获得指导搜索方向，减少全局路径规划的时间
[0118]
通过启发式搜索a*算法，评估代价值以搜索最近的路径。对于任意一个点的代价值，有f＝g+h；f表示通往目标点的代价，g表示从起始点移动到该点的距离，h则表示从该点到目标点的距离，距离的算法是采用曼哈顿距离，即从当前格子到目的格子之间水平和垂直的方格的数量总和。a*最短路径搜索算法原理如图9所示。
[0119]
3、目标
[0120]
(1)通过研究建图算法(slam)，可实现巡检机器人在水泵房内准确定位，在此基础上建立出水泵房的二维栅格地图。
[0121]
(2)通过研究路径规划算法(prm)，可实现在建立好二维栅格地图的水泵房内规划出一条经过所有水泵房设备的路径。
[0122]
(3)通过研究路径跟踪算法(pure persui)，可实现控制机器人按照规划好的路径运动，同时在规划的路径上突然出现障碍物时能及时避障。
[0123]
4、拟解决的关键问题
[0124]
(1)建图问题
[0125]
①
矿用水泵房巡检机器人遇到恶劣的自然条件和复杂的地质环境，例如巷道经常有积水、线缆、落石、碎煤等各种阻拦物；巷道表面坡度大等，因此巡检机器人如何实现对周围的障碍物建立二维地图，同时还要建立崎岖地形的二维地图，是本发明拟解决的关键问题之一。
[0126]
②
机器人在行走过程中难免会遇到斜坡路段，由于二维激光雷达是针对空间中的一个水平面上的障碍物建图，因此这个斜坡视为障碍物，造成机器人无法通过该斜坡路段，因此巡检机器人如何把激光雷达数据(二维)和深度摄像头数据(三维)结合建立斜坡的二维环境地图，是本发明拟解决的关键问题之二。
[0127]
(2)自主导航问题
[0128]
①
巡检机器人上装有摄像头或红外热成像仪对设备进行观测，观测视野内电气设备的大小决定了数据采集精度的好坏，因此巡检机器人在进行全局路径规划时不仅只体现出躲避障碍物，行走路径最短，用时最少等这些特点，还需要选取适当的观测距离和观测角来确定每个电气设备合适的观测范围，是本发明拟解决的关键问题之三。
[0129]
②
巡检机器人在保证巡检安全的前提下，从巡检的起点开始，躲避地图中的静态障碍物，安全到达目标点，由于厂房内一般会有多个设备，则会有多个目标点，因此巡检机器人如何解决阶段性路径规划问题，即把刚到达的目标点作为新的起点，重新规划出新的路径到达下一个目标点，是本发明拟解决的关键问题之四。
[0130]
③
机器人在执行全局路径规划时，经过危险地段的过程中难免会发生侧翻情况，此时机器人的巡检工作便会因为侧翻而终止，因此机器人如何在无人干预的情况下自动翻转到正常状态，继续进行导航完成巡检工作是本发明拟解决的关键问题之五。
[0131]
5、方法、技术路线、实验方案及可行性分析
[0132]
5.1方法
[0133]
本发明拟将理论分析、算法设计、试验研究与仿真模拟相结合的研究方法进行，以解决将目前ros中的定位和导航算法应用于煤矿井下设备巡检时所遇到的关键问题。根据ros巡检机器人的组成结构，根据工作流程及实时环境，完善主要研究内容，并建立ros机器人的三维模型，对模型的物理参数进行设置，并结合matlab和gazebo实现算法的仿真，找出适用于煤矿井下水泵房巡检机器人巡检的最优算法。
[0134]
5.2技术路线，如图10所示；
[0135]
5.3实验方案
[0136]
(1)水泵房巡检机器人平台运动学和动力学分析
[0137]
根据水泵房巡检机器人的主要技术指标和参数，设计轮式煤矿水泵房巡检机器人模拟平台，对机器人平台模型进行运动学和动力学分析。包括三维模型设计、运动学模型建立、动力学模型建立。水泵房巡检机器人运动底盘三维模型如图2所示。
[0138]
(2)地形感知方法的研究
[0139]
利用kinect深度相机得到有关地形参数的数据信息，实现路面危险等级的自动识别。将计算得到的地形信息作为机器人的路径规划的主要指标。结合地形因素进行路径规划的效果如图11所示。
[0140]
(3)激光雷达数据的预处理
[0141]
通过里程计辅助方法与卡尔曼滤波算法结合，去除激光雷达运动畸变。激光雷达运动畸变去除前后效果对比如图12所示。
[0142]
(4)ros中slam建图和导航算法的优化
[0143]
用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后进行全局优化，获得具有全局一致性的地图；并在路径规划算法中加入启发信息，指导搜索方向，减少全局路径规划的时间；然后制定测试环境设计和性能评价标准，并针对改进后的算法和原始算法在建图效率和导航策略等方面进行实验结果的对比分析，最后优化ros系统和单片机程序相关程序。图优化前后建图效果对比如图13所示。
[0144]
6、可行性分析
[0145]
6.1理论研究的可行性
[0146]
在机器人本体方面通过对建立的水泵房巡检机器人三维模型模型进行静力学、运动学和动力学仿真，可以不断优化巡检机器人的运动特性，使之能够达到水泵房巡检机器人要求的性能指标，在算法方面本项目采用ros中开源的slam建图和导航算法，该算法已经被广泛应用到了室内扫地机器人上，若要将该算法用于煤矿井下水泵房设备巡检机器人，由于工作环境的改变必须对原始的slam建图和导航算法做适当调整。
[0147]
同时，本发明设计科学合理，水泵房巡检机器人建图和导航算法的研究可以把水泵房巡检人员从日常简单、机械、重复性的工作中解放出来，便于人们集中精力处理更加重要和复杂的工作，对强化煤矿生产安全管理，真正实现减员增效、节能降耗具有重要意义。
[0148]
6.2实践研究的可行性
[0149]
矿山流体控制国家地方联合工程实验室，具有多年从事工业自动化的经验，已经配备多种机电设备检验试验条件。现有的设备有ni，可以测试和测量，提供模块化硬件平台和系统设计软件(labview)。可以用来进行实验满足整体的要求。
[0150]
7、针对煤矿井下水泵房巡检机器人的slam建图和自主导航算法进行研究，主要创新点如下：
[0151]
(1)建立地形感知系统，能够识别巡检机器人行进方向上的障碍物、坡度、坡向、粗糙度以及危险等级。
[0152]
(2)基于巡检机器人的地形感知信息，建立以优化崎岖地面路径长度以及机器人通过路面时危险等级为指标的运动规划模型，同时保证巡检机器人行走路线安全的情况下，规划起点与终点的最短路径。
[0153]
(3)采用线性最小二乘方法对里程计进行标定减小机器人里程计的累积误差，同时使用图优化的方式在机器人的位姿、自然特征以及人工特征之间建立约束，然后对地图进行全局优化，最终可获得全局一致性的地图。
[0154]
(4)运用激光雷达通过slam算法建立水泵房的二维栅格地图，然后在栅格地图中通过路径规划算法得到一条经过所有水泵房设备的路径，最后通过路径跟踪算法控制机器人跟踪这条路径，到达每一个水泵房设备的位置。实现了煤矿井下巡检机器人在无人控制，无轨道、线缆和gps辅助的情况下对水泵房设备进行巡检工作。
[0155]
下面结合仿真实验对本发明的技术方案作进一步的描述。
[0156]
1、工作基础
[0157]
已经完成了水泵房巡检机器人样机的结构设计，同时在matlab中完成了路径规划算法、路径跟踪算法和避障算法的仿真，下一步将完成slam建图算法的仿真。
[0158]
1.1 prm路径规划算法仿真
[0159]
在进行路径规划前，首先根据机器人的尺寸对水泵房二维栅格地图进行膨胀，以防止机器人在进行路径跟踪时与转角处的障碍物发生碰撞，然后将膨胀后的地图给prm路径规划算法，并设置规划路径的起点(1.2，2.4)，终点(2.7，3.3)，prm路径规划算法生成的随机点数目为800个，prm路径规划算法的仿真结果如图14所示。在图15中可看到该路径是由一系列坐标点连线组成，这些坐标点在接下来的路径跟踪中有重要作用。
[0160]
1.2 pure persuit路径跟踪算法仿真
[0161]
在进行路径跟踪前要在matlab中生成一个两轮差速机器人模型，设置机器人的直径为0.2m，该机器人可通过里程计得到任意时刻世界坐标系下的位姿，该位姿可以判断机
器人是否到达了目标点；再把点4到点6的规划路径存储到pure persuit路径跟踪算法中，点4到点6的规划路径由一些坐标点连线组成，这些坐标点是由prm算法生成的，他们是pure persuit算法需要跟踪的目标点；设置机器人进行路径跟踪时的线速度0.1m/s、最大角速度1rad/s，距离阈值为0.01m，执行pure persuit算法得到的仿真结果如图16所示。
[0162]
图17为pure persuit路径跟踪算法控制机器人跟踪路径4和路径5时各个时刻的姿态角、角速度和线速度的变化，在区间1内机器人处于起始点，位姿(x，y，theta)＝(1.2，2.4，0)，原始路径姿态角(原始路径与世界坐标系x轴间夹角)大于机器人的姿态角(机器人坐标系的x轴与世界坐标系的x轴之间的夹角)，因此pure persuit路径跟踪控制器给出的机器人旋转角速度大于0，机器人逆时针方向旋转，机器人的姿态角增加，在区间2内机器人的姿态角与原始路径姿态角相等，pure persuit路径跟踪控制器给出的机器人旋转角速度等于0，线速度为0.1m/s保持不变，机器人将沿着原始路径移动。在区间3内由于原始路径的姿态角进一步增大，因此pure persuit路径跟踪控制器给出的机器人旋转角速度大于0，机器人逆时针方向旋转，机器人的姿态角也增大。在区间4内机器人的姿态角与原始路径姿态角相等，pure persuit路径跟踪控制器给出的机器人旋转角速度等于0，线速度仍保持不变，机器人沿着原始路径移动。同理在区间5内也发生了原始路径姿态角的改变，同时pure persuit路径跟踪控制器也给出的机器人不同的旋转角速度，使机器人的姿态角最终与原始路径的姿态角相等。在区间6内机器人到达原始路径的最终点，此时线速度和角速度都为0，机器人的最终的姿态角仍和原始路径姿态角相等。
[0163]
1.3避障算法仿真
[0164]
机器人避障是指机器人在进行路径跟踪时，如果激光雷达检测到机器人前方存在障碍物，就运行避障算法控制机器人绕开该障碍物，然后继续跟踪原始路径。通过pure persuit路径跟踪算法跟踪路径2和路径3，并在路径2上设置一个障碍物用来进行避障算法的仿真。仿真结果如图18所示。
[0165]
图19为机器人在避障过程中各个时刻的机器人的姿态角、机器人前方、右方和左方距障碍物距离的变化，设置距离阈值为0.1m，打开激光雷达传感器，通过分析激光雷达传感器的数据可得到机器各个方位距障碍物的距离，当该距离大于距离阈值时，机器人处于路径跟踪阶段，此时路径跟踪算法控制机器人沿原始路径移动，当机器人前方或左方距障碍物的距离小于距离阈值时，机器人处于避障阶段，开始执行避障算法并停止执行路径跟踪算法，由于机器人右方距障碍物的距离明显大于前方和左方，机器人右转(顺时针旋转)，机器人的姿态角减小了，机器人前进一段距离后左转，检测到机器人各个方位距障碍物的距离已经大于距离阈值，说明机器人已经绕过障碍物，机器人回到路径跟踪阶段，停止避障算法并继续执行路径跟踪算法。
[0166]
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、cd或dvd
‑
rom的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编
程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
[0167]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。