基于有限狀態機的煤礦輔助運輸轉載自動化控制研究

王雯，吳娟，金書奎，邱建都

(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%以上。