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

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

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

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

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

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

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

1 鉆孔機器人加卸鉆桿系統

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

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

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

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

圖2 主機的調姿Fig.2 Attitude adjustment of host machine

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

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

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

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

2.1 軌跡規劃方法

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

2.2 避障方法

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

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

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

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

2.3 避障軌跡規劃的分段策略

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

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

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

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

3 加卸鉆桿的避障軌跡規劃算法

3.1 最小沖擊算法

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

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

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

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

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

2) 仿真分析

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

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

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

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

3.2 沖擊優化的避障軌跡規劃算法

1) 改進雙樹RRT 算法

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

2) B 樣條曲線路徑優化

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

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

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

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

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

3) 基于優化路徑的最小沖擊算法

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

圖7 沖擊優化軌跡規劃算法Fig.7 Impact-optimal trajectory planning algorithm

4) 仿真分析

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

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

圖9 沖擊優化算法得到的關節參數仿真曲線Fig.9 Trajectory simulation curves of manipulator joints obtained by impact-optimal trajectory planning algorithm

4 試驗驗證

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

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

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

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

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

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

圖11 各關節參數變化曲線Fig.11 Change curves of joint parameters

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

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

5 結論

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

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

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