基于末端桿件螺旋運動的鑿巖機械臂孔序規劃

王恒升　陳偉鋒　王思遠

中南大學高性能復雜制造國家重點實驗室，長沙，410083

?

基于末端桿件螺旋運動的鑿巖機械臂孔序規劃

王恒升陳偉鋒王思遠

中南大學高性能復雜制造國家重點實驗室，長沙，410083

摘要：根據隧道炮孔設計，進行孔序規劃，是對鑿巖機械臂進行自動控制所需要解決的一個問題。針對這一問題，以機械臂末端桿件的螺旋運動來定義其運動軌跡，用螺旋運動軌跡的曲線長度來定義機械臂將末端執行器從一個孔位運送到另一個孔位所付出的代價，以此代價為基礎建立目標函數，用蟻群算法進行問題求解，得到機械臂運動的孔序規劃。利用對偶四元數法推導了機械臂末端桿件螺旋運動參數的計算公式。仿真驗證了軌跡求解算法，得到的軌跡平滑、連續；針對某隧道斷面64組炮孔設計，利用蟻群算法進行左右兩個機械臂的孔序規劃仿真，得到了合理的規劃結果。

關鍵詞：孔序規劃；螺旋運動；對偶四元數；蟻群算法；螺旋線性插值；鑿巖機械臂

0引言

鉆爆法是目前礦山、隧道、地下工程施工的主要工藝方法，鑿巖臺車是實現這一工藝方法的主要工程裝備。在鉆爆工藝過程的周期循環中涉及鑿巖臺車部分的主要任務，是通過操控鑿巖臺車串聯機械臂的各個關節運動，把末端桿件上裝配的鑿巖鉆機以一定的姿態運送到炮孔的設計位置，啟動鑿巖鉆機，在設計的位置和方向上打出一定深度的炮孔。影響整個隧道開挖的工程質量、工程進度、工程造價的因素很多，涉及鑿巖鉆孔環節的主要有炮孔的精度、機械臂移動的速度等[1]。本文研究的目標是對鉆孔的順序進行規劃，以縮短機械臂在各個孔位之間移動所花的時間，提高鉆鑿工序的工作效率；同時也可減小機械臂的磨損和運動過程的能量消耗；實際控制時還需考慮機械臂的運動干涉。

對于隧道斷面布孔設計，孔序規劃就是要尋求一種優化的孔序，使機械臂按照這一順序逐個完成炮孔的鉆鑿。國外文獻一般針對某炮孔設計，直接給出孔序規劃的結果[2]。周友行等[3-4]研究了孔序規劃的理論方法，把該問題看成旅行商問題(traveling salesman problem，TSP)的變種，從遍歷孔的方案中找出最優方案，由于TSP是一個NP難題，故主要采用啟發式的算法來尋優[5-6]。對于孔之間的移動代價，或者說路程的長短，有的簡單地以炮孔的空間位置坐標直接計算兩點的空間距離，有的則以機械臂所有關節的運動轉角進行加權求和。上述方法均沒有考慮機械臂的實際運行軌跡。

本文以機械臂末端桿件的運動軌跡為基礎，定義炮孔之間的移動距離代價。末端桿件從一個炮孔對應的位姿狀態移動到另一個炮孔對應的位姿狀態，可以等價為空間坐標系之間的變換，根據Charles定理，這一變換可以等效為繞某個軸(稱為“螺旋軸”)的旋轉運動和沿該軸的平移運動(平移距離稱為“螺距”)[7]，即將空間坐標系之間的變換等效為螺旋運動。在此基礎上將螺旋運動軌跡的曲線長度定義為兩個炮孔之間的距離，再進一步利用蟻群算法[8]，搜索遍歷孔的最短路徑[9]。詳細介紹了螺旋運動的對偶四元數[10]表示、炮孔之間螺旋運動的關系推導及螺旋運動軌跡的長度計算，介紹了蟻群算法及孔序規劃的應用，最后給出了仿真及仿真結果。

1末端桿件螺旋運動軌跡計算

圖1所示為某煤礦巖石運煤通道的炮孔設計斷面圖，該斷面設計有64個炮孔，由雙臂鑿巖臺車實現炮孔鉆鑿。鑿巖臺車完成64個炮孔的鉆鑿后，進入裝填炸藥、爆破、通風、清渣等工序。

圖1　炮孔設計斷面圖

1.1末端桿件姿態的四元數描述

某鑿巖機器臺車的六自由度機械臂如圖2所示，末端桿件就是滑架6，末端執行器(即鑿巖鉆機)安裝在滑架上。工作時通過機械臂各關節的運動把末端執行器運送到對應炮孔所設計的位姿狀態，各炮孔對于末端桿件的位姿要求由炮孔的設計和機械臂的實際工作環境所確定。鑿巖臺車、機械臂及工作斷面的示意圖見圖3，炮孔對末端桿件的姿態和位置要求見表1，表1中第2、第3列給出了部分與炮孔相應的末端桿件的原始數據，省略了其他炮孔對應的末端桿件的位姿數據。

1.關節6　2.車體　3.關節1　4.關節2　5.大臂　6.滑架　7.關節3　8.關節4　9.關節5圖2　機械臂示意圖

圖3　臺車、機械臂及工作斷面的關系示意圖

孔號炮孔位置坐標(mm)末端桿件姿態(α,β,γ)末端桿件位姿的對偶四元數5(-784.5,-1400,4900)(0,20°,-90°)(0.696,-0.123,0.123,-0.696)+ε(1743.880,-86.524,-1061.434,1571.977)6(784.5,-1400,4900)(0,-20°,90°)(0.696,-0.123,-0.123,0.696)+ε(-1743.880,86.524,-1061.434,1571.977)7(784.5,-1000,4900)(0,-20°,90°)(0.696,-0.123,-0.123,0.696)+ε(-1719.323,225.797,-922.161,1571.977)8(784.5,-600,4900)(0,-20°,90°)(0.696,-0.123,-0.123,0.696)+ε(-1694.765,365.070,-782.888,1621.093)9(-1150,-800,4900)(0,7°,-90°)(0.706,-0.043,0.043,-0.706)+ε(1721.626,-229.274,-793.904,1687.092)10(-1150,-1200,4900)(0,7°,-90°)(0.706,-0.043,0.043,-0.706)+ε(1730.260,-88.117,-935.062,1678.458)11(0,-1883.5,4900)(0,0,-90°)(0.707,0,0,-0.707)+ε(1732.412,665.918,-665.918,1732.412)

表1中的數據是相對固接在車體上的基礎坐標系OvXvYvZv給出的，也就是固接在末端桿件上的坐標系O6X6Y6Z6相對于基礎坐標系的空間位姿。其中，姿態角按歐拉角給出，旋轉順序為Y-X-Z，即先繞Y軸轉β角，然后繞X軸轉α角，最后繞Z軸繞γ角到達最終姿態。以上三個旋轉變換的四元數[11]表達式為

(1)

由此，末端桿件姿態的四元數可由以上三個四元數的連續相乘得到：

qR=qYqXqZ=(q0,q1,q2,q3)

(2)

(3)

根據剛體的旋轉變換，以上三個連續旋轉等效于沿某單位矢量表示的旋轉軸n=(nx,ny,nz)旋轉θ角度的結果，即qR也可以寫成以下等價三角函數形式：

(4)

由式(2)到其等價式(4)的參數變換如下：

(5)

1.2末端桿件螺旋運動的參數計算

圖4中，空間坐標系O1X1Y1Z1到O2X2Y2Z2的坐標變換可以分為以下兩步：

圖4　坐標系O1X1Y1Z1到O2X2Y2Z2的坐標變換

(1)將坐標系O1X1Y1Z1沿向量t平移到與坐標系O2X2Y2Z2的原點重合的位置，用單位對偶四元數表示此平移變換，即

(6)

式中，qt為四元數(0,t)表示平移的量。

(2)再按式(4)將坐標軸繞通過原點的某單位向量n旋轉θ角至與O2相應的坐標軸重合。這兩步綜合起來用對偶四元數表示為

(7)

(8)

(9)

螺旋運動中的轉軸是一空間直線，該直線用Plücker坐標表示為

(10)

代入螺旋運動的參數，式(9)可表示為

(11)

對式(11)中的三角函數進行Taylor展開并整理可得以下對偶四元數表達式：

(12)

將式(7)的對偶部分展開并令其與式(12)的對偶部分相等，可得

(13)

對等式(13)進行化簡，得

(14)

利用拉格朗日等式，有以下關系：

n×t×n=(n·n)t-(t·n)n=t-dn

(15)

將式(15)的右邊與式(14)等號右邊的第一部分相似，代入式(14)，有

(16)

將式(16)的右邊兩項共同提出叉乘因子n，對比兩邊可以得到

(17)

但實際上，由于c×n=(c+λn)×n對于任意實數λ都是成立的，所以滿足式(16)的c有任意多個，從幾何上說可以是螺旋軸上的任意一點。某些文獻為了方便，直接取坐標原點的投影點，即取向量c與n相垂直，此時c由下式確定：

(18)

1.3末端桿件螺旋運動的軌跡插值

圖5　從位姿1到位姿2進行螺旋軌跡插值

(19)

(20)

將式(20)表示為類似式(9)的單位對偶四元數的三角函數形式，有

(21)

(22)

(23)

(24)

把式(23)代入式(19)可得到在車體坐標系中的插值公式：

(25)

(26)

其中，qht=(0，th)可由式(8)計算得到，向量th是關于h的函數，也就是末端桿件坐標原點的Sclerp插值軌跡點在車體坐標系OvXvYvZv中的空間函數。

2孔序規劃

2.1孔序規劃問題描述

孔序規劃問題，即釬桿末端從起始孔開始，遍歷所有設計炮孔位的炮孔排序問題。施工時按排好的孔序進行鉆鑿，因此，序列中重復孔的排序是沒有意義的。孔序規劃問題是旅行商問題的變種，與旅行商問題相比，沒有在遍歷完后重新回到起始點的要求。所以孔序規劃問題在數學求解上是一個NP難題。對于這類問題通常通過設計部分(而不是全部)搜索算法進行數值求解，也就是通過某種啟發式的算法，不遍歷全部解空間進行尋優，得到所謂的不一定是最優但實際上卻是足夠優的解[8,9,13-14]。本文采用蟻群算法進行孔序規劃的問題求解。

2.2蟻群算法求解孔序規劃問題

蟻群算法實際上是對自然界中的螞蟻群體通過合作尋找出一條優化的覓食路線的現象的數學建模，通過數學的方法模擬這一自然現象，形成算法，解決尋優問題。

設有n個孔位，m只螞蟻。在每一個時間步長內，每只螞蟻經過一個孔位，因此，在n個步長的時間內，所有螞蟻均遍歷了n個孔位，完成一次迭代循環；為了防止在一次循環中螞蟻重復已經經過的孔號，設置一個禁忌列表tabu，把經過的孔序號放入tabu中，當tabu中充滿了n個孔號時，一次循環結束。與禁忌列表tabu互補的一個列表叫allowed列表，每一步螞蟻可以從allowed列表中選擇一個孔號。

2.2.1信息素

螞蟻選擇哪個孔號是按照概率計算進行隨機選取的，螞蟻會在自己經過的路徑上留下信息素，或者說是在路徑原有信息素的基礎上增加一個信息素增量。本文采用蟻周模型[14](ant-cycle system)，即信息素增量的計算是每個螞蟻走完一個周期(迭代循環一次)后進行的。例如，第k只螞蟻在某周期完成后從第i個孔到第j個孔的路徑上所留下的信息素增量為

(27)

其中，Q為信息量常數，Lk為第k只螞蟻在本次循環中遍歷的路徑總長度。Lk值較小的螞蟻，表現優秀，留下的信息素較大，在后面的路徑概率選取上會有一定的優勢。

經過一次迭代循環后全部m只螞蟻在路徑(i,j)上的信息素增量的總和Δτij以及信息素總量τij分別為

(28)

τij←ρτij+Δτij

(29)

式(29)為路徑(i,j)上的信息素總量的更新算式，其中，系數ρ表示信息素持久性，取ρ<1以避免路徑上信息素的無限增加，同時模擬自然界的一種自然遺忘的規律。

值得一提的是，一方面，這里的信息素增量和信息素總量是隨迭代周期不斷更新的，為了簡化符號的寫法，省去了公式中的迭代循環變量。另一方面，迭代開始時，螞蟻是沒有先驗知識的，為了使狀態轉移機制可以實現，取初始值為一常數，即設τij(0)=C。

2.2.2路徑選擇概率

當螞蟻k位于第i孔，選擇下一步為第j孔的概率為

(30)

概率的大小除與路徑上的信息素τij有關外，還與ηij有關。ηij表示選擇路徑(i,j)的期望程度，是一種啟發因素，本文取ηij=1/d(i,j)，即路徑長度越小，選擇的可能性越高。α和β兩個參數分別反映螞蟻在選擇過程中信息素和啟發因素的相對重要程度。式(30)中，τij(t)中的t表示動態更新的含義，盡管在蟻周模型中信息素并不是每一步進行更新，而是一個循環更新一次；事實上如果采用蟻密(ant-density)或蟻量(ant-quantity)模型時，信息素的確是每走一步更新一次；此處為表達一致，仍然用τij(t)表示。

所有螞蟻均遍歷了n個孔位后完成一次迭代循環，選出最短路徑的遍歷方案；隨著迭代循環的進行，期望最短路徑能夠收斂到一個最優值，即找到優化的炮孔排序方案。

3仿真

3.1空間位姿變換螺旋運動插值仿真

基于鑿巖臺車六自由度機械臂參數和隧道斷面設計的幾何參數，在MATLAB中進行了仿真計算。采用第2節給出的公式，只要給出鑿巖鉆機在任意兩個炮孔要求的末端執行器位姿，即可得到連續平滑的螺旋運動軌跡。從13號孔(1150 mm,-800 mm,4900 mm，0,-7°,90°)到37號孔(1852 mm,-2871 mm,4900 mm,2°,1°,0)，對末端桿件坐標原點向量th運用Sclerp插值法進行了10 000次插值。圖6所示為末端桿件坐標系運動軌跡，描述了位姿的漸變過程。圖7所示為從13號孔到37號孔末端桿件的移動軌跡。桿件長度為4 m,圖7中已標出近車體段和近斷面端(圖7)。

圖6　從13號孔到37號孔坐標系的變化過程

圖7　從13號孔到37號孔桿件的移動過程

插值過程得到的螺旋運動角度θ1h和平移位移d1h都是關于h的線性函數，進一步驗證了第2 節的結論。為節省篇幅，此處省略了插值曲線。

從13號孔到37號孔的末端桿件螺旋曲線軌跡長度的計算結果為2430.6 mm(即圖6的曲線長度)。

為了更形象地表現出末端桿件坐標系的運動軌跡，給出右臂的一個連續工作孔序，見表2。按照第2節的計算公式，把坐標系原點的空間軌跡投影到隧道斷面上，得到圖8的結果，此孔序列始于59號孔，連續移動經過10個孔后回到59號孔的位置。

3.2孔序規劃的蟻群算法仿真

利用蟻群算法，取信息素迭代初值C=10-2，啟發因素的期望程度因素ηij=500/d(i,j)，信息素持久系數ρ=0.7，信息量常數Q=15 000，螞蟻的隨機選擇中信息素和啟發因素的重要因子α=β=1，螞蟻數量m=32，迭代次數取1000。左右臂按工作空間的區域分別平均分配32個炮孔(圖9所示的兩組炮孔)，給定左臂的起始孔為54，給定右臂的起始孔為58，在MATLAB中進行仿真計算。

表2　右臂的試驗工作孔序

圖8　右臂末端桿件坐標系的連續11次螺旋運動軌跡在斷面上的投影

圖9　孔序規劃的結果

圖9所示為兩臂孔序規劃的結果，圖中連線不代表桿件末端空間真實軌跡，只用作排序顯示。左臂末端桿件運動軌跡的最小距離為15 231 mm，右臂的最小距離為15 244 mm，兩臂軌跡總距離差為13 mm,小于任意兩個孔之間的最短距離。兩臂的孔序路徑的直觀效果也很理想。圖10所示為左右兩臂孔序規劃蟻群算法的迭代過程，迭代200次以后蟻群的路徑已經非常集中了，平均路徑與最短路徑也非常接近；可以看出最小距離和平均距離的收斂趨勢和過程非常好。

(a)左臂路徑迭代

(b)右臂路徑迭代圖10　孔序規劃蟻群算法的最優路徑和平均路徑仿真過程

4結語

本文針對隧道鑿巖機器人(鑿巖臺車)操作臂的孔序規劃展開研究。就機械臂的軌跡規劃問題，提出末端執行器螺旋運動的軌跡規劃，按照這種軌跡，末端執行器的運動過程平穩，路徑最短，有利于提高機械臂的工作效率、減少運動磨損和能量消耗。基于對偶四元數法，詳細推導了末端執行器螺旋運動的軌跡參數計算公式。運用四元數表達空間桿件的姿態及坐標變換，表達形式緊湊、直觀，運算過程簡潔明了。以末端執行器螺旋運動的軌跡長度作為從一個孔到另一個孔移動的代價，建立了孔序規劃的目標函數，尋求最短的炮孔遍歷路徑。針對某煤礦運煤巖石隧道斷面設計的64組炮孔參數，運用蟻群算法進行了優化，迭代算法的收斂過程非常理想，得到了實用的孔序規劃的結果。本文從孔序規劃的角度給出末端執行器理想的螺旋運動軌跡，未涉及關節的運動及機械臂的運動干涉問題，在實際機械臂的控制中需要考慮這些問題。

參考文獻：

[1]Kukkonen J.The Art of Drill Plan Design[C]// Proceedings of World Tunnel Congress 2008-Underground Facilities for Better Environment and Safety.Agra,2008:1017-1023.

[2]Andersson K.Simulation of a Tunnel Drilling Sequence to Determine Loads on a Rock Drilling Equipment[C]//ADAMS User Conference.Rome, 2000:1-11.

[3]周友行, 何清華,謝習華.基于遺傳算法的鑿巖機器人孔序規劃[J].機器人,2002,24(1): 62-65.

Zhou Youxing, He Qinghua, Xie Xihua. Application of Genetic Alogorithm to Bore Planning of the Tunnel Drilling Robot[J].Robot,2002,24(1): 62-65.

[4]周友行, 何清華.雙臂鑿巖機器人離散任務規劃[J]. 中國機械工程, 2006, 17(13): 1334-1337.

Zhou Youxing, He Qinghua. Discrete Task Planning of Drilling Robot with Two Booms[J].China Mechanical Engnieering,2006,17(13):1334-1337.

[5]Woeginger G J.Exact Algorithms for NP-hard Problems:a Survey[M].Berlin:Springer Berlin Heidelberg, 2003.

[6]Applegate D L.The Traveling Salesman Problem: a Computational Study[M].Princeton:Princeton University Press, 2006.

[7]武元新.對偶四元數導航算法與非線性高斯濾波研究[D].長沙:國防科學技術大學, 2005.

[8]Brand M, Masuda M, Wehner N, et al. Ant Colony Optimization Algorithm for Robot Path Planning[C]//International Conference on Computer Design and Applications.Qinhuangdao,2010:436-440.

[9]Dorigo M, Gambardella L M. Ant Colony System: a Cooperative Learning Approach to the Traveling Salesman Problem[J].IEEE Transactions on Evolutionary Computation,1997,1(1): 53-66.

[10]Sariyildiz E, Temeltas H. A New Formulation Method for Solving Kinematic Problems of Multiarm Robot Systems Using Quaternion Algebra in the Screw Theory Framework[J].Turkish Journal of Electrical Engineering & Computer Sciences, 2012,20(4):607-628.

[11]Shoemake K. Animating Rotation with Quaternion Curves[J].ACM SIGGRAPH Computer Graphics. ACM, 1985, 19(3): 245-254.

[12]Kavan L, Collins S, ?ára J, et al.Geometric Skinning with Approximate Dual Quaternion Blending[J]. ACM Transactions on Graphics,2010,27(4): 995-999.

[13]Bianchi L, Dorigo M, Gambardella L M, et al. A Survey on Metaheuristics for Stochastic Combinatorial Optimization[J]. Natural Computing an International Journal, 2009, 8(2): 239-287.

[14]李士勇.蟻群算法及其應用[M].哈爾濱：哈爾濱工業大學出版社,2004.

(編輯陳勇)

收稿日期：2015-01-27

基金項目：國家重點基礎研究發展計劃(973計劃)資助項目(2013CB035504)

中圖分類號：TP24

DOI：10.3969/j.issn.1004-132X.2016.13.010

作者簡介：王恒升，男，1963年生。中南大學機電工程學院教授、博士研究生導師。主要研究方向為機電一體化及智能機器人。陳偉鋒，男，1989年生。中南大學機電工程學院碩士研究生。王思遠，男，1992年生。中南大學機電工程學院碩士研究生。

Drilling Sequence Planning Based on Screw Motion of End Lever of a Tunneling Rig Manipulator

Wang HengshengChen WeifengWang Siyuan

State Key Laboratory for High Performance Complex Manufacturing,Central South University,Changsha,410083

Abstract:One of the problems in the development of computerized tunneling rig is to make a drilling sequence according to a drill pattern design, which is the focus of this work. ACO algorithm was used to get a satisfactory sequence for a drill pattern,where the objective function was based on the trajectory length of screw motion of the end effector transported from one hole position to the next by the manipulator. The formulas were derived for the calculation of the screw motion parameters of the end effector with dual quaternion expressions. Simulations verify the algorithm of screw motion of the interpolation of the trajectory and the trajectory calculated is smooth and continuous. It turns out to be a practical drilling sequence resulting from the ACO algorithm for a prescribed drill pattern with two manipulators (right and left) of a 64-hole tunneling design.

Key words:drilling sequence; screw motion; dual quaternion; ant colony optimization(ACO) algorithm; screw linear interpolation; tunneling rig manipulator