课程设计课程名称机器人学题目名称空间描点机器人建模仿真学生学院专业班级学号学生姓名指导教师目录1.课程设计要求 (1)2.空间描点机器人的设计 (2)2.1机器人构型及坐标 (2)2.2D-H参数表 (4)3.正运动学 (5)3.1齐次变换矩阵 (5)3.2 空间描点机器人工作空间 (6)4.几何法求逆解 (7)5.程序流程图 (8)6.总结分析 (9)7.Matlab程序附录 (10)7.1 Mov_6DOF_Rob_Lnya.m (10)7.2 DHfk6Dof_Lnya.m (12)7.3 IK_6DOF_Rob_Lnya.m (13)7.4 Build_6DOFRobot_Lnya.m (14)7.5 Erzhihua.m (14)7.6 draw_Workplace.m (15)7.7 Matrix_DH_Ln.m (16)7.8 Connect3D.m (17)1. 课程设计要求一，按照附件模板填写，要求有封面和目录，除签名处不能有手写。

二，主要内容包括下面几个部分，1，设计一款六自由度机器人，要求2，3，4，5关节中有一个是滑动关节，其余关节应为转动关节。

试构想该机器人的功能，并根据功能设定机器人的介绍参数（杆件长度及关节极限）2，建立机器人的正运动学模型，进行Matlab 运动仿真。

（分析机器人的工作空间，制作机器人各个运动的动画。

）注意事项：1）要求画出机器人的关节坐标系，列出DH 参数表，以及各个关节间的齐次变换矩阵。

2）Matlab仿真应画出工作空间的立体图和剖面图。

采用机器人产品的同学应与实际说明书的工作空间做对比。

自行设计的同学要做简单的分析讨论。

3）直接采用例程里面的三自由度机器人该部分得0 分。

3，实现逆运动学轨迹规划注意事项：1）这里特指机器人末端的轨迹规划，不是关节空间的轨迹规划。

2）要实现控制机器人末端在空间中完成某种轨迹。

（如直线，圆弧，心型，写字等）3）可以采用求解逆运动的方程或者是利用微分运动。

4）写出详细的推导过程（公式）。

5）要求有仿真截图及动画。

6）只能使用matlab 及本课程提供的例程，不能使用工具箱。

7）仅仅使用3自由度例程的同学本部分分数会很低4，自由发挥项（完成这一部分的同学才能够得到90分以上）1）机器人完整逆解的求解方式（数值解）；2）寻找奇异点，分析奇异位型。

5，Matlab程序作为附录应添加在课程设计报告书的最后面。

要求在第一页附上程序流程图，注明函数调用过程，此外，程序要排好版。

2. 空间描点机器人的设计2.1机器人构型及坐标本课程设计通过matlab对一个六自由度的空间描点机器人进行设计，并对其进行仿真分析。

该机器人的第一关节为固定关节，主要是用于机器人本体的固定，第四关节为滑动关节，剩余关节为转动关节，在第六关节的连杆末端，可以带上各类的设备，在运行时带动连杆对进行空间三维描点或其他动作。

机器人的结构和大致效果示意图分别如如图1和图2所示：图1 机器人结构图图2 机器人大致效果图机器人关节坐标系和运行图如图3和图4所示：图3 机器人关节坐标系图4 机器人运行图2.2 D-H参数表根据本设计按摩机器人的各关节的坐标系及连杆长度，我们可以得到如下表1的DH 参数表。

表1 D-H参数表3正运动学3.1 齐次变换矩阵由课堂知识可易知，机器人关节n 到关节n+1的坐标变换步骤为如下的模式：()()()()1*0,0,*,0,0*nn n n n n n T A Rot a Trans d Trans a Rot n θα+==轴,轴,具体的齐次变换矩阵为：10010001001000001000100000010001001000000100010001001S 0000n n n nn n n nn n nn n n n n n n n nn n n n n n n n n C S a S C C S T d S C C C S S a C S C C S C a S S C d θθθθααααθαθαθθθαθαθθαα+-⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦--=1⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦由上述知识，以及根据第三节的D-H 参数表，可求得如下各个关节之间的齐次变化矩阵：1111112000*********01C S S C A T θθθθ⎛⎫ ⎪- ⎪== ⎪⎪⎝⎭(1) 222222300 00001800001C S S C A T θθθθ-⎛⎫ ⎪⎪== ⎪⎪⎝⎭(2)3333333340100010001000001C S C S C S A T θθθθθθ-⎛⎫⎪⎪== ⎪- ⎪⎝⎭ (3) 445100a450010000100001A T +⎛⎫⎪⎪== ⎪⎪⎝⎭(4)555555600000011000001C S S C A T θθθθ-⎛⎫ ⎪ ⎪== ⎪⎪⎝⎭(5)6666666670500500010001C S C S C S A T θθθθθθ-⎛⎫⎪⎪== ⎪⎪⎝⎭(6)3.2 空间描点机器人工作空间 通过matlab 程序仿真，我们可以得到如下结果：图5 立体图 图6 X-Y 视角图7 Y-Z 视图 图8 X-Z 视图4 几何法求逆解本课程设计所建立的机器人模型的逆解是通过几何法来进行求解的，总体的思路是第四个关节为滑动关节，第五、六关节为固定关节，所以当知道目标点的位置信息后，可以通过第四节中的各个关节间的齐次变换矩阵，求得14T ，接着的话便可以通过3自由度机器人几何法求逆解的方法求出相应的关节转动角度。

具体的公式推导过程如下：4456756755666556665656565656565656**100a45000050010000050**0010001100001000010001000105050a450S T T T T C S C S C S C S C S C C S S C S S S C C S S C C S θθθθθθθθθθθθθθθθθθθθθθθθθθ=+--⎛⎫⎛⎫⎛⎫⎪ ⎪ ⎪⎪ ⎪ ⎪= ⎪ ⎪ ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭⎝⎭----+++-=56565656050500011000001S S C C S C C S θθθθθθθθ⎛⎫ ⎪++ ⎪ ⎪ ⎪⎝⎭（7）由于本设计的机器人模型结构特殊，后两个关节变化角度为0，即12==0θθ，所以可得4710050a4500100001100001T ++⎛⎫⎪⎪= ⎪ ⎪⎝⎭（8）由于11234567234567*****T T T T T T T = （9）可得()1114477T T T -= （10）接着便可以通过课堂上三自由度机器人对前三个关节角度求逆解的几何法进行求解。

5 程序流程图本设计主要是两个功能，一个是通过所设计机器人末端在空间中画球，另外一个是通过所设计机器人末端对在空间中对加载的图片实现一个复现，把图片描出来。

大致的流程图如下：图9 程序流程图6 分析总结由matlab的实验结果来看，本次课程设计达到了实验的目的。

所设计空间描点机器人的工作空间也符合描点机器人工作的需要。

在本次课程设计中，虽然达到了实验要求的工作，可实现三维空间的描点，相对于前一次的大作业，本次课程设计有了很大进步。

可实现对目标点逆解的求解，在画工作空间中，采用的是通过齐次变换矩阵的计算，事先在matlab中画出工作空间点，最后再对该工作空间数组直接描点，大大提高了效率。

不过还存在着一些问题需要改进：1）在描字过程中，描出来的字并不算整齐。

2）在工作空间的X-Y视角图中，出现了一个空心点。

在本次课程设计中，通过对机器人建模，增强了对这方面的了解，对机器人有了更加全面的认识，我将会在此课程设计的基础上，进一步摸索解决上述还存在问题的原因，不断从中学习机器人的知识。

7 matlab代码附录7.1 Mov_6DOF_Rob_Lnya.m：close all;clear;num = 1ToDeg = 180/pi;ToRad = pi/180;L1=150;L2=100;L3=100;th1=180;th2=0;th3=0;d4=50;th5=0;th6=0;DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,0);view(134,12);pause;stp=30;%%%%%%%%%%%%%%%%%%%正运动学%%%%%%%%%%%%%%%%%%%%%%%% for i=0:stp:360DHfk6Dof_Lnya(th1+i,th2,th3,d4,th5,th6,1);endfor i=0:stp:360DHfk6Dof_Lnya(th1,th2+i,th3,d4,th5,th6,1);endfor i=0:stp:360DHfk6Dof_Lnya(th1,th2,th3+i,d4,th5,th6,1);endfor i=0:60:360DHfk6Dof_Lnya(th1,th2,th3,d4+i,th5,th6,1);endfor i=360:-60:0DHfk6Dof_Lnya(th1,th2,th3,d4+i,th5,th6,1);endfor i=0:stp:360DHfk6Dof_Lnya(th1,th2,th3,d4,th5+i,th6,1);endfor i=0:stp:360DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6+i,1);endpause;%%%%%%%%%%%%%%%%%%%%%%%画球%%%%%%%%%%%%%%%%%%%%%%%% cla;global Link;point11=[];point12=[];point13=[];a=100;b=100;c=100;r=30;[x,y,z]=sphere(30);X=x*r+a;Y=y*r+b;Z=z*r+c;for i=1:1:31for j=1:1:31px=X(i,j); py=Y(i,j); pz=Z(i,j);[th1,th2,th3]=IK_6DOF_Rob_Lnya(L1,L2,L3,px,py,pz);th1=th1*ToDeg;th2=th2*ToDeg;th3=th3*ToDeg;DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,1);point11(num) = Link(7).p(1);point12(num) = Link(7).p(2);point13(num) = Link(7).p(3);num = num + 1;plot3(point11,point12,point13,'k*');hold on;endend% %%%%%%%%%%%%%%%%%%%%描字%%%%%%%%%%%%%%%%%%%%%global Linkpoint21=[];point22=[];point23=[];num=1;ToDeg = 180/pi;load('FP.mat');[FPh,FPl]=size(FP);for i=1:FPh;[th1,th2,th3]=IK_6DOF_Rob_Lnya(L1,L2,L3,FP(i,2),100,FP(i,1));num=num+1;th1=th1*ToDeg;th2=th2*ToDeg;th3=th3*ToDeg;th4=0;th5=0;th6=0;if i==FPhDHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,0);elseDHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,1);endpoint21(num)= Link(7).p(1);point22(num)=Link(7).p(2);point23(num)=Link(7).p(3);plot3(point11,point12,point13,'k*');hold on;plot3( point21, point22, point23,'r.');hold on;endcla;plot3(point11,point12,point13,'k*');hold on;plot3( point21, point22, point23,'r.');hold on;7.2 DHfk6Dof_Lnya.mfunction pic=DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,fcla) global LinkBuild_6DOFRobot_Lnya;radius = 10;len = 30;joint_col = 0;plot3(0,0,0,'ro');Link(2).th=th1*pi/180;Link(3).th=th2*pi/180;Link(4).th=th3*pi/180;Link(5).dx=d4;Link(6).th=th5*pi/180;Link(7).th=th6*pi/180;p0=[0,0,0]';for i=1:7Matrix_DH_Ln(i);Endfor i=2:7Link(i).A=Link(i-1).A*Link(i).A;Link(i).p= Link(i).A(:,4);Link(i).n= Link(i).A(:,1);Link(i).o= Link(i).A(:,2);Link(i).a= Link(i).A(:,3);Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;endgrid on;axis([-600,500,-500,500,-500,500]);xlabel('x');ylabel('y');zlabel('z');drawnow;pic=getframe;if(fcla)cla;end7.3 IK_6DOF_Rob_Lnya.mfunction [th1,th2,th3]=IK_6DOF_Rob_Lnya(L1,L2,L3,px,py,pz)ToDeg = 180/pi;ToRad = pi/180;th1=atan2(py,px);P=[px,py,pz];J2=[0,0,L1];th3=pi-acos( ( L2^2+L3^2-norm(P-J2)^2)/(2*L2*L3) );th3=-th3;C1=cos(th1);S1=sin(th1);C3=cos(th3);S3=sin(th3);A=C1*px + S1*py;B=pz - L1;W1=(L2+C3*L3);W2=(L3*S3);fprintf('W1^2+W2^2=%4.2f \n',W1^2+W2^2);th2=atan2( (W1*B-W2*A),(W1*A+W2*B) );fprintf('th1=%4.2f \n',th1*ToDeg);fprintf('th2=%4.2f \n',th2*ToDeg);fprintf('th3=%4.2f \n',th3*ToDeg);7.4 Build_6DOFRobot_Lnya.mToDeg = 180/pi;ToRad = pi/180;UX = [1 0 0]';UY = [0 1 0]';UZ = [0 0 1]';Link=struct('name','Body','th',0,'dz',0,'dx',0,'alf',90*ToRad,'az',UZ ); % azLink(1)=struct('name','Base','th',0*ToRad,'dz',-200,'dx',100,'alf',0* ToRad,'az',UZ); %Base To 1Link(2)=struct('name','J1','th',180*ToRad,'dz',250,'dx',0,'alf',90*To Rad,'az',UZ); %1 TO 2Link(3)=struct('name','J2','th',0*ToRad,'dz',0,'dx',100,'alf',0*ToRad ,'az',UZ); %2 TO 3Link(4)=struct('name','J3','th',0*ToRad,'dz',0,'dx',100,'alf',-90*ToR ad,'az',UZ); %3 TO 4Link(5)=struct('name','J3','th',0*ToRad,'dz',0,'dx',50,'alf',0*ToRad, 'az',UZ); %4 TO 5Link(6)=struct('name','J3','th',0*ToRad,'dz',100,'dx',0,'alf',0*ToRad ,'az',UZ); %5 TO 6Link(7)=struct('name','J3','th',0*ToRad,'dz',0,'dx',50,'alf',0*ToRad, 'az',UZ); %6 TO 77.5 Erzhihua.mclear,clcA=imread('广工.png');B=rgb2gray(A);g_max=double(max(max(B)));g_min=double(min(min(B)));T=round((g_max-g_min)/2);D=(double(B)>=T);[Dm,Dn]=size(D);num=1;for m=1:Dmif mod(m,2)==0n=Dn;for ii=1:Dnif D(m,n)==0FP(num,1)=(Dm-m);FP(num,2)=(Dn-n);num=num+1;endn=n-1;endelsen=1;for ii=1:Dnif D(m,n)==0FP(num,1)=(Dm-m);FP(num,2)=(Dn-n);num=num+1;endn=n+1;endendendsave('FP.mat','FP');7.6 draw_Workplace.mclose all;clear;ToDeg = 180/pi;ToRad = pi/180;num=1;th_interval = 30;d_interval = 10;for th1=-180:10:180for th2=-90:th_interval:90for th3=-180:th_interval:0for d4=-40:d_interval:200for th5=-180:th_interval:180for th6=-90:th_interval:90theta1=th1*ToRad;theta2=th2*ToRad;theta3=th3*ToRad;theta5=th5*ToRad;theta6=th6*ToRad;A1 =[[ cos(theta1), 0, sin(theta1), 0] [ sin(theta1), 0, -cos(theta1), 0] [ 0, 1, 0, 250][ 0, 0, 0, 1]];A2 =[[ cos(theta2 ), -sin(theta2 ), 0, 0][ sin(theta2 ), cos(theta2 ), 0, 0][ 0, 0, 1,80][ 0, 0, 0, 1]];A3 =[[ cos(theta3), 0, -sin(theta3), 100*cos(theta3)] [ sin(theta3), 0, cos(theta3), 100*sin(theta3)] [ 0, -1, 0, 0][ 0, 0, 0, 1]]; A4 =[[ 1, 0, 0, 0][ 0, 1, 0, 0][ 0, 0, 1, d4 + 50][ 0, 0, 0, 1]];A5 =[[ cos(theta5), -sin(theta5), 0, 0][ sin(theta5), cos(theta5), 0, 0][ 0, 1, 0, 100][ 0, 0, 0, 1]];A6 =[[ cos(theta6), -sin(theta6), 0, 50*cos( theta6)] [ sin(theta6), cos(theta6), 0, 50*sin( theta6)] [ 0, 0, 1, 0] [ 0, 0, 0, 1]];A = A1 * A2 * A3 * A4 * A5 * A6;point1(num) = A(1,4);point2(num) = A(2,4);point3(num) = A(3,4);num = num + 1;endendendendendendplot3(point1,point2,point3,'r*');axis([-600,600,-600,600,-400,700]);grid on;7.7 Matrix_DH_Ln.mfunction Matrix_DH_Ln(i)global LinkToDeg = 180/pi;ToRad = pi/180;C=cos(Link(i).th);S=sin(Link(i).th);Ca=cos(Link(i).alf);Sa=sin(Link(i).alf);a=Link(i).dx; %distance between zi and zi-1d=Link(i).dz; %distance between xi and xi-1Link(i).n=[C,S,0,0]';Link(i).o=[-1*S*Ca,C*Ca,Sa,0]';Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';Link(i).p=[a*C,a*S,d,1]';Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];7.8 Connect3D.mfunction Connect3D(p1,p2,option,pt)h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option); set(h,'LineWidth',pt)。