基于SLAM 导航的煤矿井下机器人设计

赵文金 朱子恒 吴云雁

（山西潞安环保能源开发股份有限公司常村煤矿，山西 长治 046100）

随着煤炭行业对无人化、智能化需求日益增加，采用机器人参与到井下探测、开采、运输、救援等工作已经成为主要发展趋势。对于井下机器人来说，自主导航能力成为判定机器人智能化的关键因素。国家煤矿安全监察局在2019 年发布的《煤矿机器人重点研发目录》中指出，对于掘进类、采煤类、运输类、安控类、救援类五大类煤矿机器人，自主导航功能是首要解决的问题[1-2]。

自主导航功能包括了定位、建图、路径规划、避障等方面，由于井下环境复杂多变，巷道狭长、转角多，且存在复杂信号干扰，因此GPS 导航无法应用于井下环境。对于井下弱GPS 信号的场景，有学者提出利用UWB 室内定位技术实现建图和导航。UWB（超宽带，Ultra Wide Band）是一种无线载波通信技术，利用纳秒级的非正弦波窄脉冲传输数据，发射信号功率谱密度低，对信号衰落不敏感，因此其定位精度高，非常适合室内定位。然而，实现UWB 定位需要在若干位置安装相应的基站，目前基站价格昂贵，对于井下环境需要投入成本较高，且UWB 定位方式不能满足所有功能情景，例如完成救援等。许多学者提出采用视觉方式实现环境识别和定位，但由于井下光线较暗、粉尘浓度高，因此无法获取清晰的图像。为解决室内机器人自主导航的问题，SLAM 算法逐渐进入人们视野[3-4]。SLAM（Simultaneous Localization and Mapping）由Randall C.Smith 和Peter Cheeseman 在1986 年 首次提出，其实现自主导航的方式是以当前位置为起始，在机器人移动过程中根据携带传感器数据（包括激光雷达、里程计等）和地图路标进行自身定位，同时构建增量式地图，完成对未知环境的建图和导航[5-7]。1999 年的机器人国际研讨会（International Foundation of Robotics Research，ISRR） 是SLAM发展史上的一个重要节点，这是第一次针对SLAM问题的讨论，主要是讨论了基于卡尔曼滤波的方法与基于概率学方法下SLAM 的收敛问题。

到目前为止，SLAM 根据搭载传感器的不同主要分为激光SLAM 与视觉SLAM。激光SLAM 发展时间长，数据处理相对简单，是目前比较成熟、比较稳定、比较主流的定位导航方法；视觉SLAM还处于发展阶段，且对光的依赖程度高，但其适应的场景多，有很大的发展空间。根据优化方法的不同，SLAM 还可分为基于粒子滤波的方法和基于图优化方法的SLAM。

国内在SLAM 上的研究也取得了较大进展。河海大学的王秉洲等[8]为了解决Fast-SLAM 算法粒子退化的问题，应用自适应渐消的无迹卡尔曼滤波，一方面减小了扩展卡尔曼滤波线性化时的误差，另一方面减小了粒子的退化；云南师范大学的罗景文等[9]在箱粒子滤波算法中引入萤火虫算法的寻优机制进行位姿估计，减小了构建地图所需的粒子数量，提高了Fast-SLAM 的构图和定位精度；上海大学的任明宇等[10]将激光雷达采集的点云数据与相机采集的三维点云数据通过贝叶斯方法对地图进行融合与更新，从而构建出更精确的地图。

虽然SLAM 定位范围相对GPS 要小很多，但摆脱了对卫星的依赖，非常适合于井下弱GPS 信号的场景，同时，只需要在机器人上搭载雷达等传感器，不需要类似UWB 技术一样在井下安装大量基站。因此，对于井下机器人的自主导航设计来说，SLAM 算法是最有效和最经济的手段。

该文将设计基于SLAM 导航的井下机器人，搭载Jetson Nano 主机运算导航算法，实现建图、定位、路径规划等，同时开发单片机以实现对车轮电机的控制，Jetson Nano 主机实时向单片机发布车轮控制指令，单片机实时反馈里程计数据等。将井下机器人在实验室进行测试，利用ROS 系统提供的rviz 三维可视化工具完成井下机器人位置及数据流监控，并对算法进行修正与参数校准，完善井下机器人的自主导航功能。

1 总体方案

自主导航功能主要分成两个部分：建图和导航。如图1 所示。

图1 自主导航功能结构图

SLAM 是自主导航的核心算法，机器人顶部搭载激光雷达实时扫描平面360°，获取周围环境数据，依赖这些数据构建增量式地图。通常建图采用的传感器主要包括激光雷达和深度相机，考虑井下环境较为复杂和多变，粉尘较多，光线较暗，因此采用激光雷达作为检测环境数据的主要传感器。导航的实现依赖路径规划算法，该算法接收实时更新的地图数据，完成基于地图的定位，同时接收来自单片机上里程计数据完成基于里程计的定位，将这两个定位信息结合完成局部路径规划和全局路径规划。全局路径规划用于指导机器人走向目标点的路径，局部路径规划用于扫描当前路径上的障碍物，实现避障。经过路径规划后，产生速度控制指令，Jetson Nano 主机将更新的速度控制指令发送给单片机，单片机根据电机控制算法实现车轮电机的速度与转角控制，最终完成机器人导航功能。

因此，该文针对运输类机器人给出设计方案，为了实现建图及导航功能，需要提供实时扫描的地图数据、里程计数据和目标点信息数据，SLAM 算法根据这些数据计算出速度控制指令，并发送给控制器。

2 硬件方案

2.1 硬件分析

机器人的硬件主要考虑三个方面：控制器类、传感器类、底盘类。

1）控制器类

控制器主要完成两部分功能：导航算法实现和运动控制。

采 用Jetson Nano 作 为 主 机，Jetson Nano 是GPU 运算平台，以Jetson Nano 为系统的总控制器，主要用于SLAM 算法及路径规划算法的计算。具体型号采用Jetson Nano B01，该款主机是高性能嵌入式计算平台，采用了NVIDIA Maxwell GPU 和四核ARM Cortex-A57 CPU 的组合，拥有强大的计算能力和低功耗特性，具有128 个CUDA 核心，支持硬件加速和视频编解码和图形处理，主频高达1.43 GHz，支持多线程并发处理。由于需要与单片机进行通信，需要充足的接口，Jetson Nano B01 包含多个USB 接口、HDMI 接口、以太网接口、WiFi、蓝牙等通信接口，以及GPIO、SPI、I2C 等嵌入式接口，方便与单片机等多种外设进行通信。

以半导体的STM32F103ZET6 芯片为单片机核心，并搭载外围电路，主要用于实现车轮电机的运动控制，包括转角和速度的调节。STM32F103ZET6是半导体公司生产的32 位具有低功耗、高性能的处理器，具有ARM Corex-M3RISC 内核，工作频率可达72 MHz，高速嵌入式存储器，包括闪存512 KB，SRAM64 KB，提供了3 个12 位ADC 通道、4 个通用16 位定时器，还有多种标准通信接口，包括2 个I2C、3 个SPI、1 个SDIO、5 个USART、1个USB 和1 个CAN 通道。

2）传感器类

SLAM 算建图主要依赖于环境数据和里程计数据，根据井下环境情况，采用360°激光雷达实时获取环境数据。这里采用单线激光雷达RPLIDAR A1M8。该激光雷达采用三角测距原理，测距范围为0.15～12 m，误差为±1%，扫描角度范围是0～360°，角度分辨率＜0.5°。

里程计数据主要包括航向角、角速度、加速度等，依赖于惯导采集，并利用单片机计算得到。采用MPU6050 作为IMU 传感器，将获取的加速度、角速度等数据解算得到欧拉角数据，为导航提供里程计数据。MPU6050 整合了3 轴陀螺仪和3 轴角速度传感器，解决了陀螺仪与加速度传感器轴间差的问题，同时自带数字运动处理器，使用InvenSense 公司提供的运动处理库实现姿态解算，实现硬件加速。

速度和方向数据主要依赖编码器传输给单片机。这里采用高精度巨磁阻编码器，该编码器集成了光电编码器的高精度特点，相较于霍尔编码器提高了数倍，在低速时表现出优异的稳定性。

3）底盘类

底盘采用阿克曼类型底盘，其控制方式主要采用舵机实现前轮转向功能、利用双电机完成后轮差速驱动功能。选用额定扭矩为13.5 kg·cm、额定电流为2.3 A、额定转速为230 r/min、减速比为27:1型后轮驱动直流无刷电动机。电动机与其配套的底盘电机轮组模块带有滑动轴承，电动机不直接驱动车轮，而是负责传递动力。

2.2 硬件方案设计

根据硬件方案，设计如图2 所示的硬件连接方式。

图2 硬件连接图

各部分主要功能包括：

1）导航算法功能

Jetson Nano 通过USB 接口连接激光雷达，获取激光雷达扫描的地图数据，用于建图功能中地图的建立与导航功能中获取实时地图用于定位；

Jetson Nano 通 过USART 与STM32 连 接，获取STM32 读取到的里程计数据实现定位功能，在路径规划时将计算得到的智能车速度控制指令传给STM32，实时控制机器人的移动，实现导航与避障功能；

Jetson Nano 作为主机，将虚拟机的Ubuntu 作为从机，通过局域网与虚拟机的Ubuntu 系统建立通信，通过虚拟机的ROS 系统提供的节点与话题工具实现对各节点与话题的监控，通过虚拟机ROS系统提供的rviz 三维可视化工具实现对智能车在二维栅格地图中的位置的监控，并可以通过rviz 发布目标点话题。

2）运动控制功能

STM32 通过调节控制舵机PWM 信号的占空比，实现对后驱电动机的控制，舵机带动前轮转动实现转向功能；

STM32 连接驱动器，驱动器连接后驱电动机，STM32 通过调节驱动器控制电路PWM 信号的占空比，实现对驱动器驱动电路驱动电机电流大小的控制，进而实现STM32 对电机转速的控制；

STM32 通过调节驱动器控制电路输入逻辑电平的高低，实现对后驱电动机电流方向及有无的控制，进而实现STM32 对电机转向的控制；

STM32 通过计数编码器输入的A、B 相脉冲数并获取A、B 相相位差，通过计算得到实时速度数据与速度方向；

STM32 通 过I2C 接 口 与IMU 通 信，实 现 对IMU 的配置与读取IMU 内各寄存器值，通过一定的计算得到机器人实时航向角、角速度、加速度等数据；

STM32 通过USART 接口与Jetson Nano 通信，实现STM32 与Jetson Nano 之间的数据交换，用来向Jetson Nano 传输各传感器数据、机器人状态数据并接收Jetson Nano 传入的控制指令。

3 软件方案

3.1 软件分析

软件功能主要包括SLAM 算法、路径规划算法、里程计数据结算、运动控制算法、主机与单片机之间的通信等功能。

3.2 软件方案设计

根据上述主要软件功能介绍，设计如图3 所示的软件方案。

图3 软件结构示意图

软件功能上主要分成两个部分：

1）导航功能

主要在Jetson Nano 上实现。Jetson Nano 安装Ubuntu 系统，并安装ROS 系统，Jetson Nano 上所有的功能包都是在ROS 环境下开发。

主要的功能包括：

① 通信功能包

主要完成STM32 与Jetson Nano 的串口通信，自定义编码协议，将需要传递的数据转换为16 进制发送/解读。在对波特率、通信端口等通信基本参数、里程计、IMU 坐标系和话题名进行配置后，启动通信文件，通过订阅速度话题、里程计话题、tf 坐标变化话题等，读取导航所需数据，通过SLAM 算法得到的控制指令再发送给单片机。

② 导航功能包

基于gampping 算法完成地图的构建；提取地图数据、里程计数据等，基于move_base 功能包实现路径规划；设定目标位置，通过auto_run 功能包实现自动巡航功能，计算电机速度控制指令。

2）运动控制功能

主要在STM32 单片机上实现。STM32 单片机主要采用C 语言进行开发编程。

主要功能包括：编码器数据解算、惯导数据解算、电机速度闭环控制、数据的串口发送编码、数据串口接收处理。

4 实验室测试

对井下机器人进行实验室测试，主要包括建图测试、导航测试。

1）建图测试

建图是导航的基础，地图是否正确影响导航效果。因此首先对建图功能进行测试。

利用ROS 的rqt 工具可以得到建图时的建图节点与话题功能图。建图节点与话题功能图如图4。

图4 建图节点与话题功能图

图中完成了雷达到底盘的静态tf 坐标变换，雷达扫描节点发布激光雷达实时扫描的地图数据，SLAM 算法接收实时地图扫描的数据和做底盘与激光雷达之间位置转换的tf 坐标变换数据，完成地图的构建。

对实验室进行实地测试，图5 为实验室的二维栅格图。

图5 实验室二维栅格图

图5 表示根据雷达数据所创建的地图，其中黑色边界表示实验室围墙，其内部黑色区域表示障碍物，包括静态和动态障碍物，白色区域为可通行区域。从建图结果可以看出，建得的地图与实际实验室的地图吻合，建图功能可以正常运行。

2）导航测试

在实验室中设定几个坐标点，测试机器人是否能够正确地运行到指定位置，并且能够绕开障碍物。

图6 中虚线为全局路径规划路线，点线所包围的网格线为局部路径规划路线。路径规划运行的结果满足预期，可以正常进行目标点的导航及巡航功能。

图6 机器人路径图

5 结论

该文设计了井下机器人，通过激光雷达对井下环境进行扫描，搭载Jetson Nano 主机接收地图数据并通过SLAM 导航算法实现建图、定位、导航、避障等功能，发送指令给单片机，单片机根据控制算法实现车轮转角与速度的控制。通过实验室的测试，验证了设计的机器人导航等功能获得较好效果，在未来的应用中，可完成井下探测、开采、运输、救援等工作。