工业机器人课程设计23655

1、SHANDJiNG UNnERnr 人丁 WTLIHAI 匸业机器人课程设计 基于Matlab的工业机器人运动学和雅克比较矩阵求解 精选文库 23 PUMA560机器人简介 PUMA560机器人的正解 1、确定D-H坐标系 2、确定各连杆 D-H参数和关节变量 3、求岀两杆间的位姿矩阵 4、求末杆的位姿矩阵 5、MATLAB 编程. 6、验证 PUMA560机器人的逆解 1、求 2、求 3、求 4、求 5、求 6、求 7、解的多重性. 8、MATLAB 编程 9、对于机器人解的分析 机器人的雅克比矩阵 1、定义 2、雅可比矩阵的求法 3、微分变换法求机器人的雅可比矩阵 4、矢量积法求机器人的雅

2、克比矩阵 5、MATLAB 编程 附录 1、程序. 2、三维图 目录 10 10 11 11 11 12 12 12 12 13 14 15 16 16 24 摘要 机器人学作为一门高度交叉的前沿学科，引起许多具有不同专业背景人们的 广泛兴趣，对其进行深入研究，并使其获得快速发展。尤其是近年来各种新兴技 术飞速发展，机械工业产品的自动化、高精度、重负载等性能指标变得越来越突 出。因此在机器人学的计算中就要求更高的精度， 计算机技术的发展很好的解决 了这一问题。本文将以PUMA560为例，利用个人电脑平台的 Matlab对其运动 学的正解、逆解以及雅克比矩阵进行计算研究。 关键词 PUMA560

3、 Matlab正解逆解雅克比矩阵微分变换法矢量积法 ABSTRACT As a highly in ters persed subject, the robotics makes many people who major in differe nt subject in terest in it, research and devel op it. Esp ecially in rece nt years, with the rap id devel opment of varieties of emerg ing tech no logies, mecha ni cal p roducts

4、 indexes of automation, high precision and the re-load are becoming more and more outstanding. There is a need of greater precision in the calculation of robotics and compu ter tech no logy makes it po ssible. In this paper we will use Matlab to research the kin ematics p roblem and Jacobia n array

5、of P UMA560. KEY WORDS P UMA560 Matlab Kin ematicsp roblemPositive-soluti onInv erse-soluti on Jacobia n arrayDiffere ntial tran sformati on Vector p roduct tran sformatio n PUMA560机器人简介 PUMA560是属于关节式机器人，6个关节都是转动关节，如图1 1所示, 前三个关节确定手腕参考点的位置，后三个关节确定手腕的方位。和大多数工业 4、 机器人一样，后三个关节轴线交于一点。该点选作为手腕参考点，也选作为 5、6

6、的原点。关节一的轴线为垂直方向，关节2和关节3的轴线为水平，且 平行，距离为a2。关节1和关节2的轴线垂直相交，关节3和关节4的轴线垂 屁: ra 直交错。距离为a3。各个 连杆坐标系如图11所 示, a2 a3 d2 d3 相应的连杆参数列于 1 2中。其中， 431.8mm 20.32mm 14909mm 433.07mm。 在更进一步了解 PUMA560 机器人的 转 动角度问题时，我们先来 Z0 山 定义一下 P UMA560机 器人的初始位姿。首先, 定义机器人的初始位置. 取大臂处于某一朝向时, 作为腰关节的初始位置. 大臂处在水平位置时，作 2 5 00 01 2 为肩关节的初始

7、位置.小 0 6兔- 连杆扭角a 两连杆距离 两杆夹角0 2、确定各连杆 D-H参数和关节变量 臂处在下垂位置，关节轴线Z4和Z0平行时，作为肘关节的初始位置.关节轴线Z5 和Z3平行时，作为腕扭转关节的初始位置.关节轴线Z6和Z4平行时，作为腕弯曲 关节的初始位置.抓手两个指尖的连线与大臂平行时，作为腕旋转关节的初始位置. 在上述初始位置的前下，各个关节的零点位置得到确定. PUMA560机器人的正解 1、确定D-H坐标系 PUMA 560的关节全为转动关节： Zi坐标轴:沿着i+1关节的运动轴； Xi坐标轴:沿着Zi和Zi-1的公法线，指向离开Zi-1轴的方向; Yi坐标轴:按右手直角坐标

8、系法则制定； 连杆长度ai; Zi和Zi-1两轴心线的公法线长度； i: Zi和Zi-1两轴心线的夹角； di: Xi和Xi-1两坐标轴的公法线距离； i :Xi和Xi-1两坐标轴的夹角； 连杆i 变量0 a-1 ai-1 di 变量范围 1 0 0 0 0 -160 160 2 02 -90 0 d2 -225 45 3 03 0 a2 0 -45 225 4 04 -90 a3 d4 -110 170 5 0 90 0 0 -100 100 6 0 -90 0 0 -266 266 确定各连杆 D-H参数和关节变量: 3、求出两杆间的位姿矩阵 第i连杆与第i-1连杆间的变换矩阵 Ai =R

9、ot(x, a-i )tra ns(a-i,0,0)Rot(z, 0)tra ns(0,0,d) 午3 0T6 0T6 c i =s ic i 1 s is i 1 0 c ic c is 0 相邻两个连杆间的位姿变换矩阵 0 0 1 0 c 1 s 1 0 0 1 i 1di i 1di 1 s 1 c 1 0 0 0 0 0 1 1T2 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 1 3T4 0 1 0 0 0 0 0 1 0 1 0 0 0 0 0 1 5T6 求末杆的位姿矩阵 0Ti（ 1）2（ 2）气（ 3）3T4（ 4）4T5（ 5）5T6（ 6） nx Ox ax

10、 0 1 0 0 0 0 0 1 ny nz 0 Oy Oz 0 ay az 0 Px Py Pz 1 由上面的矩阵, 我们可以得到最终结果: nx G 023 QGGsts3 S23S5C3 S| 846503 C4S6 ny S|023CqGGsts6 S23C6 C| S4C5C6 O4S6 nz S23 C4C5C3S4S5 C23S5C3 Ox C|C23C4C5S6S4C3 S23S5S3 Si C4C6 S4C5S6 Ci C4C6 S4C5C6 Oz S23 C4C5S6 S4C6 C23S5S5 ax q C23C4 S5 S23C5 SS4S5 ay s C23C4S5 G

11、S4S5 S1aC4C5S3S4C3 S23S5S3 Oy az S23C4S5 Px C1 82 d4S23 dbs Py S1 azQ a3C23 d4S23 dzG Pz a3S23 a2S2 Matlab编程 运行zhengjie.m,根据提示输入 的值，回车后的出T的解如下: T= 0 0 1.0000 0 1.0000 0 0 0 0 1.0000 0 0 -149.0900 864.8700 20.3200 1.0000 验证 a2 0T6 90o,2 431.8mm, 0, d2 a2 390o, 40, 149.09mm, a3 d2 d4 a3 20.32mm, d4433

12、.07mm, d656.25mm 经验证，编程所得结果与课本给出验证公 由课本给出的验证公式进行所编程序的验证， 式得到的结果一致。进一步表明所编程序是正确的。 PUMA560机器人的逆解 将PUMA 560的运动方程（3.64）写为: nx 0t6ny nz 0 Ox Oy Oz 0 ax ay az 0 Px Py0T1(1)1T2(2)2T3(3)3T4(4)4T5(5)(6) Pz 1 若末端连杆的位姿已经给定，求关节变量16的值成为运动逆解。 1、求1 0T11 1 0T6 1T2 2T3 3 3T44 4T55 5T66 C1 S1 0 0 S| C1 0 0 0 0 1 0 0

13、0 0 1 nx ny nz 0 Ox Oy Oz 0 ax ay az 0 Px Py6 Pz 1 sin( 1) d2 /; cos( atan2 込 1)/ (d2 / )2 2 atan2(Py, Px) atan2(d2, Jplp： d； 式中，正、 负号对应于1的两个可能解。 2、求3 GPxSPy a3C23d4S2332 Pza3S23 d4C23a2S2 由以上两式的平方加上-S1 Px G Py d2的平方可以得到: a3c3d4S5k (22) 2 2 2 2 在上式中，k Px Py Pz a2 af d； d： 2a2 式(22)中已经消去2，所以可以由三角代换求解

14、得到 3的解。 所以：在3的表达式中正、负号对应于3的两种可能解。 3 atan2(a3,d4)atan2(k, 屈 d2 k2) 3、求2 0T31 1, 2, 316汗4 4T55 166 C1C23 S1C23 523 a2C3 nx Ox axPx C1S23 S1S23 C23 a2S3 ny Oy ayPy 3 S, Ci 0 d2 nz Oz azPz 0 0 0 1 0 0 0 1 C4C5C6 S4S6 C4C5S6 S4S6 C4S5 a3 3T6 3T4 4T6 S5S6 S5S6 C5 d4 S4S5S6 C4S6 S4C5S6C4C6 S4S5 0 0 0 0 1 令

15、矩阵方程(2 3)两端的元素(1,4)和(2,4)分别对应相等 C1S23 Px S1C23 PyS23 Pz a2C3 a3 C1S23 P： XS1S23 Py C23Pz a2S3 d4 由以上两式可得 23的表达式: 16( 23) ，则得两方程: 2323 ata n2 3 azCO Pz (G PxSP y)(a2S3 dj (d4a2S3)p(GPx SPy)(a2C3 as) 由求得的 23，可求出 223 根据1和 3解的四种可能组合可以得到相应的 23四种可能值，于是可得到 的2四种可能解。 4、求4 ClC23 S1C23 S23 a2C3 nx Ox C1S23 S1S

16、23 Si 0 C1 0 C23 0 0 a2S3 d2 1 ny nz 0 Oy Oz 0 ax ay az 0 Px Py3T6 Pz 1 令上式的矩阵方程的两端的元素(1,4)和(2,4)分别对应相等，则得两方程: axC1C23ayS1C23az S23C4S5 axS1ayC1S4S5 当S 0 41 atan2( axSlayCl ,axC1C23ayS1C23azS23) 42 ata门2(比$ayC1 , axC1C23ayS1C23azS23) 当S5=0时,机械手处于奇异形位.此时,关节轴4和6重合，只能解出4和6的和或 差.奇异形位可以由式4的表达式中的atan2的两个变

17、量是否接近零来判别.若都接 近零,则为奇异形位，否则，不是奇异形位.在奇异形位时，可任意选取4值，再计算相 应的6值。 5、求5 根据求出的 4，可以进一步解出 0T411,2, 3, 4 0T6七 5 H 6 65, 6 因为1， 2， 3，4在前面均已解出，逆变换 0T4 1( 1, 2 ,3, C1C23C4 SS4 SIC23C4C1S4 S23C4 8203(4 d2S4 a3C4 C1C23S4 S1S4 S1C23 S4C1C4 S23S4 a2C3S4 d2C4 a3S4 C1S23 S1S23 C23 a2S3d4 0 0 0 1 4)为: 令矩阵方程两端的元素(1,3)和(

18、3,3)分别对应相等，则得两方程: ax C1C23C4S1S4ay S1C23C4C1S4az S23C4S5 axC1S23ayS1S23azC23C5 所以可以得到 5的最终表达式：5 ata门2(岂25) 6、求6 0T511, 2, 3, 4, 5T6 5T6 6 令矩阵方程两端的元素 (3,1)和(1,1)分别对应相等，则得两方程: SBnx(C1C23S4S1C4 ) C6nx C1C23C4S1S4 C5 nz(S23C4C5C23S5) ny(S1C23S4C1C4)n z(S23S4) C1S23S5n yKSSCq C1 s4 ) c5S1 s23s5 得到最后6的表达式

19、: 6 ata n2(S6,C6) 7、解的多重性 PUMA560的运动反解可能存在8种解，但是，由于结构的限制，例如各关 节变量不能在全部360度范围内运动，有些解不能实现。在机器人存在多种解的 情况下，应选取其中最满意的一组解，以满足机器人的工作要求。 8、Matlab 编程 在Matlab中运行nijie.m,根据提示输入nx,Ox,ax, ny,Oy,ay,nz,0z,az的值并回车，可得 i的8组解值如下: 90.0000 -2.6918 -84.6272 -180.0000 2.6810 180.0000 90.0000 -0.0000 -90.0000 0 0.0000 0 90

20、.0000 -2.6918 -84.6272 0.0000 -2.6810 -0.0000 90.0000 -0.0000 -90.0000 0 -0.0000 0 -70.4385 180.0000 -84.6272 104.7629 20.2581 74.3103 -70.4385 182.6918 -90.0000 97.5292 19.7387 82.0067 -70.4385 180.0000 -84.6272 -75.2371 -20.2581 -105.6897 -70.4385 182.6918 -90.0000 -82.4708 -19.7387 -97.9933 9、对于机

21、器人解的分析 通过编程可以知道，我们最终得到八组解。然后对八组解进行分析，对于1 的变化范围为从160160，所以程序中我们得到的两个解都是正确的。然后 对2进行分析，由于2的角度变化范围是从22545，所以在我们所得到的 结果中，后四组是超出2的变化范围的，所以我们可以舍去后四组解。再逐个对 3、4、5、6进行角度分析，最终可获得适合的解。 机器人的雅克比矩阵 1、定义 机械手的操作速度与关节速度间的线性变换定义为机械手的雅可比矩阵。 x X(q) 25 1、程序 1、运动学正解 附录 function zhe ngjie(c1,c2,c3,c4,c5,c6) cic; a2=431.8;

22、a3=20.32; d2=149.09; d4=433.07; d6=56.25; c1=i npu t(c1=); c2=i npu t(c2=); c3=i np ut(c3=); c4=inpu t(c4=); c5=i np ut(c5=); c6=i npu t(c6=); T1=cosd(c1) -si nd(c1) 0 0; sin d(c1),cosd(c1),0,0; 0,0,1,0; 0 0 0 1; T2=cosd(c2),-si nd(c2),0,0; 0,0,1 d2; -si nd(c2) -cosd(c2) 0 0; 0 0 0 1; T3=cosd(c3) -s

23、i nd(c3) 0 a2; sin d(c3) cosd(c3) 0 0; 精选文库 0 0 0 1； T4=cosd(c4) -si nd(c4) 0 a3 ; 0 0 1 d4; -sin d(c4) -cosd(c4) 0 0; 0 0 0 1; T5=cosd(c5) -si nd(c5) 0 0; 0 0 -1 0; sin d(c5) cosd(c5) 0 0; 0 0 0 1; T6=cosd(c6) -si nd(c6) 0 0; 0 0 1 0; -si nd(c6) -cosd(c6) 0 0; 0 0 0 1; T=T1*T2*T3*T4*T5*T6; dis p( T

24、=); dis p(T) 2、运动学逆解 fun ctio n n ijie(T) clc; n x=i npu t( nx=);ox=i npu t(ox=);ax=i npu t(ax=); n y=i npu t( ny 二);oy=i npu t(oy=);ay=i npu t(ay=); nz二inpu t( nz二);oz二 inpu t(oz=);az=inpu t(az=); px二inpu t( px二);py二inpu t( py 二);pz二inpu t( pz二); a2=431.8; a3=20.32; d2=149.09; d4=433.07; c仁ata n2(p

25、 y, px)-ata n2(d2,sqrt( px* px+py* py-d2*d2),ata n2( py, px)-ata n2(d2,-sq rt(px*px+py*py-d2*d2); %求解 c1 c1=c1/pi*180; k=( px* px+py*py+p z* pz-a2*a2-a3*a3-d2*d2-d4*d4)/(2*a2); c3=ata n2(a3,d4)-ata n2(k,sqrt(a3*a3+d4*d4-k*k),ata n2(a3,d4)-ata n2(k,-sqrt(a3 *a3+d4*d4-k*k); %求解 c3 c3=c3/pi*180; for i=

26、1:2 for j=1:2 m仁cosd(c1(i);m2=si nd(c1(i); n1=cosd(c3(j); n2=si nd(c3(j); c23(i,j)=ata n2(-(a3+a2* n1)* pz+(m1* px+m2* py)*(a2* n2-d4),(-d4+a2* n2)* pz+(m1* px+m2* py)*(a2* n1+a3); c23(i,j)=c23(i,j)/pi*180; c2(i,j)=c23(i,j)-c3(1,j); end end%求解c2 for i=1:2 for j=1:2 m1=cosd(c1(i); n仁si nd(c1(i); m2=c

27、osd(c23(i,j); n2=si nd(c23(i,j); c41(i,j)=ata n2(-ax* n1+ay*m1,-ax*m1*m2-ay* n1*m2+az* n2); c411(i,j)=ata n2(ax* n1-ay*m1,ax*m1*m2+ay* n1*m2-az* n2); c41(i,j)=c41(i,j)/pi*180; c411(i,j)=c411(i,j)/pi*180; end end%求解c4 c4=c41,c411; dis p(c4) c23=c23(1,:),c23(1,:);c23(2,:),c23(2,:); for i=1:2 for j=1:4

28、 m1=cosd(c1(i); n仁si nd(c1(i); m2=cosd(c23(i,j); n2=si nd(c23(i,j); m3=cosd(c4(i,j); n3=si nd(c4(i,j); sin c5(i,j)二ax*(m1*m2*m3* n1* n3)-ay*( n1*m2*m3-m1* n3)+az*( n2* m3); cosc5(i,j)=ax*(-m1* n2)+ay*(- n1*n 2)+az*(-m2); c5(i,j)=ata n2(si nc5(i,j),cosc5(i,j); c5(i,j)=c5(i,j)/pi*180; end end%求解c5 for

29、 i=1:2 for j=1:4 m1=cosd(c1(i); n仁si nd(c1(i); m2=cosd(c23(i,j); n2=si nd(c23(i,j); if sin d(c5(i,j)-0.01 c4(i,j)=0; end end end%奇异形位判断 for i=1:2 for j=1:4 m1=cosd(c1(i); n仁si nd(c1(i); m2=cosd(c23(i,j); n2=si nd(c23(i,j); m3=cosd(c4(i,j); n3=si nd(c4(i,j); m4=cosd(c5(i,j); n4=si nd(c5(i,j); sin c6(

30、i,j)=-nx*(m1*m2* n3-n 1*m3)-ny*( n1*m2* n4+m1*m4)+nz*( n2* n3); cosc6(i,j)=nx*(m1*m2*m3+n1* n3)*m4-m1* n2* n4)+ny*( n1*m2*m 3-m1* n3)*m4-m1*m2*m4)-nz*( n2*m3*m4+m2* n4); c6(i,j)=ata n2(si nc6(i,j),cosc6(i,j); c6(i,j)=c6(i,j)/pi*180; end end%求解 c6 C仁c1(1),c1(1),c1(1),c1(1),c1 (2),c1 (2),c1 (2) ,c1 (2

31、); C2=c2(1,:),c2(1,:),c2(2,:),c2(2,:); C3=c3(1),c3 (2) ,c3(1),c3 (2),c3(1),c3 (2) ,c3(1),c3(2); C23=c23(1,:),c23(2,:); C4=c4(1,:),c4(2,:); C5=c5(1,:),c5(2,:); C6=c6(1,:),c6(2,:);% 排序 C=C1;C2;C3;C4;C5;C6; % 输出 C=C; disp(C); 3、微分变换法求雅克比矩阵 fun ctio n Wfbh(c1,c2,c3,c4,c5,c6) c1=i npu t(c1=); c2=i npu t(

32、c2=); c3=i np ut(c3=); c4=inpu t(c4=); c5=i np ut(c5=); c6=i npu t(c6=); a2=431.8; a3=20.32; d2=149.09; d4=433.07; d6=56.25; T10=cosd(c1) -sin d(c1) 0 0; sin d(c1),cosd(c1),0,0; 0,0,1,0; 0 0 0 1; T21=cosd(c2),-si nd(c2),0,0; 29 精选文库 0,0,1 d2; -si nd(c2) -cosd(c2) 0 0; 0 0 0 1; T32=cosd(c3) -sin d(c3

33、) 0 a2; sin d(c3) cosd(c3) 0 0; 0 0 1 0; 0 0 0 1; T43=cosd(c4) -sin d(c4) 0 a3 ; 0 0 1 d4; -sin d(c4) -cosd(c4) 0 0; 0 0 0 1; T54=cosd(c5) -sin d(c5) 0 0; 0 0 -1 0; sin d(c5) cosd(c5) 0 0; 0 0 0 1; T65=cosd(c6) -sin d(c6) 0 0; 0 0 1 0; -si nd(c6) -cosd(c6) 0 0; 0 0 0 1; T64=T54*T65; T63=T43*T64; T62

34、=T32*T63; T61=T21*T62; T60=T10*T61; T(:,:,1)=T61; T(:,:,2)=T62; T(:,:,4)=T64; T(:,:,5)=T65; N=T(1:3,1,:); T(:,:,3)=T63; 31 精选文库 O=T(1:3,2,:); A=T(1:3,3,:); P=T(1:3,4,:); PN=cross( P,N); PO=cross( P, O); PO=P O(3,1,:); P A=cross( P,A); P A=PA(3,1,:); J= PN;P O; PA;N(3,1,:);O(3,1,:);A(3,1,:); J6=0;0;0;0;0;1; J=J(:,:,1),J(:,:,2),J(:,:,3),J(:,:,4),J(:,:,5),J6; R60=T60(1:3,1:3); J=R60,zeros(3