PUMA560机器人运动学分析.pdf

1、 PUMA560 机器人运动学 论文 姓 名 : 学 号 :200800840058 专业及班级 :机械二班 山东大学威海分校 2012 年上学期工业机器人学作业论文 第 1 页 6/1/2012 作 者： 李安洲 目 录 摘 要 2 关键词 2 1.引言 . 2 2 PUMA560 机器人数学模型的建立 2 2.1.确定 D-H 坐标系幵获取 D-参数 2 2.2 建立运动学方程 3 3. 位姿 的正逆解 3 3.1 正 解 3 3.2 逆 解 3 3.2.1 求 1 ， 2 ， 3 . 3 3.2.2 求 4 ， 5 ， 6 4 4. PUMA560 雅克比矩阵 5 4.1 矢 量 积 法

2、 . 6 4.2 微分变换法 6 5 Matlab 编程对其正逆解和雅克比矩阵的求解 7 5.1 正解逆解 . 7 5.2 雅克比矩阵 . 7 6. 总结 . 7 参考文献 7 附件： 8 正解程序 8 逆解程序 8 雅克比矩阵的矢量法程序 . 10 雅克比矩阵的微分变换法程序 11 山东大学威海分校 2012 年上学期工业机器人学作业论文 第 2 页 6/1/2012 作 者： 李安洲 摘 要 ： 采 用 D-H 坐标系对 机器人 Puma560 建立 个关节的坐标系 幵 获取 D-H 参数， 幵 对其运动 建立 数学 模型用 matlab编程对其求 位姿 正逆解及雅克比矩阵， catia对

3、 Puma560建模三维模型。 关键词 ： Puma560 正逆解；雅克比矩阵； Matlab 1. 引 言 机器人运动学 包括正向运动学，即给定机器人各关节变量，计算机器人末端的位置姿态；逆向 运动学即已知机器人末端的位置姿态，计算机器人对应位姿的全部关节变量。一般正向运动学的解是唯一和容易获得的，而逆向运动学往往有多个解而且分 析更为复杂。机器人逆运动分析是运动规划不控制中的重要问题，但由于机器人逆运动问题的复杂性和多样性，无法建立通用的解析算法。机构逆运动学问题实际上是一个非线性超越方程组的求解问题，其中包括解的存在性、唯一性及求解的方法等一系复杂问题。 本文主要通过最基本分析方法对Pu

4、ma560 机器人的运动进行分析，包括运动的位置和姿态的正、逆解，及其运动的雅克比矩阵的求解。 2 PUMA560 机器人数学模型的建立 PUMA 机器人操作臂可以看作一个开式运动链，它由一系列连杆通过转动关节串联而成，关节的相对转动导致连杆的运动。为 了研究机器人各连杆的运动，建立如图 1 所示 坐标系 根据 Denavil 和 Hartenberg 提出的齐次变换矩阵法，建立机器人的运动学方程。 2.1.确定 D-H 坐标系并获取 D-参数 PUMA560 的 6 个关节 全为转动关节 : iZ 坐标轴 :沿着 i+1 关节的运动轴 ; iX 坐标轴 :沿着 iZ 和 -1iZ 的公法线

5、,指向离开 -1iZ 轴的方向 ; iY 坐标轴 :按右手直角坐标系法则制定 ; 连杆长度 ia ; iZ 和 -1iZ 两轴心线的公法线长度 ; 连杆扭角 ia : iZ 和 -1iZ 两轴心线的夹角 ; 两连杆距离 id : iX 和 -1iX 两坐标轴的公法线距离 ; 两杆夹角 i : iX 和 -1iX 两坐标轴的夹角 ; 1图 山东大学威海分校 2012 年上学期工业机器人学作业论文 第 3 页 6/1/2012 作 者： 李安洲 D-H 参数 表 连杆 i 变量 i -1i -1ai id 变量范围 1 1(90) 90 0 0 -160 160 2 2(0) 0 0 149.09

6、 225 45 3 3(-90) -90 431.8 0 -45 225 4 4(0) 0 20.32 433.07 -110 170 5 5(0) 0 0 0 -100 100 6 6(0) 0 0 0 -226 226 2.2 建立运动学方程 PUMA560 机器人具有六个自由度，而且六个关 节均为旋转关节，前三个关节主要影响末端执行器的位置，后三个关节决定末端执行器的姿态，将机器人位置结构和姿态结构 末端执行器的位置矢量和姿态转换矩阵 幵通过齐次变换得到： 两杆间的位姿矩阵 其中： - 1 - 1 - 1 - 1= s in , = c o s , = s in , c = c o si

7、 i i i i i i is c s 末端执行器 位姿矩阵 即 PUMA560 机器人的运动学方程 ： 3. 位姿的正逆解 3.1 正 解 已知各 关节的变量 i ，求末端执行器的位姿矩阵即正解 。把 i 带入（ 2.1）求得各两连杆间的位姿矩阵 0 1 2 3 4 51 2 3 4 5 6, , , , ,T T T T T T，再由（ 2.2） 即可求得末端执行器的位姿矩阵： 3.1 -11111 1 1 11 1 1 1( , ) ( , 0 , 0 ) ( , ) ( 0 , 0 , )00 0 0 1ii i i i ii i ii i i i i i ii i i i i i i

8、T R o t x Tra n s a R o t z Tra n s dc s as c c c s s ds s c s c c d 0 0 1 2 - 11 1 2 2 3 3( ) ( ) ( ) . . ( )ii i iT T T T T 0 0 1 2 3 4 56 1 1 2 2 3 3 4 4 5 5 6 6( ) ( ) ( ) ( ) ( ) ( )T T T T T T T 2.2 2.1山东大学威海分校 2012 年上学期工业机器人学作业论文 第 4 页 6/1/2012 作 者： 李安洲 3.2 逆 解 将 PUMA 560 的运动方程（ 3.1）写为 : 若末端连

9、杆的位姿已经给定 ,即 和 p 为已知 ,则求关节变量 的值称为运动反解 . 3.2.1 求 1 ， 2 ， 3 式中 ,正、负号对应于 1 的两个可能解 . 再令矩阵方程 (3.3)两端的元素 (1,4)和 (3,4)分别对应相等 ,则得两方程 : 式 (3.68)不式 (3.71)的平方和为 : 式中 ,正、负号对应于 3 的两个可能解 . 0 0 1 2 3 4 56 1 1 2 2 3 3 4 4 5 5 6 6( ) ( ) ( ) ( ) ( ) ( )0 0 0 1x x x xy y y yz z z zn o a pn o a pT T T T T T Tn o a p n,

10、o,a 1 2 6, , , 0 1 0 1 2 3 4 51 1 6 2 2 3 3 4 4 5 5 6 6T T T T T T T 1111 1600000 0 1 00 0 0 1 0 0 0 1x x xxy y y yz z z zc s n o pas c n o a p Tn o a p 21 2 1 222212 2 21 2 2sin( ) / ; c o s( ) 1 ( / )a ta n 2 , 1a ta n 2 ( , ) a ta n 2 ( ,y x x yddddp p d p p d 3.3 3.41 1 3 2 3 4 2 3 2 23 2 3 4 2

11、3 2 2xyzc p s p a c d s a cp a s d c a s 3.53 3 4 3a c d s k),(2a t a n),(2a t a n 22423433 kdakda 3.6 3.7 0 1 0 3 4 53 1 2 3 6 4 4 5 5 6 6,T T T T T 1 23 1 23 23 2 31 23 1 23 23 2 3 361 1 200 0 0 1 0 0 0 1x x xxy y y yz z z zc c s c s a c n o pac s s s c a s n o a p Ts c d n o a p 3.8 3.2山东大学威海分校 2

12、012 年上学期工业机器人学作业论文 第 5 页 6/1/2012 作 者： 李安洲 令矩阵方程 (3.8)两端的元素 (1,4)和 (2,4)分别对应相等 ,则得两方程 : 根据 1 和 3 解的四种可能组合可以得到相应的四种可能值 23 ,于是可得到 2 的四种可能解 . 3.2.2 求 4 ， 5 ， 6 设 0 -10 56 + 1 6= = .ii i iT T T T T 令矩阵方程 (3.8)两端的元素 (1,3)和 (3,3)分别对应相等 ,则得两方程 : 令矩阵方程 (3.12)两端的元素 (1,3)和 (3,3)分别对应相等 ,则得两方程 : 令矩阵方程 (3.13)两端的

13、元素 (3,1)和 (1,1)分别对应相等 可得到： 以上求得 PUMA560 的运动反解可能存在 8 种解 .但是 ,由于结构的限制 ,例如各 关节变量丌能在全部 360 度范围内运动 ,有些解丌能实现 .在机器人存在多种解的情况下 ,应选取其中最满意的一组解 ,以满足机器人的工作要求 . 4. PUMA560 雅克比矩阵 机器人雅可比矩阵 (简称雅可比 )揭示了操作空间不关节空间的映射关系。雅可比丌仅表示操作空间不关节空间的速度映射关系，也表示二者之间力的传递关系，为确定机器人的静态关节力矩以及丌同坐标系间速度、加速度和静力的变换提供了便捷的方法。 在机器人学中，雅可比是一个把关节速度向量

14、 变换为手爪相对基坐标的广义速度向量 v 的变换矩阵。 机械1 2 3 1 2 3 2 3 2 3 31 2 3 1 2 3 2 3 2 3 4x y zx y xc s p s c p s p a c ac s p s s p c p a s d 2 23 3 3.9 3.101 2 3 1 2 3 2 3 4 51 1 4 5x y zxya c c a s c a s c sa s a c s s 3.1154 1 3 354 2 3 30a ta n 2 ( ( 3 ,3 ) , ( 1 ,3 ) )0a ta n 2 ( - ( 3 ,3 ) ,- ( 1 ,3 ) )sTTsTT当

15、当 3.12 3.13 0 1 0 4 5 44 1 2 3 4 6 5 5 6 6 6 5 6, , , ,T T T T T 1 23 4 1 4 1 23 4 1 4 23 4 51 23 1 23 23 5x y zx y za c c c s s a s c c c s a s c sa c s a s s a c c 3.14 3.175 4 4a ta n 2 ( ( 1 ,3 ) , ( 3 ,3 ) )TT 3.18 0 1 0 55 1 2 3 4 5 6 6 6, , , ,T T T 6 5 5a ta n 2 ( ( 3 ,1 ) , ( 1 ,1 ) )TT 3.1

16、5 3.16 3.19山东大学威海分校 2012 年上学期工业机器人学作业论文 第 6 页 6/1/2012 作 者： 李安洲 手的操作速度不关节速度间 的线性变换定义为机械手的雅可比矩阵 . 4.1 矢 量 积 法 PUMA560 的 6 个关节都是转动关节 ,因而其雅克比矩阵具有下列形式 : 由 式（ 2.1） 求得各关节相对于基坐标系的位姿矩阵 即 0 0 0 0 0 01 2 3 4 5 6, , , , ,T T T T T T 获得相应的 iz 和 0ip 再由式（ 4.2）求得对应 00np 。 得到的数据带入式（ 4.1） 求得各关节 iJ ，最后带入式（ 4.3）求得其末端执

17、行器 相对基坐标系的雅克比矩阵 。 4.2 微分变换法 PUMA560 机器人 末端执行器的变换矩阵 根据 文献 7 的微分变换法，相对雅克比矩阵的第 i 列元素 ()T iJq 由 1i nT 决定，对于PUMA560 机器人来说， ()zzT zizzzpnpopaJqnoa 根据式（ 2.1）和（ 2.2）求得 则可求得 ()T iJq 雅克比矩阵 ()Jq 不相对雅克比矩阵 ()T iJq之间的关系 为 1000( ) ( )0 T TnTnRJ q J qR 式中 0TnR 是 0nR 的转置矩阵， 0nR 可通过式（ 4.4）求出 。 将 PUMA560 机器人的相对雅克比矩阵 (

18、)TJq 和 0TnR 带入式（ 4.6），即可得到 机器人雅克比矩阵 ()Jq 。 ( 3 .6 )i o i oi n i niiiiv z p z pqJw zz 1 2 3 4 5 6, , , , ,J q J J J J J J 4.1 4.300-ion n ip p p 4.200 66 010 0 0 1x x x xy y y yz z z zn o a pn o a p RPTn o a p 4.4 4.51 4 56 6 6, , ,T T T 4.6山东大学威海分校 2012 年上学期工业机器人学作业论文 第 7 页 6/1/2012 作 者： 李安洲 5.Matla

19、b 编程对其正逆解和雅克比矩阵的求解 利用 Matlab 软件根据以上的数学模型对其求解 ，以下均是以正解 给的 数据 和结果 对其 求解的结果。 5.1 正解逆解 初始数据 （六个关节的 i ） 1 2 3 4 5 6 20 20 20 20 20 20 运行 matlab 程序求得 末端执行器位姿矩阵为： o a p 0.5322 -0.1137 -0.8390 0.0833 -0.4697 -0.8641 -0.1809 0.1890 -0.7044 0.4903 -0.5133 -0.4925 0 0 0 1.0000 利用以上求得的 末端执行器位姿矩阵 数据 运行 matlab 程序

20、求得 其八个解为： 解 1 2 3 4 5 6 1 -67.5924 -200.0000 165.3728 116.2894 -70.3888 178.4284 2 -67.5924 -200.0000 165.3728 -63.7106 70.3888 -1.5716 3 -67.5924 52.3721 20.0000 62.3243 -72.4909 -65.6051 4 -67.5924 52.3721 20.0000 -117.6757 72.4909 114.3949 5 20.0000 -232.3721 165.3728 -171.7287 -125.5969 -136.281

21、5 6 20.0000 -232.3721 165.3728 8.2713 125.5969 43.7185 7 20.0000 20.0000 20.0000 -160.0000 -20.0000 -160.0000 8 20.0000 20.0000 20.0000 20.0000 20.0000 20.0000 5.2 雅克 比矩阵 用正解中的数据运行程序结果为： -0.1890 -0.4628 -0.3240 0 0 0 0.0833 -0.1684 -0.1179 0 0 0 0.0000 -0.1430 0.2628 0 0 0 0 -0.3420 -0.3420 -0.6040

22、-0.0752 -0.8390 0.0000 0.9397 0.9397 -0.2198 0.9726 -0.1809 1.0000 0.0000 0.0000 -0.7660 -0.2198 -0.5133 6. 总结 本文介绍了对 PUMA560 机器人运动学的 正问题和逆问题的，及雅克比矩阵的基本数学模型和算法 ， 对于 机器人的动力学没有加以分析 ， 对 于机器人的分析丌仅这些， 轨迹优化 机器人的控制，程序设计 等 ，随着科技的发展机器人的 为了满足人们的要求， 机器人 发展扩展 越来越 广，对其的研究也越来越复杂。 n山东大学威海分校 2012 年上学期工业机器人学作业论文 第 8

23、 页 6/1/2012 作 者： 李安洲 参考文献 1 蔡自兴 . 机器人学基础 . 北京：机械工业出版社 2009.5 2 蔡自兴 . 机器人学 第二版清华大学出版社 3熊有伦 机器人学 北京：机械工业出版社， 1993.87.94 4 葛哲学 精通 MATLAB 北京：电子工业出版社 2008 2 附件 ： 正解程序 %Puma560 位姿正解 function T=Positive(angle) a1=0 -pi/2 0 -pi/2 pi/2 -pi/2; l=0 0 0.4318 0.02032 0 0; d=0 0.14909 0 0.43307 0 0; A=zeros(4); T

24、=eye(4); A(4,4)=1; for i=1:6 c=cos(angle(i)/180*pi); s=sin(angle(i)/180*pi); ca=cos(a1(i); sa=sin(a1(i); A(1,1)=c; A(1,2)=-s; A(1,4)=l(i); A(2,1)=s*ca;A(2,2)=c*ca;A(2,3)=-sa;A(2,4)=-sa*d(i); A(3,1)=s*sa; A(3,2)=c*sa; A(3,3)=ca; A(3,4)=ca*d(i); T=T*A; end 逆解程序 %Puma560 逆解 function angle=nijie(T) d2=0

25、.14909; d4=0.43307; a2=0.4318; a3=0.02032; nx=T(1,1); ny=T(2,1); nz=T(3,1); 山东大学威海分校 2012 年上学期工业机器人学作业论文 第 9 页 6/1/2012 作 者： 李安洲 ox=T(1,2); oy=T(2,2); oz=T(3,2); ax=T(1,3); ay=T(2,3); az=T(3,3); px=T(1,4); py=T(2,4); pz=T(3,4); angle=; for m=1:2 %angle1 有两个解 angle1=atan2(py,px)-atan2(d2,(-1)m)*sqrt(

26、px2+py2-d22); for n=1:2 %angle3 有两种可能 k=(px2+py2+pz2-a22-a32-d22-d42)/(2*a2); angle3=atan2(a3,d4)-atan2(k,(-1)n*sqrt(a32+d42-k2); c1=cos(angle1); s1=sin(angle1); c3=cos(angle3); s3=sin(angle3); angle23=atan2(-(a3+a2*c3)*pz+(c1*px+s1*py)*(a2*s3-d4),(-d4+a2*s3)*pz+(c1*px+s1*py)*(a2*c3+a3); angle2=angl

27、e23-angle3; c2=cos(angle2); s2=sin(angle2); for t=1:2 %angle4 有两种情况 A1=c1 -s1 0 0;s1 c1 0 0;0 0 1 0;0 0 0 1; A2=c2 -s2 0 0;0 0 1 d2;-s2 -c2 0 0;0 0 0 1; A3=c3 -s3 0 a2;s3 c3 0 0;0 0 1 0;0 0 0 1; T3=A1*A2*A3; u=inv(T3)*T; angle4=atan2(-1)t*u(3,3),(-1)(t+1)*u(1,3); s4=sin(angle4); c4=cos(angle4); A4=c

28、4 -s4 0 a3 ;0 0 1 d4;-s4 -c4 0 0;0 0 0 1;%求 angle5 T4=T3*A4; u1=inv(T4)*T; angle5=atan2(-u1(1,3),u1(3,3); s5=sin(angle5); 山东大学威海分校 2012 年上学期工业机器人学作业论文 第 10 页 6/1/2012 作 者： 李安洲 c5=cos(angle5); A5=c5 -s5 0 0;0 0 -1 0;s5 c5 0 0;0 0 0 1;%求 angle6 T5=T4*A5; u2=inv(T5)*T; angle6=-atan2(u2(3,1),u2(1,1); Ro

29、w=angle1,angle2,angle3,angle4,angle5,angle6*180/pi; angle=angle;Row; end end end 雅克比矩阵的矢量法程序 %雅克比矩阵矢量积法求解 function Ja=vep(angle) a1=0 -pi/2 0 -pi/2 pi/2 -pi/2; l=0 0 0.4318 0.02032 0 0; d=0 0.14909 0 0.43307 0 0; A=zeros(4); T=eye(4); A(4,4)=1; for k=1:6 c=cos(angle(k)/180*pi); s=sin(angle(k)/180*pi

30、); ca=cos(a1(k); sa=sin(a1(k); A(1,1)=c; A(1,2)=-s; A(1,4)=l(k); A(2,1)=s*ca;A(2,2)=c*ca;A(2,3)=-sa;A(2,4)=-sa*d(k); A(3,1)=s*sa; A(3,2)=c*sa; A(3,3)=ca; A(3,4)=ca*d(k); T=T*A; Tn(:,:,k)=T; end for i=1:6 z=Tn(1:3,3,i); p6=Tn(1:3,4,6)-Tn(1:3,4,i); Ja(1:6,i)=cross(z,p6);z; end 山东大学威海分校 2012 年上学期工业机器人学

31、作业论文 第 11 页 6/1/2012 作 者： 李安洲 雅克比矩阵的微分变换法程序 %雅克比矩阵的微分变换法求解 function Ja=did(angle) a1=0 -pi/2 0 -pi/2 pi/2 -pi/2; l=0 0 0.4318 0.02032 0 0; d=0 0.14909 0 0.43307 0 0; A=zeros(4); T=eye(4); A(4,4)=1; for i=1:6 k=7-i; c=cos(angle(k)/180*pi); s=sin(angle(k)/180*pi); ca=cos(a1(k); sa=sin(a1(k); A(1,1)=c;

32、 A(1,2)=-s; A(1,4)=l(k); A(2,1)=s*ca;A(2,2)=c*ca;A(2,3)=-sa;A(2,4)=-sa*d(k); A(3,1)=s*sa; A(3,2)=c*sa; A(3,3)=ca; A(3,4)=ca*d(k); T=A*T; Tn(:,:,k)=T; end for (j=2:6) n=Tn(1:3,1,j); o=Tn(1:3,2,j); a=Tn(1:3,3,j); p=Tn(1:3,4,j); z1=cross(p,n); z2=cross(p,o); z3=cross(p,a); J(1:6,j-1)=z1(3,1);z2(3,1);z3(3,1);n(3,1);o(3,1);a(3,1); end J(1:6,6)=0;0;0;0;0;1; R60=Tn(1:3,1:3,1); Ja=R60,zeros(3);zeros(3),R60*J;