排爆機械臂多目標軌跡規劃研究

馬振書， 張云峰， 孫華剛， 陸繼山

(1.軍械技術研究所，石家莊　050003；2.軍械工程學院，石家莊　050003)

?

排爆機械臂多目標軌跡規劃研究

馬振書1， 張云峰2， 孫華剛1， 陸繼山2

(1.軍械技術研究所，石家莊050003；2.軍械工程學院，石家莊050003)

摘要：針對排爆機械臂軌跡規劃問題，提出多目標軌跡規劃方法。建立三次樣條曲線分段軌跡描述方程，采用外加虛擬點法保證三次樣條曲線可任意指定軌跡初始速度、加速度，提出靈活性優化目標以提高機構靈活性；將優化目標及運動學約束以軌跡方程參數表示，轉化為對各段運動時間的尋優；采用CNSGA-Ⅱ對排爆機械臂運動進行多目標軌跡優化，處理非線性約束最優化，建立排爆機械臂多目標軌跡規劃算法流程。結果表明，該方法能有效提高規劃結果靈活性，各項目標均優于線性加權單目標規劃，所得軌跡平滑可控。

關鍵詞：排爆機械臂；軌跡規劃；多目標優化；NSGA-Ⅱ

排爆機器人(Explosive Ordnance Disposal Robot)指替人在爆炸等危險環境中完成任務的機器人[1]。大多數排爆機器人為遙控操作，通過監視器觀察作業環境，操縱機械臂關節完成作業。此控制較易產生視覺疲勞，在精神高度緊張下較難精確控制機械臂，易引發爆炸等危險。因此，自主決策、運動的排爆機器人成為發展方向[2-4]。排爆機械臂為排爆機器人的作業部分，其軌跡規劃為排爆機器人性能的關鍵因素，決定排爆機械臂的運動方式。

軌跡規劃為機器人學的基本問題，其任務即據路徑規劃結果產生隨時間連續變化的各關節運動律。機器人軌跡規劃主要有笛卡爾空間規劃[5-6]及關節空間規劃[7-9]，前者形象直觀，但要實現快速逆運動學計算，運動軌跡可能穿過奇異區域[10]；而關節空間規劃優點在于控制系統直接作用于關節，易滿足設計要求及運動約束，避免奇點、關節突變，減少正逆運動學計算[11]。軌跡規劃基本的優化條件為最小執行時間[12-13]、最小能量[14]及最小脈動[15-16]，大多混合優化方法均基于此。Gasparetto等[17]以時間-脈動最優為目標，研究該優化條件對機械臂運動性能影響，經與傳統的時間最優對比表明，該方法能有效減小關節脈動，減小機械磨損及振動，使機器人運動性能更好。龐慧[18]提出的基于三次樣條插值的時間最優軌跡規劃方法，能初步解決排爆機械臂軌跡規劃問題，但仍有待進一步解決的問題，即① 軌跡規劃目標中未考慮機械臂的靈活性，實際應用中若機械臂在某位置靈活性較低，其末端微小運動會導致關節速度突然變化。② 混合目標優化各目標間相互沖突，其權值靠經驗設定，對結果影響較大；多目標優化方法并未給出從Pareto解集中選擇最優解方法；傳統NSGA-Ⅱ較難處理復雜非線性約束組優化問題。③ 經典三次樣條插值無法同時保證第一、二類初始條件，高次樣條曲線構造復雜，龍格現象明顯。

為此，本文以某型排爆機器人二代樣機為原型，研究其排爆機械臂多目標軌跡規劃技術。在優化目標中加入靈活性函數，使軌跡規劃結果易于跟蹤；采用改進三次樣條曲線在過渡點間插值，使機械臂軌跡滿足任意初始條件；用NSGA-Ⅱ算法軌跡規劃獲得滿足多約束條件、多優化目標的Pareto解集，并提出最優解選擇方法。

1排爆機械臂結構及靈活性

1.1機械臂結構

排爆機械臂結構見圖1，機械臂有六個自由度，即腰部回轉、大臂俯仰、小臂俯仰、小臂伸縮、手腕俯仰及手腕旋轉。建立D-H坐標系，參數見表1。工作空間末端位姿與關節空間坐標可通過齊次變換矩陣相互轉化。機械臂由電液混合驅動，機構復雜，對軌跡平滑度要求較高；排爆機器人能源來自蓄電池，能耗低的工作軌跡可增加排爆機器人續航能力。

圖1　排爆機械臂結構Fig.1 Structure of explosive ordnance disposal manipulator

#θdaα0-1θ1(90)00-901-2θ2(-90)0a202-3θ3(0)0a3-903-40d40904-5θ5(0)00-905-6θ6(0)000

1.2機械臂靈活性

引起機械臂末端特定的關節空間運動需用雅克比矩陣計算。雅克比矩陣為奇異矩陣或機械臂運動到變化范圍極限時，其末端微小運動會導致關節速度突然變化，此時對應的機械臂位形稱奇異位形。

圖2為機械臂前三個關節的速度橢圓，橢圓半徑越短機械臂末端越難實現與該半徑對應方向的運動[19]。圖2(a)為正常位姿的速度橢圓，圖2(b)為接近奇異位形的速度橢圓。可見，當機械臂接近奇異位形時其末端失去某些方向的靈活性，嚴重影響機械臂關節速率的控制效果。

圖2　機械臂末端速度橢圓Fig.2 End-effector velocity ellipsoids

排爆機器人抓取重量大，工作環境危險，一旦機械臂失控會導致事故發生，因此其軌跡規劃須避開奇異位形附近區域，保證機械臂靈活可控。

2多目標軌跡規劃問題構造

關節空間軌跡規劃以工作空間路徑的點經逆運動學計算所得關節變量值作為過渡點，過渡點間采用插值函數構造排爆機械臂運動軌跡。

2.1改進三次插值函數

為滿足速度、加速度連續性要求，用三次樣條函數對過渡點插值。傳統三次樣條插值無法同時滿足第一、二類邊界條件，文獻[8]的改進方法雖能使軌跡始末速度、加速度同時為零，但不能任意指定采用始末速度、加速度，不適用于實時軌跡規劃。本文用在第二及倒數第二位置添加兩個虛擬點方式改進三次樣條函數。設N關節機械臂工作有n-2個路徑點，加兩個虛擬點，每個關節有n個過渡點。

三次樣條曲線加速度為線性函數，因此關節j在第i段加速度函數Wj,i(t)計算式為

(1)

式中：wj,i為關節j在第i過渡點處加速度；hi=ti+1-ti為通過相鄰過渡點所用時間；ti為軌跡通過第i過渡點時刻。

對式(1)積分兩次消去任意常數，得位移公式為

(i=1…n-1,j=1…N)

(2)

式中：Qj,i(t)為關節j在第i段位移函數；qj,i為關節j在第i過渡點處位移。

對式(2)求導得速度公式為

(i=1…n-1,j=1…N)

(3)

式中：Vj,i(t)為關節j在第i段的速度函數。

式(3)代入連續性條件，得

Vj,i(ti-0)=Vj,i+1(ti+0)

(4)

整理得

uiwj,i-1+2wj,i+(1-ui)wj,i+1=di,

(i=2,5,…,n-1,j=1…N)

(5)

為使機械臂能實時規劃含任意初始速度、加速度的運動軌跡，虛擬過渡點位置公式為

(6)

qj,n-1=qj,n-hn-1vj,n+

(7)

最終獲得求過渡點處加速度線性方程組，系數矩陣為嚴格對角占優的三對角陣，故方程組有解，已知wj,1，wj,n，vj,1，vj,n，qj,i，hi的值即可求得過渡點處加速度wj,i，即

2.2軌跡規劃目標

針對排爆機械臂軌跡規劃問題，提出4個軌跡規劃目標，分別為執行時間、脈動目標、加速度目標及機械臂靈活性，以此滿足排爆機械臂對工作效率、軌跡平滑度、能量消耗及機械臂靈活性要求。據三次樣條曲線構造目標函數，表達式分別為

(9)

(10)

(11)

(12)

式(9)～(13)中：J為機械臂雅克比矩陣；VCj，ACj，JCj為關節j的速度、加速度及脈動約束；Hlb為各段運動時間下限。

(13)

由以上推導過程知，機械臂最優軌跡規劃問題轉為在非線性約束下對各段運動時間hi的多目標優化問題。式(9)為執行時間目標函數，以過渡點間運動時間之和表示，目的為減小機械臂運動時間；式(10)為軌跡脈動目標函數，以各過渡點間脈動平方和表示，目的為使運動軌跡更平滑，減小加速度突然變化；式(11)為加速度目標函數，目的為減小機械臂運動過程中加速度，進而減小能量消耗；式(12)為機械臂靈活性目標函數，可增加機械臂運動中的靈活性，使運動軌跡遠離奇點；式(13)為約束條件，依次為速度約束、加速度約束、脈動約束、時間約束。傳統單目標軌跡規劃方法的目標函數表達式為

(14)

式中：k1，k2，k3，k4為權重系數；N1，N2，N3，N4為歸一化參數。

優化結果取決于憑直覺及先驗知識設置的權重系數，較難獲得多目標優化條件的近似最優解。

3非支配排序遺傳算法(NSGA-Ⅱ)

傳統線性加權多目標優化方法各優化目標間相互沖突，優化結果過度依賴人為設置的權重系數。而基于非支配排序的多目標優化方法可較好的解決此問題。對多目標優化問題，一般無法找到使所有目標最優的解，因此引入非次解概念。非次解指無法在改進一個目標函數的同時不削弱其它目標的解，基于非次解的多目標優化亦稱Pareto最優化。

NSGA-Ⅱ(Nondominated Sorting Genetic Algorithm)為由Deb等[20]提出基于Pareto的多目標進化算法，其結果能較好逼近Pareto前沿，能有效解決多目標優化問題。較NSGA，NSGA-Ⅱ引入精英機制，保留前代優秀個體，并用快速非支配排序，算法復雜度由O(mN3)降為O(mN2)，其中m為優化目標數，N為種群數；NSGA-Ⅱ無需設置主觀可調參數，能消除共享參數對算法效果影響。因此，NSGA-Ⅱ在收斂性(更接近真實Pareto前沿)及多樣性(結果分布性好)均優于其它多目標優化算法。

3.1約束非支配排序遺傳算法(CNSGA-Ⅱ)

傳統NSGA-Ⅱ難以處理含復雜非線性約束的多目標最優化問題，本文對其進行改進。

為維持NSGA-Ⅱ流程的模塊性，改變個體i，j支配關系，定義若以下任一條件成立，則個體i約束-支配個體j：①i滿足約束條件而j不滿足約束條件；②i、j均不滿足約束條件，但i擁有較小的約束超越量；③i、j均滿足約束條件，且i支配j。

個體約束超越量計算式為

(15)

式中：m為非線性約束數量；n為線性約束數量；h為個體；Cv為個體h約束超越量，非線性約束nc及線性約束lc被轉化為不等式形式，若個體在約束范圍內則Cv=0。

CNSGA-Ⅱ不改變算法的復雜度，無需增加懲罰函數，只改變支配關系定義，使符合約束條件或約束超越小的個體支配不符合約束條件或約束超越大的個體，其它過程與傳統NSGA-Ⅱ相同。

3.2多目標軌跡規劃流程

CNSGA-Ⅱ算法求解排爆機械臂多目標軌跡規劃問題算法流程見圖3。其中選擇算子為隨機遍歷抽樣；交叉算子為獨立點交叉；變異算子為高斯變異。

圖3　多目標軌跡規劃算法流程Fig.3 Process of multi-objective trajectory planning algorithm

4仿真與分析

用CNSGA-Ⅱ規劃排爆機械臂運動軌跡。機械臂運動所經關節位置序列見表3，各關節運動學約束見表4。以改進三次樣條曲線對過渡點間軌跡插值，設始末速度、加速度均為零，種群規模為500，各段運動時間上限由經驗給出，下限計算式為

(16)

式中：v，w，j為以Hlb各段運動時間所得速度、加速度、脈動。

表3　軌跡規劃輸入

表4　關節運動學約束

圖4為多目標軌跡規劃所得Pareto前沿，圖4為加入靈活性目標后的優化結果。其中圖4(b)未加入靈活性目標，可見兩條曲線形狀發生明顯變化，因軌跡規劃結果會過于靠近奇點，導致機械臂失去靈活性及可操縱性。

圖4　Pareto前沿Fig.4 Pareto front

表5、6分別為不考慮、考慮靈活性的多目標表優化所得Pareto最優解中單一目標最優個體目標值。對比看出，加入靈活性最優條件使機械臂靈活性有所提高，但會犧牲時間最優目標。由雅克比矩陣定義，機械臂軌跡繞過低靈活性區域需增加運動時間，因此目標對執行時間影響較大，而對脈動目標、加速度目標影響較小。

據混合優化目標式(14)，令N1=40，N2=0.001，N3=0.01，N4=0.000 01，k1=0.4，k2=0.2，k3=0.2，k4=0.2；其中歸一化系數N1,…,N4由單一目標優化結果獲得，權重系數k1,…,k4據實際經驗取得。用經典遺傳算法進行傳統單目標軌跡規劃，與多目標軌跡規劃所得Pareto解集中時間目標最接近的解對比，結果見表7。由表7看出，時間相同時Pareto最優解的脈動、加速度、靈活性目標均遠優于傳統混合單目標優化，表明多目標優化結果較好逼近Pareto前沿。

表5　多目標優化結果

表6　考慮靈活性的多目標優化結果

表7　優化結果比較

多目標軌跡規劃所得結果為一組近似Pareto最優解，因此用平均最優解從Pareto解集中選最優解。表達式為

(16)

式中：Fmean為平均最優解；Objmax，Objmin分別為一組Pareto最優解中某優化目標最大、最小值，可選使Fmean值最小的解作為多目標軌跡規劃最終結果。

圖5～圖8為平均最優解與采用傳統GA法所得位移、速度、加速度及脈動曲線。圖中實線為多目標優化結果，虛線為傳統線性加權法優化結果。可見位移、速度、加速度曲線平滑連續，各曲線變化范圍均在運動學約束內，滿足工作、設計要求。除關節5外，其它各關節虛線變化幅度均大于實線，表明同等運動時間下，

圖5　各關節位移Fig.5 Displacement of joints

圖6　各關節速度Fig.6 Velocity of joint

圖7　各關節加速度Fig.7 Acceleration of joints

圖8　各關節脈動Fig.8 Jerk of joints

多目標軌跡規劃的關節空間軌跡更平滑，可減小機械磨損與多余振動；優化時考慮機構靈活性，使機械臂易于跟蹤運動軌跡，該運動律可直接作為控制器輸入引導機械臂運動。

5結論

(1) 利用基于改進三次樣條插值及CNSAGA-Ⅱ多目標軌跡規劃方法，所得排爆機械臂運動軌跡平滑，較好滿足排爆機械臂動作時避免加速度突變、減小消耗能量要求。通過在目標函數中加入機構靈活性指標，能提高軌跡規劃結果的靈活性、便于機械臂跟蹤規劃軌跡，軌跡靈活性高也可提高機構的控制性能。

(2) 多目標優化的結果為Pareto解集，較難從中選擇某個合適的解。平均最優解通過多目標歸一化加合方式選擇最優解，避免結果選擇的盲目性。本文所提算法可應用于其它類型串聯機械臂系統的軌跡規劃。

參 考 文 獻

[1] 范路橋,姚錫凡,祁亨年,等.排爆機器人的研究現狀及其關鍵技術[J].機床與液壓,2008,36(6):139-143.

FAN Lu-qiao, YAO Xi-fan, QI Heng-nian, et al. Research progress and key techniques of explosive ordnance disposal robot[J].Machinetool & Hydraulics,2008,36(6):139-143.

[2] 范路橋,姚錫凡,蔣梁中,等.半智能排爆機器人的單手雙目手眼系統[J].華南理工大學學報:自然科學版,2008,36(4): 40-44.

FAN Lu-qiao,YAO Xi-fan,JIANG Liang-zhong,et al. Single-hand and binocular eye-in-hand system for semi-intelligent EOD robot[J]. Journal of South China University of Technology :Natural Science Edition,2008,36(4):40-44.

[3] 申加剛,董再勵,郝穎明,等.排爆作業機器人模擬訓練系統研究[J].機器人,2005,27(5):426-430.

SHEN Jia-gang, DONG Zai-li, HAO Ying-ming, et al. Research on a virtual training simulator for explosive-handling Robot[J].Robot, 2005,27(5):426-430.

[4] 曾建軍,楊汝清,張偉軍.有限人參與下的排爆機器人半自主抓取[J].上海交通大學學報,2007,41(8):1238-1342.

ZENG Jian-jun, YANG Ru-qing, ZHANG Wei-jun. Research of semi-automatic bomb fetching with limited support of human in explosive ordnance disposal robot[J]. Journal of Shanghai Jiaotong University, 2007,41(8):1238-1342.

[5] 陳國梁,王心漢,王敏.機械手圓周運動的軌跡規劃與實現[J].華中科技大學學報:自然科學版,2005,33(11):63-66.

CHEN Guo-liang, WANG Xin-han, WANG Min. Trajectory planning for the circular motion of manipulator and its implementation[J]. J.Huazhong Univ. of Sci. & Tech.:Nature Science Edition,2005,33(11):63-66.

[6] 任敬軼,孫漢旭.一種新穎的笛卡爾空間軌跡規劃方法[J].機器人，2002,24(3):217-221.

REN Jing-yi, SUN Han-xu.A novel method of trajectory planning in cartesian space[J].Robot,2002,24(3):217-221.

[7] Solteiro Pires E J, de Moura Oliveira P B, Tenreiro Machado J A.Manipulator trajectory planning using a MOEA[J]. Applied Soft Computing,2007(7):659-667.

[8] Gasparetto A, Zanotto V. A technique for time-jerk optimal planning of robot trajectories[J]. Robotics and Computer-Integrated Manufacturing,2008(24):415-426.

[9] Rossi C, Savino S. Robot trajectory planning by assigning positions and tangential velocities[J]. Robotics and Computer-Integrated Manufacturing, 2013 (29):139-156.

[10] Niku S B.孫富春,朱紀洪,劉國棟,等,譯.機器人學導論 分析、控制及應用[M].2版.北京：電子工業出版社，2013.

[11] Gasparetto A, Zanotto V. Optimal trajectory planning for industrial robots[J]. Advances in Engineering Software, 2010(41):548-556.

[12] Shiller Z, Chang H, Wong V. The practical implementa- tion of time-optimal control for robotic manipulators[J]. Robotics & Computer-Integreted Manufacturing,1996,12(1):29-39.

[13] Lee Y D, Lee B H, Kim H G. An evolutionary approach for time optimal trajectory planning of a robotic manipulator [J].Information Science,1999(113):245-260.

[14] Oen K T, Wang L C T. Optimal dynamic trajectory planning for linearly actuated platform type parallel manipulators having task space redundant degree of freedom [J]. Mechanism and Machine Theory,2007(42): 727-750.

[15] Boscariol P, Gasparetto A. Model-based trajectory planning for flexible-link mechanisms with bounded jerk[J]. Robotics and Computer-Integrated Manufacturing, 2013(29): 90-99.

[16] Gasparetto A, Zanotto V. A new method for smooth trajectory planning of robot manipulators[J]. Mechanism and Machine Theory,2007 (42):455-471.

[17] Gasparetto A, Lanzutti A, Vidoni R, et al. Experimental validation and comparative analysis of optimal time-jerk algorithms for trajectory planning[J]. Robotics and Computer-Integrated Manufacturing, 2012(28):164-181.

[18] 龐慧.危險彈藥處理機器人工作裝置的性能分析及優化[D].天津:河北工業大學,2010.

[19] Corke P. Robotics, vision and control[M]. Berlin: Springer, 2011.

[20] Deb K, Agrawal S, Pratap A, et al. A fast elitist nondominated sorting genetic algorithm for multi-objective optimization: NSGA-Ⅱ[J]. Evolutionary Computation, 2002,6(2):182-197.

基金項目：國家863項目(2001AA422420)

收稿日期：2014-12-16修改稿收到日期：2015-01-30

通信作者張云峰 男，碩士生，1990年生

中圖分類號:TP241.3

文獻標志碼：A

DOI:10.13465/j.cnki.jvs.2016.02.019

Multi-objective trajectory planning of explosive ordnance displosal manipulators

MA Zhen-shu1, ZHANG Yun-feng2, SUN Hua-gang1, LU Ji-shan2

(1. Institute of Mechanical Technology, Shijiazhuang 050003, China; 2. Mechanical Engineering College, Shijiazhuang 050003, China)

Abstract:A multi-ojective optimization technique was presented to solve the problem of explosive ordnance displosal manipulators’ trajectory planning. Cubic splines were used to connect each pair of consecutive via-points. Two virtual points were introduced to ensure the initial and final conditions of velocity and acceleration to be respected. A manipulability optimization objective was developed to enhance the manipulability of mechanism. The objective functions and kinematic constraints were described by the parameters of cubic splines, so that the problem was transformed into the optimization of each time interval. The constraint NSGA-Ⅱ was adopted to plan the multi-objective trajectory, which is suitable to solve optimization problems with nonlinear constraints. The procedure of the algorithm based on NSGA-Ⅱ was created. Comparing with the traditional method, the simulation results indicate that the developed technique can improve the manipulability effectively and every optimization index achieved is better than the results of linear weighting coefficient technique. The obtained trajectories are smooth enough and easy to control.

Key words:explosive ordnance displosal manipulator; trajectory planning; multi-objective optimization; NSGA-Ⅱ

第一作者 馬振書 男，博士，研究員，1966年生