懸臂式掘進機視覺伺服截割控制系統(tǒng)研究

懸臂式掘進機視覺伺服截割控制系統(tǒng)研究

張旭輝，趙建勛，張超，楊文娟（1.西安科技大學(xué)機械工程學(xué)院，陜西西安710054；2.陜西省礦山機電裝備智能監(jiān)測重點實驗室，陜西西安710054）0前言未來較長時間內(nèi)煤炭仍將是我國主體能源［1］，但我國因巷道掘進困難而導(dǎo)致采掘比例嚴重失調(diào)［2］。巷道掘進工作中環(huán)境復(fù)雜、司機視野差，通過掘進機司機手動操作進行煤巷掘進作業(yè)無法準確控制掘進機的運行位姿，導(dǎo)致巷道掘進過程中出現(xiàn)巷道成形質(zhì)量差、掘進效率低的問題。因此，實現(xiàn)掘進機的信息化、自動化和智能化對減輕工人勞動強度、提高開采效率具有重要意義［3］。目前國內(nèi)外針對懸臂式掘進機的自動截割控制的研究現(xiàn)狀如下：日本三井三池公司針對截割控制研發(fā)了截割功率自適應(yīng)控制系統(tǒng)［4］；毛君等［5］建立了斷面仿形截割控制的數(shù)學(xué)模型；吳淼等［6］利用激光測距儀和傾角傳感器設(shè)計掘進機位姿檢測系統(tǒng)；王蘇彧等［7］根據(jù)液壓缸伸縮量與截割頭的位置關(guān)系，利用傾角傳感器及旋轉(zhuǎn)編碼器測量液壓缸的伸縮量計算得到截割頭位置，并提出了掘進機記憶截割控制方法；毛清華等［8］利用行程傳感器、超聲和慣導(dǎo)等多傳感器檢測掘進機機身空間位姿，可有效準確地檢測掘進機的空間位姿。目前針對懸臂式掘進機所建立的控制系統(tǒng)中，大多利用位移傳感器的間接式位移傳感器及角度編碼器來反饋掘進機截割頭位姿信息，但利用位移傳感器間接式測量方式對安裝精度有較高的要求，且在振動環(huán)境下，現(xiàn)有方法的測量穩(wěn)定性和傳感器壽命較低。筆者采用非接觸式視覺測量方法［9］，利用視覺伺服控制技術(shù)，根據(jù)視覺測量得到的信息作為視覺伺服的信息控制掘進機關(guān)節(jié)驅(qū)動器實現(xiàn)掘進機自動截割。1視覺伺服控制1.1視覺伺服控制原理視覺伺服系統(tǒng)是利用視覺測量信息作為反饋的控制系統(tǒng)［10］，該系統(tǒng)由視覺測量系統(tǒng)、機器人和運動控制系統(tǒng)組成，利用視覺測量系統(tǒng)獲取環(huán)境信息并反饋到控制器中，并根據(jù)控制策略對機器人進行控制。視覺測量相比其他傳感器具有測量范圍廣、信息量大等特點，可以快速、有效地獲得環(huán)境信息。視覺伺服分為基于位置的視覺伺服、基于圖像的視覺伺服［11］。基于位置伺服控制系統(tǒng)根據(jù)建立的目標與攝像機之間的數(shù)學(xué)模型將圖像信息轉(zhuǎn)換成世界坐標系中的位置信息，并傳給機器人關(guān)節(jié)控制器控制機器人運動［12］?；趫D像的視覺伺服在二維圖像上通過計算圖像的雅克比矩陣得到位置誤差，該方法對標定誤差和空間模型誤差不敏感，但伺服過程中容易出現(xiàn)圖像雅克比矩陣奇異點。1.2掘進機視覺伺服將視覺伺服控制技術(shù)用于懸臂式掘進機的自動截割控制中，利用視覺測量方法實現(xiàn)對截割頭位置的檢測和精確定位，并建立掘進機運動學(xué)模型實現(xiàn)掘進機的自動截割控制。采用位置反饋型機器人視覺控制技術(shù)以及動態(tài)look-and-move的結(jié)構(gòu)［13］方式建立懸臂式掘進機視覺伺服截割控制系統(tǒng)，利用視覺進行位置反饋的掘進機視覺伺服控制結(jié)構(gòu)示意如圖1所示。圖1掘進機視覺伺服控制結(jié)構(gòu)Fig.1Visualservocontrolstructureofroadheader系統(tǒng)主要任務(wù)是根據(jù)對截割頭規(guī)劃的截割軌跡，利用固定在機體上的工業(yè)相機連續(xù)拍攝固定在截割臂上標靶圖像并進行快速圖像處理，計算得到截割頭位姿，在對掘進機進行運動學(xué)建?；A(chǔ)之上，計算截割頭在巷道中位姿，并將得到的三維截割頭位置信息反饋到控制器中，再根據(jù)截割頭位姿調(diào)整策略利用關(guān)節(jié)控制器和伺服放大器對掘進機液壓缸進行控制。與傳統(tǒng)的利用位移傳感器獲取截割頭位置信息相比，視覺測量方法通過非接觸測量可以快速、準確地獲得有效信息，不僅不需要改變掘進機的結(jié)構(gòu)加裝傳感器，并且具有較好的測量穩(wěn)定性和準確性。2懸臂式掘進機視覺伺服截割控制系統(tǒng)設(shè)計懸臂式掘進機視覺伺服系統(tǒng)由懸臂式掘進機、DSP控制器、CCD工業(yè)相機、紅外標靶機載計算機和各傳感器組成。利用視覺進行位置反饋的懸臂式掘進機自動截割控制系統(tǒng)總體方案如圖2所示，系統(tǒng)包括視覺測量模塊、截割頭位姿解算模塊、掘進機運動控制模塊、數(shù)據(jù)存儲顯示模塊等。圖2掘進機視覺伺服截割控制系統(tǒng)總體方案Fig.2Generalschemeofvisualservocuttingcontrolsystemforroadheade如圖2所示，掘進機視覺伺服控制系統(tǒng)采用機載計算機+DSP控制器的架構(gòu)，系統(tǒng)依靠人工在計算機中輸入規(guī)劃路徑信息，采用視覺測量方法測量截割頭相對于機身的位置信息，利用超聲波傳感器和捷聯(lián)慣導(dǎo)測量并計算得到掘進機在巷道空間中的機身位姿。根據(jù)截割頭相對于機身位姿信息及掘進機在巷道中的位姿信息，利用D-H方法進行運動學(xué)正向解算得到在巷道空間下截割頭的位姿。將解算得到的巷道空間下的截割頭位姿反饋到DSP控制器中，并與規(guī)劃的路徑信息比較得到截割頭的誤差信息，根據(jù)誤差信息采用反變換法對掘進機逆運動學(xué)解得到截割頭需擺動的關(guān)節(jié)角度。采用PID控制方法，利用DSP控制器輸出控制命令通過電液比例閥驅(qū)動升降油缸及回轉(zhuǎn)油缸來實現(xiàn)對截割臂擺動的控制。并將解算得到的位姿數(shù)據(jù)保存到MYSQL數(shù)據(jù)庫中，在巷道斷面中用圖形的方式顯示截割頭截割軌跡，監(jiān)視系統(tǒng)工作時的運動狀態(tài)。軟件系統(tǒng)采用多線程方式實現(xiàn)多任務(wù)并行控制，其中在視覺測量模塊中完成圖像處理及位姿解算；在位姿解算模塊中計算截割頭的位置信息；在數(shù)據(jù)存儲顯示模塊中完成各數(shù)據(jù)的存儲及截割路徑的顯示功能。在運動控制模塊中根據(jù)得到的誤差信息利用DSP的D／A模塊發(fā)出控制信號實現(xiàn)掘進機的截割控制，各模塊共同工作實現(xiàn)掘進機的視覺伺服截割控制。3掘進視覺伺服系統(tǒng)關(guān)鍵技術(shù)3.1視覺測量模塊懸臂式掘進機視覺測量系統(tǒng)如圖3所示，視覺測量模塊由機載計算機、CCD工業(yè)相機、工業(yè)以太網(wǎng)以及紅外LED標靶組成。視覺測量模塊采用單目視覺測量技術(shù)，進行截割頭的位姿計算，得到截割頭相對于機身的位姿。圖3懸臂式掘進機視覺測量系統(tǒng)Fig.3Visionmeasuringsystemofcantileverroadheade利用攝像機采用軟觸發(fā)的形式實時采集紅外標靶的圖像，將采集到的圖像通過千兆網(wǎng)GigE接口傳輸?shù)綑C載計算機中。通過視覺測量模塊實現(xiàn)了懸臂式掘進機截割頭的位姿解算。為保證實時性，設(shè)定視覺測量的周期不超過100ms。由于測量系統(tǒng)時根據(jù)圖像的信息計算得到掘進機截割頭的空間位置，因此視覺測量系統(tǒng)中的圖像分析處理過程直接影響系統(tǒng)的控制精度。為保證控制系統(tǒng)的控制精度，對測量系統(tǒng)的圖像進行預(yù)處理，減小由于圖像噪聲等問題引起的系統(tǒng)誤差。對采集得到的圖像進行去噪及圖像增強預(yù)處理，如圖4所示。圖4圖像處理流程Fig.4Flowofimageprocessing根據(jù)圖4中圖像處理流程，對輸入的圖像利用高斯濾波器去除圖像中的噪聲干擾，利用圖像二值化對圖像進一步處理，獲取圖像中的光斑區(qū)域，再在各區(qū)域中利用灰度質(zhì)心方法提取圖像中的光斑中心坐標［14-16］。最后根據(jù)文獻［9］中掘進機截割頭姿態(tài)測量方法，利用攝像機外參標定計算并得到截割頭相對于掘進機機身的位姿。3.2位姿解算模塊圖5為根據(jù)掘進機運動學(xué)模型所建懸臂式掘進機的坐標系。其中坐標系O0X0Y0Z0、O1X1Y1Z1、O2X2Y2Z2、O3X3Y3Z3、O4X4Y4Z4、OcXcYcZc、OwXwYwZw分別為懸臂式掘進機基坐標系、回轉(zhuǎn)關(guān)節(jié)坐標系、抬升關(guān)節(jié)坐標系、伸縮關(guān)節(jié)坐標系、截割頭坐標系攝像機坐標系及標靶坐標系，OdXdYdZd為巷道斷面坐標系，以截割斷面左下角作為原點，豎直方向為Zd軸方向，Xd軸方向垂直于截割面，Yd軸方向平行于截割斷面水平向左；系統(tǒng)工作時，首先將掘進機初始姿態(tài)的基坐標系與巷道斷面坐標系各方向保持一致，并設(shè)初始姿態(tài)下掘進機的各方向旋轉(zhuǎn)角度均為0。將捷聯(lián)慣導(dǎo)系統(tǒng)固定在掘進機機體上，實時測量掘進機機身位姿，即橫滾角、俯仰角及偏航角的角度變化量。利用安裝在機身上的超聲波傳感器測量掘進機機身距離兩側(cè)煤壁以及截割斷面的距離，結(jié)合掘進機姿態(tài)角，可以計算得到掘進機基坐標系原點巷道斷面坐標系下的Xd、Yd和Zd軸方向下的坐標。然后根據(jù)姿態(tài)計算方法可以得到基坐標系O0X0Y0Z0相對于巷道斷面坐標系的變換矩陣d0T：圖5掘進機坐標系Fig.5Roadheadercoordinatesystem式中：α、β、γ分別為掘進機的橫滾角、俯仰角及偏航角的變化量；SX、SY、SZ分別為機體坐標系原點在巷道斷面坐標系下的各個方向下的坐標。得到掘進機截割頭坐標系與巷道斷面坐標系之間的轉(zhuǎn)換關(guān)系之后，可以計算得到任意時刻截割頭在斷面的位置，并在軌跡顯示模塊中顯示截割頭的軌跡。同時，根據(jù)截割頭實際位置與規(guī)劃掘進機運動軌跡的誤差信息，利用反變化法對掘進機逆運動學(xué)求解，計算得到任意時刻的驅(qū)動關(guān)節(jié)角度。4系統(tǒng)控制模型懸臂式掘進機視覺伺服截割控制系統(tǒng)的閉環(huán)系統(tǒng)結(jié)構(gòu)如圖6所示。為實現(xiàn)控制系統(tǒng)的精確控制并且具有一定穩(wěn)定性，采用PID控制方法［17］對掘進機進行控制?？刂葡到y(tǒng)采用閉環(huán)控制，利用視覺反饋得到的掘進機截割頭位姿測量，經(jīng)過位姿解算模塊得到截割頭于截割斷面上的位置信息。據(jù)規(guī)劃的截割頭軌跡及與計算得到位姿的偏差經(jīng)過PID控制器的調(diào)控，利用控制器發(fā)出指令經(jīng)過比例放大器及電液比例閥控制液壓缸伸縮，使掘進機截割頭到達期望位置。圖6掘進機閉環(huán)控制系統(tǒng)結(jié)構(gòu)Fig.6Roadheaderclosed-loopcontrolsystemstructure根據(jù)文獻［18］可知，通過分析比例放大器、電液比例閥、液壓缸等各環(huán)節(jié)特性可以得到掘進機視覺伺服截割控制系統(tǒng)傳遞函數(shù)數(shù)學(xué)模型，在截割頭的視覺測量部分中，可以將視覺測量部分看作是一個比例環(huán)節(jié)，在理想情況下，位姿反饋系數(shù)為1。掘進機自動截割控制系統(tǒng)傳遞函數(shù)模型如圖7所示。圖7掘進機截割控制系統(tǒng)傳遞函數(shù)模型Fig.7Transferfunctionmodeloftunnelingcontrolsystemofroadheader5系統(tǒng)試驗及結(jié)果5.1視覺測量試驗首先對視覺測量系統(tǒng)進行精度試驗，采用視覺測量系統(tǒng)對截割頭進行跟蹤定位，采集懸臂運動過程中標靶的圖像，并計算截割頭姿態(tài)角。搭建礦用懸臂式掘進機截割頭姿態(tài)視覺測量測量試驗平臺如圖8所示，選用與EBZ160型掘進機比例為1∶5的掘進機模型，圖中①為MV-EM130M工業(yè)相機并加波長為850nm的窄波濾鏡，圖中②采用300mm×300mm的方形標靶，標靶上均勻布置16個紅外LED燈。圖8礦用懸臂式掘進機截割頭姿態(tài)視覺測量試驗平臺Fig.8Experimentalplatformforvisualmeasurementofcuttingheadattitudeofminecantileverroadheade懸臂式掘進機試驗平臺主要參數(shù)如下：由于在掘進機工作環(huán)境下會存在雜光的干擾，并且LED燈光的波長大于900nm，因此在工業(yè)相機前加濾鏡去除雜光。采集標靶圖像如圖9所示，圖9a為利用工業(yè)相機直接拍攝紅外標靶的圖像，圖9b為相機加裝濾鏡后對紅外標靶所拍攝的圖像。圖9采集標靶圖像Fig.9Acquisitionoftargetimages在不同位置下利用視覺測量傳感器對截割頭的俯仰角進行實時測量，設(shè)定圖像采集時間間隔為100ms，利用視覺測量模塊實時測量截割臂8s內(nèi)的姿態(tài)角的變化，并采用精度為0.05°的傾角傳感器測量掘進機截割臂姿態(tài)角的真實值。為方便計算設(shè)定掘進機位于巷道中間位置，并且當截割頭水平且位于掘進機中間位置時設(shè)定截割頭回轉(zhuǎn)角及升降角分別為0°和-90°。得到的試驗結(jié)果及試驗誤差如圖10、圖11所示。圖10截割頭俯仰角測量結(jié)果Fig.10MeasurementresultofpitchAngleofcuttinghead圖11截割頭俯仰角測量結(jié)果誤差Fig.11Errorofpitchingangleofcuttinghead根據(jù)試驗結(jié)果可知，2次利用傾角傳感器測量的掘進機截割臂的俯仰角度分別為-61.88°及-90.03°。使用視覺測量系統(tǒng)測量截割頭2次位置的截割頭角度時，截割頭的最大誤差為±0.4°。5.2掘進機視覺伺服截割控制試驗懸臂式掘進機視覺伺服截割控制試驗平臺如圖12所示。測試系統(tǒng)由懸臂式掘進機模型、計算機、MV-EMV130M防爆工業(yè)相機、300mm×300mm的紅外標靶、捷聯(lián)慣導(dǎo)、超聲波傳感器、DSP控制器以及掘進機組成。在懸臂式掘進機視覺伺服截割控制試驗中，通過固定在掘進機機身上的防爆工業(yè)相機采集固定在掘進機截割臂上的紅外標靶的圖像，使用加裝在鏡頭前的濾鏡去除雜光，經(jīng)過圖像預(yù)處理、光斑中心定位及截割頭位姿結(jié)算計算得到截割頭坐標系相對于機身坐標系之間轉(zhuǎn)換關(guān)系。放置兩擋板于機身兩側(cè)及機身前端模擬煤壁，利用超聲測距傳感測得機身距煤壁的距離。融合捷聯(lián)慣導(dǎo)及超聲波傳感器測得數(shù)據(jù)計算得到截割頭坐標系與巷道斷面坐標系之間的轉(zhuǎn)換關(guān)系。并將得到的數(shù)據(jù)通過RS485反饋到DSP控制器中，控制器根據(jù)規(guī)劃路徑與截割頭實際位姿之間的誤差輸出控制命令通過電液比例閥驅(qū)動升降油缸及回轉(zhuǎn)油缸控制截割頭運動。為保證系統(tǒng)的實時性，設(shè)定相機采集圖像時間間隔為100ms。預(yù)先在巷道斷面坐標系中規(guī)劃掘進機截割頭的運動軌跡。當掘進機機身為靜止時，截割頭在巷道斷面的截割路徑為弧線，其軌跡規(guī)劃示意如圖13所示。圖12懸臂式掘進機視覺伺服截割控制試驗平臺Fig.12Testplatformforvisualservocuttingcontrolofcantileverroadheade圖13軌跡規(guī)劃示意Fig.13Schematicoftrajectoryplanning按實際巷道等比例設(shè)定模擬巷道的尺寸為1000mm×1400mm，在斷面坐標系下設(shè)以A（50，-77，191.5）為起點，根據(jù)圖13中規(guī)劃軌跡進行自動截割，其中點B點坐標為（50，-1223，191.5）、C點坐標為（50，-1243，411.6）和D點坐標為（50，-56，411.6）。根據(jù)截割頭的解算模塊可計算得到截割頭位于A、B、C和D點時，截割頭的回轉(zhuǎn)角及升降角的理論值分別為（-34°，-69°）、（28°，-69°）、（28°，-82°）以及（-34°，-82°），使掘進機的截割部空載運行，根據(jù)規(guī)劃的截割軌跡截割斷面，通過精度為0.01°的傾角傳感器和精度為0.05°的角度編碼器測量得到截割頭垂直擺角和水平擺角的實際值，掘進機截割頭截割軌跡如圖14所示，紅線為掘進機截割頭截割時的實際運行軌跡，藍線為掘進機的規(guī)劃截割軌跡，如圖14可以看出截割頭運動軌跡可以穩(wěn)定地按照規(guī)劃的截割路徑運動。試驗結(jié)果及試驗誤差如圖15所示。圖14掘進機截割頭截割軌跡Fig.14Cuttingtrackofcuttingheadofroadheader圖15中，根據(jù)圖15a和圖15b中截割頭姿態(tài)角誤差可知，