基于MATLAB的6R機(jī)器人軌跡規(guī)劃速度分析

楊廣新

（1.浙江信息工程學(xué)校，浙江湖州 313000；2.湖州工程技師學(xué)院，浙江湖州 313000）

0 引言

隨著產(chǎn)業(yè)升級，迫切需要對機(jī)器人的設(shè)計(jì)進(jìn)行深入的研究[1]，因此分析和掌握機(jī)器人軌跡規(guī)劃特性至關(guān)重要。

目前機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃方法比較多，但多集中在采用多項(xiàng)式實(shí)現(xiàn)關(guān)節(jié)空間的軌跡規(guī)劃。孫增光等[2]研究了三次多項(xiàng)式和五次多項(xiàng)式實(shí)現(xiàn)IRB2400 機(jī)器人的關(guān)節(jié)空間軌跡規(guī)劃算法，并對規(guī)劃效果進(jìn)行對比。李林升[3]采用組合的5-5-7-5 次復(fù)合多項(xiàng)式實(shí)現(xiàn)PUMA560 機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃，計(jì)算量較大，需要采用高性能計(jì)算機(jī)實(shí)現(xiàn)關(guān)節(jié)空間軌跡規(guī)劃。劉貝貝等[4]在5 次多項(xiàng)式基礎(chǔ)上融合了均勻5 次B 樣條曲線進(jìn)行機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃，實(shí)現(xiàn)較好的控制效果。此外，梅江平等[5]提出5 次非均勻有理B樣條實(shí)現(xiàn)Delta機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃。

機(jī)器人速度雅可比矩陣（簡稱雅可比）表示在具體位置關(guān)節(jié)處操作空間速度與關(guān)節(jié)空間速度之間的線性映射關(guān)系。機(jī)器人速度雅克比矩陣是機(jī)器人操作空間速度分析的關(guān)鍵要素，機(jī)器人在不同位置，雅克比矩陣的值是不同的。機(jī)器人速度雅克比矩陣求解比較困難，許多學(xué)者針對機(jī)器人的構(gòu)型不同研究出不同的機(jī)器人速度雅克比矩陣求解方法，但目前主要采用微分變換法、直接構(gòu)造法和矢量叉乘法等[6-7]。

本文以6R 機(jī)器人為研究對象，根據(jù)D-H 參數(shù)法建立6R機(jī)器人運(yùn)動學(xué)方程；在此基礎(chǔ)上，根據(jù)直接構(gòu)造法構(gòu)建6R機(jī)器人速度雅克比矩陣，對機(jī)器人關(guān)節(jié)空間和操作空間速度關(guān)系進(jìn)行仿真分析，研究機(jī)器人三次多項(xiàng)式軌跡規(guī)劃對操作空間速度連續(xù)性及變化的影響。

1 機(jī)器人運(yùn)動學(xué)分析

1.1 D-H建模

D-H機(jī)器人建模方法由Denavit和Hartenberg提出。如圖1所示，兩個構(gòu)件相連，中間關(guān)節(jié)軸線有兩條公法線與其垂直，每條公法線對應(yīng)一個連桿，如Zn分別與公法線an和an+1垂直。兩公法線之間的夾角為轉(zhuǎn)動角，記為θn，其轉(zhuǎn)向按繞軸線Zn的右手螺旋法確定。

圖1 D-H連桿模型

在每個構(gòu)件上固結(jié)一坐標(biāo)系，固結(jié)在桿n 上的坐標(biāo)系為onxnynzn，其中坐標(biāo)系原點(diǎn)on為關(guān)節(jié)軸線zn與an的交點(diǎn)，xn與公法線重合，zn與關(guān)節(jié)軸線重合。坐標(biāo)系onxnynzn可以通過坐標(biāo)系on-1xn-1yn-1zn-1繞xn-1軸轉(zhuǎn)角an，沿xn-1軸平移an，繞zn軸轉(zhuǎn)角θn，然后沿zn軸平移dn四步獲得，則變換矩陣如下：

式中：cn為 cos θn；sn為 sin θn； sαn為 sinαn； cαn為 cosαn。

式中： nx、ny、nz，ox、oy、oz，ax、ay、az分別為末端執(zhí)行器的單位主矢量x、y、z 相對于i關(guān)節(jié)坐標(biāo)系的x、y、z 的方向余弦； px、py、pz為末端執(zhí)行器的坐標(biāo)原點(diǎn)相對于i關(guān)節(jié)坐標(biāo)系的 x、y、z 的位置。

當(dāng)i=0時，表示在機(jī)器人的基坐標(biāo)系與工具坐標(biāo)系之間的變換。

1.2 機(jī)器人運(yùn)動學(xué)

機(jī)器人關(guān)節(jié)連桿參數(shù)如表1所示。

表1 6R機(jī)器人連桿參數(shù)

根據(jù)D-H方法，第i個關(guān)節(jié)坐標(biāo)系到第i+1個關(guān)節(jié)坐標(biāo)系的變化如下：

2 機(jī)器人速度雅克比矩陣的構(gòu)建

2.1 機(jī)器人速度雅克比矩陣

6R機(jī)器人關(guān)節(jié)變量采用向量θ表示如下：

因此，關(guān)節(jié)空間的微小運(yùn)動可表示為：

機(jī)器人工具坐標(biāo)系在操作空間的位姿為X，其是關(guān)節(jié)變量的函數(shù)，如下所示：

式中： X 為6 維列矢量； J(θ) 為6R 機(jī)器人的速度雅可比（Jacobian）矩陣，其由函數(shù)x、y 的偏微分組成，反映了關(guān)節(jié)空間微小運(yùn)動dθ與工具坐標(biāo)系之間微小運(yùn)動dX 之間的關(guān)系，同時也反映了機(jī)器人末端工具坐標(biāo)系在操作空間的位姿變化速度與機(jī)器人關(guān)節(jié)變化速度之間的線性關(guān)系，該線性變換與機(jī)器人的位置相關(guān)。

對于6R機(jī)器人，根據(jù)機(jī)器人運(yùn)動學(xué)方程，可以獲得直角坐標(biāo)位置向量[x, y, z]T的顯式方程，但找不到方位向量[ωx，ωy，ωz]T的一般表達(dá)式。采用雅克比矩陣構(gòu)造法可以更方便求出機(jī)器人雅克比矩陣。

對于轉(zhuǎn)動關(guān)節(jié)，關(guān)節(jié)n 只繞zn軸以角速度θ˙n轉(zhuǎn)動，機(jī)器人末端在操作空間位姿速度按如下構(gòu)造：

6R機(jī)器人6個關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，6R機(jī)器人雅克比矩陣根據(jù)式（7）進(jìn)行構(gòu)造，如下所示：

2.2 機(jī)器人速度雅克比矩陣構(gòu)造

機(jī)器人速度雅克比矩陣構(gòu)造步驟如下：

（1）計(jì)算各連桿變化矩陣如式（3）；

（3）按式（8）構(gòu)造J的各列元素。

3 仿真分析

6R 機(jī)器人依次經(jīng)過點(diǎn)P0、P1、P2和P3，對應(yīng)時間序列為0 s，2 s，5 s和7 s。操作空間點(diǎn)P0～P4對應(yīng)的關(guān)節(jié)值如表2所示。經(jīng)過MATLAB 三次多項(xiàng)式關(guān)節(jié)空間軌跡規(guī)劃，各關(guān)節(jié)位置曲線和速度曲線如圖2 所示。由圖可知，6R 機(jī)器人關(guān)節(jié)空間位置和速度連續(xù)，機(jī)器人運(yùn)行平穩(wěn)。根據(jù)三次多項(xiàng)式關(guān)節(jié)空間軌跡規(guī)劃結(jié)果，對機(jī)器人相對于基坐標(biāo)系的操作空間速度進(jìn)行分析，如圖3 所示。機(jī)器人操作空間沿基坐標(biāo)系X、Y和Z 方向及合成速度均連續(xù)；沿基坐標(biāo)系X、Y 和Z 軸的旋轉(zhuǎn)角速度不連續(xù)，但數(shù)值為1×10-16級別，此不連續(xù)是由MATLAB計(jì)算誤差造成的。

表2 關(guān)節(jié)位置序列 rad

圖2 關(guān)節(jié)位置速度曲線圖

4 結(jié)束語

采用速度雅克比矩陣直接構(gòu)造法原理，給出6R機(jī)器人速度雅克比矩陣構(gòu)造過程，機(jī)器人力雅克比矩陣是速度雅克比矩陣的轉(zhuǎn)置，對于6R機(jī)器人受力研究具有重要意義；在機(jī)器人三次多項(xiàng)式關(guān)節(jié)空間軌跡規(guī)劃中，機(jī)器人關(guān)節(jié)空間及操作空間運(yùn)行速度均連續(xù)，機(jī)器人運(yùn)動平穩(wěn)，適合機(jī)器人PTP 軌跡規(guī)劃要求。