煤矿场景下基于RGBD 的视觉导航技术

王 鹏，赵红菊

（1.国家能源集团神东煤炭集团有限公司，陕西 榆林 719135；2.中煤科工集团沈阳研究院有限公司，辽宁 抚顺 113122；3.煤矿安全技术国家重点实验室，辽宁 抚顺 113122；4.中煤科工机器人科技有限公司，广东 深圳 518054）

当前，对于煤矿场景应用机器人而言，具备一定环境信息采集功能的巡检类机器人大致分2 种：轨道式巡检机器人和轮式巡检机器人[1-4]。相较于轨道式巡检机器人的轨道铺设困难、巡检空间受限等问题，轮式巡检机器人则具有应用范围广、地面适应性好等特点而成为煤矿巡检类机器人的首选。

自主导航功能是轮式巡检机器人智能化的重要特征之一。由于煤矿井下场景具有低光照、大粉尘等复杂的半结构化环境特征，进而现有普通工业机器人的自主导航算法难以在实际应用中取得良好的展示，因此，随着近年来煤矿机器人的兴起，越来越多的科研院所开始逐步探索煤矿机器人的自主导航研究[5-6]。目前，已经在煤矿机器人自行走功能取得一定成绩的导航技术有磁钉+惯性导航[7-8]、二维码视觉导航[9]和激光雷达导航[10]。磁钉+惯性导航受限于煤矿场所的施工改造、短距离定位精准长距离累积误差大；同样的二维码视觉导航也是需要现场场地改造，增加施工及维修陈本；激光雷达导航则在巷道场景特征点少时，低线束雷达建图稀疏、高线束雷达成本昂贵；与前面导航技术方案相比，视觉导航因具有特征信息完备、地图描述精准、定位算法鲁棒性强的特点，并且视觉所需的传感器即深度相机相比激光雷达更加廉价稳定，同时增加辅助高光照明系统解决低光照问题，可以较好地控制机器人完成在井下复杂环境中的巡检任务。

因此，结合现有矿用轮式巡检机器人，研究了一种煤矿场景下基于RGBD 的视觉导航方法，建立一套完整的视觉导航设计流程，用于满足机器人在煤矿井下环境的三维建图、机器人自身的重定位以及到达目标点的自主路径规划与导航等功能。

1 总体架构

以Kinect v2 相机作为数据采集平台，基于极创Warthog-01-M 轮式机器人移动底盘进行视觉导航架构设计，从而实现轮式机器人在煤矿场景环境中的自动建图、自主定位以及面向目标点的路径规划与导航等功能。依据现有煤矿轮式巡检机器人的应用环境与巡检需求，采用模块化设计思路对导航系统进行功能定义和拆分，在明确各部分输入输出的基础上，将独立的功能模块进行模块化开发，然后通过模块之间的配合完成全部的目标任务，加速系统的并行开发与测试时间，提升导航系统的开发效率，同时方便后续的阶段化调试以及功能的迭代更新。

视觉导航系统架构的主要功能模块以及模块间运行的逻辑关系如图1。

图1 总体架构原理框图Fig.1 Principle block diagram of overall architecture

视觉导航系统架构共包含3 个核心功能模块，即建图模块、定位模块以及路径规划与导航模块，通过这3 个功能模块的衔接，实现从深度相机信息采集到轮式巡检机器人路径规划与导航的全过程。

1）建图模块。基于ORB_SLAM2 算法，将Kinect v2 相机采集到的RGB 图片信息以及对应的深度信息转化为三维点云地图，构建出平面栅格地图用于实现后续的重定位。

2）定位模块。采用蒙特卡罗方法，对当前帧在已构建地图中的局部位置已经估计，从而实时产生当前位置。

3）路径规划与导航。在明确环境地图和机器人自身位置的情况下，对输入的目标点进行路径规划，通过全局路径规划产生优化路径、局部路径规划实现动态避障，最终引导机器人完成自主导航任务。

2 关键技术

煤矿井下环境复杂，机器人视觉导航研究需重点解决其应用环境如低照度、开放环境场景特征点稀疏等带来的导航可靠性、稳定性问题。在三维建图设计中，地图创建的精度直接影响机器人自主导航的定位精度，构建细节信息完备的稠密地图，采用帧间匹配算法来逐帧的进行地图拼接，同时设计回环算法来消除帧间匹配所产生的累计误差，进而实现精准的三维地图构建。对于机器人重定位需求而言，以明确机器人所处的局部区域为中心，缩小参与匹配的范围，并基于特征匹配算法实现机器人的精准定位，完成鲁棒性良好的视觉里程计构建。路径规划与自主导航功能的研究则需要实时跟踪机器人当前位置并规划路径，在实时获取视觉里程计信息的同时，基于二维栅格地图进行平面路径规划，完成准确的导航任务，最终实现机器人的自主行走功能。

2.1 地图创建

地图创建是实现煤矿场景下视觉导航的重要研究内容之一，其主要是对Kinect v2 相机传感器输入的RGBD 信息进行处理，生成包含环境完备信息的RGBD 三维点云地图，并在此基础上进一步获得可供定位和导航的平面栅格地图。地图创建过程采用ORB_SLAM2 算法来实现，算法组成包括ORB 和SLAM 2 部分，其中ORB 算法是一种快速特征点提取和描述的算法，包括图像梯度特征点的提取以及描述子的设计；SLAM 算法则是基于ORB 特征进行连续帧之间的地图配准以及后续导航过程中的重定位。由于ORB 特征稳定且可以高速获取，因此，ORB_SLAM2 算法具有良好的实时性，并通过BA 实现回环检测来保证其建图的可靠性，使得此算法在室内室外环境中都有较强的适应性和鲁棒性。

经过模拟井下巷道和现场实际测试，采用该算法完成的三维地图创建可以满足下一步的定位需求。系统环境建图流程如图2。

图2 系统环境建图流程Fig.2 System environment mapping process

地图创建依据ORB_SLAM2 算法原理，采用3个并行线程：跟踪、局部建图和闭环检测。每个线程实现的功能描述如下：

1）跟踪。提取图像帧的ORB 特征，然后查找其与局部地图匹配的位置，并通过BA 实现最小化投影误差，从而跟踪每一帧对相机进行定位。

2）局部建图。将筛选出的关键帧插入地图，构建新的地图点，同时通过局部BA 去除冗余的局部关键帧。

3）闭环检测。在机器人运行途中，将当前帧与关键帧进行对比，若发现机器人经过了重复的环境，则通过前后位姿构建起关键帧和当前帧之间的闭环，通过图优化的方式添加约束并去除累计误差。

在完成上述3 个线程后，可以实现对该环境的建图，地图包括地图点、关键帧、共视图以及生成树。但是此方法生成的地图为稀疏的特征点地图，而非细节全面的稠密地图，因此利用ORB_SLAM2方法生成的连续帧间位姿变换关系，通过逐帧的RGBD 点云拼接，获得了稠密的点云地图以供后续的定位和导航。整个地图在ROS 端的构建过程描述如下：①将轮式巡检机器人置于需要建图的环境中，根据机器人的初始位置和朝向定义坐标原点和坐标轴；②在ROS 中启动Kinect v2 相机节点，对其RGB 图像和Depth 图像进行消息发布；③在ROS 中启动ORB_SLAM2 建图节点，订阅Kinect v2 发布的图像话题，根据当前帧进行地图初始化；④开启Rviz节点，实时观测地图构建情况；⑤启动轮式巡检机器人底盘的键盘控制节点，给定机器人的移动速度和旋转速度，然后通过键盘控制机器人在场景中移动，直至完成整个场景的建图。

2.2 自主定位

自主定位功能主要利用已经构建的场景地图以及当前图像帧的RGBD 点云信息，采用蒙特卡罗定位方法，对轮式巡检机器人在当前场景中所处的位置进行估计，实现煤矿井下复杂结构化环境的精准定位。系统自主定位流程如图3。

图3 系统自主定位流程Fig.3 Independent positioning process of system

首先利用高斯分布作为巡检机器人位置信息的噪声，在此分布下通过大量粒子去表征机器人的可能位置；在此基础上，通过观测值去计算每个粒子运动后的位置可能性大小即评分；然后根据评分对粒子群进行重采样；重复进行权重的计算和粒子群的重采样，从而实现机器人运动过程中的连续定位。

2.3 路径规划

路径规划功能主要分为全局路径规划和局部路径规划，依据煤矿井下场景特征，在路径规划研究中，全局路径规划基于Dijkstra 算法，采用优先广度搜索和贪心策略路径求解；局部路径规划采用DWA动态窗口算法，在短时间内对没有碰撞区域进行搜索，同时优化算法使轮式巡检机器人在短时间内回避障碍物。系统全局路径规划流程如图4。系统局部规划流程如图5。

图4 系统全局路径规划流程Fig.4 Global path planning process of system

图5 系统局部规划流程Fig.5 Local planning process of system

全局路径规划采用Dijkstra 算法，主要通过贪心和广度搜索优先路径解算方法来实现，在巡检机器人初始位置时，算法首先通过广度优先算法对未访问节点进行遍历，循环计算未遍历节点和此时节点最短距离；当计算出最短路径节点时，将此节点视为下一个路径点；循环迭代此过程，由此可以得到起始点到目标点的1 条全局最优路径。

局部路径规划采用动态窗口算法，主要目的在于实现避障功能，针对全局路径的局部区域进行二次路径规划，主要通过在短时间内对巡检机器人周围区域进行碰撞搜索，检测巡检机器人是否发生碰撞，同时优化算法使巡检机器人在短时间内回避障碍物。

3 试验验证

针对煤矿井下环境状况，设计工厂环境下模拟光照良好和暗黑楼道模拟巷道低光照场景，对提出的地图构建方法、路径规划进行验证。巡检机器人运行速度设定为0.6 m/s，并采样10 处目标点进行定位跟踪测量。巡检机器人导航功能相关指标测试结果见表1。

表1 巡检机器人导航功能相关指标测试结果Table 1 Navigation function related index test results of inspection robot

根据测试结果可知：采用该导航技术得到的地图构建与实际环境一致，巡检机器人活动区域内没有存在未扫描到空间；定位导航精度良好，通过对多个目标点进行功能验证测试，重复定位精度最大6.8 cm，定位偏差小于14 cm，角度误差小于12°，验证了该视觉导航架构及算法流程具有较好的稳定性和鲁棒性，能够适用于煤矿井下巷道开放性场景环境下的机器人自行走功能。

4 结 语

针对井下巷道等具有特征点稀疏、低光照等环境的开放性环境，设计了一种适用于煤矿机器人的基于RGBD 的视觉导航系统，建立视觉导航架构流程，并根据流程进行了轮式巡检机器人的导航效果测试。实际测试结果表明：该导航流程可以准确地完成视觉地图的建立，并且可以实现很好的回环效果；路径规划后，机器人能够有效地完成从起始点到规划目标点的自主运动。