姚亞峰，姚寧平，彭 濤，梁春苗，彭光宇

(1.煤炭科學(xué)研究總院，北京 100013；2.中煤科工西安研究院(集團(tuán))有限公司，陜西 西安 710077)

煤礦井下鉆機(jī)主要用于瓦斯抽采、地質(zhì)勘探、探放水等領(lǐng)域，是煤礦安全生產(chǎn)不可或缺的裝備之一[1-3]。自2015 年起，國(guó)家部委陸續(xù)發(fā)布了一些提升煤礦發(fā)展水平、提高安全生產(chǎn)水平的技術(shù)性指導(dǎo)意見(jiàn)[4-5]，這對(duì)于煤礦井下鉆機(jī)向智能化轉(zhuǎn)型發(fā)展具有重要意義。自動(dòng)加卸鉆桿技術(shù)是智能化鉆機(jī)研究的關(guān)鍵技術(shù)之一。常見(jiàn)煤礦井下智能化鉆機(jī)以負(fù)載敏感電液比例多路閥為主要核心，以PLC 智能控制器為控制單元，遙控器為操作終端，位移傳感器、壓力傳感器等為檢測(cè)反饋元件，實(shí)現(xiàn)了自動(dòng)加卸鉆桿裝置中各機(jī)械臂的同步運(yùn)行和邏輯控制[6-8]，但也存在裝置結(jié)構(gòu)復(fù)雜、檢測(cè)元件多、定位精度低、重復(fù)定位誤差大等問(wèn)題，對(duì)于加卸鉆桿功能的運(yùn)行可靠性影響較大[9-10]。

針對(duì)典型加卸鉆桿系統(tǒng)存在的問(wèn)題，開(kāi)發(fā)了防爆伺服電機(jī)驅(qū)動(dòng)的六自由度機(jī)械手及其控制系統(tǒng)，并應(yīng)用于鉆孔機(jī)器人加卸鉆桿，具有加卸鉆桿效率高、重復(fù)定位精度高及工作范圍大等特點(diǎn)，可滿(mǎn)足鉆孔機(jī)器人連續(xù)快速加卸鉆桿需求[11-12]。通過(guò)將機(jī)械手、主機(jī)和鉆桿倉(cāng)等安裝在共同的履帶車(chē)體平臺(tái)上，開(kāi)發(fā)了一體式鉆孔機(jī)器人。由于鉆孔設(shè)計(jì)需求，鉆孔中心位置即主機(jī)的主軸中心孔位置會(huì)隨著主機(jī)的調(diào)姿而發(fā)生變化，同時(shí)在加卸鉆桿過(guò)程中還存在障礙物，因此，每個(gè)鉆孔的加卸鉆桿過(guò)程都存在變目標(biāo)的避障軌跡規(guī)劃問(wèn)題。

針對(duì)機(jī)器人協(xié)作中軌跡規(guī)劃問(wèn)題，相關(guān)學(xué)者提出對(duì)兩個(gè)不同自由度的機(jī)器人采用視覺(jué)系統(tǒng)識(shí)別標(biāo)記，并利用標(biāo)記點(diǎn)進(jìn)行實(shí)時(shí)跟蹤定位[13]。針對(duì)多關(guān)節(jié)平面機(jī)器人，提出了一種有效和準(zhǔn)確的避障軌跡規(guī)劃和跟蹤策略，設(shè)計(jì)了合適的二階滑?？刂破鱽?lái)保證足夠的跟蹤精度，該方法可獲得有效和準(zhǔn)確的軌跡規(guī)劃[14]。針對(duì)零件掃描建模問(wèn)題，提出了基于六軸機(jī)器人的零件掃描路徑規(guī)劃方法，有效控制了重疊掃描區(qū)域[15]。隨著機(jī)器人的規(guī)模應(yīng)用，國(guó)內(nèi)在該方向的研究逐漸成為了熱點(diǎn)。針對(duì)六自由度串聯(lián)機(jī)械臂，進(jìn)行了改進(jìn)RRT算法的機(jī)械臂避障路徑規(guī)劃研究[16]。針對(duì)六軸工業(yè)機(jī)器人軌跡規(guī)劃等的方法問(wèn)題，從單機(jī)軌跡規(guī)劃、雙機(jī)協(xié)作柔順控制、單機(jī)在線運(yùn)動(dòng)規(guī)劃等方面入手，提出合理的軌跡優(yōu)化和運(yùn)動(dòng)規(guī)劃策略[17]。針對(duì)機(jī)器人碼垛中的避障問(wèn)題，提出一種實(shí)現(xiàn)自主避障的軌跡規(guī)劃方法[18]。針對(duì)算法改進(jìn)方面，在傳統(tǒng)RRT 算法的基礎(chǔ)上，提出了一種擴(kuò)展點(diǎn)選擇策略和自適應(yīng)步長(zhǎng)策略，并結(jié)合Dijkstra 算法對(duì)改進(jìn)算法產(chǎn)生的路徑進(jìn)行優(yōu)化[19]。

國(guó)內(nèi)外學(xué)者在工業(yè)機(jī)器人的裝配、碼垛、協(xié)作等場(chǎng)景中進(jìn)行了避障軌跡規(guī)劃研究，研究重點(diǎn)集中于軌跡規(guī)劃算法優(yōu)化，但在煤礦井下鉆孔機(jī)器人等裝備上卻鮮有研究和應(yīng)用。在煤礦井下工作環(huán)境中，機(jī)械手和主機(jī)之間加卸鉆桿的避障環(huán)境、工作空間及負(fù)載特性等與工業(yè)機(jī)器人的應(yīng)用場(chǎng)景顯著不同，不僅存在電氣元件和控制系統(tǒng)的防爆問(wèn)題，還存在障礙物及目標(biāo)位置變化、工作空間小、運(yùn)動(dòng)沖擊大等問(wèn)題，這些問(wèn)題的研究對(duì)于提高鉆孔機(jī)器人加卸鉆桿控制系統(tǒng)的智能化程度具有重要意義。本文從煤礦井下鉆孔機(jī)器人加卸鉆桿系統(tǒng)組成原理、避障軌跡規(guī)劃策略、具體的避障軌跡規(guī)劃算法、仿真分析及試驗(yàn)驗(yàn)證等方面開(kāi)展研究，提出了適應(yīng)主機(jī)姿態(tài)變化的加卸鉆桿軌跡規(guī)劃方法。

1 鉆孔機(jī)器人加卸鉆桿系統(tǒng)

一體式鉆孔機(jī)器人的加卸鉆桿系統(tǒng)以防爆六自由度機(jī)械手為主體，與主機(jī)、鉆桿倉(cāng)、穩(wěn)固裝置等安裝在共同的車(chē)體平臺(tái)上，并通過(guò)控制系統(tǒng)實(shí)現(xiàn)鉆桿的自動(dòng)加卸功能，其結(jié)構(gòu)如圖1 所示。機(jī)械手采用防爆伺服電機(jī)驅(qū)動(dòng)，由6 個(gè)串聯(lián)機(jī)械臂(連桿)、6 個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)和末端手爪等組成，其中6 個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)分別為基座的回轉(zhuǎn)、腰的俯仰、小臂的俯仰、小臂的回轉(zhuǎn)、腕部關(guān)節(jié)的俯仰和腕部關(guān)節(jié)的回轉(zhuǎn)。根據(jù)機(jī)械手各連桿參數(shù)、連桿坐標(biāo)系和各關(guān)節(jié)限位角度，采用標(biāo)準(zhǔn)DH 參數(shù)法進(jìn)行運(yùn)動(dòng)學(xué)建模[20]，可得到機(jī)械手的DH 參數(shù)見(jiàn)表1。主機(jī)是鉆孔機(jī)器人的關(guān)鍵部件，包括變幅裝置、動(dòng)力頭、夾持器、給進(jìn)裝置等，其中變幅裝置可實(shí)現(xiàn)鉆孔開(kāi)孔的參數(shù)調(diào)節(jié)，包括舉升、傾角和機(jī)身平移3 個(gè)自由度。鉆桿倉(cāng)是貯存鉆桿的裝置，鉆桿倉(cāng)中的鉆桿按照固定的行間距和列間距排列。

表1 防爆六自由度機(jī)械手連桿DH 參數(shù)Table 1 DH parameters of explosion-proof six-DOF manipulator

圖1 鉆孔機(jī)器人加卸鉆桿系統(tǒng)Fig.1 Drill pipe loading and unloading system of drilling robot

鉆孔機(jī)器人的加鉆桿流程是利用機(jī)械手從鉆桿倉(cāng)中抓取鉆桿，并根據(jù)規(guī)劃算法運(yùn)送鉆桿至主機(jī)的加卸鉆桿位置，再利用夾持器夾緊鉆桿，隨后機(jī)械手松開(kāi)鉆桿并返回等待下一個(gè)流程，卸鉆桿流程則相反。隨著鉆孔設(shè)計(jì)不同，主機(jī)的調(diào)姿狀態(tài)發(fā)生變化，主機(jī)上的加卸鉆桿位置也隨之變化，如圖2 所示，因此，對(duì)于不同的鉆孔開(kāi)孔位置，機(jī)械手都要重新規(guī)劃軌跡。在機(jī)械手運(yùn)動(dòng)過(guò)程中，機(jī)械手的連桿臂及手爪中的鉆桿要避開(kāi)穩(wěn)固裝置、主機(jī)零部件等障礙物，其中穩(wěn)固裝置的位置是固定的，而主機(jī)上各零部件的位置則是變化的。

圖2 主機(jī)的調(diào)姿Fig.2 Attitude adjustment of host machine

主機(jī)的3 個(gè)自由度中，舉升和平移為線性關(guān)節(jié)，傾角則為旋轉(zhuǎn)關(guān)節(jié)，具體參數(shù)見(jiàn)表2，根據(jù)主機(jī)的機(jī)械結(jié)構(gòu)參數(shù)可知調(diào)整姿態(tài)前后加卸鉆桿位置的變化關(guān)系。設(shè)主機(jī)的舉升、傾角及機(jī)身平移參數(shù)分別為H、β、L，設(shè)定各參數(shù)為0 時(shí)加卸鉆桿位置中心的坐標(biāo)為A(x1,y1,z1)，參數(shù)變化后的坐標(biāo)為B(x2,y2,z2)，如圖3 所示，則有以下公式：

表2 主機(jī)的三自由度參數(shù)Table 2 Three-DOF parameters of host machine

圖3 鉆桿位置中心的坐標(biāo)變化Fig.3 Change of coordinates in position center of drill pipe

2 機(jī)械手加卸鉆桿的避障軌跡規(guī)劃策略

2.1 軌跡規(guī)劃方法

機(jī)械手的路徑規(guī)劃只涉及關(guān)節(jié)角度，軌跡規(guī)劃則是在路徑規(guī)劃基礎(chǔ)上對(duì)機(jī)械手的關(guān)節(jié)角速度、角加速度及角加加速度等進(jìn)行規(guī)劃，以滿(mǎn)足光滑性和連續(xù)性等要求。軌跡規(guī)劃包括笛卡爾空間規(guī)劃和關(guān)節(jié)空間規(guī)劃。笛卡爾空間規(guī)劃是根據(jù)已知點(diǎn)的笛卡爾坐標(biāo)直接插補(bǔ)，然后對(duì)插補(bǔ)的每個(gè)坐標(biāo)點(diǎn)通過(guò)機(jī)械手運(yùn)動(dòng)學(xué)逆解運(yùn)算得到對(duì)應(yīng)的關(guān)節(jié)變量。關(guān)節(jié)空間規(guī)劃只對(duì)幾個(gè)關(guān)鍵點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)逆解運(yùn)算，然后對(duì)運(yùn)動(dòng)學(xué)逆解得到的機(jī)械手關(guān)節(jié)變量通過(guò)插補(bǔ)算法進(jìn)行規(guī)劃。對(duì)于由多自由度串聯(lián)機(jī)械臂組成的機(jī)械手來(lái)說(shuō)，通常是結(jié)合關(guān)節(jié)空間和笛卡爾空間下進(jìn)行規(guī)劃。

2.2 避障方法

在機(jī)械手避障軌跡規(guī)劃研究中，需要對(duì)障礙物進(jìn)行模型簡(jiǎn)化，便于進(jìn)行碰撞檢測(cè)的運(yùn)算。障礙物一般采用球體、長(zhǎng)方體及圓柱體等規(guī)則幾何體進(jìn)行包絡(luò)。但對(duì)于復(fù)雜模型和狹窄空間，可采用由多個(gè)頂點(diǎn)坐標(biāo)確定的無(wú)規(guī)則凸包絡(luò)體進(jìn)行模型簡(jiǎn)化，可以有效減小包圍盒尺寸，提高避障環(huán)境的真實(shí)性[21-22]。在同一個(gè)坐標(biāo)系下分別建立機(jī)械手和主機(jī)的URDF 格式模型以及鉆桿倉(cāng)、穩(wěn)固裝置等環(huán)境模型，再利用包絡(luò)法建立加卸鉆桿系統(tǒng)的碰撞檢測(cè)模型，其中穩(wěn)固裝置和鉆桿倉(cāng)為規(guī)則幾何體，機(jī)械手和主機(jī)的各連桿臂為凸包絡(luò)體，如圖4 所示。

圖4 加卸鉆桿系統(tǒng)的碰撞檢測(cè)模型Fig.4 Collision detection model of drill pipe loading and unloading system

兩個(gè)三維物體之間的最近距離可用最小歐氏距離來(lái)表示，歐氏距離的計(jì)算方法如下。設(shè)三維空間中兩點(diǎn)A(x1,y1,z1)，B(x2,y2,z2)，則A、B點(diǎn)之間的歐氏距離為：

規(guī)則幾何體之間的最小歐氏距離有固定的計(jì)算公式，對(duì)于不規(guī)則的凸包絡(luò)體來(lái)說(shuō)，每個(gè)凸包絡(luò)體的外表面都可以看作是由若干個(gè)點(diǎn)云信息組成的，通過(guò)計(jì)算兩個(gè)凸包絡(luò)體外表面點(diǎn)云之間的歐氏距離，當(dāng)最小歐氏距離大于0 時(shí)，則認(rèn)為兩者無(wú)碰撞。

2.3 避障軌跡規(guī)劃的分段策略

在鉆孔機(jī)器人的鉆桿倉(cāng)中，鉆桿呈等間距縱橫排列，因此，可采用碼垛算法抓取鉆桿。從圖4 中可以看出，鉆桿倉(cāng)和主機(jī)本身就是一個(gè)凸包絡(luò)體，基于凸包絡(luò)體進(jìn)行碰撞檢測(cè)時(shí)，若直接從初始位置到終點(diǎn)位置進(jìn)行軌跡規(guī)劃，鉆桿會(huì)與這兩個(gè)凸包絡(luò)體碰撞。因此，為了簡(jiǎn)化規(guī)劃算法，可以根據(jù)鉆桿倉(cāng)和主機(jī)的結(jié)構(gòu)特點(diǎn)將軌跡分為3 段。

第一段：從抓桿位置到鉆桿倉(cāng)上方固定位置，此段路徑為直線，應(yīng)采用笛卡爾空間規(guī)劃方法，在得到每個(gè)插值路徑點(diǎn)的機(jī)械手關(guān)節(jié)變量后，根據(jù)關(guān)節(jié)變量與時(shí)間的函數(shù)關(guān)系采用一階、二階及三階導(dǎo)數(shù)法可得到關(guān)節(jié)的角速度、角加速度和角加加速度。

第二段：從鉆桿倉(cāng)上方固定位置到主機(jī)加桿位置上方，機(jī)械手要根據(jù)主機(jī)位姿變化調(diào)整軌跡并避開(kāi)障礙物，應(yīng)采用關(guān)節(jié)空間規(guī)劃方法。由于機(jī)械手在鉆桿倉(cāng)和穩(wěn)固裝置之間的路徑不受主機(jī)位姿變化影響，可通過(guò)增加一個(gè)過(guò)渡點(diǎn)來(lái)簡(jiǎn)化規(guī)劃算法。根據(jù)初始、末端和過(guò)渡點(diǎn)得到各插值路徑點(diǎn)的關(guān)節(jié)角度，并利用最小歐氏距離方法判斷是否碰撞，最后用導(dǎo)數(shù)法則得到各關(guān)節(jié)的角速度、角加速度和角加加速度。

第三段：從主機(jī)加桿位置上方到加桿位置，此段路徑同樣采用直線規(guī)劃，具體方法與第一段相似。

3 加卸鉆桿的避障軌跡規(guī)劃算法

3.1 最小沖擊算法

1) 基于七次多項(xiàng)式插值算法

機(jī)械手軌跡規(guī)劃中，通過(guò)采用不同的多項(xiàng)式插值函數(shù)，可以生成不同的軌跡。多項(xiàng)式插值軌跡規(guī)劃主要包括三次、五次、七次多項(xiàng)式插值[23]，采用七次多項(xiàng)式規(guī)劃可以保證速度、加速度和加加速度的連續(xù)性，實(shí)現(xiàn)機(jī)械手的最小沖擊軌跡規(guī)劃，有利于減小加卸鉆桿過(guò)程中機(jī)械手的高速運(yùn)動(dòng)沖擊。

機(jī)械手的關(guān)節(jié)角度或笛卡爾坐標(biāo)與時(shí)間的函數(shù)都可以用如下七次多項(xiàng)式表示：

令T=t1-t0，h=q1-q0，可求得式(3)中各系數(shù)如下：

根據(jù)以上公式再結(jié)合導(dǎo)數(shù)法則可得到任意時(shí)間節(jié)點(diǎn)對(duì)應(yīng)的速度、加速度和加加速度。根據(jù)機(jī)械手加卸鉆桿的避障軌跡規(guī)劃策略，第一段和第三段軌跡可基于直線坐標(biāo)點(diǎn)在笛卡爾空間下采用最小沖擊規(guī)劃算法進(jìn)行規(guī)劃。第二段軌跡要考慮避開(kāi)障礙物，在關(guān)節(jié)空間下進(jìn)行最小沖擊規(guī)劃，并開(kāi)展機(jī)械手運(yùn)動(dòng)中的碰撞檢測(cè)。如果檢測(cè)出碰撞，則可調(diào)整過(guò)渡點(diǎn)并重新開(kāi)始規(guī)劃和碰撞檢測(cè)，如果繼續(xù)發(fā)生碰撞，則優(yōu)先更改過(guò)渡點(diǎn)，或者繼續(xù)增加過(guò)渡點(diǎn)，直到不會(huì)發(fā)生碰撞為止，最后根據(jù)關(guān)節(jié)角度的變化得到角速度、角加速度和角加加速度。

2) 仿真分析

將主機(jī)的URDF 格式模型按照舉升H=200 mm、傾角α=45°和機(jī)身平移L=440 mm 進(jìn)行參數(shù)調(diào)節(jié)，機(jī)器人的URDF 格式模型按照開(kāi)始抓取鉆桿的姿態(tài)進(jìn)行參數(shù)調(diào)節(jié)，并在此環(huán)境下進(jìn)行加鉆桿的軌跡規(guī)劃仿真。按照最小沖擊避障軌跡規(guī)劃算法對(duì)三段軌跡分別進(jìn)行規(guī)劃，設(shè)定仿真時(shí)間段分別為0～5、5～25、25～30 s，總仿真時(shí)間為30 s，可得到如圖5 所示的仿真路徑，圖中紅色為機(jī)械手末端鉆桿中心的運(yùn)動(dòng)軌跡。在圖5中第二段中，機(jī)械手運(yùn)動(dòng)路徑平滑但路徑繞行較長(zhǎng)。

圖5 最小沖擊算法規(guī)劃的加桿路徑Fig.5 Loading path of drill pipe by minimum impact algorithm

根據(jù)機(jī)械手各關(guān)節(jié)軌跡參數(shù)得到的仿真曲線如圖6 所示，可以看出，三段軌跡中機(jī)械手各關(guān)節(jié)的角度、角速度、角加速度和角加加速度都是連續(xù)的。另一方面，第二段軌跡中關(guān)節(jié)角速度、角加速度和角加加速度的絕對(duì)值數(shù)值很大，其中角加加速度的絕對(duì)值峰值達(dá)到了52.481 rad/s3，這對(duì)于機(jī)械手的運(yùn)行來(lái)說(shuō)是非常不利的。因此，該方法雖然得到了最小的沖擊規(guī)劃，但卻是通過(guò)提高機(jī)械手關(guān)節(jié)運(yùn)行速度和運(yùn)行距離等參數(shù)實(shí)現(xiàn)的，同時(shí)也犧牲了機(jī)械手的運(yùn)行壽命。

圖6 最小沖擊軌跡規(guī)劃算法得到的機(jī)械手各關(guān)節(jié)軌跡仿真曲線Fig.6 Trajectory simulation curves of manipulator joints obtained by minimum impact trajectory planning algorithm

3.2 沖擊優(yōu)化的避障軌跡規(guī)劃算法

1) 改進(jìn)雙樹(shù)RRT 算法

為了在避障環(huán)境條件下縮短機(jī)械手的運(yùn)行距離，設(shè)計(jì)采用快速擴(kuò)展隨機(jī)樹(shù)(RRT)規(guī)劃算法，該算法也適合解決多自由度串聯(lián)機(jī)械手在復(fù)雜環(huán)境中的避障路徑規(guī)劃問(wèn)題。常見(jiàn)的RRT 算法有單樹(shù)RRT 算法和雙樹(shù)RRT 算法等[24-25]。通過(guò)采用最近路徑漸進(jìn)優(yōu)化方法，可以對(duì)雙樹(shù)RRT 算法進(jìn)行改進(jìn)。改進(jìn)雙樹(shù)RRT算法在開(kāi)始和目標(biāo)點(diǎn)處同時(shí)創(chuàng)建兩個(gè)根節(jié)點(diǎn)樹(shù)，為了擴(kuò)展每棵樹(shù)，隨機(jī)生成一個(gè)配置，如果不和環(huán)境障礙物發(fā)生碰撞，則根據(jù)設(shè)置的最大連接距離從最近的節(jié)點(diǎn)開(kāi)始邁出一步，隨后嘗試使用新的擴(kuò)展樹(shù)連接對(duì)面樹(shù)上最近的節(jié)點(diǎn)，此時(shí)在無(wú)碰撞前提下可啟用直接連接方式來(lái)縮短路徑。相比單樹(shù)RRT 算法，采用改進(jìn)雙樹(shù)RRT 算法可以得到搜索速度更高、路徑更短的優(yōu)化路徑。

2) B 樣條曲線路徑優(yōu)化

利用改進(jìn)型雙樹(shù)RRT 算法可得到初始無(wú)碰撞路徑點(diǎn)，由于該算法得到的路徑通常是不光滑的，可以通過(guò)三次均勻B 樣條曲線進(jìn)行路徑的優(yōu)化，得到一組光滑的路徑點(diǎn)。p次B 樣條曲線的表達(dá)式如下：

式中：Ni,k(t)為B 樣條曲線的基函數(shù)，可由de Boor-Cox 遞推公式求得；Pi為 B 樣條曲線的控制頂點(diǎn)；t為經(jīng)過(guò)歸一化處理的時(shí)間變量。

對(duì)于三次B 樣條曲線來(lái)說(shuō)，其基函數(shù)為：

則三次均勻B 樣條曲線可以表示為：

式中：P0、P1、P2、P4為4 個(gè)控制頂點(diǎn)。

3) 基于優(yōu)化路徑的最小沖擊算法

依據(jù)B 樣條曲線優(yōu)化的各路徑點(diǎn)，基于七次多項(xiàng)式插值的最小沖擊規(guī)劃方法進(jìn)行插值計(jì)算，并按照最小歐氏距離計(jì)算方法進(jìn)行包絡(luò)體碰撞檢測(cè)驗(yàn)證，可得到關(guān)節(jié)角度、角速度、角加速度及角加加速度都相對(duì)連續(xù)的沖擊優(yōu)化軌跡規(guī)劃算法。由于前面的第一段和第三段軌跡規(guī)劃算法能得到較優(yōu)的軌跡，因此只需對(duì)第二段軌跡按照雙樹(shù)RRT 規(guī)劃算法+B 樣條曲線優(yōu)化+最小沖擊算法的沖擊優(yōu)化規(guī)劃算法進(jìn)行改進(jìn)，具體的規(guī)劃流程如圖7 所示。

圖7 沖擊優(yōu)化軌跡規(guī)劃算法Fig.7 Impact-optimal trajectory planning algorithm

4) 仿真分析

選取同樣的主機(jī)姿態(tài)按照上述多種算法組合的沖擊優(yōu)化算法進(jìn)行軌跡規(guī)劃仿真分析，設(shè)定總仿真時(shí)間為30 s，圖8 為機(jī)械手的運(yùn)動(dòng)路徑，圖9 為機(jī)械手各關(guān)節(jié)運(yùn)行參數(shù)仿真曲線。由圖8 可以看出，相比圖5 中的路徑，路徑曲線較短，由圖9d 可知，角加速度的絕對(duì)值最大峰值為0.957 rad/s3，對(duì)比圖6d 降低了98.2%，該算法相比前面的最小沖擊軌跡規(guī)劃算法來(lái)說(shuō)，總路徑較短，角加加速度等的數(shù)值更小且相對(duì)連續(xù)。

圖8 沖擊優(yōu)化算法規(guī)劃的加桿路徑Fig.8 Loading path of drill pipe by impact-optimal trajectory planning algorithm

圖9 沖擊優(yōu)化算法得到的關(guān)節(jié)參數(shù)仿真曲線Fig.9 Trajectory simulation curves of manipulator joints obtained by impact-optimal trajectory planning algorithm

4 試驗(yàn)驗(yàn)證

調(diào)節(jié)鉆孔機(jī)器人的主機(jī)姿態(tài)與仿真模型一致，并根據(jù)沖擊優(yōu)化避障軌跡規(guī)劃方法的仿真結(jié)果采用機(jī)械手示教法進(jìn)行驗(yàn)證。為了盡可能接近仿真路徑又能減少輸入數(shù)據(jù)量，選取仿真中三段軌跡的起始點(diǎn)、終止點(diǎn)、3 個(gè)關(guān)鍵點(diǎn)及4 個(gè)過(guò)渡點(diǎn)共9 個(gè)坐標(biāo)點(diǎn)的關(guān)節(jié)角度和角速度作為輸入數(shù)據(jù)，見(jiàn)表3 和表4。在第一段軌跡和第三段軌跡中的各個(gè)輸入坐標(biāo)點(diǎn)之間采用線性插值得到插值路徑點(diǎn)的關(guān)節(jié)角度和角速度，在第二段軌跡中的各個(gè)輸入坐標(biāo)點(diǎn)之間采用多項(xiàng)式插值得到插值路徑點(diǎn)的關(guān)節(jié)角度和角速度。

表3 機(jī)械手工具坐標(biāo)系對(duì)應(yīng)的各關(guān)節(jié)角度Table 3 Each joint angle in manipulator tool coordinate system 單位：(°)

表4 機(jī)械手工具坐標(biāo)系對(duì)應(yīng)的各關(guān)節(jié)角速度Table 4 Each joint angular velocity in manipulator tool coordinate system 單位：(°)/s

將9 個(gè)坐標(biāo)點(diǎn)的關(guān)節(jié)角度和角速度通過(guò)機(jī)械手示教器輸入并設(shè)置好插值方法，操作鉆孔機(jī)器人進(jìn)行加桿過(guò)程的軌跡規(guī)劃測(cè)試，圖10 為各坐標(biāo)點(diǎn)的加桿狀態(tài)圖。圖中機(jī)械手從鉆桿倉(cāng)最遠(yuǎn)位置處(起始點(diǎn))抓取鉆桿后，直線運(yùn)行至鉆桿倉(cāng)上部關(guān)鍵點(diǎn)1，此時(shí)各關(guān)節(jié)速度變?yōu)?，然后按照一定的曲線軌跡運(yùn)行至關(guān)鍵點(diǎn)2附近，并繼續(xù)運(yùn)行至關(guān)鍵點(diǎn)3，此時(shí)各關(guān)節(jié)速度又降為0，最后，按照直線軌跡運(yùn)行至終止點(diǎn)，完成軌跡規(guī)劃的驗(yàn)證。在整個(gè)加桿過(guò)程中，機(jī)械手各機(jī)械臂及其手爪中的鉆桿都未與鉆孔機(jī)器人的穩(wěn)固裝置、主機(jī)上的零部件等障礙物碰撞，因此也驗(yàn)證了避障軌跡規(guī)劃的正確性。

圖10 各關(guān)鍵位置加桿狀態(tài)Fig.10 Loading state of drill pipe at each key position

在完成軌跡規(guī)劃測(cè)試驗(yàn)證后，根據(jù)測(cè)試的軌跡選取包含上述9 個(gè)坐標(biāo)點(diǎn)的共43 個(gè)坐標(biāo)點(diǎn)的關(guān)節(jié)角度和關(guān)節(jié)角速度，采用導(dǎo)數(shù)法則計(jì)算關(guān)節(jié)角加速度和角加加速度，并繪制各關(guān)節(jié)變化曲線，如圖11 所示，對(duì)比軌跡規(guī)劃仿真結(jié)果可知，實(shí)際的測(cè)試軌跡在關(guān)節(jié)角度、角速度、角加速度和角加加速度變化趨勢(shì)方面具有較高的相似性，但在局部峰值數(shù)據(jù)等方面有一定的誤差，這與實(shí)際測(cè)試中選取的坐標(biāo)點(diǎn)較少而影響到導(dǎo)數(shù)法則的計(jì)算結(jié)果有關(guān)。

圖11 各關(guān)節(jié)參數(shù)變化曲線Fig.11 Change curves of joint parameters

為了進(jìn)一步驗(yàn)證機(jī)械手中鉆桿的運(yùn)動(dòng)路徑，根據(jù)機(jī)械手運(yùn)動(dòng)學(xué)正解求解方法，可以得到機(jī)械手末端鉆桿中心的坐標(biāo)，根據(jù)坐標(biāo)數(shù)據(jù)繪制的運(yùn)動(dòng)路徑如圖12所示，相比仿真曲線基本一致。從圖中可以看出，第一段和第三段路徑為直線，第二段路徑由兩段曲線組成，兩段之間通過(guò)關(guān)鍵點(diǎn)2 采用圓弧過(guò)渡連接，有利于提高加卸鉆桿效率。

圖12 機(jī)械手手爪中鉆桿中心點(diǎn)運(yùn)動(dòng)軌跡Fig.12 Trajectory of drill pipe center in manipulator claw

5 結(jié)論

a.提出了加卸鉆桿避障軌跡規(guī)劃策略，采用三段式方法，第一段從待抓鉆桿至鉆桿倉(cāng)上方，在笛卡爾空間下采用直線規(guī)劃；第二段從第一段的終點(diǎn)至主機(jī)待加桿位置上方，該位置可根據(jù)主機(jī)調(diào)整姿態(tài)前后加卸鉆桿位置的函數(shù)關(guān)系得到，在關(guān)節(jié)空間下采用多項(xiàng)式插值算法進(jìn)行規(guī)劃，并可通過(guò)增加過(guò)渡點(diǎn)解決避障問(wèn)題；第三段從第二段終點(diǎn)至鉆桿放置位置，該位置也由主機(jī)調(diào)整姿態(tài)前后加卸鉆桿位置得到，并在笛卡爾空間下采用直線規(guī)劃。

b.提出了基于改進(jìn)雙樹(shù)RRT 算法+B 樣條曲線路徑優(yōu)化+七次多項(xiàng)式插值組合算法的沖擊優(yōu)化避障軌跡規(guī)劃算法，該算法得到的路徑較短，不僅可保證關(guān)節(jié)角度、角速度、角加速度和角加加速度的連續(xù)性，且角加加速度的絕對(duì)值峰值相比直接采用基于最小沖擊的七次多項(xiàng)式插值規(guī)劃算法降低了98.2%，可顯著提升機(jī)械手的使用工況。

c.采用給定少數(shù)坐標(biāo)點(diǎn)并結(jié)合插值方法進(jìn)行了機(jī)械手加桿試驗(yàn)，通過(guò)繪制關(guān)節(jié)曲線和鉆桿中心實(shí)際運(yùn)動(dòng)路徑并進(jìn)行了對(duì)比分析，相比仿真曲線具有較高的相似度。試驗(yàn)驗(yàn)證了沖擊優(yōu)化避障軌跡規(guī)劃算法的正確性，為鉆孔機(jī)器人六自由度機(jī)械手加卸鉆桿的避障軌跡規(guī)劃提供了一種可行的方法。