基于有限状态机的煤矿辅助运输转载自动化控制研究

王雯，吴娟，金书奎，邱建都

(1.太原理工大学机械工程学院，山西太原 030024；2.矿山流体控制国家地方联合工程实验室，山西太原 030024)

0 前言

煤矿运输分为主运输和辅助运输[1]。辅助运输承担着人员、设备和材料等运输的重要任务。我国西部部分矿区地质构造异常复杂，巷道断面为10～15 m2，采煤工作面推进长度短，一般为400～600 m，一个采区每年搬家倒面3-4次，辅助运输线路随工作地点延伸或迁移频繁。巷道运输路线错综复杂、变坡及拐弯多，辅助运输转载环节多，单轨吊车与矿用平板车间转载环节的自动化水平仍不够完善，需人工手动操作完成起吊转载作业，耗时耗力，安全性不高，严重制约了煤矿辅助运输智能化发展。

为实现煤矿辅助运输智能化，2019年1月国家煤矿安全监察局发布的《煤矿机器人重点研发目录》明确提出：重点研发煤矿运输类机器人(无人驾驶运输车、搬运机器人等)。

陈杨阳等[4]提出我国煤矿运输机器人发展趋势及关键技术，阐述了机械臂类、清理类、无人车等机器人的应用场景，初步判断这3类方向是煤矿运输机器人的发展趋势。

本文作者提出一种采用煤矿辅助运输转载机器人实现容器自动化转载的控制方案，重点研究辅助运输转载机器人在转载过程中各子系统(单轨吊车、转载机械臂、平板车、锁具)协同工作的运动控制策略，最后借助机器人操作系统(Robot Operating System，ROS)进行转载仿真实验，进而验证该方法的合理性。

1 辅助运输转载机器人概述

为提高辅助运输效率和安全性，文中提出采用辅助运输转载机器人协助工人完成转载工作，如图1所示，转载机器人主要包括单轨吊车、机械臂、平板车、转载容器和锁具。锁具安装在平板车车板四角位置，与转载容器角座位置相对应。单轨吊车沿着巷道顶部固定的轨道运动，机械臂底座安装在单轨吊车的车厢顶部，机械臂抓取转载容器，将装有不同物料的转载容器搬运至单轨吊车上，实现物料的装载。单轨吊车到达指定位置后由机械臂将装有物料的转载容器放置在平板车上，实现物料的转载。

图1 转载机器人

2 辅助运输转载控制系统设计

2.1 转载控制系统方案

辅助运输转载控制主要是实现转载过程中单轨吊车、转载机械臂、锁具和平板车之间的协同工作，保证转载的每一环节都能顺利完成。文中给出的转载控制实施方案如下：首先工人通过上位机监控单轨吊车和平板车的运动，当其到达指定的转载点位置时，输入指令控制单轨吊车和平板车停止运动。接着工人输入转载指令，系统切换到机械臂工作模式，机械臂先回初始位置，自检成功后通过机器视觉算法实现转载容器的种类及坐标位置的检测，当容器的种类符合转载要求时，平板车上的锁具解锁，机械臂开始抓取，实时根据容器当前在机械臂坐标系下的位姿计算机械臂各关节角度和夹爪开度，将数据发送至PLC控制器，驱动机械臂运动实现对转载容器的抓取，抓取成功后将转载容器放置在平板车上，机械臂重新回到初始位置。最后工人输入平板车运动指令，系统切换到平板车工作模式，锁具上锁后，平板车开始运动，至此完成了容器的转载工作。

根据上述转载控制实施方案，文中设计了辅助运输转载控制系统框架，分为上中下3层，分别为上位机监控层、核心算法层和转载驱动层，如图2所示。

图2 转载控制系统框架

上层为上位机监控层，由PC机和工业路由器组成。PC机和工控机接入煤矿井上和井下由工业路由器建立的局域网中，在PC机上运行辅助运输转载控制相关软件，软件功能主要包括单轨吊车的调度、转载现场视频监控、人工转载控制和平板车的调度等。

中层是转载控制核心算法层，其硬件主要由工控机和工业摄像头组成。工业摄像头采集转载容器图像数据，工控机内部对图像数据进行处理，通过运行目标检测、识别和定位算法得到容器种类数据和容器位姿数据。将容器种类和位姿数据打包，通过机械臂抓取参数估计获得机械臂各关节角度、机械臂夹爪姿态和夹爪开度等数据，经LAN网发送至转载驱动层。

下层为转载驱动层，通过防爆PLC控制器实现对单轨吊车、转载机械臂、锁具和平板车的控制，所有控制程序烧录到防爆PLC控制器内，根据上层下达的控制指令统一执行。

基于转载控制系统框架，选用合适的硬件实现转载控制的自动化，文中主要对机械臂和锁具控制的硬件连接进行研究。转载机械臂的底座关节和末端夹爪姿态调整关节都采用液压马达作为关节旋转驱动源，剩余的连杆关节采用液压缸作为关节旋转驱动源，夹爪开度控制采用液压缸作为驱动源。锁具锁杆旋转带动锁头转动，其驱动源采用扭转油缸，配合防松油缸使得锁头能够固定在指定旋转角度。

控制系统硬件连接如图3所示，控制台A为防爆计算机，输入指令可人工控制转载过程，且人工控制优先于其他通信控制。锁具控制和转载机械臂运动控制分别采用独立PLC控制器，锁具控制采用2个二位二通电磁阀分别控制锁头旋转和锁紧。转载机械臂运动控制采用3个三位四通换向阀分别控制机械臂底座旋转关节和两连杆旋转关节，实现机械臂末端在XY平面上的调整；采用1个三位四通换向阀控制末端旋转关节，实现机械臂末端夹具姿态的调整；采用1个三位四通换向阀控制夹紧油缸，实现夹爪开度的调整。2个PLC控制器分别接收各自执行端反馈的信号，通过PID算法对实际装载过程中产生的偏差进行调整。

图3 辅助运输转载控制硬件连接示意

工业摄像头通过USB接口连接工控机，其采集的图像数据作为工控机内部核心算法的输入，PLC控制器E在接收到工控机计算得到的转载容器位姿、机械臂各关节的旋转角度、夹爪的开度等数据后，开启油泵2，驱动各关节电磁阀工作，实现转载机械臂抓取的控制。PLC控制器B在接收转载机械臂抓取状态信号后，开启油泵1，当机械臂从平板车上抓取转载容器时，PLC控制器B关闭防松电磁阀，使得锁头可以相对于锁具底座转动，打开扭锁电磁阀，将锁头旋转90°，实现转载容器与平板车解锁。当机械臂把转载容器放置在平板车上后，PLC控制器B打开扭锁电磁阀，将锁头再次旋转90°，实现转载容器与平板车上锁固定，打开防松电磁阀，将锁头固定在当前角度，防止锁头意外旋转使得容器与平板车固定失效。两PLC控制器互为冗余，与工控机、本地WiFi基站间通过LAN线进行数据传输，采用备用电源应急，保证了辅助运输转载控制的安全性能。

2.2 辅助运输转载控制系统模型建立

煤矿辅助运输转载需要单轨吊车、平板车、锁具、转载机器人之间协同工作，各个子系统之间不是相互独立运行的，其运行的时间和顺序需要一个总的控制系统进行统一管理，文中以此为基础建立了辅助运输转载控制系统模型，如图4所示。

图4 煤矿辅助运输转载控制系统模型

该模型主要包括一个系统任务切换状态机、机械臂抓取控制子系统、平板车运动控制子系统、锁具运动控制子系统和单轨吊车运动控制子系统。其中系统任务切换状态机根据输入的用户指令结合各个子系统的当前状态，对下一步的工作做出判断。当系统状态切换到机械臂抓取控制子系统时，用户可以手动输入机械臂4个关节旋转角度和夹爪开度来控制机械臂完成转载；也可使机械臂根据传入的图像数据，经过转载容器目标检测、图像处理和机械臂抓取参数估计后自主获得机械臂各关节旋转角度和夹爪开度信息，完成容器的转载。当系统状态切换到锁具控制子系统时，根据用户输入的指令和机械臂抓取的状态完成锁具的解锁和上锁。

文中通过系统状态机实现各子系统协同工作，使得辅助运输转载能够安全进行，图5所示为文中设计的辅助运输转载任务切换状态机，主要包括初始化状态、单股吊车运动控制状态、平板车运动控制状态和机械臂抓取控制状态四部分。

图5 煤矿辅助运输转载系统状态机

图6所示为初始化状态的详细框图，系统进入初始化状态后，各个子系统的运动状态位全部停止，使得系统在一个安全的状态下运行。

图6 初始化状态框图

图7所示为从初始化状态切换到单轨吊车运动控制状态的详细框图。系统完成初始化后，通过输入用户指令单轨吊车运动，在判断机械臂是否处于回中位状态后，触发状态切换，系统由初始化状态切换到单轨吊车运动控制状态，在该控制状态下又包含单轨吊运动和停止2个子模块。当进入单轨吊车控制子系统后，首先使单轨吊车控制开启，通过用户输入指令使得单轨吊车运动控制关闭或返回到开启状态。

图7 单轨吊车运动控制状态框图

图8所示为从初始化状态切换到平板车运动控制状态的详细框图，由于和单轨吊车运动控制过程相似，文中不再进行说明。

图8 平板车运动控制状态框图

图9所示为机械臂抓取控制状态框图，将机械臂抓取控制状态分为回中位、开始抓取和结束抓取3个环节。首次进入机械臂抓取控制状态，机械臂先回到中位，如果机械臂当前的状态为已经回到中位，则进入机械臂开始抓取状态，同时使得锁具控制状态位解锁；此时若抓取成功，则进入结束抓取状态，若用户输入指令结束机械臂抓取，则机械臂又重新回到中位，当接收到控制其他子系统的用户指令后，进入对应的子系统状态。

图9 机械臂抓取控制状态框图

3 辅助运输转载控制系统仿真

3.1 系统信号列表建立

系统状态机跳转和各子系统间工作控制都是通过信号传递实现的，文中把系统中传递的信号分为用户指令信号、运动状态反馈信号和控制位信号三大类，具体信号内容如表1所示。

3.2 转载控制系统仿真分析

将需要输入的用户指令信号和传感器信号写入Signal Builder子模块中，触发状态机协调各个子系统工作。图10所示为建立的转载控制系统仿真环境。

图10 转载控制系统仿真环境

在Signal Builder子模块中模拟用户输入自动转载指令，系统状态机根据当前各子系统状态反馈类信号和用户指令信号对辅助运输转载进行控制，其输入和输出的信号时序如图11所示。

图11 辅助运输转载控制时序

用户输入自动转载指令后，系统状态机判断平板车和单轨吊车都处于静止状态，机械臂控制位置1，机械臂抓取子系统状态开始运行，机械臂反馈状态为开始抓取，此时锁具控制位置1。锁具开锁完成后，机械臂进行转载容器的抓取，机械臂的反馈状态为抓取中，当用户观测到机械臂反馈状态为完成抓取时，输入终止转载指令，此时机械臂控制位置0，机械臂由完成抓取状态进入回中位状态，机械臂反馈状态位回中位后，锁具控制位置0，锁具上锁。用户输入平板车运动指令，平板车控制位置1，平板车反馈状态为开始运动后，用户输入单轨吊车运动指令，单轨吊车控制位置1，单轨吊车反馈状态为开始运动，至此容器的自动转载工作已完成。

在系统状态机的协同控制下，严格把控各子系统工作顺序和时间，避免由于系统工作时序混乱引发的转载安全事故。

4 机械臂抓取实验

在ROS中建立了转载机械臂抓取仿真环境，工业相机安装在两指夹爪上端，加载Gazebo仿真环境，机械臂前端放置一张实验桌，桌子上散乱摆放着不同类别转载容器和抓取后放置的箱子，如图12所示。将该机械臂接入系统状态机，在外部模拟平板车、单轨吊、锁具的状态信号。通过系统状态机控制机械臂完成容器的抓取和搬运。

图12 机械臂实验场景

在机械臂抓取控制子系统中包括了机械臂完成抓取所需要的目标检测、目标位姿获取和抓取放置相关算法，启动机械臂抓取控制子系统，机械臂回到程序设定的初始位置。此时，工业相机与桌面平行，工业相机获取转载容器图像信息并输入到事先训练好的模型，输出识别到的容器检测结果和对应的三维空间位姿。将准备抓取的容器位姿输入到抓取参数估计算法中，输出置信度最优的夹爪旋转角度和夹爪抓取宽度，然后打包成消息发送给机械臂控制节点。机械臂接收到目标抓取容器信息后，夹爪中心运动到模型输出的最优抓取点位置，然后闭合夹爪实施抓取，夹取容器后机械臂重新回到初始位置，运动规划到平板车上方并松开夹爪，容器放置到箱子后，机械臂重新回到初始位置准备下次抓取。在抓取过程中，机械臂时刻向系统状态机反馈当前的工作状态。机械臂抓取过程如图13所示。

图13 机械臂抓取过程

一次成功抓取的标准为机械臂抓取容器到箱子上方的整个过程中不出现掉落，以该标准对转载容器进行20次抓取实验，结果如表2所示。

表2 抓取实验结果

由表2可以看出：在Gazebo环境中机械臂抓取物体的成功率可以达到80%以上，由于夹爪闭合时，物体和夹爪会产生碰撞和摩擦，可能会造成“空抓”现象。

5 结论

文中主要对辅助运输转载自动化控制系统展开研究。首先概述了辅助运输转载机器人的组成及工作原理；然后设计了辅助运输转载控制系统框架，并在此框架基础上建立了转载控制系统模型，在MATLAB中搭建了转载控制系统模型的仿真环境，生成系统信号列表，对自动转载控制过程进行仿真分析；最后在ROS中建立了转载机械臂抓取仿真实验环境，将Gazebo中的机械臂接入系统状态机，在外部模拟平板车、单轨吊、锁具的状态信号，通过系统状态机控制机械臂完成容器的抓取和搬运。在Gazebo中仿真转载容器抓取实验，不同位置下抓取的成功率均在80%以上。