智能錨注一體臺車鉆臂運(yùn)動學(xué)研究

牛孔肖，荊留杰，陳 帥，李鵬宇

（中鐵工程裝備集團(tuán)有限公司，河南 鄭州 450016）

1 引言

近年來，交通［1］、礦山［2］領(lǐng)域的部分企業(yè)開始逐步引入大型智能化裝備［3］，以取代人工粗放的作業(yè)模式。目前集鉆孔、注漿、安裝錨桿等功能為一體的緊湊型液壓錨注一體臺車已經(jīng)比較成熟，這種臺車依靠人工進(jìn)行鉆臂定位，精度低且速度慢，不利于隧道施工效率和質(zhì)量的提升。實(shí)現(xiàn)錨注一體臺車的信息化和智能化是錨注一體臺車的重要發(fā)展方向，而錨桿的快速自動定位是其中最關(guān)鍵的步驟。

為了實(shí)現(xiàn)錨桿的快速自動定位，首先需要建立錨注一體臺車鉆臂的運(yùn)動學(xué)模型，然后根據(jù)不同的求解方向，分別對正向運(yùn)動學(xué)和逆向運(yùn)動學(xué)［4］進(jìn)行研究。目前國內(nèi)外針對隧道施工專用設(shè)備臂架運(yùn)動學(xué)的研究集中在鑿巖臺車，而對智能錨注一體臺車研究較少。文獻(xiàn)［5］使用D-H法研究了液壓鑿巖臺車鉆臂的正運(yùn)動學(xué)和工作空間。文獻(xiàn)［6］通過現(xiàn)場試驗(yàn)和數(shù)值模擬總結(jié)關(guān)節(jié)間隙和撓度分布規(guī)律，然后采用蒙特卡洛法對運(yùn)動學(xué)模型進(jìn)行修正。文獻(xiàn)［7］基于機(jī)器視覺和激光測距的臂架柔度檢測方法建立了錨桿臺車定位誤差補(bǔ)償模型，并建立了迭代法求兩次逆解確定關(guān)節(jié)變量的臂架定位算法。文獻(xiàn)［8］對鑿巖臺車雙三角鉆臂建立非齊次增量型時變線性模型，并提出了基于該模型的自適應(yīng)預(yù)測控制算法。文獻(xiàn)［9］設(shè)計了一種改進(jìn)型鑿巖機(jī)器人，并對其鉆孔定位方法進(jìn)行了研究。文獻(xiàn)［10］提出了一種利用啟發(fā)式搜索算法研究鑿巖機(jī)器人逆運(yùn)動學(xué)的方法。文獻(xiàn)［11］提出了一種交叉精英反向粒子群優(yōu)化算法求解鑿巖機(jī)器人鉆臂運(yùn)動學(xué)逆解。文獻(xiàn)［12］基于改進(jìn)差分進(jìn)化算法研究了9自由度液壓機(jī)械臂的運(yùn)動學(xué)逆解。文獻(xiàn)［13］提出一種與智能掘支錨聯(lián)合機(jī)組配套的機(jī)器人化鉆錨車，并對其進(jìn)行工作空間分析和軌跡規(guī)劃。

上述研究在進(jìn)行臂架運(yùn)動學(xué)分析時均沒有有效利用臂架的結(jié)構(gòu)特點(diǎn)，而是采用數(shù)值解的方式對逆向運(yùn)動學(xué)進(jìn)行求解，這種方式一般計算時間較長，很難滿足視覺伺服［14］、精細(xì)路徑規(guī)劃［15］等高頻計算場景的使用需求。這里采用D-H方法建立8自由度鉆臂的運(yùn)動學(xué)方程，基于蒙特卡洛法分析鉆臂的可達(dá)工作空間，根據(jù)鉆臂的結(jié)構(gòu)特點(diǎn)和關(guān)節(jié)運(yùn)動范圍提出一種鉆臂運(yùn)動學(xué)逆解算法，針對大斷面隧道錨桿作業(yè)過程，在臺車?yán)硐霠顟B(tài)和傾斜狀態(tài)下對鉆臂運(yùn)動學(xué)逆解算法進(jìn)行了驗(yàn)證。

2 鉆臂運(yùn)動方程的建立

2.1 智能錨注一體臺車鉆臂結(jié)構(gòu)特點(diǎn)

智能錨注一體臺車鉆臂由雙三角油缸、伸縮油缸、擺動油缸、外插角油缸和補(bǔ)償油缸驅(qū)動，包含6個回轉(zhuǎn)關(guān)節(jié)和2個移動關(guān)節(jié)，是一種8自由度冗余機(jī)械臂，如圖1所示。機(jī)頭上裝有鉆孔梁和錨桿梁，兩者配合形成雙推進(jìn)梁結(jié)構(gòu)，在對錨桿作業(yè)過程中的鉆臂運(yùn)動學(xué)進(jìn)行分析時僅需考慮錨桿梁的位姿。

圖1 智能錨注一體臺車鉆臂結(jié)構(gòu)Fig.1 Structure of the Bolting Jumbo Boom

智能錨注一體臺車在自動化和智能化的過程中需要兼容手動模式，這樣在某些特殊作業(yè)場景下操作手仍可以掌握一定的自主權(quán)。充分考慮手動模式下的可操作性，對智能錨注一體臺車鉆臂運(yùn)動進(jìn)行動作分解，這也是智能錨注一體臺車鉆臂自由度冗余的一個重要原因，如表1所示。

表1 鉆臂運(yùn)動的動作分解Tab.1 Action Decomposition of Boom Movement

2.2 D-H模型

根據(jù)智能錨注一體臺車鉆臂的結(jié)構(gòu)參數(shù)，建立各關(guān)節(jié)坐標(biāo)系（均為右手系），得到鉆臂運(yùn)動簡圖，如圖2所示。鉆臂D-H參數(shù)表，如表2所示。αi-1為連桿扭轉(zhuǎn)角，ai-1為連桿長度，θi為關(guān)節(jié)角，di為連桿偏移。

表2 鉆臂D-H參數(shù)表Tab.2 The D-H Parameters of the Boom

圖2 鉆臂運(yùn)動簡圖Fig.2 Movement Diagram of the Boom

其中：

2.3 正運(yùn)動學(xué)方程

智能錨注一體臺車鉆臂相鄰關(guān)節(jié)坐標(biāo)系之間的齊次變換矩陣為：

式中：i—關(guān)節(jié)編號；

鉆臂末端相對于基座的位姿為：

式（2）即為智能錨注一體臺車鉆臂的正向運(yùn)動學(xué)方程。

3 可達(dá)工作空間計算

為滿足大部分隧道的斷面施工要求，鉆臂最大作業(yè)高度（圖1中Z向）需要達(dá)到12m，最大作業(yè)寬度（圖1中Y向）需要達(dá)到15m。基于蒙特卡羅法利用matlab仿真計算智能錨注一體臺車鉆臂的可達(dá)工作空間，計算流程，如圖3所示。

圖3 計算可達(dá)工作空間的流程圖Fig.3 Flow Chart for Computing Reachable Workspace

采用與圖1一致的坐標(biāo)軸方向，仿真得到智能錨注一體臺車鉆臂可達(dá)工作空間云圖，如圖4 所示?？蛇_(dá)工作空間在XOZ和YOZ平面上的投影，如圖5、圖6所示。由鉆臂可達(dá)工作空間可以得出智能錨注一體臺車可以在-2m≤X≤12m、-9m≤Y≤9m、-8m≤Z≤11m范圍內(nèi)進(jìn)行隧道支護(hù)作業(yè)，可以滿足大部分隧道施工使用需求。

圖4 鉆臂三維可達(dá)工作空間Fig.4 3D Reachable Workspace of the Boom

圖5 鉆臂可達(dá)工作空間在XOZ平面上的投影Fig.5 Reachable Workspace of the Boom in XOZ

圖6 鉆臂可達(dá)工作空間在YOZ平面上的投影Fig.6 Reachable Workspace of the Boom in YOZ

4 鉆臂逆運(yùn)動學(xué)算法

4.1 算法原理

雙三角油缸的結(jié)構(gòu)特點(diǎn)是為了完成大臂的擺臂和起落動作，使這兩個動作不引起錨桿梁姿態(tài)的變化，于是：

伸縮油缸和補(bǔ)償油缸不會引起錨桿梁姿態(tài)的變化，可知：

由式（3）、式（4）得：

為滿足式（5），可使：

因?yàn)閝4的取值范圍與-q2相同，q5的取值范圍與-q1相同，所以式（6）符合鉆臂結(jié)構(gòu)特點(diǎn)。

按照鉆臂結(jié)構(gòu)特點(diǎn)將8個關(guān)節(jié)進(jìn)行功能劃分，關(guān)節(jié)6和關(guān)節(jié)7用于調(diào)整錨桿梁的方向，關(guān)節(jié)1到關(guān)節(jié)5用于調(diào)整錨桿梁末端的位置，關(guān)節(jié)8用于補(bǔ)償定位不準(zhǔn)等因素引起的末端間隙。

基于上述條件和各關(guān)節(jié)坐標(biāo)系方向，可得：

由式（7）可以解出：

式中：acos—C語言中的反余弦函數(shù)；

atan 2—C語言中的反正切函數(shù)。

采用迭代搜索方法獲取q8，將q8從最小值開始按一定的步長依次設(shè)置為不同的值，直到成功求出符合條件的q1-q7。把q6、q7和q8代入式（2），經(jīng)變換得到：

由式（10）可得出：

基于表2中的關(guān)節(jié)運(yùn)動范圍，本逆解算法得到了明確的計算公式，不存在多解的情況而且計算精度高。

4.2 算法實(shí)現(xiàn)

基于C++語言實(shí)現(xiàn)了智能錨注一體臺車鉆臂逆運(yùn)動學(xué)算法，程序流程，如圖7所示。

圖7 運(yùn)動學(xué)逆解流程圖Fig.7 Flow Chart for Inverse Kinematics

4.3 算法驗(yàn)證

模擬智能錨注一體臺車在φ15m級大斷面隧道中的施工作業(yè)情況，選取23組錨桿目標(biāo)位姿，其中分布在隧道面上的23個圓代表錨桿定位位置，其上引出的線段代表錨桿的方向，如圖8所示。理想情況下，臺車車體與隧道軸線平行，當(dāng)鉆臂基座距目標(biāo)截面6m，高于隧道底面3m時，依據(jù)錨桿末端在鉆臂基座坐標(biāo)系中的位置和方向得到對應(yīng)的運(yùn)動學(xué)逆解計算結(jié)果，如表3所示。每組數(shù)據(jù)計算時間均小于1ns，且計算結(jié)果符合鉆臂結(jié)構(gòu)特點(diǎn)。

表3 鉆臂運(yùn)動學(xué)逆解結(jié)果Tab.3 The Results of Inverse Kinematics for the Boom

圖8 錨桿孔布置圖Fig.8 Anchor Hole Diagram

為保證施工效率，操作手憑經(jīng)驗(yàn)將臺車停在隧道的合適位置，而并不會特意將車體調(diào)到非常理想的狀態(tài)。車體一般會與隧道軸線有一個較小的夾角γ，如圖9所示。車體傾斜時依據(jù)錨桿末端在鉆臂基座坐標(biāo)系中的位置和方向得到對應(yīng)的運(yùn)動學(xué)逆解計算結(jié)果，如表4所示。表中：前11組數(shù)據(jù)是在車體左偏3°情況下的結(jié)果，后12組數(shù)據(jù)是在車體右偏3°情況下的結(jié)果。每組數(shù)據(jù)計算時間均小于1ns，且計算結(jié)果符合鉆臂結(jié)構(gòu)特點(diǎn)。

表4 當(dāng)車體傾斜時鉆臂運(yùn)動學(xué)逆解結(jié)果Tab.4 The Results of Inverse Kinematics for the Boom When the Jumbo Is Tilted

圖9 臺車位置示意圖Fig.9 Schematic Diagram of Jumbo Position

5 結(jié)語

（1）應(yīng)用D-H法建立了智能錨注一體臺車8自由度鉆臂的運(yùn)動學(xué)模型，并列出了鉆臂結(jié)構(gòu)的分解動作，為后續(xù)分析提供依據(jù)。

（2）基于蒙特卡羅法在matlab中仿真出鉆臂工作空間，驗(yàn)證智能錨注一體臺車鉆臂可以滿足隧道施工使用需求。

（3）提出一種基于結(jié)構(gòu)特點(diǎn)的智能錨注一體臺車鉆臂逆運(yùn)動學(xué)算法，該算法計算效率高，在臺車?yán)硐霠顟B(tài)和傾斜狀態(tài)下均表現(xiàn)良好。