基于SLAM的井下巡检机器人的研究

刘璐 高振航 张灿 胡飘

摘  要：文章设计了基于多传感器融合SLAM的井下巡检机器人。作为SLAM的感知设备，相机、激光雷达和惯性传感器的性能和可靠性在不同的运行速度和环境下相差较大，针对这一特点，结合激光和视觉SLAM子系统以及子系统的误差项对机器人的位姿进行非线性优化，提高了机器人在光线昏暗、地形复杂的井下环境中运行的定位准确性和稳定性。

关键词：SLAM；巡检机器人；数据融合

中图分类号：TP242  文献标识码：A  文章编号：2096-4706（2023）05-0060-05

Research on Underground Inspection Robot Based on SLAM

LIU Lu， GAO Zhenhang， ZHANG Can， HU Piao

（China University of Mining and Technology-Beijing， Beijing  100083， China）

Abstract： This paper designs an underground inspection robot based on multi-sensor fusion SLAM. As the sensing equipment of SLAM， the performance and reliability of cameras， laser radars and inertial sensors vary greatly under different operating speeds and environments. Aiming at this feature， combining the laser and vision SLAM subsystems and error items of subsystems， the robot's position and attitude are optimized nonlinearly to improve the positioning accuracy and stability of the robot's operation in the dark and complex underground environment.

Keywords： SLAM; inspection robot; data fusion

0  引  言

现代化矿井的生产设备日益向大型、连续、高速和自动化的方向发展，基于天然因素和人为因素的各种故障影响着生产效率和人员安全，给企业和工人造成损失。因此需要对矿井生产设备和系统进行定期、定点的检查，对照标准查验设备的异常现象和隐患及时提出维护方案[1]。煤炭生产过程常采用需要工人直接或间接参与的人工巡检方式进行故障排除，由于井下通信距离长、通信信号易受干扰、照明不足等环境原因导致井下人工作业存在安全隐患。近年来煤炭行业正进一步实现生产过程智能化、无人化和通信设备网络化，并在关键工作区和相对危险的工作岗位采用机器人代替工人进行设备和仪器的操作和检查以提高煤炭井下作业的工作效率和安全性[2]。设计一款在易发生特大事故的重要场所可通过远程操控或自主移动的方式进入危险区域进行巡检和测绘工作的井下巡检机器人，对提高煤炭生产的效率和保障煤炭工人的安全有重要意义。

1  总体设计

井下巡检机器人由感知模块、主控制器模块、运动控制模块和通信模块组成。数据采集模块将摄像头采集的图像信息、激光雷达返回的距离数据、IMU陀螺儀采集的加速度信息发送到主控制器模块，主控制器模块将采集到的数据进行数据融合和计算得到机器人的位姿信息，构建地图并进行路径规划，主控制器模块通过通信模块向运动控制模块发送指令和接收运动信息并将机器人的位姿信息和地图信息反馈到上位机，运动控制模块通过控制电机转速控制机器人的移动并返回运动状态信息。井下巡检机器人的总体设计如图1所示。

2  硬件设计

硬件部分包括主控制器模块、运动控制模块、感知模块、电源模块、速度检测模块和通信模块。主控制器模块根据感知模块采集到的环境信息和速度检测模块返回的运动状态信息通过基于多传感器融合的SLAM算法进行定位、建图和路径规划并通过通信模块向运动控制模块发送指令控制机器人的运动。

2.1  主控制器模块

主控制器采用NVIDIA的Jetson Nano开发套件，总体尺寸为69 mm×45 mm，采用4核Quad-core ARM A57微处理器的CPU，采用NVIDIA研发的128-core Maxwell作为GPU，使用4 GB 64-bit LPDDR4 25.6 GB/s的内存。板载2路CSI摄像头接口、4个USB 3.0接口和一个Micro USB接口，提供外接GPIO口、两个I2C接口、两个UART接口和两个SPI接口。Jetson Nano是业内先进的AI计算平台，支持多种深度学习框架，如TensorFlow、PyTorch、Caffe、Keras、MXNet等，可实现图像分类、目标检测、语义分割和智能分析等功能，可用于构建自主机器人和复杂人工智能系统。同时具有低功耗和并行处理快速的优势，可以并行处理多个传感器并在每个传感器上运行多个现代神经网络。Jetson Nano搭载Ubuntu 18.04系统，支持Melodic版本的ROS系统，可实现井下巡检机器人的同步定位与建图功能和自主导航功能。

采用电源模块输出的5 V电压为主控制器Jetson Nano供电，通过串口通信模块和运动模块相连实现运动指令的下发和里程计信息的获取，通过USB接口连接感知模块的摄像头和激光雷达实时获取环境信息。

2.2  运动控制模块

运动控制模块基于STM32F407VET6微控制器设计，STM32F407VET6是基于32位ARM Contex M4的微控制器，工作频率可达168 MHz，采用RISC精简指令集、LQFP表面贴装封装，具有14个定时器、2个UART接口、3个I2C接口、3个SPI通道和3路16位AD转换器。

运动控制模块的STM32F407VET6微处理器的主要功能为接收来自主控制器的运动指令，通过运动学公式根据运动指令计算左右车轮的转速，并通过PID控制输出到电机驱动模块的PWM波控制左右车轮的转速。同时结合光电编码器返回的信息，计算出电机的实时转速得到机器人的实时运动状态信息，并将得到的信息和IMU返回的加速度信息上传到主控制器。

2.3  感知模块

感知模块由双目摄像头、2D激光雷达和IMU惯性测量装置组成。采用微雪的IMX219双目摄像头作为视觉传感器实时获取环境中的图像信息。IMX219双目摄像头的分辨率为800万像素、视场角为83°、支持红外夜视功能，采用双CSI排线作为摄像头接口。双目摄像头成本较低且易于获取环境中的深度信息，同时IMX219的红外夜视功能可以在昏暗环境下更好地获取环境信息。激光雷达采用思岚科技的RPLIDAR-A1型雷达，通过USB转串口模块与主控制器相连，RPLIDAR-A1型雷达的正常工作功率为3 W，测距范围为12 m，工作频率为5.5 Hz，测量频率为8 000次/秒。利用高精度激光雷达可以准确测量机器人周边环境的距离信息，有效提高煤炭井下机器人建图的准确性。IMU惯性测量装置采用内置输出16位数字量的三轴加速度计、三轴陀螺仪和三轴磁力计的MPU9250，通过I2C通信向单片机发送机器人的运动和姿态信息。

目前基于视觉传感器的SLAM框架已经较为成熟，但由于煤炭井下光照不足、空气中有较多粉尘和水雾的特殊环境，基于视觉的SLAM方法会产生一定误差，因此加入了可对周边环境的障碍物进行测距的激光雷达和实时提供机器人加速度信息的IMU惯性测量装置，用于数据融合，增加井下巡检机器人定位与路径规划的精度和鲁棒性。

2.4  电机驱动模块

电机驱动模块选用东芝半导体公司生产的TB6612电机驱动元件，TB6612电机驱动模块的工作温度为-20 ℃～85 ℃，具有两路最高频率为100 kHz、最高电流为1.2 A直流电流的PWM波输出，可同时驱动两路直流电机实现正转、反转和急停功能。相比于常用的L298N电机驱动模块散热性能更好，进行电路设计时不需设计额外的散热元件。电机选择带有高精度编码器的12 V直流减速电机。直流减速电机通过在直流电机外加装一套齿轮通过齿轮线速度的变化，实现在低转速时为车轮提供较大力矩。高精度光电编码器通过光电装置实时测量车轮的实时转速数据，通过运动学公式计算得到机器人的里程计信息。

2.5  电源模块

采用12 V锂电池供电，其中直流减速电机由12 V电源电压直接供电，电源电压经过降压得到5 V和3.3 V电压为各个模块供电。将12 V电源电压通过RT8289模块降压得到5 V电压，再将5 V电压通过CP2102模块进一步降压得到3.3 V电压。

2.6  通信模块

主控制器模块和运动控制模块通过串口通信实现数据传输。运功控制模块通过CP2102 USB转串口模块实现单片机的UART接口和Jetson Nano USB接口的连接，CP2102模块使用5 V电压供电，支持300 bit/s -1 Mbit/s波特率的数据传输。主控制器模块采用intel 8265NGW双频网卡，通过Wi-Fi通信和上位机连接满足传输视频流的需求。

3  软件设计

软件设计基于ROS开发平台和Keil5开发平台，采用C++和Python语言进行程序编写，根据功能主要分为通信子程序、SLAM子程序、运动控制子程序和路径规划子程序。

用户通过通信子程序远程控制机器人的运动和查看周边环境的信息。SLAM子程序利用激光雷达和视觉传感器返回的数据，通过提取运动环境的特征同时结合里程计信息得到机器人的位置信息和周边环境信息并生成地图。运动控制子程序进行机器人的运功控制，根据IMU传感器和光电编码器的信息通过卡尔曼滤波进行数据融合，计算机器人的位姿信息，并通过SLAM子程序进行建图。路径规划子程序根据地图结合里程计信息和传感器信息，进行路径规划并向运动控制模块发送运动指令，从而在移动过程中通过传感器信息进行移动路径的优化和局部避障。

3.1  通信子程序设计

单片机通过通信子程序接收主控模块发送的前进、后退、变速、转向等指令并控制机器人的运动。单片机和主控模块通过波特率为115 200串口实现通信。单片机以20 Hz的频率向主控模块发送数据帧，单片机发送的数据帧如表1所示，每个数据帧由帧头、使能标志位、XYZ三轴速度、IMU三轴加速度、三轴角速度、电源电压、数据校验位和帧尾组成。

单片机采用中断接收的方式接收主控制器发送的数据。单片机接收的数据帧由帧头、使能标志位、机器人XYZ三轴目标速度数据校验位和帧尾组成。

3.2  同步定位与建图子程序设计

近年来以ORB-SLAM框架为代表的传统视觉SLAM技术已经较为成熟，但煤炭井下光线暗、多粉尘的复杂环境限制了视觉传感器在井下环境的应用。因此煤炭井下等不利于视觉传感器的特殊环境多采用基于激光雷达的SLAM技术[3]，虽然激光SLAM的效果优于传统的视觉SLAM，但低价格的激光雷达存在激光点不够密集、噪声大、闭环检测效果不好的问题，而视觉SLAM由于图像包含的丰富信息在闭环检测方面存在天然的优势。因此采用激光结合视觉的方式提高定位精度是井下移動机器人的研究热点[4]。

井下巡检机器人采用一种激光和视觉相结合的定位与建图方法，通过融合激光点云数据和图像特征点数据基于稀疏姿态调整对机器人位姿进行优化的同时，采用基于视觉的词袋模型进行闭环检测并生成基于3D点云的栅格地图。加入视觉信息的激光视觉联合优化的SLAM整体框架，如图2所示。

常用的SLAM数学模型有基于滤波的SLAM算法和基于图优化的SLAM算法，图优化通过构造节点和构造边表示一系列位姿变量的关系，通过非线性优化求出最优变量得到准确的位姿估计。

将视觉相邻帧误差函数公式和激光误差函数公式改写成李代数形式并合并，整体误差函数为：

将机器人位姿作为唯一变量并进行优化，视觉观测量和激光点云数据作为位姿之间的约束，误差函数为：

采用L-M法求解非线性优化问题。

3.3  运动控制子程序设计

单片机接收指令后按照由运动控制子程序根据运动学公式进行计算得到左右轮的转速，并根据光电编码器返回的左右轮实时转速信息通过输出PWM脉冲控制电机实现对车轮转速的PID控制从而实现控制机器人按照指定速度运动，车轮转速的控制流程图如图3所示。

车轮转速的PID控制算法采用增量式离散PID公式，根据单位时间内光电编码器获取的脉冲数得到车轮的速度并与目标值进行比较得到偏差，通过控制偏差的比例、积分和微分使偏差趋向于零。

增量式离散PID控制的计算公式为：

Pwm+=Kp[e（k）-e（k-1）]+Kie（k）+Kd[e（k）-2e（k-1）+（k-2）]

式中Pwm为增量输出的结果，e（k）为第k次车轮转速与目标转速的偏差，Kp为比例项参数，Ki为积分项参数，Kd为微分项参数。井下巡检机器人的控制闭环系统采用PI控制，将增量离散PID公式简化为：

Pwm+=Kp[e（k）-e（k-1）]+Kie（k）

采用串级PID进行机器人的位置控制，位置控制的流程图如图4所示，将外环位置环的输出作为速度环的输入，并由内环速度控制器的输出控制作为执行器的电机转动，从而实现在控制车轮转速的同时使机器人到达目标位置。

差速履带式机器人在经过某些复杂地形时会出现空转或打滑的现象，IMU惯性传感器在长时间使用时会出现零点漂移的情况，采用单一方法得到的里程计信息会存在较大的误差[5]。为了得到更准确的里程计信息，单片机通过扩展卡尔曼滤波将IMU传感器和编码器的信息进行数据融合，并将得到的里程计信息发送到主控制器。

3.4  路径规划子程序设计

井下巡检机器人采用基于A*算法改进的D*算法进行路径规划和自主导航。D*算法是一种启发式的路径搜索算法，适合于井下动态未知环境和动态障碍物的场景。D*算法首先通过dijstra或A*算法从目标点向初始点进行反向搜索，在机器人向目标点移动的过程中遇到障碍物时对路线进行局部更改，算法流程如图5所示。

设目标点为点G，Y为当前节点，X为Y的下一个节点，c（X， Y）为X到Y的路径，open集合为待访问的节点集合，close集合为已经访问的节点和待访问的节点，h函数为当前点到目标点的距离，k函数为当前点到G点的最小代价。当机器人到目标点的最短路径上的节点发生变化时，D*算法通过传播状态和搜索临接点的方法寻找地图发生改变后的最优路径。

4  实验结果

在实验室环境下进行了样机搭建和测试，样机运行图如图6所示。将样机和上位机连接在同个网络下后进行ROS的主从机配置和通信。

进行系统初始化，给雷达连接的串口访问权限后通过launch配置文件启动建图节点。如图7所示。

控制样机在环形实验空间以0.2 m/s的速度运动两圈，通过rviz可视化工具得到环境地图，如图8所示。

5  结  论

基于多传感器融合的巡检机器人在实验室环境下相对于单一传感器有更好的稳定性，且不受环境光的影响，缺点是由于并行运行多个系统导致运行帧率下降，以及激光雷达在收镜面反射干扰产生误差，今后的研究可从关联视觉信息和激光信息的方向出发，在降低误差的同时尽可能提高巡检机器人的运动速度。

参考文献：

[1] 黄骞，刘帅，任伟，等.基于双通信模式的综采工作面巡检机器人研制 [J].煤炭科学技术，2021，49（S1）：167-169.

[2] 张爱民，翟德华，李栋庆.煤矿综采工作面牵引式巡检机器人的设计 [J].自动化应用，2021（7）：133-135.

[3] HOUBEN S，DROESCHEL D，BEHNKE S. Joint 3D Laser and Visual Fiducial Marker Based SLAM for a Micro Aerial Vehicle [C]//2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems（MFI）.Baden：IEEE，2016：609-614.

[4] Shi J Q，He B w，Zhang L w，et al. Vision-Based Real-Time 3D Mapping for UAV with Laser Sensor [C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）.Daejeon：IEEE，2016：4524-4529.

[5] 张保龙，周观民，黄海燕，等.矿用巡检机器人及其控制管理系统的设计研究 [J].煤炭技术，2021，40（9）：165-167.

作者简介：刘璐（1997—），女，汉族，山东济宁人，硕士研究生在读，研究方向：视觉SLAM；高振航（1998—），男，汉族，河南开封人，硕士研究生在读，研究方向：嵌入式技术、自动控制；张灿（1998—），女，汉族，河北盐山人，硕士研究生在读，研究方向：机器人；胡飘（1998—），男，汉族，四川遂宁人，硕士研究生在读，研究方向：嵌入式技術。

收稿日期：2022-10-11