当前位置：文档库 › 机器人技术大作业

机器人技术大作业

大作业：PUMA 机器人

1. 坐标系建立

利用D-H 参数法建立坐标系：

2．D-H 参数表

关节

i θ(?) i α(?) i l i d

运动范围

1 90 -90 0 0 -160°~160°

2 0 0 a 2 d 2

-225°~45° 3 90 90 -a 3 0 -45°~225° 4 0 -90 0 4d

-110°~170° 5 0 90 0 0

-100°~100° 6

-266°~266°

3. 正运动学推导 3.1变换矩阵求取

i-1i 11Rot(,) Trans(0,0,d ) Trans(l ,0,0) Rot(,) = 00

1i i i i i i i i i i i i i i i i i i i i i i c s c s s l c c c c c s l s A z x s c d θθαθαθθ

θαθαθθααα----????-?

?=??????

列各D-H 变换矩阵如下：

110101010010000

01-?????

?=??-?

???c s s c A 2212222

02220200

001c s a c s c a s A d -??????=??

???? 33233

3303301000001c s a c s c a s A -??

??--??=??

344404040400100001-??????=??-????c s s c A d 45

0505001000001c s s c A ????-??=??

?? 5666-6

066000010

001c s s c A d ??

????=??

???? 注：为书写方面，本文中cos ,sin i i ci si θθ==

又由00123456123456T A A A A A A =?????，利用Matlab 进行符号运算，运行程序PUMAzhengyundongxue （程序详见附录）得：

???????

???=10

0060

pz az sz nz py ay sy ny px ax

sx nx T

其中

- c6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - s6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3))s6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3)) - c6*(s5*(c2*s1*s3 + c3*s1*s2) nx ny == - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3)))s4*s6*(c2*s3 + c3*s2) - c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2))nz ???

???

?=??

s6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - c6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3))s6*(s5*(c2*s1*s3 + c3*s1*s2) - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))) sx sy ==+ c6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3))s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) + c6*s4*(c2*s3 + c3*s2)sz ???

???

?=??

c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) c5*(c2*s1*s3 + c3*s1*s2) + s5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)ax ay az =??

=??=?

24623346- d *s1+d *(c1*c2*s3 + c1*c3*s2) - d *s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) + a *c1*c2 + c5*d6*(c1*c2*s3 + c1*c3*s2) +a *c1*s2*s3 -a *c1*c2*c3

c1*d2 + d *(c2*s1*s3 + c3*s1*s2) + d *s5*(c px py ==263342631*s4 + c4*(c2*c3*s1 - s1*s2*s3)) + a *c2*s1 + c5*d *(c2*s1*s3 + c3*s1*s2) +a *s1*s2*s3 - a *c2*c3*s1d *(c2*c3 - s2*s3) - a *s2 + d *(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)) + a pz =3*c2*s3 +a *c3*s2

???

????

???? 3.2变换矩阵验证 3.2.1初步验证

根据DH 参数表中θi 的初始值，将其带入

T 后得到末端执行器的坐标为

px= -d 2

py=a 2+d 4+d 6 pz=a 3

与初始设定一致，因此初步判断变换矩阵正确。

3.2.2使用matlab 机器人工具箱详细验证

9090-a3=-0.02

032

0-45°

~225°40-900d4=0.43

307

-110°

~170°509000-100°

~100°6000d6=0.05

625

-266°

~266°

利用matlab机器人工具箱按照以上DH参数，编写并运行程序PUMAyanzheng（程序详见附录）得到如下图，与PUMA560机器人一致，表明DH参数法建模正确。

4. 逆运动学推导

反变换法求逆0012345

6123456

T A A A A A A

=?????，且有

（1）求

由01012345

1623456

161

=?=????=

B A T A A A A A T得

11111111111111110001''''''''''''0001nx*c ny*s sx*c sy*s ax*c ay*s px*c py*s -nz -sz -az -pz ny*c - nx*s sy*c - sx*s ay*c - ax*s py*c - px*s nx sx ax px ny sy ay py nz sz az pz ++++??

???

?????

??=???

4*6*(2*32*3)6*(5*(2*33*2)4*5*(2*32*3))

6*(5*(2*32*3)4*5*('''c4*s6 2*33*2))4*6*(2*33*+ c5*c6*s42)--+---++=??

=?++?=-?

s s c c s s c s c s c s c c c c s s c s c c s s c c c s c s s s c s c s nx ny nz

's6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - c6*s4*(c2*c3 - s2*s3)'s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2)'c4*c6 + c5*s4*s6=-??

=??=?

sx sy sz 'c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3)'c4*s5*(c2*s3 + c3*s2) - c5*(c2*c3 - s2*s3)'s4*s5ax ay az =??

=??=?

24633246px'=a *c2 + d *(c2*s3 + c3*s2) - d *(c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3)) - a *c2*c3 + a *s2*s3

'a *s2 - d *(c2*c3 - s2*s3) + d *(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2))

py =3326- a *c2*s3 - a *c3*s2'd - d *s4*s5pz ???

???

??=???

由矩阵对应元素均相等，1(3,3)T61(3,3)B =，1(3,4)T61(3,4)B =得

2611s4*s5

11d - d *s4*s5ay*c - ax*s py*c - px*s =??

得到：

()261111-=-py*c px*s d d *ay*c - ax*s

整理有

66211+-+=c *(py d *ay)(px d *ax)*s d

三角变换

66sin cos μμ+=??

+=?px d *ax r py d *ay r

其中

66*arctan *μ?=??+=?

r px d ax ()py d ax 得到

21arccos μ=±-d θ(

（2）求3θ

由21110103453216456B3=63A A A T A A A T ---==得

66461112

1314 c4*c5*c6 + s4*s6- c6*s4 + c4*c5*s6c4*s5-d *c4*s521222324-c4*s6 + c5*c6*s4c4*c6 + c5*s4*s6s4*s5-d *s4*s531323334-c6*s5-s5*s6c5d d *c541

4243440001b b b b b b b b b b b b b b b b ????

????????=????

-????

????

其中B3个元素如下：

32111c2323123121232312313123231231412323123*3

b ny*s * - nz*s +

c *nx*c b sx*c *c - sz*s + sy*s *c b ax*c *c - az*s + ay*s *c b py*s *c - pz*s + c *px*c a a c =??=?

=??=+-? 2

b21= ny*c1 - nx*s1b22= sy*c1 -sx *s1b23= ay*c1 - ax*s1b24= py*c1 - px*s1-d ????

???

23123123123322312312333231231233423123123*3

b nz*

c + ny*s *s + nx*c *s b sz*c + sx*c *s + sy*s *s b az*c + ax*c *s + ay*s *s b pz*c + py*s *s + px*c *s a s =??=??=??=-? 410420430441

b b b b =??=??

=??=?

注：为书写方便，本文中232323cos(),23sin()c s θθθθ=+=+。

由矩阵元素对应相等，63(1,4)*3(1,3)B d B =-，463(3,4)*3(3,3)B d d B =- 得

[][]36624662a (pz + az*d )*s23 + d *(ax*c1 + ay*s1) + px*c1 + py*s1*c23 = a *c3

-d + d *(ax*c1 + ay*s1) + px*c1 + py*s1*s23+(pz + az*d )*c23 = a *s3

?-??

?? 两式平方求和，整理得到：

4334(2**2**)*23(2**2**)*23G d E a F c a E d F s -+=+

其中

622223422

*F=*(*c1*E 1)*1*1G=pz az d d ax ay s px c py s E F a d a ?+?

+++??+=+-+?，进行三角变换

43342**2**2**2*sin co *s d E a F a E d F ρ?ρ?-==+??

其中

4334ar 2**2**2**2*ctan *d ()E a F a E d F ρ?-?=+=???

从而得到

23arctan G θ(θ?

+=±-

由63(1,4)*3(1,3)B d B =-，463(3,4)*3(3,3)B d d B =-得

3242

a E*s23 + F*c23 = a *c3

-d + F*s23+E*c23 = a *s3-??

? 解得

433-d + F*s23+E*c23arctan a E*s23 + F*c23

(

)

θ=-

（3）求2θ

(

)422333-d + F*s23+E*c23arctan G arctan a E*s23 + F*c23

((

)

θθθθ?=+-=±---

（4）求5θ

由3(3,3)T6(3,3)B =得

523123123c az*c + ax*c *s + ay*s *s =

解得

5arccos(23123123)az*c + ax*c *s + ay*s *s θ=±

（5）求4θ

由3(1,4)T63(1,4)B =，3(2,4)T63(2,4)B =得

63262

-d *c4*s512323123*3

-d *s4*s511py*s *c - pz*s + c *px*c a a c c *py - px*s d =+-??

=-? s50≠时，解得

432*1*1arctan *1*23*23*1*23*3

py c px s d (

)py s c pz s px c c a a c θ--=-++-

（6）求6θ

由3(3,1)T63(3,1)B =，3(3,2)T63(3,2)B =得

-c6*s5*23*1*23*1*23

-s5*s6*23*1*23*1*23sz c sx c S sy s s nz c ny s s nx c c =++??

=++?

s50≠时，解得

6*23*1*23*1*23arctan *23*1*23*1*23

sz c sx c S sy s s (

)

nz c ny s s nx c c θ++=++

5. 工作空间求取

末端操作手的坐标为(,,)px py pz ，根据正运动学推导结果，用Matlab 编程求取其工作空间，根据参考文献获得PUMA560机器人各关节长度为：

22346149.09,431.8,20.32,433.07,56.25d mm a mm a mm d mm d mm =====

编写并运行程序PUMAgongzuokongjian （程序详见附录），得到：

变换角度:

俯视：

综上，PUMA560运动的逆解可能存在8种。但是，由于结构的限制，例如各关节变量不能在全部360°范围内运动，有着特定的范围，有些解不能实现。这种多解情况下，应选取其中最满意的一组解，以满足机器人的工作要求。

附录：

（1）PUMAzhengyundongxue程序

clear

syms a2 a3 d2 d4 d6 c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6;

A1=[c1 0 -s1 0;s1 0 c1 0;0 -1 0 0;0 0 0 1];

A2=[c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 d2;0 0 0 1];

A3=[c3 0 s3 -a3*c3;s3 0 -c3 -a3*s3;0 1 0 0;0 0 0 1];

A4=[c4 0 -s4 0;s4 0 c4 0;0 -1 0 d4;0 0 0 1];

A5=[c5 0 s5 0;s5 0 -c5 0;0 1 0 0;0 0 0 1];

A6=[c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1];

T60=A1*A2*A3*A4*A5*A6

T60=subs(T60,{c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6},{0 1 0 1 1 1 1 0 1 0 0 0})

（2）PUMAyanzheng程序

clear

L1=Link([-pi/2 0 0 0 0],'standard')

L2=Link([0 0.4318 0 0.14909 0],'standard')

L3=Link([pi/2 -0.02032 pi/2 0 0],'standard')

L4=Link([-pi/2 0 0 0.43307 0],'standard')

L5=Link([pi/2 0 0 0 0],'standard')

L6=Link([0 0 0 0.05625 0],'standard')

r=robot({L1 L2 L3 L4 L5 L6 })

drivebot(r)

（3）PUMAgongzuokongjian程序

clear

a2=431.8;a3=20.32;d2=149.09;d4=433.07;d6=56.25;

c1=-0.5*pi;c3=0.5*pi;c4=-0.5*pi;c5=0.5*pi;

for b1=-160.*pi/180:32.*pi/180:160.*pi/180

for b2=-225.*pi/180:54.*pi/180:45.*pi/180

for b3=-45.*pi/180:54.*pi/180:225.*pi/180

for b4=-110.*pi/180:140.*pi/180:170.*pi/180

for b5=-100.*pi/180:100.*pi/180:100.*pi/180

x=d4*(cos(b3)*sin(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1)) +...

sin(b3)*sin(c3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2)) + cos(c3)*sin(b1)*sin(c1)) + ...

d6*(cos(c5)*(cos(c4)*(cos(b3)*sin(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1)) +

sin(b3)*sin(c3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2)) + ...

cos(c3)*sin(b1)*sin(c1)) + cos(b4)*sin(c4)*(cos(b3)*cos(c3)*(cos(b1)*sin(b2) +

cos(b2)*cos(c1)*sin(b1)) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) - ...

cos(c1)*sin(b1)*sin(b2)) - sin(b1)*sin(c1)*sin(c3)) + sin(b4)*sin(c4)*(cos(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2)) - sin(b3)*(cos(b1)*sin(b2) + ...

cos(b2)*cos(c1)*sin(b1)))) + sin(b5)*sin(c5)*(cos(b4)*(cos(b3)*(cos(b1)*cos(b2) -

cos(c1)*sin(b1)*sin(b2)) - sin(b3)*(cos(b1)*sin(b2) + ...

cos(b2)*cos(c1)*sin(b1))) - sin(b4)*(cos(b3)*cos(c3)*(cos(b1)*sin(b2) +

cos(b2)*cos(c1)*sin(b1)) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2)) - ...

sin(b1)*sin(c1)*sin(c3))) + cos(b5)*sin(c5)*(cos(b4)*cos(c4)*(cos(b3)*cos(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1)) + cos(c3)*sin(b3)*(cos(b1)*cos(b2) - ...

cos(c1)*sin(b1)*sin(b2)) - sin(b1)*sin(c1)*sin(c3)) - sin(c4)*(cos(b3)*sin(c3)*(cos(b1)*sin(b2) + cos(b2)*cos(c1)*sin(b1)) + sin(b3)*sin(c3)*(cos(b1)*cos(b2) - ...

cos(c1)*sin(b1)*sin(b2)) + cos(c3)*sin(b1)*sin(c1)) +

cos(c4)*sin(b4)*(cos(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2)) - sin(b3)*(cos(b1)*sin(b2) + ...

cos(b2)*cos(c1)*sin(b1))))) + a2*cos(b1)*cos(b2) + d2*sin(b1)*sin(c1) -

a3*cos(b3)*(cos(b1)*cos(b2) - cos(c1)*sin(b1)*sin(b2)) + a3*sin(b3)*(cos(b1)*sin(b2) + ...

cos(b2)*cos(c1)*sin(b1)) - a2*cos(c1)*sin(b1)*sin(b2)

y=d4*(cos(b3)*sin(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1)) +

sin(b3)*sin(c3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)) - cos(b1)*cos(c3)*sin(c1)) + ...

d6*(cos(c5)*(cos(c4)*(cos(b3)*sin(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1)) +

sin(b3)*sin(c3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)) - ...

cos(b1)*cos(c3)*sin(c1)) + cos(b4)*sin(c4)*(cos(b3)*cos(c3)*(sin(b1)*sin(b2) -

cos(b1)*cos(b2)*cos(c1)) + cos(c3)*sin(b3)*(cos(b2)*sin(b1) + ...

cos(b1)*cos(c1)*sin(b2)) + cos(b1)*sin(c1)*sin(c3)) +

sin(b4)*sin(c4)*(cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)) - sin(b3)*(sin(b1)*sin(b2) - ...

cos(b1)*cos(b2)*cos(c1)))) +

cos(b5)*sin(c5)*(cos(b4)*cos(c4)*(cos(b3)*cos(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1)) +

cos(c3)*sin(b3)*(cos(b2)*sin(b1) + ...

cos(b1)*cos(c1)*sin(b2)) + cos(b1)*sin(c1)*sin(c3)) - sin(c4)*(cos(b3)*sin(c3)*(sin(b1)*sin(b2) - cos(b1)*cos(b2)*cos(c1)) + sin(b3)*sin(c3)*(cos(b2)*sin(b1) + ...

cos(b1)*cos(c1)*sin(b2)) - cos(b1)*cos(c3)*sin(c1)) +

cos(c4)*sin(b4)*(cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)) - sin(b3)*(sin(b1)*sin(b2) - ...

cos(b1)*cos(b2)*cos(c1)))) + sin(b5)*sin(c5)*(cos(b4)*(cos(b3)*(cos(b2)*sin(b1) +

cos(b1)*cos(c1)*sin(b2)) - sin(b3)*(sin(b1)*sin(b2) - ...

cos(b1)*cos(b2)*cos(c1))) - sin(b4)*(cos(b3)*cos(c3)*(sin(b1)*sin(b2) -

cos(b1)*cos(b2)*cos(c1)) + cos(c3)*sin(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)) + ...

cos(b1)*sin(c1)*sin(c3)))) + a2*cos(b2)*sin(b1) - d2*cos(b1)*sin(c1) -

a3*cos(b3)*(cos(b2)*sin(b1) + cos(b1)*cos(c1)*sin(b2)) + a3*sin(b3)*(sin(b1)*sin(b2) - ...

cos(b1)*cos(b2)*cos(c1)) + a2*cos(b1)*cos(c1)*sin(b2)

z=d4*(cos(c1)*cos(c3) - cos(b2)*cos(b3)*sin(c1)*sin(c3) + sin(b2)*sin(b3)*sin(c1)*sin(c3)) + d2*cos(c1) + d6*(cos(c5)*(cos(c4)*(cos(c1)*cos(c3) - ...

cos(b2)*cos(b3)*sin(c1)*sin(c3) + sin(b2)*sin(b3)*sin(c1)*sin(c3)) +

sin(b4)*sin(c4)*(cos(b2)*sin(b3)*sin(c1) + cos(b3)*sin(b2)*sin(c1)) - ...

cos(b4)*sin(c4)*(cos(c1)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(c1) -

cos(c3)*sin(b2)*sin(b3)*sin(c1))) + sin(b5)*sin(c5)*(cos(b4)*(cos(b2)*sin(b3)*sin(c1) + ...

cos(b3)*sin(b2)*sin(c1)) + sin(b4)*(cos(c1)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(c1) -

cos(c3)*sin(b2)*sin(b3)*sin(c1))) - cos(b5)*sin(c5)*(sin(c4)*(cos(c1)*cos(c3) - ...

cos(b2)*cos(b3)*sin(c1)*sin(c3) + sin(b2)*sin(b3)*sin(c1)*sin(c3)) +

cos(b4)*cos(c4)*(cos(c1)*sin(c3) + cos(b2)*cos(b3)*cos(c3)*sin(c1) - cos(c3)*sin(b2)*sin(b3)*sin(c1)) - ...

cos(c4)*sin(b4)*(cos(b2)*sin(b3)*sin(c1) + cos(b3)*sin(b2)*sin(c1)))) + a2*sin(b2)*sin(c1) - a3*cos(b2)*sin(b3)*sin(c1) - a3*cos(b3)*sin(b2)*sin(c1)

plot3(x,y,z,'*')

hold on

grid on

title('PUMA?ú?÷è?μ?1¤×÷????')

xlabel('x');

ylabel('y');

zlabel('z');

end

（4）PUMA560niyundongxue

clear

syms theta1 theta2 theta3 theta4 theta5 theta6 c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6 a2 a3 d2 d4 d6 nx ny nz sx sy sz ax ay az px py pz;

c1=cos(theta1);

c2=cos(theta2);

c3=cos(theta3);

c4=cos(theta4);

c5=cos(theta5);

c6=cos(theta6);

s1=sin(theta1);

s2=sin(theta2);

s3=sin(theta3);

s4=sin(theta4);

s5=sin(theta5);

s6=sin(theta6);

A1=[c1 0 -s1 0;s1 0 c1 0;0 -1 0 0;0 0 0 1];

A2=[c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 d2;0 0 0 1];

A3=[c3 0 s3 -a3*c3;s3 0 -c3 -a3*s3;0 1 0 0;0 0 0 1];

A4=[c4 0 -s4 0;s4 0 c4 0;0 -1 0 d4;0 0 0 1];

A5=[c5 0 s5 0;s5 0 -c5 0;0 1 0 0;0 0 0 1];

A6=[c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1];

T60=[nx sx ax px;ny sy ay py;nz sz az pz;0 0 0 1];

A11=inv(A1);

A22=inv(A2);

A33=inv(A3);

A44=inv(A4);

A55=inv(A5);

A6=inv(A6);

B1=simple(A11*T60)

T61=simple(A2*A3*A4*A5*A6)

B2=simple(A22*A11*T60)

T62=simple(A3*A4*A5*A6)

B3=simple(A33*A22*A11*T60)

T63=simple(A4*A5*A6)

f=simple(B3(1,4)+d6*B3(1,3));

f=collect(f,[cos(theta2 + theta3),sin(theta2 + theta3)]) g=simple(B3(3,4)+d6*B3(3,3)-d4);

g=collect(g,[cos(theta2 + theta3),sin(theta2 + theta3)]) h=(f+a2*c3)^2+(g+a2*s3)^2-a2^2;

h=collect(h,[cos(theta2 + theta3),sin(theta2 + theta3)]) h=simple(h)