基于運(yùn)動(dòng)過(guò)程還原法的液壓支架巡檢機(jī)器人位姿檢測(cè)

楊學(xué)軍，王然風(fēng)，王懷法，李衛(wèi)國(guó)

(太原理工大學(xué) a.礦業(yè)工程學(xué)院，b.工程訓(xùn)練中心，太原 030024)

在煤礦開(kāi)采少人化和無(wú)人化[1-2]進(jìn)程中，液壓支架位姿和直線度的正確性是保證工作面能夠自動(dòng)連續(xù)推進(jìn)的關(guān)鍵[3-4]。只有保證液壓支架排成直線，才能把與之互為支點(diǎn)進(jìn)行推拉前進(jìn)的刮板輸送機(jī)推直，然后才能保證以刮板輸送機(jī)為運(yùn)行軌道的采煤機(jī)走直線，從而把煤壁切直[5]。單個(gè)支架位姿不僅反映了整個(gè)支架群直線度，還部分反映了工作面地板地質(zhì)情況和頂部來(lái)壓情況。因此，國(guó)內(nèi)外許多學(xué)者積極研究工作面的直線度檢測(cè)和控制問(wèn)題[6]，有些學(xué)者以刮板輸送機(jī)為主要研究對(duì)象[7-9]，有些學(xué)者以液壓支架為主要研究對(duì)象[10]。

澳大利亞的LASC技術(shù)[11-12]是通過(guò)在采煤機(jī)上安裝三維陀螺儀和慣性導(dǎo)航儀，得到采煤機(jī)機(jī)身的運(yùn)動(dòng)速度、姿態(tài)和位置信息，利用行走軌跡判定刮板輸送機(jī)的軌道直線度，進(jìn)而通過(guò)液壓支架調(diào)整刮板輸送機(jī)的位置，實(shí)現(xiàn)直線度控制?；谝曈X(jué)的刮板機(jī)直線度測(cè)量方法[13-14]通過(guò)控制刮板輸送機(jī)溜槽上安裝的LED燈點(diǎn)亮和熄滅，利用攝像儀測(cè)量計(jì)算每節(jié)溜槽偏移度。基于超聲波傳感器測(cè)量千斤頂位移量的刮板輸送機(jī)調(diào)直方法[15]采用超聲波傳感器測(cè)量千斤頂位移量并傳輸至控制臺(tái)，經(jīng)數(shù)據(jù)比對(duì)后，自動(dòng)調(diào)整刮板輸送機(jī)的直線度。

基于激光對(duì)位傳感器測(cè)量液壓支架相對(duì)位移的直線度控制方法[16-17]是在每臺(tái)液壓支架上各安裝一個(gè)激光對(duì)位傳感器，根據(jù)傳感器輸出值進(jìn)行支架移動(dòng)量控制?；贚abVIEW的礦用液壓支架姿態(tài)監(jiān)測(cè)系統(tǒng)設(shè)計(jì)[18]是利用激光測(cè)距傳感器測(cè)量液壓支架底板與頂板的距離以及對(duì)支架頂板的姿態(tài)，實(shí)時(shí)顯示液壓支架的高度變化和頂板姿態(tài)變化。

上述研究工作均取得了一定的成果，但是還存在著一些問(wèn)題：1) 在檢測(cè)工作面直線度的過(guò)程中沒(méi)有一個(gè)固定參考體系，只檢測(cè)到了自身的直線度，沒(méi)有檢測(cè)工作面與開(kāi)采方向是否垂直，等等[19]；2) 為了適應(yīng)檢測(cè)方法，需要改造被檢測(cè)對(duì)象，需要在液壓支架上安裝大量的位置和姿態(tài)傳感器，這樣會(huì)造成大量前期改造費(fèi)用和后續(xù)維護(hù)費(fèi)用；3) 通過(guò)檢測(cè)相鄰支架位姿關(guān)系來(lái)推算所有支架位姿關(guān)系及直線度的方法，有一定的累積誤差[20]；4) 對(duì)液壓支架進(jìn)行6個(gè)自由度位姿檢測(cè)的研究尚未見(jiàn)文獻(xiàn)報(bào)道；5) LASC技術(shù)由于涉及軍工產(chǎn)品禁運(yùn)及價(jià)格昂貴等問(wèn)題，在國(guó)內(nèi)的推廣使用受到一定的限制。

王國(guó)法[21]總結(jié)了基于支架相對(duì)位移的工作面直線度控制方法后認(rèn)為：可以選取激光傳感器作為所有支架相對(duì)位移的參考，將其發(fā)射端安設(shè)在工作面的一端支架上，其他支架都安裝有接收裝置。每個(gè)支架的相對(duì)位置均與安裝有發(fā)射端的支架位置相比較，這樣工作面整體直線度就有一個(gè)統(tǒng)一標(biāo)準(zhǔn)。

1 液壓支架位姿檢測(cè)方法

1.1 支架位姿描述方法

描述剛體位姿的方法很多。本文借鑒描述船舶在航海時(shí)姿態(tài)的RPY角法，采用運(yùn)動(dòng)過(guò)程還原法對(duì)液壓支架的位姿進(jìn)行描述。

一個(gè)剛體在空間有6個(gè)自由度，只要這6個(gè)自由度已知，則其在空間的位置和姿態(tài)就會(huì)唯一確定。設(shè)液壓支架群首架上固接的絕對(duì)坐標(biāo)系為{H}，待測(cè)支架上固接的相對(duì)坐標(biāo)系為{S}，則{S}系相對(duì)于{H}系的位姿可以這樣來(lái)描述：剛開(kāi)始時(shí)兩個(gè)坐標(biāo)系完全重合；經(jīng)過(guò)了一系列復(fù)雜運(yùn)動(dòng)后{S}系到達(dá)了某一位姿?，F(xiàn)在不考慮其具體運(yùn)動(dòng)過(guò)程，認(rèn)為目前任何位姿都可以通過(guò)6次有序運(yùn)動(dòng)到達(dá)，即首先繞{H}系的X軸、Y軸、Z軸各旋轉(zhuǎn)一定的角度(θ,φ，ψ)，再沿相應(yīng)坐標(biāo)軸各平移一定距離(u,v,w)，最后到達(dá)了現(xiàn)在的位姿(如圖1所示)，此時(shí)就可以分別用6個(gè)有序數(shù)列來(lái)表示這兩個(gè)坐標(biāo)系。

圖1 支架位姿形成過(guò)程示意圖Fig.1 Schematic of support pose formation process

設(shè){H}系位姿為(0,0,0,0,0,0)，則{S}系相對(duì){H}系的位姿就可以表示為：

LOC(S,H)=(θ,φ,ψ,u,v,w) .

運(yùn)動(dòng)過(guò)程還原法采用從一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的6個(gè)運(yùn)動(dòng)參數(shù)來(lái)描述兩個(gè)坐標(biāo)系的相對(duì)位姿，把運(yùn)動(dòng)和位姿有機(jī)地聯(lián)系起來(lái)，以便以后在三維虛擬環(huán)境下還原再現(xiàn)各液壓支架位姿和運(yùn)行情況，從而進(jìn)一步實(shí)現(xiàn)工作面支架位姿信息的透明化，并為調(diào)直支架直線度提供更全面的信息。

1.2 支架位姿檢測(cè)方法

綜上所述，本文提出了一種液壓支架位姿和直線度檢測(cè)方法模型，如圖2和圖3所示。

圖2 液壓支架位姿及直線度檢測(cè)三維模型Fig.2 3D model for measuring the pose and straightness of hydraulic supports

圖3 液壓支架位姿及直線度檢測(cè)平面模型Fig.3 Planar model for measuring the pose and straightness of hydraulic supports

該檢測(cè)模型選取工作面兩端支架作為所有支架的固定基準(zhǔn)，其上各安裝一個(gè)激光發(fā)射裝置和激光定位裝置以形成一組固定的參考激光束。在一個(gè)巡檢機(jī)器人上安裝有一個(gè)激光接收裝置(檢測(cè)自身絕對(duì)位姿)和一個(gè)激光雷達(dá)裝置(檢測(cè)支架相對(duì)自身位姿)。機(jī)器人檢測(cè)液壓支架位姿及直線度的流程如圖4所示。

圖4 液壓支架位姿及直線度檢測(cè)流程Fig.4 Pose and straightness measurement process of hydraulic supports

設(shè)機(jī)器人坐標(biāo)系為{R}.機(jī)器人行走在第i個(gè)支架人工行走平臺(tái)上時(shí)，采集自身攜帶的激光接收裝置坐標(biāo)值及距首架的距離值解算出機(jī)器人絕對(duì)位姿(逆運(yùn)動(dòng)解算)為L(zhǎng)OC(Ri,H)=(αi,βi,γi,ai,bi,ci)；用激光雷達(dá)裝置檢測(cè)出待測(cè)支架相對(duì)機(jī)器人的位姿為L(zhǎng)OC(SiRi)=(δi,εi,ζi,di,ei,fi)；通過(guò)位姿轉(zhuǎn)換，得到第i個(gè)支架絕對(duì)位姿為L(zhǎng)OC(Si,H)=(θi,φi,ψi,ui,vi,wi).完成位姿和直線度檢測(cè)后，把所有支架位姿信息實(shí)時(shí)傳輸給地面控制室，就可以在地面控制室還原井下所有支架的實(shí)時(shí)位置和姿態(tài)，并對(duì)如何調(diào)整支架位姿和支架群直線度做出決策[22]。

該檢測(cè)方法在基本不改造液壓支架結(jié)構(gòu)的前提下，以激光束為統(tǒng)一參考基準(zhǔn)，對(duì)支架位姿和直線度進(jìn)行檢測(cè)，所獲得的直線度信息沒(méi)有累積誤差。另外，巡檢機(jī)器人還可以攜帶攝像頭及其他傳感器，給工作面外的人員提供視頻圖像和數(shù)據(jù)，以便他們及時(shí)了解工作面的情況。

1.3 巡檢機(jī)器人行走機(jī)構(gòu)

行走在液壓支架上的機(jī)器人，除具備防爆防塵等要求外，其主要功能是能攜帶檢測(cè)裝置，識(shí)別和跨越兩個(gè)支架之間的溝壑，完成每個(gè)支架的位姿檢測(cè)。本文設(shè)計(jì)的巡檢機(jī)器人行走機(jī)構(gòu)為六履帶四搖臂移動(dòng)底盤(pán)。履帶機(jī)器人[23-25]結(jié)構(gòu)簡(jiǎn)單，故障率低，是煤礦理想的機(jī)器人行走機(jī)構(gòu)，可以通過(guò)前后四個(gè)獨(dú)立搖臂與主體履帶的配合來(lái)越過(guò)支架之間的溝壑[26-27]。

本文主要以激光接收裝置和激光測(cè)距儀的檢測(cè)值為依據(jù)研究巡檢機(jī)器人位姿的檢測(cè)和解算。

2 巡檢機(jī)器人位姿檢測(cè)及解算

2.1 激光接收裝置結(jié)構(gòu)設(shè)計(jì)及坐標(biāo)系建立

本文將在前期激光定位矩陣研究[28-29]的基礎(chǔ)上，對(duì)激光接收裝置進(jìn)行設(shè)計(jì)研究。

對(duì)于在液壓支架上行走的機(jī)器人，需要在其上安裝激光接收裝置，以便參考從首架發(fā)射的激光束來(lái)檢測(cè)自身位姿。如圖5所示，首架上安裝的激光發(fā)射裝置由兩個(gè)激光發(fā)射器L2、L3和一個(gè)兼有測(cè)距和發(fā)射激光兩種功能的測(cè)距儀L1組成。

圖5 激光發(fā)射裝置和激光接收裝置結(jié)構(gòu)模型Fig.5 Structural model of the laser launching device and the laser receiving device

將這三個(gè)儀器平行安裝在液壓支架群的首架和尾架(當(dāng)機(jī)器人從首架向尾架移動(dòng)時(shí)參考首架激光，反之參考尾架激光)，且處于同一垂面，L1和L3相距300 mm，L2和L3相距200 mm.將激光接收裝置安裝在巡檢機(jī)器人上；該裝置由兩個(gè)平行的光敏電阻矩陣面板組成，兩者相距700 mm，前面低的為第一面板，后面高的為第二面板。當(dāng)機(jī)器人在液壓支架上從首架向尾架移動(dòng)時(shí)，激光接收裝置可以檢測(cè)到三束激光投射在兩個(gè)面板上的激光點(diǎn)的坐標(biāo)，同時(shí)激光發(fā)射裝置檢測(cè)第一面板與其距離。

固定激光束是由激光發(fā)射器發(fā)射的散射度很小的點(diǎn)狀激光線形成，主要起著固定參考作用。激光接收裝置通過(guò)安裝在其上的若干個(gè)光敏電阻形成的矩陣來(lái)指示激光照射的坐標(biāo)，從而確定其相對(duì)激光束在徑向的偏移量。

如圖5所示，在這個(gè)模型中共有三個(gè)坐標(biāo)系，其中OXYZ為首架絕對(duì)坐標(biāo)系{H}，其余兩個(gè)為相對(duì)坐標(biāo)系；O1X1Y1Z1為第一面板的坐標(biāo)系{R}，同時(shí)也是激光接收裝置和機(jī)器人坐標(biāo)系；O2X2Y2Z2為第二面板坐標(biāo)系{E}，其與{R}系有固定位置關(guān)系；O3點(diǎn)是O1點(diǎn)在第二面板的投影點(diǎn)?，F(xiàn)有三束激光沿X軸照射到這兩個(gè)坐標(biāo)系上，由此可以獲得這三個(gè)激光束的坐標(biāo)值。

A點(diǎn)是L1照射在第一面板上的光斑點(diǎn)，B，C兩點(diǎn)是L2和L3照射在第二面板上的光斑點(diǎn)，D點(diǎn)是L1與第二面板的虛擬交點(diǎn)。兩個(gè)面板上的激光接收裝置可以實(shí)時(shí)顯示檢測(cè)到的A，B，C點(diǎn)的坐標(biāo)值。根據(jù)“六點(diǎn)定位原理”，只要有6個(gè)獨(dú)立的已知量就可以確定機(jī)器人在空間的位姿，即只需要A點(diǎn)的3個(gè)坐標(biāo)值、B點(diǎn)2個(gè)坐標(biāo)值、C點(diǎn)1個(gè)坐標(biāo)值就可以把激光接收裝置(也就是機(jī)器人)的位姿唯一確定。

2.2 巡檢機(jī)器人位姿的逆運(yùn)動(dòng)解算

機(jī)器人上安裝的激光接收裝置所顯示的坐標(biāo)可以理解為該裝置是從{H}系開(kāi)始，經(jīng)過(guò)3次旋轉(zhuǎn)和3次平移后得到的，其位姿解算過(guò)程就是根據(jù)激光接收裝置的坐標(biāo)來(lái)逆向推算自身是經(jīng)過(guò)了怎樣的3次旋轉(zhuǎn)和3次平移后到達(dá)該位姿。由于3次旋轉(zhuǎn)時(shí)，其結(jié)果與旋轉(zhuǎn)順序有關(guān)，因此本文認(rèn)為機(jī)器人是經(jīng)過(guò)了下面運(yùn)動(dòng)順序形成了最終的位姿：先依次繞X，Y，Z軸旋轉(zhuǎn)，再依次沿X，Y，Z軸平移。

2.2.1原始檢測(cè)數(shù)據(jù)

通過(guò)測(cè)距儀和激光接收裝置測(cè)到的坐標(biāo)見(jiàn)表1.

表1 可測(cè)得的A，B，C三點(diǎn)坐標(biāo)Table 1 Measurable coordinates of A, B and C

表1中，Ax通過(guò)激光測(cè)距儀測(cè)得，Ay和Az通過(guò)激光接收裝置第一面板測(cè)得，By0，Bz0，Cy0和Cz0通過(guò)第二面板測(cè)得。

把B，C兩點(diǎn)坐標(biāo)全部轉(zhuǎn)換到{R}坐標(biāo)系中，則

RB=(700,By,Bz)=(700,By0,Bz0+500) ,RC=(700,Cy,Cz)=(700,Cy0,Cz0+500) .

式中，上標(biāo)R表示選定的參考坐標(biāo)系{R}.

根據(jù)“六點(diǎn)定位原理”，實(shí)際上只需要知道Ax坐標(biāo)，再加上Ay，Az，By，Bz，Cy，Cz這6個(gè)坐標(biāo)值中的5個(gè)就可以求解；另外一個(gè)相當(dāng)于 “過(guò)定位尺寸”，其值的大小可以通過(guò)其他5個(gè)值表達(dá)出來(lái)。如果現(xiàn)在能多測(cè)出一個(gè)坐標(biāo)值，就會(huì)給后面的計(jì)算帶來(lái)一些便利，也可以利用這個(gè)值對(duì)其他值進(jìn)行驗(yàn)算。

機(jī)器人從初始位姿(圖5)開(kāi)始，首先繞X，Y，Z軸旋轉(zhuǎn)，旋轉(zhuǎn)的角度分別為α，β，γ，如圖6所示。其次再沿這三個(gè)坐標(biāo)軸分別移動(dòng)，移動(dòng)的距離為a，b，c，如圖7所示。經(jīng)過(guò)這樣的6次運(yùn)動(dòng)變換，形成了

圖6 三次姿態(tài)變換Fig.6 Three changes of orientation

最后的位姿，位姿逆運(yùn)動(dòng)解算就是按照這個(gè)逆序進(jìn)行還原求逆解。

圖7 三次位置變換Fig.7 Three changes of position

2.2.2解算要點(diǎn)

位姿逆運(yùn)動(dòng)解算要遵循機(jī)器人運(yùn)動(dòng)的逆序進(jìn)行還原，即先沿Z，Y，X軸逆向平移，再繞Z，Y，X軸逆向旋轉(zhuǎn)。即從圖7開(kāi)始，最終將機(jī)器人上的激光接收裝置位姿還原為圖5的初始位姿。由于解算過(guò)程比較復(fù)雜，本文分四步對(duì)主要思路和步驟進(jìn)行說(shuō)明，詳細(xì)計(jì)算過(guò)程另撰文論述。

第一步，解算機(jī)器人沿X，Y，Z軸的平移值a，b，c.

采用坐標(biāo)法求解a,b,c，具體原理如下。在{R}坐標(biāo)系中，O1，A，B，D的坐標(biāo)均為已知或可求；在{H}系中，A，B，D三點(diǎn)坐標(biāo)也是已知或可求，需要求O1點(diǎn)坐標(biāo)(a,b,c).根據(jù)同一線段在不同坐標(biāo)中長(zhǎng)度不變的原理，利用O1A，O1B，O1D三線段長(zhǎng)度不變?cè)砜傻萌齻€(gè)方程，解三個(gè)未知數(shù)，即為所求。

在{R}坐標(biāo)系中，利用等比關(guān)系可以求出D點(diǎn)坐標(biāo)：

RD=(700,Dy,Dz) .

其中

在{H}坐標(biāo)系中，A，B，D三點(diǎn)坐標(biāo)為：

HA=(Ax,0,0) ,HB=(Bx,0,500) ,HD=(Dx,0,0) .

其中，Ax為已知，Bx和Dx可由下式得出：

當(dāng)Dz

綜上得O1，A，B，D四個(gè)點(diǎn)在兩個(gè)不同坐標(biāo)系中的坐標(biāo)，如表2所示。

表2 四個(gè)點(diǎn)在兩個(gè)不同坐標(biāo)系中的坐標(biāo)Table 2 Coordinates of four points in two different coordinate systems mm

列三個(gè)方程，解之得：

(1)

(2)

(當(dāng)Ay<0時(shí)取負(fù)號(hào)，Ay≥0時(shí)取正號(hào)) .

(3)

根據(jù)求解結(jié)果進(jìn)行相應(yīng)的運(yùn)動(dòng)和坐標(biāo)還原。在{R}坐標(biāo)系內(nèi)平移激光接收裝置使點(diǎn)A與點(diǎn)O1重合，則A，B，D三點(diǎn)在{R}坐標(biāo)系中坐標(biāo)還原為

RA=(0,0,0)=RO1,RB=(700,By1,Bz1)=(700,By-Ay,Bz-Az) ,RD=(700,Dy1,Dz1)=(700,Dy-Ay,Dz-Az) .

式中，坐標(biāo)中的下標(biāo)1表示還原次數(shù)，以下同。

在{H}坐標(biāo)系內(nèi)沿X軸方向平移激光接收裝置，使得O1和O重合，A，B，D三點(diǎn)在{R}坐標(biāo)系中坐標(biāo)不變，在{H}坐標(biāo)系中的坐標(biāo)還原為

HA=(0,0,0)=HO,HB=(Bx1,0,500)=(Bx-Ax,0,500) ,HD=(Dx1,0,0)=(Dx-Ax,0, 0) .

至此，激光接收裝置就還原為圖6所示狀態(tài)。

第二步，求激光接收裝置繞Z軸旋轉(zhuǎn)的角度γ.

采用幾何向量法進(jìn)行計(jì)算。在{R}坐標(biāo)系內(nèi)，求過(guò)A，B，D三點(diǎn)的平面方程；然后求解B點(diǎn)在平面YOZ上的投影點(diǎn)E的坐標(biāo)；再計(jì)算過(guò)E，A，O3三點(diǎn)的平面方程；兩平面ABD和EAO3的夾角即為γ.

(當(dāng)BD與Y軸交點(diǎn)Ey>0時(shí)取負(fù)號(hào)) .

(4)

式中，A1，B1，C1和A2，B2，C2分別為平面ABD和EAO3方程的系數(shù)。

求解后，B和D點(diǎn)的坐標(biāo)相應(yīng)還原為

RB=(700,By2,Bz2) ,RD=(700,Dy2,Dz2) .

第三步，求激光接收裝置繞Y軸旋轉(zhuǎn)的角度β.

用幾何法求解得：

(當(dāng)Dz2>0時(shí)取負(fù)號(hào)) .

(5)

求解后，D點(diǎn)坐標(biāo)還原為初始值，B點(diǎn)坐標(biāo)還原為

RB=(700,By3,Bz3) .

第四步，求激光接收裝置繞X軸旋轉(zhuǎn)的角度α.

用幾何法求解：

(6)

求解后，B點(diǎn)坐標(biāo)還原為初始值。激光接收裝置同時(shí)還原為圖5初始狀態(tài)。

需要說(shuō)明的是，文中繞X，Y，Z軸旋轉(zhuǎn)的角度單位均為(°)，沿X，Y，Z軸移動(dòng)的距離單位均為mm.

綜上，α，β，γ，a，b，c這6個(gè)參數(shù)就組成了機(jī)器人在絕對(duì)坐標(biāo)系內(nèi)的位姿。也就是說(shuō)，機(jī)器人相當(dāng)于經(jīng)過(guò)了這樣6次有序運(yùn)動(dòng)，從首架的初始位姿運(yùn)動(dòng)到了目前的位姿。

3 巡檢機(jī)器人位姿解算結(jié)果驗(yàn)證

機(jī)器人的位姿計(jì)算完成后，需要對(duì)其正確性進(jìn)行驗(yàn)證。本文采用兩種方法進(jìn)行驗(yàn)證：一種是正運(yùn)動(dòng)解算法，另外一種是虛擬裝置運(yùn)動(dòng)模擬法。

3.1 驗(yàn)證過(guò)程設(shè)計(jì)

機(jī)器人位姿逆運(yùn)動(dòng)解算需要A，B，C三個(gè)點(diǎn)的坐標(biāo)值。因受激光接收裝置尺寸的限制，不能任意取值，所以設(shè)計(jì)驗(yàn)證過(guò)程為：

1) 擬定機(jī)器人的運(yùn)動(dòng)過(guò)程(即機(jī)器人的位姿)，如LOC(R0,H)=(α0,β0,γ0,a0,b0,c0).

2) 根據(jù)機(jī)器人運(yùn)動(dòng)過(guò)程，應(yīng)用這兩種驗(yàn)證方法分別得到A，B，C三點(diǎn)在{R}系的坐標(biāo)Ay，Az，By，Bz，Cy，Cz和A點(diǎn)在{H}系的坐標(biāo)Ax.如果結(jié)果相同，說(shuō)明這兩方法都是正確的。

3) 把用這兩種方法得到的三個(gè)點(diǎn)坐標(biāo)值代入位姿逆運(yùn)動(dòng)解算公式，還原機(jī)器人運(yùn)動(dòng)過(guò)程(機(jī)器人位姿)，LOC(Rn,H)=(αn,βn,γn,an,bn,cn).

4) 比較LOC(R0,H)和LOC(Rn,H)的值，如果相同就說(shuō)明機(jī)器人位姿的逆運(yùn)動(dòng)解算結(jié)果正確。

表3是擬定的三組機(jī)器人的原始運(yùn)動(dòng)過(guò)程數(shù)據(jù)

表3 擬定的三組機(jī)器人位姿Table 3 Proposed three sets of robot pose

LOC(R0,H).

3.2 正運(yùn)動(dòng)解算法驗(yàn)證

3.2.1正運(yùn)動(dòng)解算思路

正解算就是按照擬定的位姿參數(shù)，讓機(jī)器人從初始位置開(kāi)始順序運(yùn)動(dòng)，從而追蹤激光接收裝置上A，B，C三個(gè)光斑點(diǎn)坐標(biāo)的變化情況。即依照?qǐng)D6和圖7運(yùn)動(dòng)順序，逐步計(jì)算機(jī)器人每次運(yùn)動(dòng)后坐標(biāo)Ax，Ay，Az，By，Bz，Cy，Cz的值。

機(jī)器人上的激光接收裝置原始位姿如圖5所示，其上需要計(jì)算的各點(diǎn)初始坐標(biāo)見(jiàn)表4.沒(méi)有列出的坐標(biāo)為定值，由裝置結(jié)構(gòu)決定，在機(jī)器人運(yùn)動(dòng)過(guò)程中不發(fā)生變化。

表4 A，B，C三點(diǎn)初始坐標(biāo)Table 4 Initial coordinates of point A, B, C mm

3.2.2正運(yùn)動(dòng)解算過(guò)程要點(diǎn)

機(jī)器人每運(yùn)動(dòng)一步，相應(yīng)三個(gè)點(diǎn)的坐標(biāo)值都可能有變化，所以本文僅對(duì)每一步的計(jì)算要點(diǎn)進(jìn)行說(shuō)明，略去了部分具體表達(dá)式。

第一步，繞X軸旋轉(zhuǎn)α.

激光接收裝置形成了新的位姿，在{R}坐標(biāo)系中，有以下坐標(biāo)發(fā)生了變化：

By1=500sinα,Bz1=500cosα,Cy1=300sinα,Cz1=300cosα.

第二步，繞Y軸旋轉(zhuǎn)β.

在{R}坐標(biāo)系中，形成了以下新的坐標(biāo)：

第三步，繞Z軸旋轉(zhuǎn)γ.

同理，在{R}坐標(biāo)系中，形成了以下新的坐標(biāo)：By3，Bz3，Cy3，Cz3.

第四步，沿X軸平移a.

在{H}坐標(biāo)系，Ax坐標(biāo)發(fā)生改變，其余坐標(biāo)無(wú)變化。

Ax4=a.

第五步、第六步，沿Y軸平移b，沿Z軸平移c.

在{R}坐標(biāo)系，形成了以下新的坐標(biāo)：Ay6，Az6，By6，Bz6，Cy6，Cz6.在{H}坐標(biāo)系，形成了以下新坐標(biāo)：Ax6.

最后，把B，C兩點(diǎn)沿Z軸坐標(biāo)轉(zhuǎn)換到{E}系內(nèi)：

Bz7=Bz6-500 ,Cz7=Cz6-500 .

綜上，Ax6為激光測(cè)距儀應(yīng)檢測(cè)的距離值；Ay6，Az6為第一面板應(yīng)顯示的坐標(biāo)值；By6，Bz7，Cy6，Cz7為第二面板應(yīng)顯示的坐標(biāo)值。

根據(jù)正運(yùn)動(dòng)解算公式，對(duì)表3的運(yùn)動(dòng)數(shù)據(jù)進(jìn)行計(jì)算，得到表5的結(jié)果。該值為激光測(cè)距儀應(yīng)測(cè)出的A點(diǎn)理論距離值和激光束L1，L2，L3落在激光接收裝置上的光斑點(diǎn)A，B，C的坐標(biāo)值。

表5 根據(jù)擬定位姿進(jìn)行的正運(yùn)動(dòng)解算結(jié)果Table 5 Rusults of positive motion calculation using the proposed robot pose mm

3.3 虛擬裝置運(yùn)動(dòng)模擬法驗(yàn)證

圖8是在Solidworks三維機(jī)械設(shè)計(jì)軟件里建立并裝配的位姿調(diào)整器、激光發(fā)射裝置及激光接收裝置虛擬模型。安裝了激光接收裝置的位姿調(diào)整器可以模擬機(jī)器人經(jīng)過(guò)六次運(yùn)動(dòng)調(diào)整后，從初始位姿到達(dá)最終位姿后激光束在兩個(gè)面板的坐標(biāo)變化情況。把原始運(yùn)動(dòng)過(guò)程數(shù)據(jù)依次代入，然后測(cè)量在兩個(gè)坐標(biāo)面板上三個(gè)點(diǎn)的坐標(biāo)值，就得到模擬運(yùn)動(dòng)的結(jié)果，該值應(yīng)該與正解算結(jié)果相同。該模擬方法不需要計(jì)算公式，能真實(shí)展現(xiàn)激光接收裝置的實(shí)際運(yùn)動(dòng)過(guò)程，把運(yùn)動(dòng)過(guò)程和位姿結(jié)果直觀地聯(lián)系了起來(lái)。

表6是根據(jù)表3的運(yùn)動(dòng)數(shù)據(jù)進(jìn)行的三維虛擬裝置運(yùn)動(dòng)模擬測(cè)量結(jié)果。

圖8 用SolidWorks軟件建立的位姿調(diào)整器、激光接收裝置 和激光發(fā)射裝置三維模型Fig.8 3 d models of pose adjuster, laser receiving device and laser launching device established with SolidWorks software

表6 根據(jù)擬定位姿進(jìn)行的虛擬裝置運(yùn)動(dòng)模擬結(jié)果Table 6 Motion simulation results of 3D digital virtual device using the proposed robot pose mm

3.4 驗(yàn)證結(jié)果分析

正運(yùn)動(dòng)解算和三維虛擬裝置運(yùn)動(dòng)模擬兩種驗(yàn)證方法的結(jié)果比較如表7所示。從表中可以看出，兩種方法得出的結(jié)果有一定的誤差，這是由于正運(yùn)動(dòng)解算過(guò)程和軟件模擬計(jì)算過(guò)程不盡相同，但誤差均非常小，可以忽略不計(jì)。

表7 正運(yùn)動(dòng)解算與虛擬裝置運(yùn)動(dòng)模擬結(jié)果比較Table 7 Comparison of positive motion calculation and virtual assembly motion simulation mm

表8是根據(jù)表5進(jìn)行的機(jī)器人位姿逆運(yùn)動(dòng)解算結(jié)果。由于三維虛擬裝置運(yùn)動(dòng)不能進(jìn)行位姿逆運(yùn)動(dòng)模擬，所以只有正運(yùn)動(dòng)模擬結(jié)果。

表9是擬定位姿與逆運(yùn)動(dòng)解算位姿比較的結(jié)果。從表中可以看出，機(jī)器人位姿理論計(jì)算是完全正確的，而且具有唯一性，誤差屬于正常范圍，可以忽略不計(jì)。

表3、表5-表9是從大量驗(yàn)證數(shù)據(jù)中摘錄的三

表8 根據(jù)表5計(jì)算的機(jī)器人位姿逆運(yùn)動(dòng)解算結(jié)果Table 8 Robot pose solved by using the inverse motion formula according toTable 5

表9 逆運(yùn)動(dòng)解算位姿與擬定位姿比較Table 9 Comparison of inverse motion solution and the proposed robot pose

組數(shù)據(jù)，這三組數(shù)據(jù)基本代表了該裝置的測(cè)量范圍和檢測(cè)特征。巡檢機(jī)器人位姿檢測(cè)理論和逆運(yùn)動(dòng)解算的正確性，為后續(xù)液壓支架姿態(tài)及直線度檢測(cè)和調(diào)整實(shí)驗(yàn)研究奠定了基礎(chǔ)。

4 結(jié)論

1) 提出了描述液壓支架6個(gè)自由度位姿(3個(gè)位置自由度(u,v,w)和3個(gè)姿態(tài)自由度(θ,φ,ψ))的運(yùn)動(dòng)過(guò)程還原法。基于該方法，設(shè)計(jì)了利用巡檢機(jī)器人對(duì)液壓支架位姿和直線度進(jìn)行檢測(cè)的方法模型。該檢測(cè)模型先利用激光接收裝置檢測(cè)巡檢機(jī)器人位姿，再用激光雷達(dá)裝置檢測(cè)待測(cè)支架相對(duì)機(jī)器人位姿，最后通過(guò)位姿轉(zhuǎn)換獲得各支架位姿，進(jìn)而再計(jì)算出整個(gè)支架群的直線度。用該方法模型檢測(cè)支架位姿，不用在待測(cè)支架上安裝任何傳感器，計(jì)算得到的直線度沒(méi)有累積誤差。

2) 探索了利用激光束、激光測(cè)距儀和激光接收裝置對(duì)機(jī)器人位姿進(jìn)行檢測(cè)的裝置結(jié)構(gòu)及尺寸。通過(guò)分析裝置和建立坐標(biāo)系，推導(dǎo)了機(jī)器人位姿的逆運(yùn)動(dòng)解算公式，實(shí)現(xiàn)了機(jī)器人位姿的運(yùn)動(dòng)過(guò)程還原。該機(jī)器人位姿檢測(cè)方法只利用激光接收裝置上3個(gè)點(diǎn)的6個(gè)坐標(biāo)值來(lái)計(jì)算機(jī)器人的3個(gè)位置坐標(biāo)和3個(gè)姿態(tài)角度，這為其他場(chǎng)合的位姿檢測(cè)提供了理論依據(jù)。

3) 通過(guò)正運(yùn)動(dòng)解算和虛擬裝置運(yùn)動(dòng)模擬兩種方法驗(yàn)證了巡檢機(jī)器人位姿檢測(cè)的逆運(yùn)動(dòng)解算的正確性，這為后續(xù)液壓支架姿態(tài)及直線度的檢測(cè)和調(diào)整實(shí)驗(yàn)研究奠定了基礎(chǔ)。