基于Matlab的工业机器人运动学和雅克比运动分析.doc

- 0 - 工业机器人课程设计 基于 Matlab 的工业机器人运动学和雅克比运动分析 班级： 学号 姓名： 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 1 - 目录目录 摘要摘要.- 2 - PUMA560 机器人简介机器人简介- 3 - 一、一、PUMA560 机器人的正解机器人的正解- 4 - 1.1、确定 D-H 坐标系.- 4 - 1.2、确定各连杆 D-H 参数和关节变量.- 4 - 1.3、求出两杆间的位姿矩阵- 4 - 1.4、求末杆的位姿矩阵- 5 - 1.5、MATLAB编程求解.- 6 - 1.6、验证- 7 - 二、二、PUMA560 机器人的逆解机器人的逆解- 7 - 2.1、求.- 7 - 1 2.2、求.- 8 - 3 2.3、求.- 8 - 2 2.4、求.- 9 - 4 2.5、求.- 10 - 5 2.6、求.- 10 - 6 2.7、解的多重性- 11 - 2.8、MATLAB编程求解.- 11 - 2.9、对于机器人解的分析- 11 - 三、机器人的雅克比矩阵三、机器人的雅克比矩阵.- 12 - 3.1、定义- 12 - 3.2、雅可比矩阵的求法- 12 - 3.3、微分变换法求机器人的雅可比矩阵- 13 - 3.4、矢量积法求机器人的雅克比矩阵- 14 - 3.5、MATLAB编程求解.- 15 - 附录附录.- 17 - 1、MATLAB程序- 17 - 2、三维图.- 26 - 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 2 - 摘要摘要 机器人学作为一门高度交叉的前沿学科，引起许多具有不同专业背景人们 的广泛兴趣，对其进行深入研究，并使其获得快速发展。尤其是近年来各种新 兴技术飞速发展,机械工业产品的自动化、高精度、重负载等性能指标变得越来 越突出。因此在机器人学的计算中就要求更高的精度，计算机技术的发展很好 的解决了这一问题。本文将以 PUMA560 为例，利用个人电脑平台的 Matlab 对 其运动学的正解、逆解以及雅克比矩阵进行计算研究。 关 键 词 PUMA560 Matlab 正解 逆解 雅克比矩阵 微分变换法 矢量积法 ABSTRACT As a highly interspersed subject, the robotics makes many people who major in different subject interest in it, research and develop it. Especially in recent years, with the rapid development of varieties of emerging technologies, mechanical products 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 computer technology makes it possible. In this paper we will use Matlab to research the kinematics problem and Jacobian array of PUMA560. KEY WORDS PUMA560 Matlab Kinematics problem Positive-solution Inverse-solution Jacobian array Differential transformation Vector product transformation 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 3 - PUMA560 机器人简介机器人简介 PUMA560 是属于关节式机器人，6 个关节都是转动关节，如图 11 所示， 前三个关节确定手腕参考点的位置，后三个关节确定手腕的方位。和大多数工 业机器人一样，后三个关节轴线交于一点。该点选作为手腕参考点，也选作为 4、5、6的原点。关节一的轴线为垂直方向，关节 2 和关节 3 的轴线为水 平，且平行，距离为。关节 1 和关节 2 的轴线垂直相交，关节 3 和关节 4 的 a2 轴线垂直交错。距离为 。各个连杆坐标系如 a3 图 11 所示，相应的连 杆参数列于表 12 中。 其中，mm a 8 . 431 2 ，mm a 32.20 3 ， mm d 04.149 2 。mm d 07.433 3 在更进一步了解 PUMA560 机器人的转动角度问题时，我们先来定义一下 PUMA560 机器人的初 始位姿。首先,定义机器 人的初始位置.取大臂处 于某一朝向时,作为腰关 节的初始位置.大臂处在 水平位置时,作为肩关节 的初始位置.小臂处在下 垂位置,关节轴线 Z4 和 Z0 平行时,作为肘关节 的初始位置.关节轴线 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 4 - Z5 和 Z3 平行时,作为腕扭转关节的初始位置.关节轴线 Z6 和 Z4 平行时,作为腕 弯曲关节的初始位置.抓手两个指尖的连线与大臂平行时,作为腕旋转关节的初始 位置.在上述初始位置的前下,各个关节的零点位置得到确定. 一、一、PUMA560 机器人的正解机器人的正解 1.1、确定、确定 D-H 坐标系坐标系 PUMA 560 的关节全为转动关节: Zi 坐标轴:沿着 i+1 关节的运动轴; Xi 坐标轴:沿着 Zi 和 Zi-1 的公法线,指向离开 Zi-1 轴的方向; Yi 坐标轴:按右手直角坐标系法则制定; 连杆长度 ai; Zi 和 Zi-1 两轴心线的公法线长度; 连杆扭角 i: Zi 和 Zi-1 两轴心线的夹角; 两连杆距离 di: Xi 和 Xi-1 两坐标轴的公法线距离; 两杆夹角 i :Xi 和 Xi-1 两坐标轴的夹角; 1.2、确定各连杆、确定各连杆 D-H 参数和关节变量参数和关节变量 确定各连杆 D-H 参数和关节变量： 连杆连杆 i变量变量 ii-1ai-1di变量范围变量范围 11000-160160 22-900d2-22545 330a20-45225 44-90a3d4-110170 559000-100100 66-9000-266266 1.3、求出两杆间的位姿矩阵、求出两杆间的位姿矩阵 第 i 连杆与第 i-1 连杆间的变换矩阵 Ai =Rot(x, i-1)trans(ai-1,0,0)Rot(z, i)trans(0,0,di) 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 5 - = 1 1111 1111 0 0001 iii iiiiiii iiiiiii csa s cc cssd s sc sccd 相邻两个连杆间的位姿变换矩阵 11 110 1 00 00 0010 0001 cs sc T 22 1 2 22 00 0010 00 0001 cs T sc 33 332 3 00 00 0010 0001 cs sc T 44 3 4 44 00 0010 00 0001 cs T sc 55 4 5 55 00 0010 00 0001 cs T sc 66 5 6 66 00 0010 00 0001 cs T sc 1.4、求末杆的位姿矩阵、求末杆的位姿矩阵 0012345 6112233445566 ( )()()()()()TTTTTTT 001 616 0001 xxxx yyyy zzzz noap noap TT T noap 由上面的矩阵，我们可以得到最终结果： 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 6 - 1234 5 64 623 5 614 5 64 6 1234 5 64 623 5 614 5 64 6 234 5 64 623 5 6 1234 5 64 623 5 614 64 5 6 1234 5 64 623 5 614 x y z x y nccc c cs ss s css c cc s nscc c cs ss s ccs c cc s nsc c cs sc s c occc c ss cs s ss c cs c s oscc c ss cs s sc c c 64 5 6 234 5 64 623 5 6 123 4 523 51 4 5 123 4 523 51 4 5 23 4 523 5 1223 234 232 1 1223 234 232 1 3 232 2423 z x y z x y z s c c osc c ss cc s s ac c c ss cs s s as c c ss cc s s as c sc c pc a ca cd sd s ps a ca cd sd c pa sa sd c 1.5、Matlab 编程编程求解求解 根据变量的取值范围，随机取 1 = 0o, 2 = -90o, 3 = 90o, 4 = 0o, 5 = 0o, 6 = 0o, 运行Positive.m，根据提示输入的值，回车后的出 T 的解如下： 16 1.6、验证、验证 1 = 0o, 2 = -90o, 3 = 90o, 4 = 0o, 5 = 0o, 6 = 0o, , , , mm a 8 . 431 2 mm a 32.20 3 mm d 04.149 2 mm d 07.433 3 s 2 240 6 3 010 001 100 0001 d ad T a 由课本给出的验证公式进行所编程序的验证，经人工运算验证，编程所得结果与课本 给出验证公式得到的结果一致。 T= 1.0000 0 0 20.3200 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 7 - 0 -1.0000 0 149.0400 0 0 -1.0000 -1.2700 0 0 0 1.0000 进一步表明所编程序是正确的。 二、二、PUMA560 机器人的逆解机器人的逆解 将 PUMA 560 的运动方程写为: 0012345 6112233445566 ( )()()()()() 0001 xxxx yyyy zzzz noap noap TTTTTTT noap 若末端连杆的位姿已经给定,求关节变量 的值成为运动逆解。 16 2.1、求、求 1 01012345 1162233445566 TTTTTTT 11 111 6 00 00 0010 00010001 xxxx yyyy zzzz csnopa scnoap T noap 2 1212 2 22 1 222 122 sin()/;cos()1 (/) atan2,1 atan2(,)atan2(, yxxy dd dd ppdppd 式中,正、负号对应于的两个可能解。 1 2.2、求、求 3 113 234 2322 3 234232 2 xy z c ps pa cd sa c pa sd ca s 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 8 - 由以上两式的平方加上的平方可以得到： 112 -s xy pc pd （22） 3 34 3 a cd sk 在上式中， 2222222 2324 2 2 xyz pppaadd k a 式（22）中已经消去，所以可以由三角代换求解得到的解。 2 3 所以：在的表达式中正、负号对应于的两种可能解。 3 3 222 33434 atan2(,)atan2( ,)a dkadk 2.3、求、求 2 010345 31236445566 ,TTTTT （23） 1 231 23232 3 1 231 23232 33 6 112 0 00010001 xxxx yyyy zzzz c cs csa cnopa c ss sca snoap T scdnoap 4 5 64 64 5 64 64 53 5 65 654334 646 4 5 64 64 5 64 64 5 0 0001 c c cs sc c ss sc sa s ss scd TT T s s sc ss c sc cs s 令矩阵方程(23)两端的元素(1,4)和(2,4)分别对应相等,则得两方程: 1 231 23232 33 1 231 23232 34 xyz xyz c s ps c ps pa ca c s ps s pc pa sd 由以上两式可得的表达式： 23 232332 3112 34 42 3112 33 tan2 ()()(), ()()() zxy xy aaa cpc ps pa sd da spc ps pa ca 由求得的，可求出： 23 2 2233 根据解的四种可能组合可以得到相应的四种可能值，于是可得到 13 和 23 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 9 - 的四种可能解。 2 2.4、求、求 4 1 231 23232 3 1 231 23232 33 6 112 0 00010001 xxxx yyyy zzzz c cs csa cnopa c ss sca snoap T scdnoap 令上式的矩阵方程的两端的元素(1,4)和(2,4)分别对应相等,则得两方程： 1 231 23234 5 114 5 xyz xy a c ca s ca sc s a sa cs s 5 41111 231 2323 42111 231 2323 0 atan2(,) atan2(,) xyxyz xyxyz s a sa ca c ca s ca s a sa c a c ca s ca s 当 当 S5=0 时,机械手处于奇异形位.此时,关节轴 4 和 6 重合,只能解出和的和 4 6 或差.奇异形位可以由式的表达式中的 atan2 的两个变量是否接近零来判别.若 4 都接近零,则为奇异形位,否则,不是奇异形位.在奇异形位时,可任意选取值,再计 4 算相应的值。 6 2.5、求、求 5 根据求出的，可以进一步解出： 4 5 010454 4123465566656 ,TTTTT 因为，在前面均已解出，逆变换为： 1 2 3 4 01 41234 ( ,)T 1 23 41 41 23 41 423 42 3 42 43 4 1 23 41 41 23 41 423 42 3 4243 4 1 231 23232 34 0001 c c cs ss c cc ss ca c cd sa c c c ss ss c sc cs sa c sd ca s c ss sca sd 令矩阵方程两端的元素(1,3)和(3,3)分别对应相等,则得两方程： 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 10 - 1 23 41 41 23 41 423 45 1 231 23235 xyz xyz acc cs sas c cc sas cs ac sas sacc 所以可以得到的最终表达式： 5 555 atan2( ,)s c 2.6、求、求 6 0105 512345666 ,TTT 令矩阵方程两端的元素(3,1)和(1,1)分别对应相等,则得两方程： 61 23 41 41 23 41 423 4 61 23 41 451 23 51 23 41 451 23 5 23 4 523 5 ()()() () () xyz xy z sn c c ss cns c sc cn s s cnc c cs scc s sns c cc s cs s s n s c cc s 得到最后的表达式： 6 666 atan2(,)s c 2.7、解的多重性、解的多重性 PUMA560 的运动反解可能存在 8 种解，但是，由于结构的限制，例如各关 节变量不能在全部 360 度范围内运动，有些解不能实现。在机器人存在多种解 的情况下，应选取其中最满意的一组解，以满足机器人的工作要求。 2.8、Matlab 编程编程求解求解 在 Matlab 中运行 Inverse.m,根据提示输入的值并回车，, xxxyyyzzz n o a n o an o a 可得的 8 组解值如下： i 2.9、对于机器人解的分析、对于机器人解的分析 通过编程可以知道，我们最终得到八组解。然后对八组解进行分析，在、 1 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 11 - 、的变化范围内，只有第 1、3、8 组符合。 2 3 4 5 6 三、机器人的雅克比矩阵 3.1、定义、定义 机械手的操作速度与关节速度间的线性变换定义为机械手的雅可比矩阵。 ( )( )xx qxJ q q ( ) ( ),1,2,6,1,2, i ij j x q Jqijn q 3.2、雅可比矩阵的求法、雅可比矩阵的求法 （1）矢量积法 对移动关节 , 00 ii ii vzz qJ w 对转动关节 iooi nin pR p 求得各关节相对于基坐标系的位姿矩阵即， iooi nin pR p 得到的数据带入式 ，求得其末端执 行器相对基坐标系的雅克比矩阵。 , i o n i i ii i o n i i z pz Jq z pz w v , i o n i i ii i o n i i z pz Jq z pz w v 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 12 - （2）微分变换法 对于转动关节 i,相对连杆 i-1,绕坐标系i的轴所作微分转动,其微分运动 i z i d 矢量为(3-117),对应的夹持器的微分运动矢量为(3-118)： 00 0 ,0(3 117)(3 118 01 T x z T y z T z z ii T zx T zy T zz pnd p od pad ddd n o a ） 于是,J(q)的第 i 列如下:对转动关节 i： , z z TT liaiz z z z pnn Jp oJo paa 对移动关节 i： 0 ,0 0 z TT lizai z n JoJ a 3.3、微分变换法求机器人的雅可比矩阵、微分变换法求机器人的雅可比矩阵 PUMA560 的 6 个关节都是转动关节,所以利用(3-119)求取雅克比矩阵的列矢量。 对于第 1 个关节来说,将中的 n,o,a,p 向量代入式(3-119)，得到雅克比矩阵的 1 6 T 列矢量。 (3-119) 1 1 1 1 234 5 64 62356 234 5 64 62356 23 4 523 5 T x T y T Tz J J J Jq sc c cs sc s c sc c ss cc s s s c sc c 其中、的表达式如下所示： x TJ 1x TT 2x TT 3 14 5 64 53 232342234 5 64 623 5 6 14 5 64 63 232342234 5 64 623 5 6 14 53 23234223 4 523 5 ()() ()()() ()() T x T y T z Js s sc sa cs dd cc c cs ss s s Js c sc ca cs dd cc c cs ss s s Js s a cs dd c c ss c 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 13 - 对于第 2 个关节来说,将中的 n,o,a,p 向量代入式(3-119),得到雅克比矩阵的列 2 6 T 矢量 2 TJ q 可以得到： 2 2 2 2 4 54 66 4 5 64 6 4 5 T x T y T Tz J J J Jq s cc sc s c sc c s s 其中、三个参数的表达式如下所示： x TJ 2y TJ 2z TJ 2 同理,可求得： 234 5 64 63 5 63 3342 34 5 64 63 5 63 334 234 5 64 63 5 63 3342 34 5 64 63 5 63 334 23 53 4 53 33423 4 53 53 () () () () ()()()( T x T y T z Jsc c cs sc s sa cs da cc c cs ss s sa sc d Jsc c ss sc s sa cs da cc c ss ss s s a sc d Jc cs c sa cs dac c ss ca 334) sc d 3456 , TTTT JqJqJqJq 44 5 64 635 6 44 5 64 635 6 44 53 6 3 4 5 64 6 4 5 64 6 4 5 T dc c cs sas c dc c ss cas s d c sa c Jq s c cc s s c sc c s s 456 5 66 5 66 5 000 000 000 , 0 0 01 TTT JqJqJq s cs s sc c 所以又以上六个矩阵便最后组成了的雅克比矩阵。1666 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 14 - 3.4、矢量积法求机器人的雅克比矩阵、矢量积法求机器人的雅克比矩阵 PUMA560 的 6 个关节都是转动关节,因而其雅克比矩阵具有下列形式： 102060 162666 126 zpzpzp J q zzz 3.5、Matlab 编程编程求解求解 （1）用微分变换法求解雅克比矩阵 在 Matlab 中运行 Differential.m，根据提示输入的值并回车，得雅克比矩 16 阵如下： （2）用矢量积法求解雅克比矩阵 在 Matlab 中运行 Integration.m，根据提示输入的值并回车，得雅克比矩 16 阵如下： 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 15 - 从以上结果中我们可以看出其运行结果与用微分变换编程所得到的结果是一致 的，进一步证明了所编写程序的正确性。 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 16 - 附录附录 1、Matlab 程序程序 1、运动学正解 function Positive (c1,c2,c3,c4,c5,c6) clc; a2=431.8; a3=20.32; d2=149.04; d4=433.07; d6=56.25; c1=input(c1=); c2=input(c2=); c3=input(c3=); c4=input(c4=); c5=input(c5=); c6=input(c6=); T1=cosd(c1) -sind(c1) 0 0; sind(c1),cosd(c1),0,0; 0,0,1,0; 0 0 0 1; T2=cosd(c2),-sind(c2),0,0; 0,0,1 d2; -sind(c2) -cosd(c2) 0 0; 0 0 0 1; T3=cosd(c3) -sind(c3) 0 a2; sind(c3) cosd(c3) 0 0; 0 0 1 0; 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 17 - 0 0 0 1; T4=cosd(c4) -sind(c4) 0 a3 ; 0 0 1 d4; -sind(c4) -cosd(c4) 0 0; 0 0 0 1; T5=cosd(c5) -sind(c5) 0 0; 0 0 -1 0; sind(c5) cosd(c5) 0 0; 0 0 0 1; T6=cosd(c6) -sind(c6) 0 0; 0 0 1 0; -sind(c6) -cosd(c6) 0 0; 0 0 0 1; T=T1*T2*T3*T4*T5*T6; disp(T=); disp(T); end 2、运动学逆解 function Inverse (T) clc; nx=input(nx=);ox=input(ox=);ax=input(ax=); ny=input(ny=);oy=input(oy=);ay=input(ay=); nz=input(nz=);oz=input(oz=);az=input(az=); px=input(px=);py=input(py=);pz=input(pz=); a2=431.8; a3=20.32; d2=149.04; d4=433.07; c1=atan2(py,px)-atan2(d2,sqrt(px*px+py*py-d2*d2),atan2(py,px)-atan2(d2,- 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 18 - sqrt(px*px+py*py-d2*d2);%求解 c1 c1=c1/pi*180; k=(px*px+py*py+pz*pz-a2*a2-a3*a3-d2*d2-d4*d4)/(2*a2); c3=atan2(a3,d4)-atan2(k,sqrt(a3*a3+d4*d4-k*k),atan2(a3,d4)-atan2(k,- sqrt(a3*a3+d4*d4-k*k);%求解 c3 c3=c3/pi*180; for i=1:2 for j=1:2 m1=cosd(c1(i);m2=sind(c1(i); n1=cosd(c3(j);n2=sind(c3(j); c23(i,j)=atan2(-(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);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); c41(i,j)=atan2(-ax*n1+ay*m1,-ax*m1*m2-ay*n1*m2+az*n2); c411(i,j)=atan2(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; disp(c4) c23=c23(1,:),c23(1,:);c23(2,:),c23(2,:); for i=1:2 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 19 - for j=1:4 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); m3=cosd(c4(i,j);n3=sind(c4(i,j); sinc5(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*n2)+az*(-m2); c5(i,j)=atan2(sinc5(i,j),cosc5(i,j); c5(i,j)=c5(i,j)/pi*180; end end%求解 c5 for i=1:2 for j=1:4 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); if sind(c5(i,j)-0.01 c4(i,j)=0; end end end%奇异形位判断 for i=1:2 for j=1:4 m1=cosd(c1(i);n1=sind(c1(i); m2=cosd(c23(i,j);n2=sind(c23(i,j); m3=cosd(c4(i,j);n3=sind(c4(i,j); m4=cosd(c5(i,j);n4=sind(c5(i,j); sinc6(i,j)=-nx*(m1*m2*n3-n1*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*m3-m1*n3)*m4-m1*m2*m4)- 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 20 - nz*(n2*m3*m4+m2*n4); c6(i,j)=atan2(sinc6(i,j),cosc6(i,j); c6(i,j)=c6(i,j)/pi*180; end end%求解 c6 C1=c1(1),c1(1),c1(1),c1(1),c1(2),c1(2),c1(2),c1(2); 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); end 3、微分变换法求雅克比矩阵 function Differential (c1,c2,c3,c4,c5,c6) c1=input(c1=); c2=input(c2=); c3=input(c3=); c4=input(c4=); c5=input(c5=); c6=input(c6=); a2=431.8; a3=20.32; d2=149.04; d4=433.07; d6=56.25; T10=cosd(c1) -sind(c1) 0 0; 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 21 - sind(c1),cosd(c1),0,0; 0,0,1,0; 0 0 0 1; T21=cosd(c2),-sind(c2),0,0; 0,0,1 d2; -sind(c2) -cosd(c2) 0 0; 0 0 0 1; T32=cosd(c3) -sind(c3) 0 a2; sind(c3) cosd(c3) 0 0; 0 0 1 0; 0 0 0 1; T43=cosd(c4) -sind(c4) 0 a3 ; 0 0 1 d4; -sind(c4) -cosd(c4) 0 0; 0 0 0 1; T54=cosd(c5) -sind(c5) 0 0; 0 0 -1 0; sind(c5) cosd(c5) 0 0; 0 0 0 1; T65=cosd(c6) -sind(c6) 0 0; 0 0 1 0; -sind(c6) -cosd(c6) 0 0; 0 0 0 1; T64=T54*T65; T63=T43*T64; T62=T32*T63; T61=T21*T62; T60=T10*T61; T(:,:,1)=T61; T(:,:,2)=T62; 工业机器人的运动学和雅克比运动分析 机械与动力工程学院 1/26/2015 - 22 - T(:,:,3)=T63; T(:,:,4)=T64; T(:,:,5)=T65; N=T(1:3,1,:); O=T(1:3,2,:); A=T(1:3,3,:); P=T(1:3,4,:); PN=cross(P,N); PN=PN(3,1,:); PO=cross(P