基于Matlab的空间描点机器人建模与仿真报告

课程设计
课程名称机器人学
题目名称空间描点机器人建模仿真学生学院
专业班级
学号
学生姓名
指导教师
目录
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*n
n n n n n n T A Rot a Trans d Trans a Rot n θα+==轴,轴,
具体的齐次变换矩阵为：
10010
001
0010
000010001000000
1000
10
0100000010001000100
1S 00
00n n n n
n n n n
n n n
n n n n n n n n n
n 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 参数表，可求得如下各个关节之间的齐次变化矩阵：
1111
112000*********
01C S S C A T θθθθ⎛⎫ ⎪- ⎪
== ⎪
⎪⎝⎭
(1) 22222
230
0 00001800001C S S C A T θθθθ-⎛⎫ ⎪
⎪
== ⎪
⎪⎝⎭
(2)
3333
333
340
100010001000001C S C S C S A T θθθθθθ-⎛⎫
⎪
⎪
== ⎪- ⎪
⎝⎭ (3) 4
45100a450010000100
001A T +⎛⎫
⎪
⎪
== ⎪
⎪
⎝⎭
(4)
555
55
560
0000011000001C S S C A T θθθθ-⎛⎫ ⎪ ⎪
== ⎪
⎪⎝⎭
(5)
66666666705005000100
01C S C S C S A T θθθθθθ-⎛⎫
⎪
⎪
== ⎪
⎪
⎝⎭
(6)
3.2 空间描点机器人工作空间 通过matlab 程序仿真，我们可以得到如下结果：
图5 立体图 图6 X-Y 视角
图7 Y-Z 视图 图8 X-Z 视图
4 几何法求逆解
本课程设计所建立的机器人模型的逆解是通过几何法来进行求解的，总体的思路是第四个关节为滑动关节，第五、六关节为固定关节，所以当知道目标点的位置信息后，可以通过第四节中的各个关节
间的齐次变换矩阵，求得1
4T ，接着的话便可以通过3自由度机器人几
何法求逆解的方法求出相应的关节转动角度。

具体的公式推导过程如下：
4
4567567
55666556665656565656565656**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θθ，所以可得
4
71
0050a4500100001100
001T ++⎛⎫
⎪
⎪= ⎪ ⎪
⎝⎭
（8）
由于
1
1234567234567*****T T T T T T T = （9）
可得
()1
1
14477T 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 = 1
ToDeg = 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:360
DHfk6Dof_Lnya(th1+i,th2,th3,d4,th5,th6,1);
end
for i=0:stp:360
DHfk6Dof_Lnya(th1,th2+i,th3,d4,th5,th6,1);
end
for i=0:stp:360
DHfk6Dof_Lnya(th1,th2,th3+i,d4,th5,th6,1);
end
for i=0:60:360
DHfk6Dof_Lnya(th1,th2,th3,d4+i,th5,th6,1);
end
for i=360:-60:0
DHfk6Dof_Lnya(th1,th2,th3,d4+i,th5,th6,1);
end
for i=0:stp:360
DHfk6Dof_Lnya(th1,th2,th3,d4,th5+i,th6,1);
end
for i=0:stp:360
DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6+i,1);
end
pause;
%%%%%%%%%%%%%%%%%%%%%%%画球%%%%%%%%%%%%%%%%%%%%%%%% 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:31
for j=1:1:31
px=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;
end
end
% %%%%%%%%%%%%%%%%%%%%描字%%%%%%%%%%%%%%%%%%%%%
global Link
point21=[];
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==FPh
DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,0);
else
DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,1);
end
point21(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;
end
cla;
plot3(point11,point12,point13,'k*');hold on;
plot3( point21, point22, point23,'r.');hold on;
7.2 DHfk6Dof_Lnya.m
function pic=DHfk6Dof_Lnya(th1,th2,th3,d4,th5,th6,fcla) global Link
Build_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:7
Matrix_DH_Ln(i);
End
for i=2:7
Link(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;
end
grid on;
axis([-600,500,-500,500,-500,500]);
xlabel('x');
ylabel('y');
zlabel('z');
drawnow;
pic=getframe;
if(fcla)
cla;
end
7.3 IK_6DOF_Rob_Lnya.m
function [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.m
ToDeg = 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 ); % az
Link(1)=struct('name','Base','th',0*ToRad,'dz',-200,'dx',100,'alf',0* ToRad,'az',UZ); %Base To 1
Link(2)=struct('name','J1','th',180*ToRad,'dz',250,'dx',0,'alf',90*To Rad,'az',UZ); %1 TO 2
Link(3)=struct('name','J2','th',0*ToRad,'dz',0,'dx',100,'alf',0*ToRad ,'az',UZ); %2 TO 3
Link(4)=struct('name','J3','th',0*ToRad,'dz',0,'dx',100,'alf',-90*ToR ad,'az',UZ); %3 TO 4
Link(5)=struct('name','J3','th',0*ToRad,'dz',0,'dx',50,'alf',0*ToRad, 'az',UZ); %4 TO 5
Link(6)=struct('name','J3','th',0*ToRad,'dz',100,'dx',0,'alf',0*ToRad ,'az',UZ); %5 TO 6
Link(7)=struct('name','J3','th',0*ToRad,'dz',0,'dx',50,'alf',0*ToRad, 'az',UZ); %6 TO 7
7.5 Erzhihua.m
clear,clc
A=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:Dm
if mod(m,2)==0
n=Dn;
for ii=1:Dn
if D(m,n)==0
FP(num,1)=(Dm-m);
FP(num,2)=(Dn-n);
num=num+1;
end
n=n-1;
end
else
n=1;
for ii=1:Dn
if D(m,n)==0
FP(num,1)=(Dm-m);
FP(num,2)=(Dn-n);
num=num+1;
end
n=n+1;
end
end
end
save('FP.mat','FP');
7.6 draw_Workplace.m
close all;
clear;
ToDeg = 180/pi;
ToRad = pi/180;
num=1;
th_interval = 30;
d_interval = 10;
for th1=-180:10:180
for th2=-90:th_interval:90
for th3=-180:th_interval:0
for d4=-40:d_interval:200
for th5=-180:th_interval:180
for th6=-90:th_interval:90
theta1=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;
end
end
end
end
end
end
plot3(point1,point2,point3,'r*');
axis([-600,600,-600,600,-400,700]);
grid on;
7.7 Matrix_DH_Ln.m
function Matrix_DH_Ln(i)
global Link
ToDeg = 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-1
d=Link(i).dz; %distance between xi and xi-1
Link(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.m
function Connect3D(p1,p2,option,pt)
h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option); set(h,'LineWidth',pt)。