張 程，張 卓

(1.吉林大學(xué) 珠海學(xué)院，廣東 珠海 519041;2.廣東科學(xué)技術(shù)職業(yè)學(xué)院，廣東 珠海 519041)

5-DOF仿人型機械臂關(guān)節(jié)空間軌跡規(guī)劃研究*

張 程1，張 卓2

(1.吉林大學(xué) 珠海學(xué)院，廣東 珠海 519041;2.廣東科學(xué)技術(shù)職業(yè)學(xué)院，廣東 珠海 519041)

文章以5-DOF仿人型機械臂為研究對象，討論了工業(yè)機器人軌跡規(guī)劃方法及相關(guān)問題。對該機械臂進行了運動學(xué)分析，建立了機械臂運動學(xué)方程，并針對典型搬運作業(yè)方式，以距離最優(yōu)為目標選取空間梯形路徑關(guān)鍵點，設(shè)計了基于關(guān)節(jié)空間五項多項式插值算法的軌跡規(guī)劃方法，利用Robotics Toolbox工具箱進行機械臂虛擬建模及關(guān)節(jié)空間軌跡規(guī)劃仿真。經(jīng)驗證，相對于傳統(tǒng)矩形路徑作業(yè)方式，該方法可以更好完成工業(yè)機器人搬運作業(yè)的軌跡規(guī)劃問題，對進一步結(jié)合工藝參數(shù)研究工業(yè)機器人控制系統(tǒng)具有必要的理論及實踐意義。

仿人型機械臂；軌跡規(guī)劃；關(guān)節(jié)空間

0 引言

工業(yè)機器人是迄今為止機器人技術(shù)最重要的商業(yè)應(yīng)用。工業(yè)機器人在執(zhí)行某項操作任務(wù)時，會依據(jù)工藝參數(shù)附加某些實時的約束條件，如軌跡參數(shù)、運動位置、速度及加速度等，這些參數(shù)的確定即是軌跡規(guī)劃的問題[1-2]。運動軌跡規(guī)劃的優(yōu)劣將直接影響機器人作業(yè)的質(zhì)量。隨著國內(nèi)外學(xué)者在運動軌跡規(guī)劃領(lǐng)域(如，機器學(xué)習(xí)、相似性運動及運動解析算法等)研究的不斷深入，運動軌跡規(guī)劃已經(jīng)成為機器人研究領(lǐng)域的重要課題之一[3-5]。本文針對機器人運動軌跡規(guī)劃的問題，以5-DOF仿人型機械臂為研究對象，利用分段軌跡規(guī)劃的方法針對典型搬運作業(yè)進行軌跡規(guī)劃研究，設(shè)計了一種5-DOF機械臂運動軌跡規(guī)劃方法。該方法依據(jù)搬運作業(yè)任務(wù)設(shè)計梯形路徑關(guān)鍵節(jié)點，采用關(guān)節(jié)空間法計算出預(yù)期的運動軌跡，使得機械臂可沿規(guī)劃軌跡穩(wěn)定、平滑且快速地到達位置，且能夠移植到其它型號的串聯(lián)機器人之中，具有良好的科研意義。

1 機械臂運動學(xué)模型

1.1 機械結(jié)構(gòu)

仿人型機械臂也稱為關(guān)節(jié)型機械臂，是由一系列通過關(guān)節(jié)相連的連桿組成的一個運動鏈[6]。關(guān)節(jié)通常分為旋轉(zhuǎn)和平移兩種，在機械臂和末端執(zhí)行器之間的運動鏈中的關(guān)節(jié)稱為手腕，機械臂和手腕組件主要用于定位末端執(zhí)行器(以及其攜帶的工具)完成作業(yè)操作。圖1展示了本文所研究的5-DOF仿人型機械臂的機械結(jié)構(gòu)，其依據(jù)人類手臂的特征而設(shè)計，包括肩部、肘部和腕部?？梢酝瓿杉绮啃D(zhuǎn)、大臂俯仰、肘部俯仰、小臂俯仰及腕部旋轉(zhuǎn)的人體上肢運動，并配有末端執(zhí)行器——夾持器，完成抓放、夾取等人體手部運動。該仿人型機械臂可以用于完成如搬運、碼垛、噴漆及弧焊等作業(yè)。

圖1 5-DOF仿人型機械臂機械結(jié)構(gòu)

1.2 運動學(xué)正解模型

(1)D-H參數(shù)確定

根據(jù)Denavit-Hartenberg參數(shù)法定義連桿坐標系。設(shè)x0y0z0為基座坐標系，其原點位于z0和z1的交點；z2平行于z3及z4；z4垂直于z5。xiyizi與機械臂第i個桿件固連，坐標原點在第i+1個關(guān)節(jié)的中心點處，選擇xi軸與相應(yīng)連桿的方向一致且處于(x0,y0)平面上，桿件長度αi-1為沿xi軸從zi-1到zi的距離；扭轉(zhuǎn)角αi-1為繞xi-1從zi-1到zi旋轉(zhuǎn)的角度；偏移距di為沿zi從xi-1到xi距離；關(guān)節(jié)角θi為繞zi從xi-1到xi旋轉(zhuǎn)的角度[2,6]。由此確定5-DOF機械臂連桿坐標系如圖2所示，連桿參數(shù)與關(guān)節(jié)變量的D-H參數(shù)表見表1。

圖2 機械臂連桿坐標系

表1 機械臂D-H參數(shù)與關(guān)節(jié)變量表

(2)變換矩陣計算

(1)

(2)

(3)

(4)

(5)

將各連桿變換矩陣相乘，即可得到機械臂的運動學(xué)方程：

(6)

式(6)為關(guān)節(jié)變量θ1，θ2，……，θ5的函數(shù)。該方程描述了末端執(zhí)行器位置坐標與各關(guān)節(jié)轉(zhuǎn)角關(guān)系，即機械手坐標系n的位置和方向相對基坐標系的坐標變換矩陣。至此，機械臂的運動學(xué)模型建立完畢。

2 關(guān)節(jié)空間軌跡插補算法

工業(yè)機器人軌跡規(guī)劃可以在兩種空間中進行：關(guān)節(jié)空間和操作空間。對于在奇異位形鄰域內(nèi)和存在冗余自由度情形的運動，操作空間軌跡規(guī)劃可能面臨難于求解的問題[7]。在此情形下，例如抓放作業(yè)和搬運作業(yè)的機器人比較適合于在關(guān)節(jié)空間進行規(guī)劃，本文選擇關(guān)節(jié)空間法進行軌跡規(guī)劃。

2.1 運動學(xué)逆解

由上文所建立的運動方程，當已知機械臂末端執(zhí)行器相對于基座標系的位姿矩陣T時，求解方程即可求出機械手的各個關(guān)節(jié)角度，此為運動學(xué)逆解[7]。關(guān)節(jié)空間規(guī)劃軌跡，即使用關(guān)節(jié)角度的平滑插值函數(shù)θ(t)表達機器人的運行軌跡，得到一組滿足軌跡約束條件的關(guān)節(jié)變量時間序列。

2.2 關(guān)節(jié)軌跡多項式插值算法

在軌跡規(guī)劃之前，需設(shè)定約束條件。以機械臂的搬運作業(yè)為例，當抓取并移動釋放物體時，要知道機械臂末端執(zhí)行器在移動中的每個節(jié)點(起始點、上升點、下降點和目標點)的位姿、速度以及加速度的要求，且其極值不能超出每個關(guān)節(jié)變量可以移動的范圍[6]。在滿足上述條件的情況下，利用平滑插值函數(shù)θ(t)產(chǎn)生不同的軌跡曲線。假設(shè)起始點時刻的起始關(guān)節(jié)角度為θ0，經(jīng)過運動學(xué)逆解能夠獲得終止點時刻tf的終止關(guān)節(jié)角度為θf。該軌跡函數(shù)θ(t)需要滿足五個關(guān)節(jié)角度、角速度和角加速度在起始點和終止點的約束條件。其中，各關(guān)節(jié)角度在起始點和終止點的約束條件為式(7)：

(7)

各關(guān)節(jié)角速度在起始點和終止點的約束條件為式(8)：

(8)

各關(guān)節(jié)角加速度在起始點和終止點的約束條件為式(9)：

(9)

在上述各關(guān)節(jié)邊界約束條件下，可以確定一個五階多項式：

θ(t)=k0+k1t+k2t2+k3t3+K4t4+k5t5

(10)

其中，多項式(10)的系數(shù)k1，k2，……，k5必須滿足上述各關(guān)節(jié)約束條件：

(11)

該線性方程組含有6個未知數(shù)和6個方程，其解為：

(12)

由此得到在約束條件下，對于初速度及終速度為零的關(guān)節(jié)運動的五次多項式插值函數(shù)為θ(t)，該插值函數(shù)表達了從起始點關(guān)節(jié)角度到終止點關(guān)節(jié)角度θf的變化。至此，完成了5階多項式插值軌跡規(guī)劃。

3 機械臂關(guān)節(jié)空間軌跡規(guī)劃仿真

基于上述5-DOF機械臂的運動學(xué)模型，利用Robotics Toolbox工具箱進行關(guān)節(jié)空間插補算法的運動軌跡仿真[8-10]。首先根據(jù)本文中5-DOF機械臂的D-H參數(shù)，建立相應(yīng)的機械臂對象模型5D-ROBT，如圖3所示。

>>L{1} = link([alpha(1) a(1) theta(1) d(1)],'standard');

>>L{2} = link([alpha(2) a(2) theta(2) d(2)],'standard');

……

>>ROBT = robot(L,'ROBT');

圖3 機械臂Robotics Toolbox模型5D-ROBT

以機械臂的搬運作業(yè)為研究對象，預(yù)先對軌跡途徑的關(guān)鍵點進行選定，將較復(fù)雜的運動路徑分解成在關(guān)鍵點間的分段運動。在整個搬運任務(wù)實現(xiàn)過程中，在工作單元內(nèi)設(shè)定5個工作點(機械臂預(yù)備點、物料初始點、抬手點、落手點和物料目地點)分別為Q0、Q1、Q2、Q3和Q4。機器人末端執(zhí)行器從初始位置Q0出發(fā)，經(jīng)規(guī)劃路徑到達Q1，抓取物塊后經(jīng)規(guī)劃路徑順次經(jīng)過Q2和Q3完成抬手、平移及落手動作，到達終點Q4并釋放物料，最終完成對物料的搬運。給定末端執(zhí)行器初始位置及依次到達的位姿矩陣如下：

>>Q0= [0 -1.57 1.57 -0.79 0];

>>Q1= [0 -0.95 1.67 -1.00 0];

>>Q2= [-0.21 -1.25 1.87 -1.00 0.79];

>>Q3= [-1.36 -1.25 1.87 -1.00 0];

>>Q4= [-1.57 -0.95 1.67 -1.00 0];

該分段軌跡規(guī)劃方法與傳統(tǒng)分段軌跡規(guī)劃方法有所不同：傳統(tǒng)分段軌跡規(guī)劃方法抬手點處于物料原點空間位置的正上方，同時，落手點也處于物料目的點空間位置的正上方，運動軌跡為矩形軌跡，如圖4所示，這樣做是為了增加正確拾取的幾率，但犧牲了時間。本文抬手點和落手點的空間位置是其投影在物料原點與物料目的點空間連線上的一點，即在物料抬起與落下的同時，在水平位置上還存在一段位移，運動軌跡為梯形軌跡。這種軌跡規(guī)劃方法可以使物料從原點到目的點所經(jīng)過的空間距離在一定程度上縮短，即距離最優(yōu)。值得一提的是，若需多次往返的搬運動作，則可通過搬運策略的優(yōu)化即可極大地縮短搬運時間，更好地提高工作效率及正確率。

圖4 傳統(tǒng)分段軌跡規(guī)劃路徑

利用Robotics Toolbox工具箱進行逆運動學(xué)求解，得到各關(guān)節(jié)運動學(xué)變量。其計算結(jié)果是有關(guān)各個路徑段的一組數(shù)據(jù)，在仿真圖中對機械臂各關(guān)節(jié)的運動情況進行觀察，根據(jù)關(guān)節(jié)空間內(nèi)變量變化情況對末端執(zhí)行器軌跡合理性進行驗證，得到搬運作業(yè)過程及機械臂末端執(zhí)行器軌跡路徑如圖5和圖6所示。

>>plot(ROBT,theta)

>>hold on;hidden off;plot3(x,y,z)

>>hold on;hidden off;

>>plot3(T00(1,4),T00(2,4),T00(3,4),′ro′,′linewidth′,5)

……

>>plot3(T34(i,4,length(t)),T34(i+1,4,length(t)),T34(i+2,4,length(t)),′ro′,′linewidth′,5)

圖5 搬運作業(yè)軌跡規(guī)劃(起始點)

圖6 搬運作業(yè)軌跡規(guī)劃(終止點)

經(jīng)算法仿真得到各關(guān)節(jié)角位移、角速度及角加速度數(shù)值對比曲線分別見圖7、圖8及圖 9，圖中實線為梯形規(guī)劃參數(shù)曲線，虛線為矩形規(guī)劃參數(shù)曲線。

>>plot(T,[Q01(:,i);Q12(:,i);Q23(:,i);Q34(:,i)])

>>plot(T,[QD01(:,i);QD12(:,i);QD23(:,i);QD34(:,i)])

>>plot(T,[QDD01(:,i);QDD12(:,i);QDD23(:,i);QDD34(:,i)])

圖7 各關(guān)節(jié)角位移隨時間變化曲線對比

圖8 各關(guān)節(jié)角速度隨時間變化曲線對比

圖9 各關(guān)節(jié)角加速度隨時間變化曲線對比

根據(jù)圖示對5個關(guān)節(jié)角運動學(xué)參數(shù)進行分析，可見本文方法從整體上改善了各關(guān)節(jié)軌跡運動參數(shù)。如關(guān)節(jié)1的角速度最大值減少了1.6°/s，角加速度最大值減少了10°/s2，使得角速度變化幅度較小，運動參數(shù)明顯趨于平穩(wěn)，運動曲線較平滑；與此同時，關(guān)節(jié)3角速度最大值增加了0.37°/s，角加速度最大值增加了2.3°/s2，該變化中機械臂關(guān)節(jié)角始終保持處于安全范圍以內(nèi)，且增幅較小，相對于關(guān)節(jié)1的明顯改善，關(guān)節(jié)3的參數(shù)調(diào)整是完全符合作業(yè)運動要求的。據(jù)此可得本文所規(guī)劃運動軌跡在整個運動過程中平穩(wěn)光滑且速度較快，相對于傳統(tǒng)矩形運動軌跡可以更好地滿足搬運作業(yè)的實際需求。

4 結(jié) 論

本文以5-DOF仿人型機械臂為研究對象，設(shè)計了基于關(guān)節(jié)空間五次多項式的插補算法，以搬運作業(yè)為例，根據(jù)設(shè)定的作業(yè)關(guān)鍵點進行軌跡規(guī)劃，設(shè)計基于梯形路徑的關(guān)鍵點規(guī)劃方法，計算各插補點的位姿，并通過運動學(xué)逆解得到各關(guān)節(jié)角度值。經(jīng)Robotics Toolbox工具箱仿真驗證，該算法可以滿足機械臂運動平穩(wěn)、連續(xù)的要求，相對于傳統(tǒng)矩形規(guī)劃路徑具有距離最優(yōu)、運動參數(shù)更穩(wěn)定的優(yōu)勢，為下一步將工業(yè)機器人空間軌跡同現(xiàn)場工藝參數(shù)進行聯(lián)合規(guī)劃并實現(xiàn)提供了必要的理論及實踐基礎(chǔ)。

[1] 楊前明,劉亞瓊,王偉,等.基于D-H算法的棉桶更換復(fù)合機器人運動學(xué)建模與仿真分析[J].組合機床與自動化加工技術(shù),2016(12):52-55.

[2] Bruno Siciliano.機器人手冊[M]. 機器人手冊編譯委員會,譯.北京:機械工業(yè)出版社,2016.

[3] 任曉平,侯濤,蘇建良.仿人雙臂多自由度工業(yè)機器人設(shè)計[J].工業(yè)控制計算機,2016,29(11):64-65.

[4] 王魯平,朱華炳,秦磊.基于MATLAB的工業(yè)機器人碼垛單元軌跡規(guī)劃[J].組合機床與自動化加工技術(shù),2014(11):128-132.

[5] Madjid Hank, Moussa Haddad. A hybrid approach for autonomous navigation of mobile robots in partially-known environments[J].Robotics and Autonomous Systems,2016,86(9): 113-127.

[6] 蔡自興.機器人學(xué)基礎(chǔ)[M].2版.北京:機械工業(yè)出版社,2016.

[7] Guo-Shing Huang. Inverse kinematics analysis trajectory planning for arobot arm[C]. Control Conference,2011: 965-970.

[8] 高帥.工業(yè)機器人軌跡規(guī)劃研究[J].計算機仿真,2015,32(10):409-413.

[9] XIAOYUN DENG. Robot Workcell Modeling and Collision Detection with MATLAB Robotics Toolbox[D].Master of Science Thesis, Tampere University of Technology,2012.

[10] Mark W Spong.機器人建模和控制[M]. 賈振中,譯.北京:機械工業(yè)出版社,2016.

(編輯李秀敏)

ResearchonTrajectoryPlanningof5-DOFHumanoidManipulatorinJointSpace

ZHANG Cheng1, ZHANG Zhuo2

(1.Zhuhai College of Jilin University, Zhuhai Guangdong 519041, China; 2.Guangdong Polytechnic of Science and Technology, Zhuhai Guangdong 519041, China)

In this paper, the 5-DOF humanoid robot arm is taken as the research object, and the trajectory planning method of industrial robot is discussed. The kinematics of the manipulator is analyzed in this paper, and the kinematics model of manipulator is established. Based on the key points of the task, a trajectory planning method based on joint space five polynomial interpolation algorithm is designed. By using the Robotics Toolbox toolbox, the virtual modeling of the manipulator and the simulation of joint space trajectory planning are carried out. This method can be applied to the trajectory planning of industrial robot, which is of great theoretical and practical significance for the further study of industrial robot control system.

humanoid manipulator; trajectory planning; joint space

TH166;TG659

：A

1001-2265(2017)09-0023-04

10.13462/j.cnki.mmtamt.2017.09.007

2017-02-12；

：2017-03-23

廣東省科技發(fā)展專項資金(基礎(chǔ)與應(yīng)用基礎(chǔ)研究方向)項目(2016A030313658)；廣東省工業(yè)高新技術(shù)領(lǐng)域科技計劃項目(2013B010101020)；吉林大學(xué)珠海學(xué)院創(chuàng)新能力培育工程項目(2016XJCQCX19)；廣東科學(xué)技術(shù)職業(yè)學(xué)院科研項目(XJPY2016022)；吉林大學(xué)珠海學(xué)院教學(xué)質(zhì)量工程項目(ZLG20170701)

張程(1986—)，女，沈陽人，吉林大學(xué)珠海學(xué)院講師，碩士，研究方向為機器人控制系統(tǒng)，(E-mail)orange1221@163.com。