礦用鉆孔機器人加卸鉆桿的避障軌跡規(guī)劃研究

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 軌跡規(guī)劃方法

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

2.2 避障方法

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

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

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

規(guī)則幾何體之間的最小歐氏距離有固定的計算公式，對于不規(guī)則的凸包絡體來說，每個凸包絡體的外表面都可以看作是由若干個點云信息組成的，通過計算兩個凸包絡體外表面點云之間的歐氏距離，當最小歐氏距離大于0 時，則認為兩者無碰撞。

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

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

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

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

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

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

3.1 最小沖擊算法

1) 基于七次多項式插值算法

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

機械手的關節(jié)角度或笛卡爾坐標與時間的函數(shù)都可以用如下七次多項式表示：

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

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

2) 仿真分析

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

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

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

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

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

1) 改進雙樹RRT 算法

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

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

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

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

對于三次B 樣條曲線來說，其基函數(shù)為：

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

式中：P0、P1、P2、P4為4 個控制頂點。

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

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

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

4) 仿真分析

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

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

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

4 試驗驗證

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

表3 機械手工具坐標系對應的各關節(jié)角度Table 3 Each joint angle in manipulator tool coordinate system 單位：(°)

表4 機械手工具坐標系對應的各關節(jié)角速度Table 4 Each joint angular velocity in manipulator tool coordinate system 單位：(°)/s

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

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

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

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

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

圖12 機械手手爪中鉆桿中心點運動軌跡Fig.12 Trajectory of drill pipe center in manipulator claw

5 結(jié)論

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

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

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