趙建偉，馬嘯飛，劉成祥，韓濤，房建華，王琨

（中國礦業(yè)大學(xué)(北京) 機(jī)電與信息工程學(xué)院, 北京，100083）

0 引言

近年來，針對煤礦開采中的安全問題，國家煤礦安監(jiān)局研究制定公布了《煤礦機(jī)器人重點(diǎn)研發(fā)目錄》[1]，重點(diǎn)研發(fā)應(yīng)用掘進(jìn)、采煤、運(yùn)輸、安控和救援5類38種煤礦機(jī)器人，并提出各機(jī)器人基本要求?，F(xiàn)有工作面巡檢方式主要有2種：人工巡檢和自動化檢測[2]。人工巡檢采用合理安排工作人員定時巡回檢查的方式，具有人工資源利用率低，實(shí)時性差，效率低等特點(diǎn)。自動化檢測采用計算機(jī)技術(shù)、網(wǎng)絡(luò)通訊技術(shù)，檢測效果較好，但是監(jiān)視范圍有限，若想全面覆蓋整個工作面，所需成本高，任務(wù)量大，維護(hù)任務(wù)重，綜合效率低下。

目前國家大力推行煤礦開采自動化、機(jī)器人化、智慧礦山等方針[3-6]，針對工作面環(huán)境與設(shè)備數(shù)據(jù)實(shí)時采集、信息實(shí)時傳輸和快速預(yù)警，建立工作面安全管控系統(tǒng)等需求，煤礦巡檢機(jī)器人具有環(huán)境感知、實(shí)施決策和行為控制與執(zhí)行等功能，可通過裝載不同裝置實(shí)現(xiàn)不同監(jiān)察任務(wù)。

在煤礦巡檢機(jī)器人實(shí)際運(yùn)行中，由于工作環(huán)境的非完整約束，使煤礦巡檢機(jī)器人系統(tǒng)具有非線性特性，難以建立精確的煤礦巡檢機(jī)器人模型[7]。而煤礦巡檢機(jī)器人控制系統(tǒng)若采用常規(guī)的 PID控制器，則對控制對象的穩(wěn)定性要求很高，需要對被控對象或控制過程建立精確的數(shù)學(xué)模型，并給出嚴(yán)格的數(shù)學(xué)方程[8]。因此，常規(guī) PID控制難以達(dá)到理想的控制效果。

基于模糊規(guī)則的模糊控制算法能有效地控制非線性時變對象，因?yàn)椴恍枰⒕_的數(shù)學(xué)模型，所以其魯棒性強(qiáng)、適應(yīng)性良好[9-11]。針對煤礦巡檢機(jī)器人運(yùn)動和控制特點(diǎn)，根據(jù)雙輪差速驅(qū)動原理結(jié)合通過實(shí)驗(yàn)獲得的控制經(jīng)驗(yàn)確定模糊規(guī)則，設(shè)計了一種軌跡跟蹤模糊控制器。經(jīng)過在Matlab環(huán)境中搭建控制器模型并進(jìn)行仿真驗(yàn)證，結(jié)果表明該控制器能有效提高控制系統(tǒng)的穩(wěn)定性和魯棒性可以應(yīng)用于煤礦井下回采工作面巡檢領(lǐng)域。

1 煤礦巡檢機(jī)器人總體設(shè)計

本文控制對象為應(yīng)用于現(xiàn)代智慧礦山井下回采工作面作業(yè)環(huán)境專用巡檢機(jī)器人。該巡檢系統(tǒng)結(jié)構(gòu)如圖1所示。通過巡檢機(jī)器人上裝載的溫度、濕度、粉塵傳感器和本安型防爆相機(jī)以及本安型紅外熱成像系統(tǒng)完對回采工作面煤壁、片幫、大塊煤、有害氣體、溫度、粉塵等信息的監(jiān)測。監(jiān)測信息通過無線通訊模塊模擬AP上傳到無線基站，最終通過交換機(jī)連接以太網(wǎng)上傳到遠(yuǎn)程回采工作面檢測中心服務(wù)器終端，實(shí)現(xiàn)組態(tài)界面展示。

圖1 回采工作面檢測系統(tǒng)結(jié)構(gòu)示意圖

■1.1 機(jī)械系統(tǒng)結(jié)構(gòu)設(shè)計

煤礦巡檢機(jī)器人在總體方案設(shè)計圖如圖2所示。煤礦巡檢機(jī)器人是整個監(jiān)測系統(tǒng)的核心載體，利用磁導(dǎo)航和RFID聯(lián)合進(jìn)行自主導(dǎo)引與定位，搭載各種傳感器對工作面內(nèi)溫度、濕度、粉塵等狀態(tài)進(jìn)行實(shí)時監(jiān)測。

圖2 煤礦巡檢機(jī)器人本體結(jié)構(gòu)

煤礦巡檢機(jī)器人底盤結(jié)構(gòu)可分為三輪、四輪和六輪結(jié)構(gòu)，其性能分析如表1所示。三輪結(jié)構(gòu)不能自轉(zhuǎn)而且穩(wěn)定性較差。四輪結(jié)構(gòu)克服了三輪結(jié)構(gòu)不能自轉(zhuǎn)和穩(wěn)定性的缺點(diǎn)，但是承重性差。采用六輪雙驅(qū)動輪差速運(yùn)動結(jié)構(gòu)，在四輪結(jié)構(gòu)基礎(chǔ)上增加兩個萬向輪，既提升了車輛平衡性，也能輔助轉(zhuǎn)向，而且穩(wěn)定性和承重性強(qiáng)、運(yùn)行可靠，整體性能優(yōu)于三輪和四輪結(jié)構(gòu)[12]。

表1 底盤結(jié)構(gòu)性能分析表

■1.2 控制系統(tǒng)硬件組成

控制系統(tǒng)硬件主要由三大部分組成，即傳感系統(tǒng)、控制系統(tǒng)和驅(qū)動系統(tǒng)，如圖2所示。

圖3 煤礦巡檢機(jī)器人控制系統(tǒng)組成圖

（1）傳感裝置感知工作環(huán)境和工作對象狀態(tài)，負(fù)責(zé)信息的收集與輸入，包括溫度、濕度、粉塵監(jiān)測模塊，音頻傳感器，視覺傳感模塊，紅外熱成像系統(tǒng)模塊，自動導(dǎo)引模塊以及超聲波避障模塊。

自動導(dǎo)引模塊包括磁導(dǎo)航傳感器和RFID射頻識別模塊。磁導(dǎo)航傳感器將底盤與磁條的偏差信息傳入STM32主控板中，RFID射頻識別模塊負(fù)責(zé)機(jī)器人在工作環(huán)境中的定位。超聲波傳感器負(fù)責(zé)感知煤礦巡檢機(jī)器人工作環(huán)境，識別是否存在障礙物。

（2）控制裝置對傳感裝置獲得的信息進(jìn)行處理，并對伺服驅(qū)動裝置發(fā)出控制指令。選用自主設(shè)計的STM32-F103單片機(jī)開發(fā)板作為核心控制模塊，根據(jù)需要設(shè)計了5個串口、2個CAN接口、2個SPI接口、2個I2C接口和10個端口，在滿足實(shí)際需要的同時留出了升級空間。

（3）驅(qū)動系統(tǒng)依據(jù)控制指令實(shí)現(xiàn)驅(qū)動動作。主要由電機(jī)驅(qū)動模塊組成，包括兩個直流無刷電機(jī)驅(qū)動器和伺服直流無刷電機(jī)，以實(shí)現(xiàn)煤礦巡檢機(jī)器人的運(yùn)動。

2 煤礦巡檢機(jī)器人運(yùn)動學(xué)分析

為了得到驅(qū)動輪速度和煤礦巡檢機(jī)器人運(yùn)行狀態(tài)之間的明確關(guān)系，在不影響整體分析的情況下，將煤礦巡檢機(jī)器人運(yùn)動機(jī)構(gòu)抽象為雙輪運(yùn)動模型[13]。煤礦巡檢機(jī)器人運(yùn)動示意圖如圖4所示。假設(shè)地面平整，煤礦巡檢機(jī)器人車體為剛性且驅(qū)動輪做純滾動運(yùn)動。

圖4 煤礦巡檢機(jī)器人運(yùn)動示意圖

圖4 中，XOY為全局坐標(biāo)系，C點(diǎn)為驅(qū)動輪連線中心點(diǎn)，L為左右兩驅(qū)動輪輪距，R為煤礦巡檢機(jī)器人車體轉(zhuǎn)彎半徑，VL、VR和V分別為左、右驅(qū)動輪和質(zhì)心C的速度，導(dǎo)航角θ為車體中心與X軸的夾角。

根據(jù)車體機(jī)械結(jié)構(gòu)，結(jié)合運(yùn)動學(xué)分析可得，煤礦巡檢機(jī)器人運(yùn)動的轉(zhuǎn)彎半徑為：

車體質(zhì)心C的速度V和角速度ω與左右驅(qū)動輪速度VL、VR之間關(guān)系可分別表示為：

在全局坐標(biāo)系XOY坐標(biāo)系中，將式（2）車體質(zhì)心C的速度V沿X、Y軸分解，得到煤礦巡檢機(jī)器人在X軸與Y軸方向上的速度分量為：

對式（3）、（4）分別積分，可得煤礦巡檢機(jī)器人每一時刻位姿Pt的運(yùn)動狀態(tài)方程為

其中，P0為t = 0時刻煤礦巡檢機(jī)器人的初始狀態(tài)值。由式（5）可得，若煤礦巡檢機(jī)器人初始位置P0已知，通過對煤礦巡檢機(jī)器人左、右兩輪不同的速度規(guī)劃，減小煤礦巡檢機(jī)器人的角度偏差和位置偏差，實(shí)現(xiàn)煤礦巡檢機(jī)器人對預(yù)定軌跡的跟蹤[14-15]。

3 模糊控制系統(tǒng)設(shè)計

■3.1 控制系統(tǒng)結(jié)構(gòu)設(shè)計

通過對自動導(dǎo)引模塊即磁導(dǎo)航傳感器輸出信號，以及驅(qū)動輪編碼器的分析得到煤礦巡檢機(jī)器人在XOY坐標(biāo)系中的位姿狀態(tài) [ Xt,Yt,θt]T,與理論軌跡對比，得到實(shí)際位置偏差e和偏差變化率 e = Pi+2- Pi。當(dāng)e>0時，煤礦巡檢機(jī)器人車體軸向相對于理論軌跡偏右；當(dāng)e小于0時，相對于理論軌跡偏左。當(dāng)ec數(shù)值較大時，左右輪轉(zhuǎn)速偏差較大，轉(zhuǎn)向速度快；反之，左右輪轉(zhuǎn)速偏差較小，轉(zhuǎn)向速度慢。

將其作為控制系統(tǒng)的輸入變量，輸入至軌跡跟蹤模糊控制器中，經(jīng)過模糊推理，得到煤礦巡檢機(jī)器人左、右驅(qū)動輪速度差ΔV（左輪速度大于右輪速度設(shè)為正，反之則為負(fù)）作為輸出，通過速度控制器調(diào)節(jié)左右輪驅(qū)動電機(jī)轉(zhuǎn)速，最終控制煤礦巡檢機(jī)器人實(shí)現(xiàn)軌跡跟蹤。軌跡跟蹤控制框圖如圖5所示。

圖5 軌跡跟蹤控制框圖

■3.2 模糊控制器設(shè)計

模糊控制器結(jié)構(gòu)框圖如圖6所示。本文將磁導(dǎo)航傳感器反饋的位置偏差e及其變化率ec作為模糊控制器的輸入變量，左、右驅(qū)動輪速度差ΔV作為輸出變量。最終確定該煤礦巡檢機(jī)器人模糊控制器為雙輸入-單輸出的二維結(jié)構(gòu)。具體設(shè)計過程如下：

圖6 模糊控制器結(jié)構(gòu)框圖

（1）輸入輸出量模糊化

結(jié)合實(shí)際情況，位置偏差e基本論域設(shè)為[-0.1，0.1](單位：m)，量化因子為ke=60，位置偏差變化率ec實(shí)際論域設(shè)為[-0.05,0.05](單位：m)，量化因子為kec=120。速度差ΔV實(shí)際論域設(shè)為[-2，2](單位：m/s)，量化因子kv=0.33。為保證模糊集的完備性和一致性，提高控制精度，將以上變量量化為13個等級｛-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6｝；模糊集數(shù)設(shè)為7，其模糊集均表示為｛NB，NM，NS，ZO，PS，PM，PB｝，分別定義為：

E={高度偏左，中度偏左，低度偏左，偏差為零，低度偏右，中度偏右，高度偏右}

Ec={高速向左，中速向左，低速向左，偏差不變，低速向右，中速向右，高速向右}

V={快速左轉(zhuǎn)，中速左轉(zhuǎn)，慢速左轉(zhuǎn)，直線行駛，慢速右轉(zhuǎn)，中速右轉(zhuǎn)，快速右轉(zhuǎn)}

為保證控制器具有靈敏度高、穩(wěn)定性強(qiáng)、魯棒性好以及響應(yīng)速度快的特性，對于e、ec、ΔV的模糊論域的隸屬度函數(shù)取三角形隸屬度函數(shù)。隸屬度函數(shù)圖如圖7所示。

圖7 輸入、輸出變量隸屬度函數(shù)圖

（2）建立模糊規(guī)則

模糊規(guī)則是模糊控制器的核心，它的正確與否直接影響到模糊控制器的性能，其數(shù)目的多少也是衡量模糊控制器性能的一個重要因素[16]。

采用Mamdani推理法，根據(jù)雙輪差速驅(qū)動原理結(jié)合通過實(shí)驗(yàn)獲得的控制經(jīng)驗(yàn)確定模糊規(guī)則。本文模糊控制器為雙輸入-單輸出結(jié)構(gòu)，且兩個輸入量之間是與的關(guān)系，模糊規(guī)則如表1所示。模糊規(guī)則曲面如圖8所示。

圖8 模糊規(guī)則曲面圖

表2 模糊控制規(guī)則表

NM PB PB PM PM PS ZO ZO NS PB PB PM PS ZO NM NM ZO PB PB PM ZO NM NB NB PS PM PM ZO NS NM NB NB PM ZO ZO NS NM NM NB NB PB ZO ZO NS NM NM NB NB

（3）輸出量逆模糊化

為了將模糊推理得到的結(jié)果V(t)進(jìn)行輸出，還需要對V(t)進(jìn)行逆模糊化環(huán)節(jié)，以此來得到關(guān)于ΔV的精確值。

常用的逆模糊化方法有最大隸屬度法、面積中心法（重心法）、加權(quán)平均法等[17]。本文選用加權(quán)平均法對輸出量V(t)進(jìn)行逆模糊化處理，其表達(dá)式如下：

式中：vi—輸出值所對應(yīng)的精確控制量；Ki—根據(jù)實(shí)際情況選擇，影響系統(tǒng)的響應(yīng)特性。

4 仿真結(jié)果分析與電機(jī)實(shí)驗(yàn)調(diào)試

根據(jù)設(shè)計要求，煤礦巡檢機(jī)器人左右輪輪距為0.53m，設(shè)置期望狀態(tài)為 V = 1m / s, ω= 1r ad/s ,利用Matlab/Simulink平臺分別搭建模糊控制器和常規(guī)PID控制器對機(jī)器人在直線路徑和圓形路徑兩種路徑下自動跟蹤效果進(jìn)行仿真。

■4.1 直線路徑

圖9 直線路徑仿真結(jié)果

■4.2 圓形路徑

圖10 圓形路徑仿真結(jié)果

由圖9和圖10仿真結(jié)果可知，通過模糊控制的煤礦巡檢機(jī)器人在1.5s時達(dá)到預(yù)設(shè)路徑，方向偏差xe、ye超調(diào)量均≤0.1m,導(dǎo)航角θe超調(diào)量≤0.1rad，而常規(guī)PID控制調(diào)節(jié)時間為2.2s ,y方向超調(diào)量為0.2m，航向偏角 θ最大超調(diào)量為0.35rad。相較于常規(guī)PID控制，本文設(shè)計的模糊控制器不需要精確計算AGV小車的控制律，而是根據(jù)運(yùn)行過程中的偏差實(shí)時調(diào)整參數(shù)以達(dá)到穩(wěn)定，且收斂速度快，超調(diào)量小，穩(wěn)定性強(qiáng)，能夠滿足實(shí)際軌跡跟蹤的需要。

■4.3 實(shí)驗(yàn)調(diào)試

良好的伺服電機(jī)精度決定了控制系統(tǒng)最終輸出結(jié)果執(zhí)行度高低。為了提高伺服電機(jī)的精度，對電機(jī)控制環(huán)路進(jìn)行調(diào)試。該款電機(jī)調(diào)試軟件如圖11所示。其位置環(huán)、電流環(huán)參數(shù)默認(rèn)自動調(diào)整，故通過調(diào)整速度環(huán)參數(shù)進(jìn)行精度調(diào)試。

圖11 上位機(jī)界面

將電機(jī)轉(zhuǎn)速設(shè)置為500rpm，通過不斷調(diào)整速度比例增益Vp和速度積分增益Vi，最終獲得了較為理想的實(shí)驗(yàn)結(jié)果。實(shí)驗(yàn)結(jié)果如圖12(a)和圖12（b）所示。

圖12 電機(jī)速度環(huán)調(diào)試前后對比圖

由圖12(a)和圖12(b)可以得出經(jīng)調(diào)試后電機(jī)實(shí)際速度雖略有延遲，但精度較高，可較好的執(zhí)行控制系統(tǒng)的輸出結(jié)果。

5 結(jié)論

（1）本文研究了一種基于高精度磁導(dǎo)航傳感器和RFID射頻識別模塊的雙輪差速驅(qū)動煤礦巡檢機(jī)器人，完成了其機(jī)械系統(tǒng)結(jié)構(gòu)設(shè)計和控制系統(tǒng)硬件設(shè)計

（2）通過對該煤礦巡檢機(jī)器人進(jìn)行數(shù)學(xué)建模及分析，結(jié)合通過實(shí)驗(yàn)獲得的控制經(jīng)驗(yàn)確定模糊規(guī)則，提出了一款適用于該模型的模糊控制軌跡跟蹤方法，將磁導(dǎo)航傳感器反饋的位置偏差e及其變化率ec作為輸入變量，左、右驅(qū)動輪速度差ΔV作為輸出變量，采用模糊控制方法調(diào)節(jié)驅(qū)動輪速度以實(shí)現(xiàn)軌跡跟蹤的目的。

（3）仿真結(jié)果表明：相比于常規(guī)PID控制器，本文設(shè)計的模糊控制器具有響應(yīng)快、超調(diào)量小、穩(wěn)定性強(qiáng)，系統(tǒng)可靠性和魯棒性更強(qiáng)的特性，能夠滿足實(shí)際軌跡跟蹤的需要。