三臂鑿巖臺車鉆臂協同鉆孔防碰撞路徑規劃研究

呂為盛, 崔孟豪, 姬會福, 張中偉

(1. 河南工業大學機電工程學院, 河南 鄭州 450001; 2. 河南農業大學煙草學院, 河南 鄭州 450002)

0 引言

鉆爆法[1]作為隧道、巷道掘進施工的主要方式,對不同地質條件及施工環境具有較好的適應性和通用性[2]。在公路、鐵路隧道等較大型地下工程中,主要采用三臂鑿巖臺車作為施工設備。3個鉆臂協同鉆孔,施工范圍大、掘進施工效率高[3]。在3個鉆臂協同鉆孔作業時,鉆臂之間可能會發生碰撞,造成設備損壞、影響施工進度,甚至威脅到作業人員的生命安全,因此有必要對三臂鑿巖臺車3個鉆臂的鉆孔路徑進行合理規劃。

三臂鑿巖臺車鉆臂可視為多關節機械臂結構,目前,機械臂路徑規劃方法主要有RRT[4-5]、A*[6-7]、人工勢場法[8-9]、粒子群算法[10-11]等。與其他算法相比,人工勢場法更易于理解,通過構建引力勢場和斥力勢場,使機械臂在引力和斥力的合力作用下向目標點運動,計算量相對較小,被廣泛應用于機械臂和移動機器人路徑規劃中[12]。郝林佳等[13]將人工勢場法應用于手術機器人位姿規劃中,以實現手術機器人在避免碰撞的情況下實現其位姿規劃; 翟麗等[14]通過改進勢場環境和勢場函數設計人工勢場法,對無人車動態實時避障規劃進行研究,通過仿真驗證了該算法的可行性; 李龍等[15]提出一種接近覺的方法對UR10機械臂進行路徑避障規劃,將勢場函數與關節空間結合,對障礙物“繞行”,基于路徑規劃實驗驗證了所用方法的有效性; 鮑久圣等[16]在傳統勢場法基礎上引入斥力場修正因子和出逃力,并建立速度場,將其應用于井下無軌膠輪車的局部路徑規劃,通過仿真驗證了所用方法的安全性。然而,傳統人工勢場法通常以笛卡爾空間作為搜索空間對機械臂進行路徑規劃,對于超過6個自由度的機械臂,其位姿均與每個關節的運動相關,需要實時計算其運動學逆解,計算量大、搜索效率低,且可能得到機械臂奇異位解,導致計算失敗。Sepehri等[17]將RRT*和人工勢場法相結合應用于機械臂路徑規劃,以7自由度Kinova JACO和Kinova Gen3機械臂為例,通過實驗所用算法可在障礙物環境中實現機械臂免碰撞。

針對目前國內外尚未開展多鉆臂協同防碰撞干涉的研究現狀,本文提出一種基于改進人工勢場法的三臂鑿巖臺車鉆臂協同鉆孔防碰撞路徑規劃方法。首先,建立三臂鑿巖臺車鉆臂運動學模型; 其次,依據鉆臂包絡體模型建立鉆臂碰撞干涉檢測數學模型; 然后,對傳統勢場法勢場函數進行改進,基于碰撞干涉檢測結果對三臂鑿巖臺車鉆臂進行防碰撞路徑規劃研究; 最后,通過仿真實驗驗證了所用方法的可行性。

1 三臂鑿巖臺車路徑規劃模型

三臂鑿巖臺車3個鉆臂協同鉆孔作業是提高掘進施工效率的重要手段。為防止3個鉆臂協同鉆孔時發生碰撞,需要對三臂鑿巖臺車鉆臂協同鉆孔進行防碰撞路徑規劃。在進行路徑規劃前,首先要判斷各鉆臂關節之間是否發生碰撞,基于判斷結果采用改進人工勢場法對鉆臂進行路徑規劃; 而鉆臂運動學是判斷各鉆臂關節之間是否發生碰撞的基礎,因此需先得到三臂鑿巖臺車鉆臂運動學模型。本文所研究的三臂鑿巖臺車的3個鉆臂結構相同,如圖1所示。

圖1 三臂鑿巖臺車

1.1 鉆臂運動學模型

1.1.1 鉆臂結構

三臂鑿巖臺車的每個鉆臂均為7自由度機械臂,包括5個轉動關節和2個移動關節,鉆臂整體結構安裝在基座上,基座固定在車體上,采用液壓方式驅動。7自由度鉆臂結構模型如圖2所示。

圖2 7自由度鉆臂結構模型

為實現鉆臂有效驅動控制,首先采用改進的D-H法建立三臂鑿巖臺車鉆臂運動學模型,獲得3個鉆臂結構簡圖和各關節坐標系,如圖3所示。

(a) 左鉆臂

(b) 中間鉆臂

(c) 右鉆臂

a、d、θ分別為連桿偏距、連桿長度、連桿轉角,下同。

圖3 三臂鑿巖臺車鉆臂結構簡圖和各關節坐標系

Fig. 3 Boom joint coordinate system of three-boom rock drilling jumbo

1.1.2 鉆臂變換矩陣

以中間鉆臂基坐標系作為三臂鑿巖臺車鉆臂全局基坐標系,左鉆臂基坐標系相對于全局坐標系沿X軸負方向平移440 mm,沿Y軸正方向平移1 125 mm,沿Z軸正方向平移340 mm; 右鉆臂基坐標系與左鉆臂基坐標系關于全局坐標系對稱。依據圖3及鉆臂結構參數,且本文所研究3個鉆臂結構相同,以中間鉆臂為例建立鉆臂D-H參數表,如表1所示。

表1 中間鉆臂D-H參數

基于改進D-H法建立鉆臂相鄰2坐標系之間的坐標變換矩陣

(1)

通過式(1)可建立鉆臂末端執行器坐標系相對鉆臂基座的位姿變換矩陣,如式(2)所示。

(2)

對于左右鉆臂末端位姿矩陣,將左右鉆臂基座坐標系相對于全局坐標系的變換矩陣左乘式(2)即可得到左右鉆臂末端相對于全局坐標系的位姿變換矩陣,如式(3)所示。

(3)

1.2 鉆臂碰撞干涉檢測模型

1.2.1 鉆臂包絡模型

由于鉆臂各關節結構非規則幾何結構,若直接計算各鉆臂的各個關節之間的距離,其計算量大、運算過程復雜。為便于計算不同鉆臂各關節之間的最短距離,本文采用包絡盒法對3個鉆臂模型進行合理簡化。鉆臂包絡模型如圖4所示。

圖4 鉆臂包絡模型

根據鉆臂各關節結構特點,關節1—5均采用圓柱體包絡盒,關節6和關節7采用長方體包絡盒,其中底座固定安裝在三臂鑿巖臺車車體上,俯仰油缸的安裝底座分別在關節1和關節2上,隨關節1和關節2運動,無需進行包絡。基于鉆臂包絡模型將各鉆臂之間的碰撞干涉檢測轉換為計算各個包絡體之間最短距離的問題,從而簡化計算過程,且能保證計算精度。

鉆臂各關節包絡體模型尺寸如表2所示。以圓柱包絡體為例建立碰撞檢測模型,設2個圓柱體包絡盒中心線之間的最短距離為dmin,2個圓柱體包絡盒的半徑分別為r1、r2。為保證2個圓柱體避免碰撞,則需滿足dmin>r1+r2。每個圓柱體包絡盒端部圓心均為鉆臂各關節節點,該節點坐標可通過鉆臂正運動學計算獲得,故進一步將鉆臂之間碰撞干涉檢測問題轉換為求解空間中直線與直線及直線與點之間位置關系的問題。

1.2.2 空間中2直線之間的最短距離

設2個包絡體的中心線分別為L1、L2,其中:L1的2個端點坐標分別為A1(x1,y1,z1)、A2(x2,y2,z2);L2的2個端點坐標分別為B1(x3,y3,z3)、B2(x4,y4,z4)。2條中心線上任意一點的坐標如式(4)所示。

(4)

式中: 向量A=A2-A1;B=B2-B1;α、β為系數,α∈[0,1]、β∈[0,1]。

此時L1、L2之間的最短距離問題即為求解約束函數的F(α,β)最優值問題,如式(5)所示。

F(α,β)=‖A(α)-B(β)‖2=‖(A1+αA)-(B1+βB)‖2。

(5)

求F(α,β)的極小值,令F(α,β)關于α、β偏導均為0,如式(6)所示。

(6)

由此計算出:

(7)

1.2.3 空間中點到直線的最短距離

與求解2直線最短距離過程類似,將點到直線的最短距離問題轉換為求解約束函數的最優值問題,設空間中一點M(x,y,z),以計算點M到中心線L1的最短距離為例進行說明,其約束函數如式(8)所示。

F(α)=‖M-A(α)‖2=‖M-(A1+αA)‖2。

(8)

令F(α)關于α的偏導為0。

(9)

結合式(4)、(8)、(9),可得:

(10)

通過dmin即可判斷鉆臂之間是否發生碰撞。

2 鉆臂防碰撞路徑規劃方法

為避免鉆臂運動過程中存在奇異位解,導致計算失敗,同時避免按照鉆孔順序運動過程中發生鉆臂碰撞干涉,本文首先建立鉆臂鉆孔路徑規劃模型,其次以關節空間作為搜索鉆臂下一個動作姿態的搜索空間,采用改進人工勢場法對三臂鑿巖臺車鉆臂進行防碰撞位姿調整。

2.1 鉆臂鉆孔路徑規劃模型

2.1.1 鉆臂工作區域劃分

根據三臂鑿巖臺車運動學模型,基于蒙特卡洛法計算出三臂鑿巖臺車工作范圍。三臂鑿巖臺車工作空間如圖5所示。

圖5 三臂鑿巖臺車工作空間

隧道掌子面所布孔位數量為188個,依據所得3個鉆臂的工作空間,對每個鉆臂工作區域進行劃分,并以鉆臂末端行駛距離最短和各關節變量之和最小為優化目標對鉆孔順序進行優化。鉆臂工作空間劃分及鉆孔順序如圖6所示。

(a) 距離最短鉆孔順序

2.1.2 鉆孔順序優化

從左到右依次為左鉆臂、中間鉆臂、右鉆臂工作區域及各鉆臂優化后鉆孔順序,鉆孔數分別為64、63、61,鉆孔順序優化參考文獻[18],考慮節約鉆孔施工能耗,分別以鉆臂末端移動行駛距離最短作為和各關節變量之和最小優化目標,采用改進遺傳算法對鉆孔順序進行優化,優化目標如式(11)所示。

(11)

式中:S為鉆臂末端執行器移動距離;l為2空位之間的距離;n為孔位數量;R為鉆臂各關節變量總和;θ為關節變量。

孔位關于隧道中線對稱分布,依據所劃分工作空間,每個鉆臂僅在所劃分工作空間內作業,由此可知鉆臂之間的碰撞主要發生在中間鉆臂與左右鉆臂之間。圖6中,在中間鉆臂工作區域內,起點1為中間鉆臂與左鉆臂之間路徑規劃時中間鉆臂起點,終點1既是中間鉆臂與左鉆臂路徑規劃終點,也是中間鉆臂與右鉆臂路徑規劃時的起點。

2.2 勢場函數建立

2.2.1 引力勢場函數

基于建立的鉆臂鉆孔路徑模型,考慮傳統勢場函數僅對鉆臂末端起約束作用,而鉆臂作業過程中由各關節控制鉆臂運動。為提高對鉆臂的控制精度,使所規劃的路徑更加合理,將勢能引入鉆臂各關節,保證勢場函數可以約束鉆臂位姿。

基于關節空間重新建立引力勢能函數:

(12)

則與其對應的引力函數為:

(13)

式中:Fatt(q)為鉆臂關節的引力;Fatt為鉆臂各關節引力之和。

2.2.2 斥力勢場函數

傳統勢場中,當障礙物在目標點附近時,鉆臂受到的斥力較大,導致鉆臂無法到達目標點,因此對傳統斥力勢能函數進行重建,避免在目標點周圍時所受斥力過大,新斥力函數如式(14)所示。

(14)

式中:Urep(q)為鉆臂關節斥力勢能;Urep為鉆臂各關節斥力勢能總和;λ為斥力勢能因子;qobs為障礙物位置;d(qi,qobs)為鉆臂關節當前位置到障礙物之間的最小距離;d(qi,qgoal)為鉆臂關節當前位置到目標位置的距離;dA為障礙物的斥力勢能作用范圍。

由此,使鉆臂遠離障礙物運動時,對應的斥力函數為:

(15)

式中:Freq(q)為鉆臂第i個關節所受到的斥力;Freq為鉆臂所有關節所受斥力總和;NOR和NRG為2個向量,分別表示鉆臂關節到障礙物的距離和鉆臂關節到目標距離的單位向量,Freq1、Freq2如式(16)所示。

(16)

由上述分析在進行防碰撞路徑規劃時,各個鉆臂所受到勢能和力的總和如式(17)所示。

(17)

3 仿真實驗研究

為分析基于改進人工勢場法用于三臂鑿巖臺車防碰撞路徑規劃的可行性及高效性,基于圖6所建立的鉆孔順序,引入改進的人工勢場法進行多鉆臂協同鉆孔防碰撞位姿調整。由于在本文所提到的三臂鑿巖車在實際工作時的鉆孔深度較淺,所面臨的環境與工況不復雜,一般不會出現鉆臂釬轉壓力和推進壓力陡然上升到最大值引起卡釬的情況,卡釬出現概率很低,因此本文暫不考慮因卡釬導致的鉆孔時間不一致的情況。

3.1 基于距離最短鉆孔順序的路徑規劃

3.1.1 實驗仿真

為實現三臂鑿巖臺車的精準路徑規劃,施工過程中需要先將鑿巖臺車放置于巷道中間,按照設定的路徑進行鉆孔作業,三臂起始孔位置在施工中一般均由程序設定為固定位置。因此,本文基于距離最短鉆孔順序時中間鉆臂與左右鉆臂之間的最短距離衡量鉆臂碰撞干涉。依據圖6(a)所示各個鉆臂起始點分別對中間鉆臂與左鉆臂、中間鉆臂與右鉆臂進行無碰撞路徑規劃仿真,結果如圖7和圖8所示。

(a) 兩鉆臂初始工作姿態

(b) 中間鉆臂完成鉆孔任務

(c) 兩鉆臂完成鉆孔任務

(a) 兩鉆臂初始工作姿態

(b) 中間鉆臂完成鉆孔任務

(c) 兩鉆臂完成鉆孔任務

圖7(a)、8(a)為2個鉆臂開始鉆孔任務時的初始狀態,該姿態由三臂鑿巖臺車車體定位實現,在車體定位過程中,鉆臂之間不會發生碰撞,故本文省去定位過程中各個鉆臂之間碰撞干涉檢測; 圖7(b)、8(b)為中間鉆臂完成其鉆孔任務后2個鉆臂之間的狀態,中間鉆臂處于停止運動狀態,左右鉆臂繼續鉆孔作業,圖7(c)、8(c)為2個鉆臂均完成鉆孔任務后的工作狀態。由圖7和圖8可知,2個鉆臂在鉆孔時完全按照所優化的鉆孔順序進行。

3.1.2 碰撞判定

進一步對鉆臂簡化后各關節之間距離依據從大到小進行排序,并提取2個鉆臂同時鉆孔時各鉆臂包絡盒之間最短距離,以判斷兩鉆臂之間是否發生碰撞。鉆臂之間最短距離如圖9所示。

圖9 基于距離最短鉆孔順序時鉆臂之間最短距離

由圖9可以看出,中間鉆臂與左右鉆臂之間的最短距離分別為984.6、580.8 mm,當中間鉆臂與左右鉆臂協同鉆孔時均沒有發生碰撞,由此可知所用改進人工勢場法可以實現鉆臂無碰撞鉆孔作業,能夠滿足三臂鑿巖臺車鉆臂協同鉆孔作業要求。

3.2 基于關節變量最小鉆孔順序的路徑規劃化

3.2.1 實驗仿真

依據圖6(b)所示,各個鉆臂起始點分別對中間鉆臂與左鉆臂、中間鉆臂與右鉆臂進行無碰撞路徑規劃仿真,結果如圖10和圖11所示。

圖10(a)、11 (a)分別為中間鉆臂與左右鉆臂和開始鉆孔任務時的初始狀態,該姿態由三臂鑿巖臺車車體定位實現,在車體定位過程中,鉆臂之間不會發生碰撞; 圖10(b)、11(b)為中間鉆臂完成工作區域內鉆孔任務后2個鉆臂之間狀態,此時中間鉆臂完成鉆孔任務處于停止狀態,左右鉆臂繼續鉆孔作業; 圖10(c)、11(c)為2個鉆臂均完成鉆孔任務后的工作狀態。由圖10和圖11可知,2個鉆臂在鉆孔時完全按照所優化的鉆孔順序進行。

(a) 兩鉆臂初始工作姿態

(b) 中間鉆臂完成鉆孔任務

(c) 兩鉆臂完成鉆孔任務

(a) 兩鉆臂初始工作姿態

(b) 中間鉆臂完成鉆孔任務

(c) 兩鉆臂完成鉆孔任務

3.2.2 碰撞判定

進一步對鉆臂簡化后各包絡盒之間的距離依據從大到小進行排序,并提取2個鉆臂同時鉆孔時各包絡盒之間最短距離,以判斷2個鉆臂之間是否發生碰撞。鉆臂之間最短距離如圖12所示。可以看出,中間鉆臂與左右鉆臂之間的最短距離分別為193.5、580.8 mm。

綜合考慮鉆臂實際結構,如鉆臂各關節外殼尺寸、鉆孔施工安全等因素,中間鉆臂與左鉆臂之間存在碰撞的風險。

圖12 基于關節變量最小鉆孔順序時鉆臂之間最短距離

4 結論與討論

1)基于改進的D-H法得到三臂鑿巖臺車運動學模型。為簡化計算過程、提高運算效率,依據鉆臂各關節結構特點,采用圓柱體+長方體的包絡盒法對鉆臂模型進行合理簡化,基于包絡體模型建立碰撞干涉檢測數學模型。

2)對傳統勢場法中勢場函數重建,將勢能引入鉆臂各關節,保證勢場對鉆臂位姿有約束作用,鉆臂按照優化后鉆孔順序工作,基于改進人工勢場法對鉆臂進行位姿調整規劃。仿真結果表明,基于距離最短鉆孔順序所得中間鉆臂與左右鉆臂之間的最短距離分別為984.6、580.8 mm,均未發生碰撞干涉; 基于關節變量最小鉆孔順序所得中間鉆臂與左右鉆臂之間的最短距離分別為193.5、580.8 mm。此時,綜合考慮施工安全性及鉆臂結構尺寸,存在碰撞風險,故在鉆孔施工時,應選取鉆臂末端移動距離作為優化目標時所得的鉆孔順序,此時可以實現三臂鑿巖臺車3個鉆臂協同無碰撞鉆孔施工。

考慮到現場作業中巖石硬度質地不同會導致鑿巖車出現卡鉆、斷釬等問題,需要針對不同工況及時調整鑿巖車的鉆進參數,有必要結合自適應鉆進開展進一步研究。