掘進(jìn)鉆車自動(dòng)定位方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種多關(guān)節(jié)多自由度設(shè)備的定位方法，具體是指掘進(jìn)鉆車鉆孔時(shí)自動(dòng)定位方法，該方法適用礦山掘進(jìn)鉆車在打眼時(shí)釬桿的定位。
【背景技術(shù)】
[0002]在礦山開(kāi)采過(guò)程中一般是用掘進(jìn)鉆車進(jìn)行打孔，然后填埋炸藥進(jìn)行爆破。目前的掘進(jìn)鉆車工作部分是在能夠行走的機(jī)身上鉸接鉆臂，鉆臂分為左鉆臂和右鉆臂，左、右鉆臂均通過(guò)軸套連接在鉆車的操作臺(tái)架上，鉆臂主要部件包括軸套、鉆臂架、支管、支架、定向滑道、滑架和釬桿，鉆臂的前端鉸接有滑架，滑架上安裝有釬桿，鉆臂上連接有使鉆臂水平方向轉(zhuǎn)動(dòng)的油缸和使鉆臂實(shí)現(xiàn)仰俯的油缸，同時(shí)鉆臂自身能夠伸縮；滑架上同樣連接有使滑架仰俯和使滑架相對(duì)鉆臂水平轉(zhuǎn)動(dòng)的油缸；另外釬桿在滑架上僅能夠前進(jìn)和后退，單個(gè)鉆臂的具體結(jié)構(gòu)見(jiàn)附圖3a、附圖3b等。在定位過(guò)程釬桿相對(duì)于滑架不動(dòng)，因此將釬桿和滑架看做一個(gè)整體，這樣掘進(jìn)鉆車工作部分的鉆臂、滑架、釬桿就至少有六個(gè)自由度，因此要想使釬桿的釬頭對(duì)準(zhǔn)鉆眼位置需要反復(fù)調(diào)整鉆臂、滑架、釬桿。目前掘進(jìn)鉆車的操作主要還是人工控制，操作人員開(kāi)動(dòng)掘進(jìn)鉆車到達(dá)指定位置，然后操作各油缸，調(diào)整鉆臂、滑架、釬桿的位置，使釬桿的釬頭對(duì)準(zhǔn)需鉆孔位置，然后起到釬桿使釬桿前進(jìn)，打出爆破孔。這樣人工操作打出的爆破孔一般誤差都很大，這樣是實(shí)施爆破后會(huì)導(dǎo)致巷道成型質(zhì)量差。同時(shí)操作者的勞動(dòng)強(qiáng)度非常大。如果將目前的掘進(jìn)鉆車改造成能夠自動(dòng)找孔定位的自動(dòng)控制方式，目前的多關(guān)節(jié)多自由度設(shè)備的定位方法程序復(fù)雜，程序運(yùn)算時(shí)間長(zhǎng)，對(duì)計(jì)算機(jī)運(yùn)算速度要求高，導(dǎo)致無(wú)法應(yīng)用于礦上巷道工程上，由于掘進(jìn)鉆車本身也是移動(dòng)的，目前的定位方法的運(yùn)算結(jié)果誤差也較大。

【發(fā)明內(nèi)容】

[0003]本發(fā)明要解決的技術(shù)問(wèn)題是提供一種運(yùn)算速度快且準(zhǔn)確的掘進(jìn)鉆車自動(dòng)定位方法，能夠在給定所打炮眼和機(jī)身位姿后便可快速準(zhǔn)確計(jì)算出鉆車鉆臂和滑架位置參數(shù)值，以使釬桿能夠準(zhǔn)確對(duì)準(zhǔn)鉆孔位置。
[0004]為解決上述技術(shù)問(wèn)題，本發(fā)明所采取的技術(shù)方案是:
[0005]掘進(jìn)鉆車自動(dòng)定位方法，將鉆車機(jī)架上的軸套繞自身軸心線轉(zhuǎn)動(dòng)時(shí)帶動(dòng)鉆臂的左右擺動(dòng)設(shè)定為第一動(dòng)作，將鉸接在軸套上的鉆臂架的上下擺動(dòng)而帶動(dòng)鉆臂的上下擺動(dòng)設(shè)定為第二動(dòng)作，將鉆臂架前端套接的支管繞自身軸線相對(duì)于鉆臂架的旋轉(zhuǎn)設(shè)定為第三動(dòng)作，將鉆臂的支架帶動(dòng)定向滑道、滑架、釬桿繞支架與支管的橫向鉸接軸的上下擺動(dòng)設(shè)定為第四動(dòng)作，將定向滑道帶動(dòng)滑架、釬桿繞支架與定向滑道的豎向鉸接軸做水平轉(zhuǎn)動(dòng)設(shè)定為第五動(dòng)作，將滑架帶動(dòng)釬桿在定向滑道上前后伸縮移動(dòng)設(shè)定為第六動(dòng)作；
[0006]在巷道三維空間內(nèi)建立全局坐標(biāo)系，在全局坐標(biāo)系內(nèi)建立鉆臂的局部坐標(biāo)系；全局坐標(biāo)系以掘進(jìn)工作面中心垂線與巷道底板中心線的交點(diǎn)為坐標(biāo)原點(diǎn)，自坐標(biāo)原點(diǎn)沿掘進(jìn)工作面豎直向上設(shè)定為Y軸正向，自坐標(biāo)原點(diǎn)沿巷道底板向后設(shè)定為Z軸正向，面向掘進(jìn)工作面時(shí)右手方向?yàn)閄軸正向；局部坐標(biāo)系是以第一動(dòng)作繞軸中心線和第二動(dòng)作繞軸中心的交點(diǎn)為原點(diǎn)，局部坐標(biāo)系的各坐標(biāo)軸正向與全局坐標(biāo)系一致；
[0007]在全局坐標(biāo)系中選取包括局部坐標(biāo)原點(diǎn)在內(nèi)的8個(gè)點(diǎn)作為關(guān)鍵點(diǎn)并將這8個(gè)關(guān)鍵點(diǎn)固聯(lián)在鉆臂的釬桿、滑架、定向滑道、支架、支管、鉆臂架、軸套的相應(yīng)位置或者虛擬位置上;
[0008]所述8個(gè)關(guān)鍵點(diǎn)包括:第I關(guān)鍵點(diǎn)是釬桿的釬尖所在的點(diǎn)；第2關(guān)鍵點(diǎn)是釬桿中心線上距離釬尖一定距離的點(diǎn)；第3關(guān)鍵點(diǎn)是位于與滑架的上平面平行的釬桿中心線所在平面內(nèi)、距離釬尖特定距離的點(diǎn)，第I關(guān)鍵點(diǎn)和第3關(guān)鍵點(diǎn)連線垂直于釬桿軸線，面向掘進(jìn)工作面時(shí)第3關(guān)鍵點(diǎn)位于釬桿(I)的右側(cè)；第4關(guān)鍵點(diǎn)是由鉆臂的實(shí)際結(jié)構(gòu)尺寸確定的位于釬桿中心線上是距離第I關(guān)鍵點(diǎn)固定距離的點(diǎn)，第4關(guān)鍵點(diǎn)到第I關(guān)鍵點(diǎn)的距離大于第2關(guān)鍵點(diǎn)到第I關(guān)鍵點(diǎn)的距離，當(dāng)鉆臂上所有動(dòng)作部件均處于零位時(shí)，第4關(guān)鍵點(diǎn)是第五動(dòng)作的豎軸中心線與釬桿中心線的交點(diǎn)；第5關(guān)鍵點(diǎn)與第4關(guān)鍵點(diǎn)的連線垂直于與滑架的上平面平行的釬桿中心線所在平面，第5關(guān)鍵點(diǎn)到第4關(guān)鍵點(diǎn)的距離是由鉆臂各部件的實(shí)際機(jī)械結(jié)構(gòu)尺寸決定的固定數(shù)值，當(dāng)鉆臂上所有動(dòng)作部件均是初始零位置時(shí)，第5關(guān)鍵點(diǎn)是第五動(dòng)作的轉(zhuǎn)動(dòng)豎軸中心線與第三動(dòng)作的旋轉(zhuǎn)繞軸中心線的交點(diǎn)；第6關(guān)鍵點(diǎn)與第5關(guān)鍵點(diǎn)的連線平行于釬桿中心線，第6關(guān)鍵點(diǎn)到第5關(guān)鍵點(diǎn)的距離是由鉆臂各部件的實(shí)際機(jī)械結(jié)構(gòu)所決定的固定數(shù)值，當(dāng)鉆臂各部件均處于初始零位置時(shí)，第6關(guān)鍵點(diǎn)位于第四動(dòng)作旋轉(zhuǎn)繞軸的中心線上；第7關(guān)鍵點(diǎn)位于支管的端部，是第三動(dòng)作的旋轉(zhuǎn)繞軸的中心線和第四動(dòng)作旋轉(zhuǎn)繞軸中心線的交點(diǎn)，在鉆臂的各部件處于初始零位置時(shí)，第6關(guān)鍵點(diǎn)與第7關(guān)鍵點(diǎn)重合；第8關(guān)鍵點(diǎn)是第一動(dòng)作旋轉(zhuǎn)繞軸的中心線和第二動(dòng)作旋轉(zhuǎn)繞軸中心線的交點(diǎn)；
[0009]使鉆車位于巷道中、全局坐標(biāo)系的Z軸方向，并確定巷道掘進(jìn)工作面上需打的炮眼位置，選定打眼的鉆臂并確定釬桿的最終位姿，確定鉆桿的最終位姿時(shí)是先計(jì)算第I關(guān)鍵點(diǎn)、第2關(guān)鍵點(diǎn)、第3關(guān)鍵點(diǎn)在全局坐標(biāo)系中的坐標(biāo)值，然后將這三個(gè)關(guān)鍵點(diǎn)的全局坐標(biāo)系的坐標(biāo)值轉(zhuǎn)換為所選定的打眼鉆臂的局部坐標(biāo)系的坐標(biāo)值，然后計(jì)算出第4關(guān)鍵點(diǎn)、第5關(guān)鍵點(diǎn)、第6關(guān)鍵點(diǎn)在局部坐標(biāo)系中的坐標(biāo)值，得知第I關(guān)鍵點(diǎn)?第6關(guān)鍵點(diǎn)的局部坐標(biāo)系的坐標(biāo)值后，通過(guò)逆向求解計(jì)算出鉆臂第一動(dòng)作?第六動(dòng)作的參數(shù)值，然后按照計(jì)算出的第一動(dòng)作?第六6個(gè)動(dòng)作參數(shù)值驅(qū)動(dòng)鉆臂各部件動(dòng)作確定鉆臂的最終位姿，使釬桿對(duì)正打眼位置進(jìn)行鉆孔。
[0010]本發(fā)明的技術(shù)方案的進(jìn)一步限定在于:
[0011]在逆向求解第一動(dòng)作?第六動(dòng)作的參數(shù)值時(shí)，首先是運(yùn)用迭代搜索方法求解第六動(dòng)作和第五動(dòng)作的參數(shù)值，之后通過(guò)逐步歸位的方法依次求解第一動(dòng)作、第二動(dòng)作、第三動(dòng)作和第四動(dòng)作的參數(shù)值，在用迭代搜索方法和逐步歸位的方法求解的過(guò)程中是調(diào)用包括剛體任意運(yùn)動(dòng)后剛體上一點(diǎn)坐標(biāo)變換的計(jì)算方法、空間點(diǎn)沿空間直線延伸一定長(zhǎng)度后坐標(biāo)變換的計(jì)算方法、兩空間平面之間夾角的計(jì)算方法、空間點(diǎn)繞空間軸線旋轉(zhuǎn)一定角度后坐標(biāo)變換的計(jì)算方法；
[0012]剛體任意動(dòng)后剛體上一點(diǎn)坐標(biāo)變換的計(jì)算方法是用于計(jì)算處于最終位置的釬桿上第四關(guān)鍵點(diǎn)、第五關(guān)鍵點(diǎn)、第六關(guān)鍵點(diǎn)坐標(biāo)，該計(jì)算方法是輸入釬桿任意動(dòng)前第I關(guān)鍵點(diǎn)、第2關(guān)鍵點(diǎn)、第3關(guān)鍵點(diǎn)的坐標(biāo)值以及動(dòng)后第I關(guān)鍵點(diǎn)、第2關(guān)鍵點(diǎn)、第3關(guān)鍵點(diǎn)的坐標(biāo)值，然后輸入所需求解的第4關(guān)鍵點(diǎn)、第5關(guān)鍵點(diǎn)、第6關(guān)鍵點(diǎn)的動(dòng)前的坐標(biāo)值即可求出相應(yīng)關(guān)鍵點(diǎn)動(dòng)后的坐標(biāo)值；
[0013]空間點(diǎn)沿空間直線延伸一定長(zhǎng)度后坐標(biāo)變換的計(jì)算方法是用于計(jì)算釬桿在最終位置狀態(tài)下釬桿沿著釬桿軸線移動(dòng)一段距離后釬桿上第I關(guān)鍵點(diǎn)?第6關(guān)鍵點(diǎn)的坐標(biāo)值，運(yùn)用該計(jì)算方法時(shí)，輸入第I關(guān)鍵點(diǎn)坐標(biāo)值和第2關(guān)鍵點(diǎn)坐標(biāo)值以及所求的關(guān)鍵點(diǎn)移動(dòng)之前的坐標(biāo)值和所求關(guān)鍵點(diǎn)釬桿沿軸線移動(dòng)的向量即可求出延伸后的該點(diǎn)坐標(biāo)值；
[0014]空間點(diǎn)繞空間軸線旋轉(zhuǎn)一定角度后坐標(biāo)變換的計(jì)算方法是用于計(jì)算第一動(dòng)作至第五動(dòng)作的每一旋轉(zhuǎn)動(dòng)作歸位后各相關(guān)關(guān)鍵點(diǎn)坐標(biāo)值，運(yùn)用該計(jì)算方法時(shí)輸入空間點(diǎn)繞空間軸線旋轉(zhuǎn)的角度向量和關(guān)鍵點(diǎn)旋轉(zhuǎn)之前坐標(biāo)值即可求出旋轉(zhuǎn)后該點(diǎn)坐標(biāo)值。
[0015]本發(fā)明的技術(shù)方案的進(jìn)一步限定在于:
[0016]確定鉆臂的最終位姿的計(jì)算流程為:
[0017]步驟1、確定鉆車的左鉆臂局部坐標(biāo)系原點(diǎn)、右鉆臂局部坐標(biāo)系原點(diǎn)在全局坐標(biāo)系中的位置坐標(biāo)值，并確定所要打的炮眼在全局坐標(biāo)系中坐標(biāo)值，選定打炮眼的鉆臂是左鉆臂還是右鉆臂；
[0018]步驟2、計(jì)算出第I關(guān)鍵點(diǎn)?第6關(guān)鍵點(diǎn)在全局坐標(biāo)系中的全局坐標(biāo)值并將各個(gè)關(guān)鍵點(diǎn)的全局坐標(biāo)值轉(zhuǎn)換為所選定的鉆臂局部坐標(biāo)系內(nèi)的局部坐標(biāo)值；
[0019]步驟3、運(yùn)用迭代搜索方法計(jì)算出第六動(dòng)作和第五動(dòng)作的參數(shù)值；
[0020]步驟4、計(jì)算出第六動(dòng)作和第