PUMA机器人大作业

1 坐标系建立：

坐标系可以简化为：

2 D-H参数表：

PUMA机器人的杆件参数:

d1 0.6604m，d2 0.14909m，d4 0.43307m，d6 0.05625m，a2 0.4318m，a3 0.02032m

3 正运动学推导

s i0ai 1 c i

s c c c s ds ii 1ii 1i 1ii 1 由式i 1Ti 可得： s is i 1c is i 1c i 1dic i 1

0001

c1 s1 sc

10

T1 1

00

00 c4 03

T4

s4 0

s40 c40

0010

0 c2

00 1 T2 s20

1 ， 0

s2

0 c20 s50c50

00 c3

s1d2 2 T3 3 000

01 ， 00 100

0 c6

00 5T6 s60

1 ， 0

s3

c300 s60 c600a2 00 10

01 01000 0 0 1

0a3 c5

01d4 4T5 s500

01 ， 0

T6 0T11T22T33T44T55T6

机械手变换矩阵

nx n0

T6 y

nz 0

oxoyoz0axayaz0

px py pz 1

nx c23(c6c5c4c1 s6s4c1) s23c6s5c1 c6c5s4s1 s6c4s1ny c23(c6c5c4s1 s6s4s1) s23c6s5s1 c6c5s4c1 s6c4c1 nz s23(c6c5c4 s6s4) c23c6s5

ox c23(s6c5c4c1 c6s4c1) s23s6s5c1 s6c5s4s1 c6c4s1oy c23(s6c5c4s1 c6s4s1) s23s6s5s1 s6s4c5c1 c6c4c1 oz s23(s6c5c4 s6s4) c23c6s5ax c23s5c4c1 s23c5c1 s5s4s1ay c23s5c4s1 s23c5s1 s5s4c1 az c23c5 s23s5c4

px a3c23c1 a2c2c1 d4s23c1 d2s1py a3c23s1 a2c2s1 d4s23s1 d2c1pz d4c23 a3s23 a2s2

4 逆运动学推导

1．求 1

用逆变换0T1 1左乘方程0T6 0T11T22T33T44T55T6两边：

T1 10T6 1T22T33T44T55T6

c1s1 sc 11 00

00

0010

0 nx

n0 y0 nz 1 0

oxoyoz0axayaz0

px py 1T6 pz 1

得 s1px c1py d2

三角代换px cos ，py sin

式中， 得到 1的解

atan2(px,py)

1 atan2(py,px) atan2(d2,

2．求 3

矩阵方程两端的元素（1，4）和（2，4）分别对应相等

c1px s1py a3c23 a2c2 d4s23

p dc as asz42332322

平方和为：

d4s3 a3c3 k

其中k

2222222

px py pz d2 d4 a2 a3

2a2

解得： 3 atan2(a3,d4) atan2(k, 3．求 2

在矩阵方程0T6 0T11T22T33T44T55T6两边左乘逆变换0T3 1。

T3 10T6 3T44T55T6

c1c23

cs 123 s1 0

s1c23 s1s23c10 s23 c2300

a2c3 nx

na2s3 y

d2 nz

1 0

oxoyoz0axayaz0

px py 3T6 pz 1

方程两边的元素（1，4）和（3，4）分别对应相等，得

c1c23px s1c23py s23pz a3 a2c3 0

csp ssp cp as d 04 123x123y23z23

联立，得s23和c23

a2s3 d4 c1px s1py pz a2c3 a3 s23 2 pxc1 pys1 pz2

a2c3 a3 c1px s1py pz a2s3 d4 c23 22

pc ps p x1y1z

s23和c23表达式的分母相等，且为正，于是

23 2 3 atan2 a2s3 d4 c1px s1py pz a2c3 a3 , a2c3 a3 c1px s1py pz a2s3 d4

根据解 1和 3的四种可能组合，可以得到相应的四种可能值 23，于是可得到 2的四种可能解

2 23 3

式中 2取与 3相对应的值。 4．求 4

令两边元素（1，3）和（2，3）分别对应相等，则可得

c1c23ax s1c23ay s23az c4s5

sa ca ss1x1y45

只要s5 0，便可求出 4

4 atan2 s1ax c1ay,c1c23ax s1c23ay s23az

当s5 0时，机械手处于奇异形位。 5．求 5

T4 10T6 4T55T6

s1c4c23 c1s4

s4s1c23 c1c4

s1s23

s23c4s23s4 c230

c3c4a2 d2s4 c4a3 nx

nc3s4a2 d2c4 s4a3 y nzs3a2 d4

1 0

ox

oyoz0

axayaz0

px py 4T6 pz 1

c1c4c23 s1s4 scc sc 412314 c1s23

0

根据矩阵两边元素（1，3）和（2，3）分别对应相等，可得

azs23c4 ax c1c4c23 s1s4 ay s1c4c23 c1s4 s5

acs ass ac c x123y231z235

5 atan2 azs23c4 ax c1c4c23 s1s4 ay s1c4c23 c1s4 , axc1s23 ays23s1 azc23

6．求 6

T5 10T6 5T6

根据矩阵两边元素（2，1）和（1，1）分别对应相等，可得

nx c1s4c23 s1c4 ny s4s1c23 c1c4 nzs23s4 s6

nx c1c4c5c23 s1c5s4 c1s5s23 ny c4s1c5c23 s1s5s23 s4c1c5 nz s5c23 c4s5s23 c6

从而求得

6 atan2 s6,c6

5 Matlab编程得出工作空间

可以将连杆6的原点做为机器人的动点，连杆6原点相对于坐标系0就是0T6的px、py、pz，已知：

px a3c23c1 a2c2c1 d4s23c1 d2s1py a3c23s1 a2c2s1 d4s23s1 d2c1 pz d4c23 a3s23 a2s2

Matlab程序如下：

clc,clear

length2=431.8;length3=20.32; d2=149.09;d4=433.07; a=pi/180;

for a1=-160*a:20*a:160*a for a2=-225*a:20*a:45*a a3=-45*a:20*a:225*a for k=1:length(a3)

px(k)=cos(a1)*(length2*cos(a2)+length3*cos(a2+a3(k))-d4*sin(a2+a3(k)))-d2*sin(a1)

py(k)=sin(a1)*(length2*cos(a2)+length3*cos(a2+a3(k))-d4*sin(a2+a3(k)))+d2*cos(a1)

pz(k)=-a3(k)*sin(a2+a3(k))-length2*sin(a2)-d4*cos(a2+a3(k)) end

plot3(px,py,pz),title('机器人的工作空间'),xlabel('x mm'),ylabel('y mm'),zlabel('z mm') holdon gridon end end

机器人工作空间——三维空间：

-7-