基于視覺測量的快速掘進(jìn)機(jī)器人糾偏控制研究

張旭輝， 周創(chuàng)， 張超， 謝楠， 張楷鑫， 劉博興

(1.西安科技大學(xué) 機(jī)械工程學(xué)院， 陜西 西安 710054；2.陜西省礦山機(jī)電裝備智能監(jiān)測重點(diǎn)實(shí)驗(yàn)室， 陜西 西安 710054)

0 引言

目前，煤礦井下仍存在大量人工手動(dòng)操控的懸臂式掘進(jìn)機(jī)，導(dǎo)致巷道掘進(jìn)效率低，成形質(zhì)量差[1-2]。發(fā)展巷道快速掘進(jìn)成套技術(shù)裝備、提高掘進(jìn)智能化水平已成為保障煤礦安全高效生產(chǎn)的先決條件[3-4]。其中，具有精確定位和自動(dòng)糾偏控制功能的快速掘進(jìn)機(jī)器人是實(shí)現(xiàn)巷道掘進(jìn)智能化的關(guān)鍵[5-6]。

許多學(xué)者對煤礦井下掘進(jìn)機(jī)定位與控制技術(shù)進(jìn)行了深入研究。陶云飛[7]提出了一種基于激光導(dǎo)向系統(tǒng)的掘進(jìn)機(jī)位姿自動(dòng)測量方法。楊文娟等[8]提出了一種基于激光束和紅外光斑特征的懸臂式掘進(jìn)機(jī)機(jī)身及截割頭位姿視覺測量系統(tǒng)。田原[9]建立了掘進(jìn)機(jī)慣性定位系統(tǒng)，將PHINS慣性導(dǎo)航系統(tǒng)用于掘進(jìn)機(jī)自動(dòng)定位，利用零速修正法提高其定位精度。吳淼等[10]提出了一種基于空間交匯測量技術(shù)的掘進(jìn)機(jī)自主位姿測量方法。張旭輝等[11]構(gòu)建了虛擬設(shè)備及工作場景的模型，提出虛擬工作面的動(dòng)態(tài)修正方法和設(shè)備的碰撞檢測方法，實(shí)現(xiàn)了監(jiān)測數(shù)據(jù)驅(qū)動(dòng)下的虛擬交互式設(shè)備遠(yuǎn)程操控。朱偉等[12]設(shè)計(jì)了一種基于嵌入式軟PLC技術(shù)的掘進(jìn)機(jī)控制系統(tǒng)，實(shí)現(xiàn)了掘進(jìn)機(jī)位姿監(jiān)測與遙控控制。瞿圓媛等[13]建立了掘進(jìn)機(jī)行走位姿偏差模型，在此基礎(chǔ)上對掘進(jìn)機(jī)行進(jìn)糾偏調(diào)度規(guī)劃與控制進(jìn)行了研究。但上述掘進(jìn)機(jī)控制技術(shù)主要以掘進(jìn)機(jī)自主定位為基礎(chǔ)，未考慮在井下高粉塵、低照度環(huán)境中掘進(jìn)機(jī)定位與控制精度不高的問題。本文提出一種基于視覺測量的快速掘進(jìn)機(jī)器人糾偏控制系統(tǒng)，對快速掘進(jìn)機(jī)器人進(jìn)行位姿調(diào)整，實(shí)現(xiàn)對快速掘進(jìn)機(jī)器人的精確控制。

1 快速掘進(jìn)機(jī)器人糾偏控制系統(tǒng)

1.1 快速掘進(jìn)機(jī)器人總體結(jié)構(gòu)

快速掘進(jìn)機(jī)器人是集斷面連續(xù)截割、支護(hù)、自動(dòng)定位、無線遙控、快速裝運(yùn)和機(jī)械除塵等功能于一體的快速巷道掘進(jìn)裝備[14]。快速掘進(jìn)機(jī)器人總體結(jié)構(gòu)，由掘進(jìn)機(jī)器人、臨時(shí)支護(hù)機(jī)器人1&2(組成護(hù)盾結(jié)構(gòu))、鉆錨機(jī)器人、錨網(wǎng)運(yùn)輸機(jī)器人、電液控平臺(tái)、運(yùn)輸及通風(fēng)除塵系統(tǒng)等組成，如圖1所示。

快速掘進(jìn)機(jī)器人通過前后盾體之間的推拉作用實(shí)現(xiàn)推進(jìn)，并反復(fù)進(jìn)行截割-推進(jìn)過程，直至完成整個(gè)巷道的掘進(jìn)工作。推進(jìn)過程中通過視覺測量方法檢測快速掘進(jìn)機(jī)器人位姿并反饋給電液控平臺(tái)，電液控平臺(tái)根據(jù)位姿偏差，控制臨時(shí)支護(hù)機(jī)器人上的水平和垂直伸縮油缸來調(diào)整整個(gè)系統(tǒng)方向、位置和姿態(tài)，保證下一階段巷道的截割精度。

1-掘進(jìn)機(jī)器人； 2-臨時(shí)支護(hù)機(jī)器人1； 3-臨時(shí)支護(hù)機(jī)器人2；4-鉆錨機(jī)器人； 5-錨網(wǎng)運(yùn)輸機(jī)器人； 6-電液控平臺(tái)；7-運(yùn)輸及通風(fēng)除塵系統(tǒng)。

1.2 快速掘進(jìn)機(jī)器人糾偏控制系統(tǒng)架構(gòu)

快速掘進(jìn)機(jī)器人糾偏控制系統(tǒng)由激光指向儀、防爆相機(jī)、防爆計(jì)算機(jī)、DSP控制器等組成，如圖2所示。

圖2 快速掘進(jìn)機(jī)器人糾偏控制系統(tǒng)架構(gòu)Fig.2 Framework of deviation correction control system for rapid tunneling robot

利用安裝在快速掘進(jìn)機(jī)器人上的防爆相機(jī)，對后方的激光指向儀進(jìn)行圖像采集，通過以太網(wǎng)將采集的圖像信息傳輸給防爆計(jì)算機(jī)，利用防爆計(jì)算機(jī)對圖像進(jìn)行預(yù)處理，根據(jù)位姿解算模型解算出快速掘進(jìn)機(jī)器人位姿信息，并將解算出的位姿信息保存在MySQL數(shù)據(jù)庫中。將解算的位姿信息與巷道設(shè)計(jì)軸線信息進(jìn)行比較，計(jì)算出快速掘進(jìn)機(jī)器人位姿與巷道設(shè)計(jì)軸線間的位姿偏差。根據(jù)位姿偏差的類型與大小，采用不同的控制策略計(jì)算出修正后控制量，并輸出控制指令，數(shù)字信號(hào)的控制指令通過數(shù)模轉(zhuǎn)換(D/A)模塊轉(zhuǎn)換成模擬信號(hào)，模擬信號(hào)經(jīng)放大器放大后控制電磁比例閥的閥芯移動(dòng)，實(shí)現(xiàn)對伸縮油缸的控制，根據(jù)各區(qū)域伸縮油缸的伸縮量不同，對快速掘進(jìn)機(jī)器人進(jìn)行位姿調(diào)整，實(shí)現(xiàn)糾偏控制。

2 快速掘進(jìn)機(jī)器人位姿視覺測量

2.1 快速掘進(jìn)機(jī)器人位姿視覺測量方案

快速掘進(jìn)機(jī)器人位姿視覺測量由臨時(shí)支護(hù)機(jī)器人、防爆相機(jī)和2個(gè)激光指向儀實(shí)現(xiàn)，如圖3所示。防爆相機(jī)安裝在臨時(shí)支護(hù)機(jī)器人1的內(nèi)壁頂端，2個(gè)激光指向儀安裝在掘進(jìn)后方巷道頂端，平行且指向巷道軸線方向。

圖3 快速掘進(jìn)機(jī)器人位姿視覺測量方案Fig.3 Pose vision measurement scheme of rapid tunneling robot

快速掘進(jìn)機(jī)器人上帶有濾鏡的防爆相機(jī)對巷道后方的激光指向儀進(jìn)行圖像采集，對圖像進(jìn)行去噪、增強(qiáng)、畸變校正等處理，利用霍夫變換提取直線特征，根據(jù)建立的位姿解算模型，得到防爆相機(jī)在激光指向儀坐標(biāo)系下的位姿信息。通過快速掘進(jìn)機(jī)器人坐標(biāo)系與防爆相機(jī)坐標(biāo)系間的轉(zhuǎn)換關(guān)系，得到快速掘進(jìn)機(jī)器人在激光指向儀坐標(biāo)系下的位姿信息。利用全站儀測量激光指向儀在巷道坐標(biāo)系下的絕對坐標(biāo)，得到激光指向儀坐標(biāo)系與巷道坐標(biāo)系的轉(zhuǎn)換關(guān)系，結(jié)合快速掘進(jìn)機(jī)器人在激光指向儀坐標(biāo)系下的位姿信息，最終得到快速掘進(jìn)機(jī)器人在巷道坐標(biāo)系中的位姿信息。

2.2 快速掘進(jìn)機(jī)器人位姿解算

快速掘進(jìn)機(jī)器人坐標(biāo)系如圖4所示，主要涉及巷道坐標(biāo)系OXYZ、激光指向儀坐標(biāo)系OjXjYjZj、防爆相機(jī)坐標(biāo)系OcXcYcZc和快速掘進(jìn)機(jī)器人坐標(biāo)系OoXoYoZo。

圖4 快速掘進(jìn)機(jī)器人坐標(biāo)系Fig.4 Coordinate system of rapid tunneling robot

快速掘進(jìn)機(jī)器人位姿解算模型如圖5所示。通過巷道-激光指向儀-防爆相機(jī)-快速掘進(jìn)機(jī)器人間的坐標(biāo)轉(zhuǎn)換關(guān)系，計(jì)算快速掘進(jìn)機(jī)器人在巷道坐標(biāo)系下的位姿。其中，Q1，Q2為2個(gè)激光光斑中心，L1，L3為2條激光束直線，L2為Q1和Q2連線，l1，l2，l3，q1，q2分別為L1，L2，L3，Q1，Q2在相機(jī)投影平面上的投影，Q3為激光束L1上的一點(diǎn)，Q4為激光束L3上的一點(diǎn)。

圖5 快速掘進(jìn)機(jī)器人位姿解算模型Fig.5 Pose calculation model for rapid tunneling robot

圖5中，平面OcQ1Q4，OcQ1Q2，OcQ2Q3的法向量為N1，N2，N3。有

V1=V3=N1×N3

(1)

V2=V1×N2

(2)

根據(jù)式(1)與式(2)求出防爆相機(jī)坐標(biāo)系下L1的方向向量V1，L2的方向向量V2和L3的方向向量V3。

激光束L1與L3間的距離為M，投影平面上l1與l3間的距離為m。有

(3)

已知激光指向儀尺寸參數(shù)，建立激光指向儀坐標(biāo)系，求出激光指向儀坐標(biāo)系下L1的方向向量W1，L2的方向向量W2，L3的方向向量W3，光斑中心Q1，Q2在激光指向儀坐標(biāo)系下的坐標(biāo)為p1，p2。

防爆相機(jī)坐標(biāo)系到激光指向儀坐標(biāo)系的旋轉(zhuǎn)矩陣為R，平移矩陣為T，有

V=RW

(4)

Q=Rp+T

(5)

由式(4)與式(5)求出防爆相機(jī)坐標(biāo)系到激光指向儀坐標(biāo)系的旋轉(zhuǎn)矩陣、平移矩陣。根據(jù)旋轉(zhuǎn)矩陣和平移矩陣得到激光指向儀坐標(biāo)系下快速掘進(jìn)機(jī)器人的位姿信息。通過巷道-激光指向儀-防爆相機(jī)-快速掘進(jìn)機(jī)器人間的坐標(biāo)轉(zhuǎn)換關(guān)系，最終得到巷道坐標(biāo)系下快速掘進(jìn)機(jī)器人的位姿。

3 快速掘進(jìn)機(jī)器人糾偏控制

3.1 控制策略

快速掘進(jìn)機(jī)器人推進(jìn)系統(tǒng)如圖6所示。16個(gè)伸縮油缸均勻地布置在臨時(shí)支護(hù)機(jī)器人的殼體內(nèi)側(cè)。水平方向的伸縮油缸分為上下和左右2個(gè)區(qū)域，上下區(qū)域的伸縮油缸用于調(diào)整豎直方向的位姿偏差和俯仰角，左右區(qū)域的伸縮油缸用于調(diào)整水平方向的位姿偏差和偏航角。針對不同偏差，調(diào)整相應(yīng)區(qū)域伸縮油缸行程可實(shí)現(xiàn)快速掘進(jìn)機(jī)器人的位姿控制。

圖6 快速掘進(jìn)機(jī)器人推進(jìn)系統(tǒng)Fig.6 Propulsion system of rapid tunneling robot

快速掘進(jìn)機(jī)器人糾偏控制目標(biāo)是使快速掘進(jìn)機(jī)器人的機(jī)頭位姿偏差保持在設(shè)計(jì)誤差范圍內(nèi)?？焖倬蜻M(jìn)機(jī)器人沿巷道設(shè)計(jì)軸線前進(jìn)的過程中，利用位姿視覺測量方法檢測快速掘進(jìn)機(jī)器人的位姿，計(jì)算位姿偏差，在位姿偏差小于設(shè)計(jì)誤差時(shí)保持當(dāng)前推進(jìn)狀態(tài)，在位姿偏差大于設(shè)計(jì)誤差時(shí)，根據(jù)偏差類型及大小選擇合適的控制策略，控制快速掘進(jìn)機(jī)器人在設(shè)計(jì)誤差范圍內(nèi)正常運(yùn)行。

快速掘進(jìn)機(jī)器人姿態(tài)類型如圖7所示。規(guī)定水平方向向左偏為負(fù)，向右偏為正；豎直方向向上偏為正，向下偏為負(fù)。圖7(a)為快速掘進(jìn)機(jī)器人軸線與巷道設(shè)計(jì)軸線間的位姿誤差在設(shè)計(jì)誤差范圍內(nèi)，此時(shí)不需要糾偏，可以繼續(xù)保持當(dāng)前控制量推動(dòng)掘進(jìn)機(jī)器人前進(jìn)。圖7(b)為快速掘進(jìn)機(jī)器人向左偏，偏航角為負(fù)，應(yīng)增大左區(qū)伸縮油缸油壓，減小右區(qū)伸縮油缸油壓，將其向右調(diào)整。圖7(c)為快速掘進(jìn)機(jī)器人向右偏，偏航角為正，應(yīng)增大右區(qū)伸縮油缸油壓，減小左區(qū)伸縮油缸油壓，將其向左調(diào)整。圖7(d)為快速掘進(jìn)機(jī)器人向上偏，俯仰角為正，應(yīng)增大上區(qū)伸縮油缸油壓，減小下區(qū)伸縮油缸油壓，將其向下調(diào)整。圖7(e)為快速掘進(jìn)機(jī)器人向下偏，俯仰角為負(fù)，應(yīng)增大下區(qū)伸縮油缸油壓，減小上區(qū)伸縮油缸油壓，將其向上調(diào)整。

3.2 控制模型

快速掘進(jìn)機(jī)器人推進(jìn)過程如圖8所示。臨時(shí)支護(hù)機(jī)器人2固定，伸縮油缸的活塞桿向上推出，臨時(shí)支護(hù)機(jī)器人1向上移動(dòng)；臨時(shí)支護(hù)機(jī)器人1固定，伸縮油缸的活塞桿收回，臨時(shí)支護(hù)機(jī)器人2向上移動(dòng)。

(a) 正常

圖8 快速掘進(jìn)機(jī)器人推進(jìn)過程Fig.8 Propulsion process of rapid tunneling robot

通過控制伸縮油缸中液壓油的流向、壓力和流量，實(shí)現(xiàn)對伸縮油缸運(yùn)動(dòng)方向、推力大小和運(yùn)動(dòng)速度的控制。

快速掘進(jìn)機(jī)器人位姿控制系統(tǒng)模型如圖9所示。系統(tǒng)采用PID控制方法，引入位姿視覺負(fù)反饋，形成閉環(huán)控制系統(tǒng)，控制各伸縮油缸的行程，以實(shí)現(xiàn)快速掘進(jìn)機(jī)器人位姿的精確控制。

圖9 快速掘進(jìn)機(jī)器人位姿控制系統(tǒng)模型Fig.9 Model of pose control system of rapid tunneling robot

輸入巷道設(shè)計(jì)軸線，利用視覺測量方法測得掘進(jìn)機(jī)器人位姿，將測得的位姿信息反饋到輸入端形成閉環(huán)控制系統(tǒng)。測得的位姿與巷道設(shè)計(jì)軸線下的期望位姿進(jìn)行比較，得到位姿偏差，PID控制器根據(jù)偏差類型與大小，選擇相應(yīng)的控制策略，并發(fā)送控制指令，對快速掘進(jìn)機(jī)器人進(jìn)行位姿調(diào)整。

4 實(shí)驗(yàn)與結(jié)果分析

搭建基于視覺測量的快速掘進(jìn)機(jī)器人糾偏控制系統(tǒng)實(shí)驗(yàn)平臺(tái)，如圖10所示。實(shí)驗(yàn)以樓道模擬巷道，以煙霧制造器模擬工況中的粉塵，采用電動(dòng)機(jī)驅(qū)動(dòng)的履帶式探測機(jī)器人模擬快速掘進(jìn)機(jī)器人進(jìn)行推進(jìn)過程的位姿測量與控制實(shí)驗(yàn)。

實(shí)驗(yàn)平臺(tái)的主要參數(shù)見表1。

1-礦用本質(zhì)安全型激光指向儀； 2-煙霧制造器； 3-棱鏡；

表1 實(shí)驗(yàn)平臺(tái)參數(shù)Table 1 Parameters of experiment platform

4.1 位姿視覺測量精度

建立位姿視覺測量系統(tǒng)，并通過全站儀與慣導(dǎo)對其進(jìn)行位姿精度驗(yàn)證。選取機(jī)器人移動(dòng)過程中14個(gè)位置作為測量點(diǎn)，利用位姿視覺測量系統(tǒng)測量移動(dòng)機(jī)器人在巷道中的位姿。將全站儀測量得到的機(jī)器人位置和慣導(dǎo)測量得到的機(jī)器人姿態(tài)作為真實(shí)值，與位姿視覺測量系統(tǒng)得到的機(jī)器人位姿進(jìn)行對比。通過對比機(jī)器人X方向與Y方向坐標(biāo)、機(jī)器人偏航角變化來說明位姿視覺測量精度，結(jié)果如圖11所示。

由圖11可知，位姿視覺測量系統(tǒng)得到的位置信息與全站儀測量結(jié)果相吻合，X方向平均偏差為21.334 mm，Y方向平均偏差為34.154 mm；位姿視覺測量系統(tǒng)得到的姿態(tài)信息與慣導(dǎo)測量結(jié)果相吻合，偏航角平均偏差為0.493°。根據(jù)實(shí)際工況要求，位姿視覺測量精度滿足行業(yè)標(biāo)準(zhǔn)，可以作為快速掘進(jìn)機(jī)器人糾偏控制的基礎(chǔ)。

4.2 機(jī)器人糾偏控制精度

將機(jī)器人位姿視覺測量結(jié)果作為糾偏控制系統(tǒng)的反饋信息，通過DSP控制器操控機(jī)器人沿掘進(jìn)方向移動(dòng)14 m，同時(shí)利用全站儀記錄機(jī)器人移動(dòng)過程中的三維坐標(biāo)。機(jī)器人運(yùn)動(dòng)軌跡如圖12所示。

圖12中巷道中線表示機(jī)器人參考運(yùn)動(dòng)軌跡，折線為機(jī)器人實(shí)際運(yùn)動(dòng)軌跡。機(jī)器人從X=-20 mm，Y=0的起始位置向前運(yùn)動(dòng)。糾偏控制系統(tǒng)根據(jù)視覺測量的位姿信息對機(jī)器人兩側(cè)履帶進(jìn)行變速調(diào)整，使機(jī)器人不斷改變運(yùn)動(dòng)方向，保證其一直在參考軌跡附近運(yùn)動(dòng)。結(jié)果表明：機(jī)器人在X方向的位置誤差控制在30 mm之內(nèi)，滿足實(shí)際工況要求。

(a) 機(jī)器人X坐標(biāo)值

圖12 機(jī)器人運(yùn)動(dòng)軌跡Fig.12 Robot trajectory

5 結(jié)論

(1) 建立了快速掘進(jìn)機(jī)器人位姿視覺測量方案，利用防爆相機(jī)拍攝激光指向儀圖像，并對圖像進(jìn)行處理，根據(jù)位姿解算模型，解算出快速掘進(jìn)機(jī)器人的位姿。

(2) 針對快速掘進(jìn)機(jī)器人移動(dòng)過程中出現(xiàn)的各種位姿偏差，提出對應(yīng)的糾偏控制策略，將測量的位姿反饋給PID控制器進(jìn)行糾偏控制以提高快速掘進(jìn)機(jī)器人的控制精度。

(3) 實(shí)驗(yàn)結(jié)果表明，位姿視覺測量精度為X方向平均偏差21.334 mm、Y方向平均偏差34.154 mm、偏航角平均偏差0.493°，糾偏控制精度在30 mm以內(nèi)。