本發明涉及掘進(jin)機械(xie)智能操作，尤(you)其是涉及一種鑿巖臺車鉆孔定位與鉆臂作業(ye)的集成控制方法和系(xi)統。

背景技術：

1、隨著科學(xue)技術的(de)進步以及社會(hui)的(de)發展，人(ren)(ren)(ren)們越來(lai)越注重工(gong)(gong)作(zuo)環境(jing)的(de)好壞，由此，針對一些工(gong)(gong)作(zuo)環境(jing)惡(e)(e)劣(lie)，勞動(dong)強度大的(de)場景，越來(lai)越多的(de)人(ren)(ren)(ren)希望能(neng)夠減少(shao)人(ren)(ren)(ren)力(li)的(de)投入，降低(di)工(gong)(gong)作(zuo)強度，而工(gong)(gong)程(cheng)機(ji)械的(de)工(gong)(gong)作(zuo)環境(jing)大多比較(jiao)惡(e)(e)劣(lie)，強度比較(jiao)高，因(yin)此需要通過提(ti)高工(gong)(gong)程(cheng)機(ji)械的(de)自動(dong)化水平，以此降低(di)工(gong)(gong)人(ren)(ren)(ren)的(de)勞動(dong)強度。

2、由于(yu)復(fu)雜的(de)智(zhi)能技術方法難以(yi)(yi)應用于(yu)地下礦山(shan)工程(cheng)機械(xie)，或難以(yi)(yi)取(qu)得良好的(de)應用效(xiao)果，因(yin)此地下礦山(shan)工程(cheng)機械(xie)的(de)自動化程(cheng)度(du)比較低，且(qie)普及程(cheng)度(du)不高。如現(xian)有技術中(zhong)(zhong)的(de)鑿(zao)巖臺車在(zai)工作(zuo)時，鉆(zhan)臂在(zai)萬向節鎖死狀態(tai)下無(wu)法求(qiu)解鉆(zhan)臂姿(zi)態(tai)信息，而且(qie)鉆(zhan)臂運(yun)行(xing)(xing)過程(cheng)中(zhong)(zhong)會對各個(ge)關(guan)(guan)節產生的(de)沖擊(ji)，現(xian)有控制方法無(wu)法對各個(ge)關(guan)(guan)節進(jin)行(xing)(xing)控制以(yi)(yi)使鉆(zhan)臂按照(zhao)預設軌(gui)跡進(jin)行(xing)(xing)自主精確運(yun)動，作(zuo)業效(xiao)率(lv)低，定位誤差大(da)。

技術實現思路

1、為克服現有技術中鉆臂在(zai)萬向節(jie)鎖(suo)死狀態下(xia)無(wu)法(fa)求(qiu)解(jie)鉆臂姿態信息，現有控(kong)制(zhi)方法(fa)無(wu)法(fa)對各個關節(jie)進行控(kong)制(zhi)的問題。本發明提出一種鑿巖臺車鉆孔定位與鉆臂作業的集(ji)成控(kong)制(zhi)方法(fa)，所(suo)述方法(fa)包括以(yi)下(xia)步驟：

2、s1、輸(shu)入工作平面(mian)的斷面(mian)幾何數(shu)據以(yi)及(ji)爆(bao)破參數(shu)，依據所述(shu)斷面(mian)幾何數(shu)據繪制斷面(mian)輪廓線(xian)，根據炮(pao)孔(kong)的類(lei)型以(yi)及(ji)所述(shu)斷面(mian)輪廓線(xian)完(wan)成(cheng)炮(pao)孔(kong)布(bu)置算法，輸(shu)出(chu)炮(pao)孔(kong)位置信息以(yi)及(ji)炮(pao)孔(kong)布(bu)置圖；

3、s2、通過單目視覺(jue)測(ce)距(ju)算法獲取視覺(jue)傳感器(qi)到工作(zuo)(zuo)平(ping)面(mian)的距(ju)離(li)，以(yi)確(que)定工作(zuo)(zuo)平(ping)面(mian)的深(shen)度(du)信息(xi)(xi)，將(jiang)所述工作(zuo)(zuo)平(ping)面(mian)的深(shen)度(du)信息(xi)(xi)與所述炮(pao)孔位(wei)置信息(xi)(xi)相(xiang)結合，獲得炮(pao)孔位(wei)置的三坐標信息(xi)(xi)，確(que)定鉆臂目標位(wei)姿；

4、s3、對鉆(zhan)(zhan)(zhan)臂進行運(yun)動學(xue)分析，針對鉆(zhan)(zhan)(zhan)臂的(de)各個關節構建(jian)關節坐標(biao)(biao)系，獲得鉆(zhan)(zhan)(zhan)臂運(yun)動學(xue)方程，根據(ju)所述鉆(zhan)(zhan)(zhan)臂運(yun)動學(xue)方程構建(jian)鉆(zhan)(zhan)(zhan)臂逆運(yun)動學(xue)算法(fa)，獲取(qu)并輸(shu)出在所述鉆(zhan)(zhan)(zhan)臂目(mu)標(biao)(biao)位姿下的(de)關節目(mu)標(biao)(biao)位姿；

5、s4、根據關(guan)節初(chu)始位姿(zi)以及所述(shu)關(guan)節目標位姿(zi)，采(cai)用五次(ci)多項式(shi)對各(ge)個(ge)關(guan)節運動(dong)(dong)軌(gui)跡進行(xing)規劃，基于pid控(kong)制算法(fa)對各(ge)個(ge)關(guan)節進行(xing)控(kong)制,使鉆(zhan)臂(bei)按照規劃軌(gui)跡移動(dong)(dong)到所述(shu)鉆(zhan)臂(bei)目標位姿(zi)。

6、在本發明的(de)一(yi)個優選實施例(li)中，所述步驟(zou)s1中的(de)炮孔包(bao)括掏槽孔，周邊孔、內圈孔以(yi)及崩落孔。

7、在本發明的一個優選(xuan)實施(shi)例中，所(suo)述單目視覺(jue)測距算法是在工作平面設(she)定(ding)一形狀規則(ze)的標定(ding)點p，測定(ding)所(suo)述標定(ding)點p到視覺(jue)傳(chuan)感(gan)器(qi)(qi)的距離來表(biao)示視覺(jue)傳(chuan)感(gan)器(qi)(qi)到工作平面的距離d，具體包括如下(xia)步(bu)驟：

8、圖(tu)(tu)像處(chu)(chu)理(li)過(guo)程，通過(guo)視覺傳感(gan)器獲(huo)取(qu)工作平面的圖(tu)(tu)像，將所述圖(tu)(tu)像進行(xing)灰度化處(chu)(chu)理(li)，根據背(bei)景以及灰度化信息設定閾值，然(ran)后，將圖(tu)(tu)像二值化去除干擾信息，獲(huo)得標定點輪廓；

9、單目測(ce)距(ju)過(guo)程(cheng)(cheng)，根據所(suo)述標(biao)定點輪廓，利用圖(tu)像矩獲得標(biao)定點p中心坐標(biao)，通過(guo)小(xiao)孔成像原(yuan)理獲得視覺傳(chuan)感器到(dao)工作平面的距(ju)離(li)d，完成測(ce)距(ju)過(guo)程(cheng)(cheng)。

10、優選(xuan)地，所述標定(ding)點p中心坐標計算方法為(wei)：

11、以所(suo)述視(shi)(shi)覺(jue)(jue)傳(chuan)感器的光心為(wei)原點(dian)建立視(shi)(shi)覺(jue)(jue)傳(chuan)感器坐標系，p'為(wei)標識點(dian)p在視(shi)(shi)覺(jue)(jue)傳(chuan)感器中(zhong)的成像點(dian)，所(suo)述p在所(suo)述視(shi)(shi)覺(jue)(jue)傳(chuan)感器坐標系中(zhong)的坐標（xc,yc,zc）如下所(suo)示：

12、

13、其(qi)中，為p到(dao)地面(mian)(mian)的距(ju)離，為p'到(dao)成(cheng)像平(ping)面(mian)(mian)主(zhu)點(dian)的垂直距(ju)離，d表(biao)示(shi)視覺傳感器到(dao)工作平(ping)面(mian)(mian)的距(ju)離，f表(biao)示(shi)焦距(ju)，x，y分別為p'在像素坐(zuo)(zuo)標系中的橫坐(zuo)(zuo)標和縱坐(zuo)(zuo)標。

14、在本發明的(de)一(yi)個(ge)優選(xuan)實施例中，所述(shu)s3中對鉆臂(bei)進(jin)行運動學分析，包(bao)括如(ru)下步驟：

15、對鉆臂的結構進(jin)行分(fen)析，獲取鉆臂的關(guan)(guan)節(jie)數(shu)量、類型、連(lian)桿長度、連(lian)桿扭角(jiao)以及關(guan)(guan)節(jie)變量，針對各個關(guan)(guan)節(jie)構建關(guan)(guan)節(jie)坐(zuo)標系，生成參數(shu)表；

16、對所述(shu)關節坐(zuo)(zuo)(zuo)標(biao)(biao)系進(jin)行運動學分析(xi)，以便獲(huo)取相(xiang)鄰兩(liang)個關節坐(zuo)(zuo)(zuo)標(biao)(biao)系之(zhi)間的齊次(ci)變換矩陣，從而獲(huo)取鉆臂末端坐(zuo)(zuo)(zuo)標(biao)(biao)系與基座(zuo)坐(zuo)(zuo)(zuo)標(biao)(biao)系之(zhi)間的變換關系，并與所述(shu)鉆臂目(mu)標(biao)(biao)位(wei)姿矩陣建立(li)聯系，獲(huo)得鉆臂運動學方程；

17、根據所(suo)述(shu)鉆(zhan)(zhan)臂(bei)(bei)運動(dong)學(xue)方(fang)程構建(jian)鉆(zhan)(zhan)臂(bei)(bei)逆運動(dong)學(xue)方(fang)程，求(qiu)解所(suo)述(shu)鉆(zhan)(zhan)臂(bei)(bei)逆運動(dong)學(xue)方(fang)程的解，得到所(suo)述(shu)關節目標(biao)位姿。

18、在本發明的一個優選實施例中，相鄰兩個關(guan)節坐標系之間的齊(qi)次(ci)變(bian)換矩陣如下式所(suo)示：

19、

20、其(qi)中，i表(biao)(biao)示(shi)關節(jie)，表(biao)(biao)示(shi)連桿扭角，表(biao)(biao)示(shi)連桿長度，表(biao)(biao)示(shi)關節(jie)偏(pian)置，表(biao)(biao)示(shi)關節(jie)扭角。

21、優選(xuan)地，在(zai)求解所述(shu)鉆臂逆運動學方程的(de)過程中(zhong)，采用(yong)δ1，δ2兩個閾值(zhi)，其中(zhong)δ1>δ2，當鉆臂當前位(wei)(wei)姿與(yu)目標(biao)位(wei)(wei)姿的(de)差(cha)值(zhi)小(xiao)于δ1時，將閾值(zhi)改為(wei)δ2繼(ji)續迭代。

22、在(zai)本發明的(de)一個(ge)(ge)(ge)優選實施例中，所述步(bu)驟s4中采用(yong)五次(ci)多項(xiang)(xiang)式對各個(ge)(ge)(ge)關節(jie)(jie)運動軌(gui)跡進行規劃，具體為(wei)：利(li)用(yong)關節(jie)(jie)初始時刻和終止(zhi)時刻的(de)位置(zhi)、速度和加速度列(lie)出六個(ge)(ge)(ge)等(deng)式，通過具有六個(ge)(ge)(ge)系數的(de)五次(ci)多項(xiang)(xiang)式計算不同時刻t的(de)關節(jie)(jie)位置(zhi)，通過差(cha)值的(de)方法確定六個(ge)(ge)(ge)系數的(de)值，建立關節(jie)(jie)位置(zhi)的(de)函數，從而獲得(de)各個(ge)(ge)(ge)關節(jie)(jie)變化曲線。

23、優選地，所述關節位(wei)置θ（t）的五次(ci)多項式的基本形式為：

24、

25、其中，、、、、、表示待(dai)確定的六(liu)個系數。

26、本(ben)發明還提(ti)供了一(yi)種鑿巖臺(tai)車鉆孔(kong)定(ding)位(wei)與(yu)鉆臂作(zuo)(zuo)業(ye)的(de)集成控(kong)制(zhi)系統，所述(shu)系統采用上述(shu)鑿巖臺(tai)車鉆孔(kong)定(ding)位(wei)與(yu)鉆臂作(zuo)(zuo)業(ye)的(de)集成控(kong)制(zhi)方法(fa)，對鑿巖臺(tai)車鉆臂的(de)作(zuo)(zuo)業(ye)進行自動控(kong)制(zhi)。

27、本發明有益效果：

28、1、本發明利用四元(yuan)數(shu)表示鉆(zhan)臂(bei)位姿(zi)，解決了鉆(zhan)臂(bei)在萬向節鎖死(si)狀態下無法求(qiu)解鉆(zhan)臂(bei)位姿(zi)信息的(de)問(wen)題(ti)；

29、2、本(ben)發明利用(yong)單(dan)目視(shi)覺方(fang)法(fa)，求解工作平面(mian)的(de)深度(du)信息(xi)，智(zhi)能識別定位功能簡化(hua)了復雜的(de)實(shi)時測(ce)距(ju)過(guo)程(cheng)；

30、3、本發(fa)明根(gen)據炮(pao)(pao)孔的類型以及所(suo)述斷面(mian)輪廓線完成(cheng)炮(pao)(pao)孔布(bu)置(zhi)算法，使得(de)用(yong)戶(hu)通(tong)過用(yong)戶(hu)界(jie)面(mian)即可完成(cheng)炮(pao)(pao)孔布(bu)置(zhi)，可針對工作(zuo)平(ping)面(mian)實時生成(cheng)炮(pao)(pao)孔位置(zhi)信(xin)息和炮(pao)(pao)孔布(bu)置(zhi)圖；

31、4、本發明采用五次多項式(shi)對(dui)鉆(zhan)(zhan)臂(bei)軌(gui)跡進行(xing)規(gui)劃，相較(jiao)于人為控(kong)(kong)制(zhi)，降低了在(zai)鉆(zhan)(zhan)臂(bei)運(yun)(yun)行(xing)過(guo)程中(zhong)對(dui)關(guan)(guan)節產生(sheng)的沖(chong)擊(ji)，所述控(kong)(kong)制(zhi)方法對(dui)鉆(zhan)(zhan)臂(bei)各個關(guan)(guan)節進行(xing)控(kong)(kong)制(zhi)，使鉆(zhan)(zhan)臂(bei)能夠(gou)按照預設軌(gui)跡進行(xing)自(zi)主精確(que)運(yun)(yun)動，提高了作業效率。

技術特征：

1.一種(zhong)鑿巖臺車鉆(zhan)孔定位與鉆(zhan)臂作業的(de)集成控制(zhi)方(fang)(fang)法(fa)，其特征(zheng)在于，所述方(fang)(fang)法(fa)包括如下步驟：

2.根(gen)據權利(li)要求1所述(shu)的(de)鑿巖臺車鉆孔(kong)定(ding)位與鉆臂作(zuo)業的(de)集成控制方法(fa)，其特征在于(yu)，所述(shu)步驟s1中的(de)炮(pao)孔(kong)包括(kuo)掏槽孔(kong)，周邊孔(kong)、內圈孔(kong)以(yi)及崩落孔(kong)。

3.根據權(quan)利(li)要(yao)求1所述的(de)鑿巖臺車鉆(zhan)孔(kong)定(ding)位與鉆(zhan)臂作(zuo)(zuo)業(ye)的(de)集(ji)成控制方法，其特征在(zai)(zai)于，所述單目視覺(jue)測距算法是在(zai)(zai)工作(zuo)(zuo)平面設(she)定(ding)一形狀規則的(de)標定(ding)點p，測定(ding)所述標定(ding)點p到視覺(jue)傳感(gan)器的(de)距離(li)來表示視覺(jue)傳感(gan)器到工作(zuo)(zuo)平面的(de)距離(li)d，具體包括如下(xia)步驟：

4.根據權(quan)利要(yao)求3所述(shu)的(de)鑿巖臺車鉆(zhan)孔定位與(yu)鉆(zhan)臂作(zuo)業(ye)的(de)集成控(kong)制方法(fa)(fa)，其特征在于，所述(shu)標定點(dian)p中心坐標計算方法(fa)(fa)為：

5.根(gen)據(ju)權(quan)利要求1所(suo)述(shu)的鑿(zao)巖臺車鉆孔(kong)定位與鉆臂(bei)作業的集成控(kong)制方法，其特征(zheng)在于，所(suo)述(shu)s3中對鉆臂(bei)進行運動學分(fen)析(xi)，包括如下步驟：

6.根(gen)據(ju)權利(li)要求(qiu)5所述的鑿巖(yan)臺車鉆孔定位(wei)與鉆臂作業的集(ji)成控制方(fang)法(fa)，其(qi)特征在于(yu)，相鄰(lin)兩個關(guan)節坐標系之間的齊次變換(huan)矩(ju)陣(zhen)如(ru)下式所示：

7.根據權利要求6所(suo)述(shu)的鑿巖臺車(che)鉆孔(kong)定(ding)位(wei)(wei)與(yu)鉆臂作業的集成控制(zhi)方法(fa)，其(qi)特(te)征在(zai)于，在(zai)求解所(suo)述(shu)鉆臂逆運(yun)動(dong)學(xue)方程(cheng)的過程(cheng)中(zhong)，采用δ1，δ2兩個閾值，其(qi)中(zhong)δ1>δ2，當(dang)鉆臂當(dang)前(qian)位(wei)(wei)姿(zi)(zi)與(yu)目(mu)標(biao)位(wei)(wei)姿(zi)(zi)的差值小于δ1時，將閾值改為(wei)δ2繼續迭代。

8.根據權(quan)利要求1所(suo)述的(de)(de)鑿巖臺車鉆(zhan)孔定位(wei)(wei)與鉆(zhan)臂作業的(de)(de)集成控制方法(fa)，其特征在于，所(suo)述步驟s4中采用(yong)(yong)五次多項式對各(ge)個(ge)關(guan)(guan)節(jie)(jie)運動(dong)軌跡進行(xing)規(gui)劃，具體(ti)為(wei)：利用(yong)(yong)關(guan)(guan)節(jie)(jie)初(chu)始時刻和終止時刻的(de)(de)位(wei)(wei)置、速度和加速度列(lie)出(chu)六(liu)個(ge)等(deng)式，通(tong)過具有(you)六(liu)個(ge)系數的(de)(de)五次多項式計(ji)算不同時刻t的(de)(de)關(guan)(guan)節(jie)(jie)位(wei)(wei)置，通(tong)過差值的(de)(de)方法(fa)確(que)定六(liu)個(ge)系數的(de)(de)值，建立關(guan)(guan)節(jie)(jie)位(wei)(wei)置的(de)(de)函(han)數，從而獲得各(ge)個(ge)關(guan)(guan)節(jie)(jie)變化(hua)曲線。

9.根據(ju)權利(li)要求8所(suo)述的(de)(de)鑿巖臺車鉆孔定位(wei)與鉆臂作業的(de)(de)集成控制方法，其特征在于，所(suo)述關節位(wei)置(zhi)θ（t）的(de)(de)五次多項式(shi)的(de)(de)基本形式(shi)為：

10.一種鑿(zao)(zao)(zao)巖(yan)(yan)臺車(che)鉆孔定(ding)位與(yu)鉆臂(bei)作業的(de)(de)集成控(kong)(kong)制系統，其(qi)特(te)征(zheng)在于，所述系統采用(yong)如(ru)權利(li)要求1~9中任一項鑿(zao)(zao)(zao)巖(yan)(yan)臺車(che)鉆孔定(ding)位與(yu)鉆臂(bei)作業的(de)(de)集成控(kong)(kong)制方法，對鑿(zao)(zao)(zao)巖(yan)(yan)臺車(che)鉆臂(bei)的(de)(de)作業進行自動(dong)控(kong)(kong)制。

技術總結
本發明涉及掘進機械智能操作技術領域，尤其涉及一種鑿巖臺車鉆孔定位與鉆臂作業的集成控制方法和系統，所述方法包括：輸入斷面幾何數據以及爆破參數，完成炮孔布置算法；通過單目視覺測距算法獲取視覺傳感器到工作平面的距離，確定鉆臂目標位姿；對鉆臂進行運動學分析，獲取并輸出所述鉆臂目標位姿下的關節目標位姿；采用五次多項式對各個關節運動軌跡進行規劃，使鉆臂按照規劃軌跡移動到所述鉆臂目標位姿。本發明采用單目視覺測距算法確定炮孔位置的三坐標信息，智能識別定位功能簡化了復雜的實時測距過程；采用五次多項式對鉆臂軌跡進行規劃，降低了鉆臂運行過程中對關節產生的沖擊，使鉆臂能夠按照預設軌跡進行自主精確運動，提高了作業效率。

技術研發人員：隋曦,隋自清,杜憲峰,顧孟豪,趙廣學,李棟良
受保護的技術使用者：煙臺興業機械股份有限公司
技術研發日：
技術公布日：2024/10/14