工业机器人课程设计

工业机器人课程设计

基于Matlab的工业机器人运动学和雅克比较矩阵求解

目录

PUMA560机器人简介 ........................................................................................................................... 4 PUMA560机器人的正解 ....................................................................................................................... 5 1、确定D-H坐标系 ........................................................................................................................... 5 2、确定各连杆D-H参数和关节变量 ............................................................................................... 5 3、求出两杆间的位姿矩阵 ................................................................................................................ 5 4、求末杆的位姿矩阵 ........................................................................................................................ 6 5、MATLAB编程 ................................................................................................................................. 7 6、验证 ................................................................................................................................................ 7 PUMA560机器人的逆解 ....................................................................................................................... 8 1、求2、求3、求4、求5、求6、求

1 ............................................................................................................................................... 8

3 ............................................................................................................................................... 8 2 ............................................................................................................................................... 9 4 ............................................................................................................................................. 10 5 ............................................................................................................................................. 10 6 ............................................................................................................................................. 11

7、解的多重性 .................................................................................................................................. 11 8、MATLAB编程 ............................................................................................................................... 11 9、对于机器人解的分析 .................................................................................................................. 12 机器人的雅克比矩阵 ............................................................................................................................ 12 1、定义 .............................................................................................................................................. 12 2、雅可比矩阵的求法 ...................................................................................................................... 12 3、微分变换法求机器人的雅可比矩阵 .......................................................................................... 13 4、矢量积法求机器人的雅克比矩阵 .............................................................................................. 14 5、MATLAB编程 ............................................................................................................................... 15 附录 ........................................................................................................................................................ 16 1、程序 .............................................................................................................................................. 16 2、三维图 .......................................................................................................................................... 24

摘要

机器人学作为一门高度交叉的前沿学科，引起许多具有不同专业背景人们的广泛兴趣，对其进行深入研究，并使其获得快速发展。尤其是近年来各种新兴技术飞速发展,机械工业产品的自动化、高精度、重负载等性能指标变得越来越突出。因此在机器人学的计算中就要求更高的精度，计算机技术的发展很好的解决了这一问题。本文将以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

PUMA560机器人简介

PUMA560是属于关节式机器人，6个关节都是转动关节，如图1—1所示，前三个关节确定手腕参考点的位置，后三个关节确定手腕的方位。和大多数工业机器人一样，后三个关节轴线交于一点。该点选作为手腕参考点，也选作为{4}、{5}、{6}的原点。关节一的轴线为垂直方向，关节2和关节3的轴线为水平，且平行，距离为a2。关节1和关节2的轴线垂直相交，关节3和关节4的轴线垂

直交错。距离为a3。各个连杆坐标系如图1—1所示，相应的连杆参数列于表1—2中。其中，

aadd

在更进一步了解PUMA560机器人的转动角度问题时，我们先来定义一下PUMA560机器人的初始位姿。首先,定义机器人的初始位置.取大臂处于某一朝向时,作为腰关节的初始位置.大臂处在水平位置时,作为肩关节的初始位置.小

2431.8mm20.32mm149.09mm433.07mm。

，，，

323

臂处在下垂位置,关节轴线Z4和Z0平行时,作为肘关节的初始位置.关节轴线Z5和Z3平行时,作为腕扭转关节的初始位置.关节轴线Z6和Z4平行时,作为腕弯曲关节的初始位置.抓手两个指尖的连线与大臂平行时,作为腕旋转关节的初始位置.在上述初始位置的前下,各个关节的零点位置得到确定.

PUMA560机器人的正解

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两坐标轴的夹角; 2、确定各连杆D-H参数和关节变量 确定各连杆D-H参数和关节变量： 连杆i 变量θi αi-1 ai-1 1 2 3 4 5 6 θ1 θ2 θ3 θ4 θ5 θ6 0° -90° 0° -90° 90° -90° 0 0 a2 a3 0 0 di 0 d2 0 d4 0 0 变量范围 -160°~160° -225°~45° -45°~225° -110°~170° -100°~100° -266°~266° 3、求出两杆间的位姿矩阵

第i连杆与第i-1连杆间的变换矩阵

Ai =Rot(x, αi-1)trans(ai-1,0,0)Rot(z, θi)trans(0,0,di)

si0cisccici1si1 =ii1sisi1cisi1ci1000c1s0T1100c3s2T3300c504T5s50

si1di ci1di1s20c20s40c40s60c600010 00010010 00010010 0001ai1相邻两个连杆间的位姿变换矩阵

s100c20c1001 T2s20100010s3c300s50c5000c4000 3T4s4100100c6010 5T6s60001004、求末杆的位姿矩阵

0T60T1(1)1T2(2)2T3(3)3T4(4)4T5(5)5T6(6)

nxn0T60T11T6ynz0oxoyoz0axayaz0pxpy pz1由上面的矩阵，我们可以得到最终结果：

nxc1c23c4c5c6s4s6s23s5c6s1s4c5c6c4s6nys1c23c4c5c6s4s6s23s5c6c1s4c5c6c4s6nzs23c4c5c6s4s6c23s5c6oxc1c23c4c5s6s4c6s23s5s6s1c4c6s4c5s6oys1c23c4c5s6s4c6s23s5s6c1c4c6s4c5c6ozs23c4c5s6s4c6c23s5s6axc1c23c4s5s23c5s1s4s5ays1c23c4s5s23c5c1s4s5azs23c4s5c23c5pxc1a2c2a3c23d4s23d2s1pys1a2c2a3c23d4s23d2c1pza3s23a2s2d4c235、Matlab编程

运行zhengjie.m，根据提示输入16的值，回车后的出T的解如下： T=

0 1.0000 0 -149.0900 0 0 1.0000 864.8700 1.0000 0 0 20.3200 0 0 0 1.0000 6、验证

190o,20,390o,40,5060a2431.8mm,d2149.09mm,a320.32mm,d4433.07mm,d656.25mm000T61010d201a2d4 00a3001

由课本给出的验证公式进行所编程序的验证，经验证，编程所得结果与课本给出验证公

式得到的结果一致。进一步表明所编程序是正确的。

PUMA560机器人的逆解

将PUMA 560的运动方程（3.64）写为:

nxn0T6ynz0oxoyoz0axayaz0pxpy0T1(1)1T2(2)2T3(3)3T4(4)4T5(5)5T6(6) pz1若末端连杆的位姿已经给定,求关节变量16 的值成为运动逆解。 1、求1

0T1110T61T222T333T444T555T66

00nxn00y10nz010oxoyoz0axayaz0pxpy1T6 pz1c1s1sc110000sin(1)d2/;cos(1)1(d2/)22dd1atan22,12

2221atan2(py,px)atan2(d2,pxpyd2式中,正、负号对应于1的两个可能解。

2、求

3

c1pxs1pya3c23d4s23a2c2

pza3s23d4c23a2s2由以上两式的平方加上-s1pxc1pyd2的平方可以得到：

a3c3d4s3k（2—2）

在上式中，k222222pxpypz2a2a3d2d42a2

式（2—2）中已经消去2，所以可以由三角代换求解得到3的解。 所以：在3的表达式中正、负号对应于3的两种可能解。

223atan2(a3,d4)atan2(k,a3d4k2)

3、求

02

T311,2,30T63T444T555T66

c1c23cs123s10s1c23s1s23c10s23c2300a2c3nxna2s3yd2nz10s5s6s4c5s6c4c60oxoyoz0axayaz0c5s4s50pxpy3T6 （2—3） pz1a3d4 01c4c5c6s4s6s5s6334T6T4T6s4s5s6c4s60c4c5s6s4s6c4s5

令矩阵方程(2—3)两端的元素(1,4)和(2,4)分别对应相等,则得两方程:

c1s23pxs1c23pys23pza2c3a3

c1s23pxs1s23pyc23pza2s3d4

由以上两式可得23的表达式：

2323atan2[(a3a2c3)pz(c1pxs1py)(a2s3d4),(d4a2s3)p(c1pxs1py)(a2c3a3)]由求得的23，可求出2：

2233

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

4、求

4

s1c23s1s23c10s23c2300a2c3nxna2s3yd2nz10oxoyoz0axayaz0pxpy3T6 pz1c1c23cs123s10令上式的矩阵方程的两端的元素(1,4)和(2,4)分别对应相等,则得两方程：

axc1c23ays1c23azs23c4s5

axs1ayc1s4s5当s5041atan2(axs1ayc1,axc1c23ays1c23azs23) 42atan2(axs1ayc1,axc1c23ays1c23azs23)当S5=0时,机械手处于奇异形位.此时,关节轴4和6重合,只能解出4和6的和或差.奇异形位可以由式4的表达式中的atan2的两个变量是否接近零来判别.若都接近零,则为奇异形位,否则,不是奇异形位.在奇异形位时,可任意选取4值,再计算相应的6值。

5、求

5

根据求出的4，可以进一步解出5：

0T411,2,3,40T64T555T664T65,6

因为1，2，3，4在前面均已解出，逆变换0T41(1,2,3,4)为：

c1c23c4s1s4ccsss123414c1s230s1c23c4c1s4s1c23s4c1c4s1s230s23c4s23s4c230a2c3c4d2s4a3c4a2c3s4d2c4a3s4

a2s3d41令矩阵方程两端的元素(1,3)和(3,3)分别对应相等,则得两方程：

axc1c23c4s1s4ays1c23c4c1s4azs23c4s5

axc1s23ays1s23azc23c5

所以可以得到5的最终表达式：5atan2(s5,c5)

6、求

06

T511,2,3,4,50T65T66

令矩阵方程两端的元素(3,1)和(1,1)分别对应相等,则得两方程：

s6nx(c1c23s4s1c4)ny(s1c23s4c1c4)nz(s23s4)c6nx[c1c23c4s1s4c5c1s23s5]ny[(s1c23c4c1s4)c5s1s23s5] nz(s23c4c5c23s5)得到最后6的表达式：6atan2(s6,c6) 7、解的多重性

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

在Matlab中运行nijie.m,根据提示输入nx,ox,ax,ny,oy,ay,nz,oz,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.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、对于机器人解的分析

通过编程可以知道，我们最终得到八组解。然后对八组解进行分析，对于1的变化范围为从1600~1600，所以程序中我们得到的两个解都是正确的。然后对2进行分析，由于2的角度变化范围是从2250~450，所以在我们所得到的结果中，后四组是超出2的变化范围的，所以我们可以舍去后四组解。再逐个对

3、4、5、6进行角度分析，最终可获得适合的解。

机器人的雅克比矩阵

1、定义

机械手的操作速度与关节速度间的线性变换定义为机械手的雅可比矩阵。

xx(q)xJ(q)q

Jij(q)xi(q),i1,2,qj,6,j1,2,,n

2、雅可比矩阵的求法 （1）矢量积法 对移动关节

vziziq,Jiw0i0对转动关节

ooziipnvziipnq,Ji,iwziziioipnoiRpn

（2）微分变换法

对于转动关节i,相对连杆i-1,绕坐标系{i}的zi轴所作微分转动di,其微分运动矢量为(3-117),对应的夹持器的微分运动矢量为(3-118)：

TdxpnzTpodzyTdzpaz(3117)TdinzxTozyTazz00,0dd0i01(3118）

pnznzo于是,J(q)的第i列如下:对转动关节i：TJlipoz,TJaizpaazz

nz,TJo对移动关节i：TJlizaiaz000

3、微分变换法求机器人的雅可比矩阵

PUMA560的6个关节都是转动关节,所以利用(3-121)求取雅克比矩阵的列矢量。 对于第1个关节来说,将1T6中的n,o,a,p向量代入式(3-121)，得到雅克比矩阵的列矢量。

TJ1xTJ1yTJ1zTJ1q

s23c4c5c6s4s6c23s5c6sccssccss23562345646scscc2345235TTT其中J1x、T2x、T3x的表达式如下所示：

TTTJ1x(s4s5s6c4s5)(a3c23s23d4)d2[c23c4c5c6s4s6s23s5s6]J1y(s4c5s6c4c6)(a3c23s23d4)d2[c23(c4c5c6s4s6)s23s5s6] J1zs4s5(a3c23s23d4)d2(c23c4s5s23c5)对于第2个关节来说,将2T6中的n,o,a,p向量代入式(3-121),得到雅克比矩阵的列矢

量TJ2q

TJ2xTJ2yTJ2zT可以得到：J2q

sccsc45646scscc45646s4s5其中TJ2x、TJ2y、TJ2z三个参数的表达式如下所示：

同理,可求得：

TJ2x[s3c4c5c6s4s6c3s5s6](a3c3s3d4a2)J2y[s3c4c5s6s4s6c3s5s6](a3c3s3d4a2)J2z(c3c5s3c4s5)(a3c3s3d4a2)(c3c4s5s3c5)(a3s3c3d4)

[c3c4c5c6s4s6s3s5s6](a3s3c3d4)T[c3c4c5s6s4s6s3s5s6(a3s3c3d4)TTJ3q,TJ4q,TJ5q,TJ6q

d4c4c5c6s4s6a3s5c6dccsscass356445646d4c4s5a3c6TJ3q

scccs45646s4c5s6c4c6s4s50000000T0T0TJ4q,Jq,Jq 56s5c6s60s5s6c60c051所以又以上六个61矩阵便最后组成了66的雅克比矩阵。 4、矢量积法求机器人的雅克比矩阵

PUMA560的6个关节都是转动关节,因而其雅克比矩阵具有下列形式：

0z11p6Jqz10z22p6z20z66p6 z65、Matlab编程

（1）用微分变换法求解雅克比矩阵

在Matlab中运行wfbh.m，根据提示输入16的值并回车，得雅克比矩阵如下： -864.8700 0 0 0 0 0 -149.0900 20.3200 20.3200 0 0 0 0 -864.8700 -433.0700 0 0 0 0 -1.0000 -1.0000 0 -1.0000 0 0 0 0 1.0000 0 1.0000 1.0000 0 0 0 0 0 （2）用矢量积法求解雅克比矩阵

在Matlab中运行slj.m，根据提示输入16的值并回车，得雅克比矩阵如下： -864.8700 0 0 0 0 0 -149.0900 20.3200 20.3200 0 0 0 0 -864.8700 -433.0700 0 0 0 0 -1.0000 -1.0000 0 -1.0000 0 0 0 0 1.0000 0 1.0000 1.0000 0 0 0 0 0

从以上结果中我们可以看出其运行结果与用微分变换编程所得到的结果是一致的，进一步证明了所编写程序的正确性。

附录

1、程序

1、运动学正解

function zhengjie(c1,c2,c3,c4,c5,c6) clc; a2=431.8; a3=20.32; d2=149.09; 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;

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) 2、运动学逆解 function nijie(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.09; d4=433.07;

c1=[atan2(py,px)-atan2(d2,sqrt(px*px+py*py-d2*d2)),atan2(py,px)-atan2(d2,-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 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&&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)-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);

3、微分变换法求雅克比矩阵 function wfbh(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.09; d4=433.07; d6=56.25;

T10=[cosd(c1) -sind(c1) 0 0; 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; 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,O); PO=PO(3,1,:); PA=cross(P,A); PA=PA(3,1,:);

J=[PN;PO;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);zeros(3),R60]*J; disp(J);

4、矢量积法求雅克比矩阵 function wfbh(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.09; d4=433.07; d6=56.25;

T10=[cosd(c1) -sind(c1) 0 0; 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; 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,O); PO=PO(3,1,:); PA=cross(P,A); PA=PA(3,1,:);

J=[PN;PO;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);zeros(3),R60]*J; disp(J); 2、三维图