程序是基于Matlab2016a，工具箱版本为Robotic Toolbox 10.2
参考博客：
MATLAB机器人工具箱使用
Matlab Robotic Toolbox V9.10工具箱(三)：轨迹规划
六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现
效果：


代码：

clear;
clc;
% 建立连杆系
L1=Link([0 12.4   0   pi/2  0  -pi/2 ]);
L2=Link([0 0      0  -pi/2           ]);
L3=Link([0 15.43  0   pi/2           ]);
L4=Link([0 0      0  -pi/2  0   0    ]);
L5=Link([0 15.925 0   pi/2           ]);
L6=Link([0 0      0  -pi/2           ]);
L7=Link([0 15.0   0   0     0   pi/2 ]);% 机器人模型对象建立
Rbt=SerialLink([L1 L2 L3 L4 L5 L6 L7]);% 6轨迹点的关节变量值
q0=[0 0 0 0 0 0 0];
qsq1=[0.46088 0.37699 0 1.31 0 1.4451 0];
qsq2=[.81681 0.56549 0 1.0681 0 1.2566 0 ];
qsq3=[2.36 0.69115 0 0.848 0 1.4451 0 ];
qsq4=[2.66 0.37699 0 1.31 0 1.4451 0];
qsq5=[pi/2 0.62831 0 1.5708 0 0.94249 0];
qsq6=[0 0.62831 0 1.5708 0 0.94249 0];% 轨迹点规划 五次多项式来规划轨迹
t=0:.04:1;
sqtraj1=jtraj(q0,qsq1,t); 
sqtraj2=jtraj(qsq1,qsq2,t); 
sqtraj3=jtraj(qsq2,qsq3,t); 
sqtraj4=jtraj(qsq3,qsq4,t);
sqtraj5=jtraj(qsq4,qsq1,t);
sqtraj6=jtraj(qsq1,q0,t);
sqtraj7=jtraj(q0,qsq6,t);
hold on%变量初始化
atj=zeros(4,4);
view(-35,40)
xlim([-40,40])
ylim([-40,40])
zlim([0,60])% 绘制第1段轨迹线
for i=1:1:length(t)atj=Rbt.fkine(sqtraj1(i,:));JTA(i,:)=transl(atj);  % 提取位姿矩阵的平移分量（3元素列向量）储存进JTA向量数组jta=JTA; plot2(jta(i,:),'r.')   % 绘制轨迹点（红色点）Rbt.plot(sqtraj1(i,:)) % 绘制轨迹动画plot2(JTA,'b')         % 绘制轨迹线（蓝色线）
end
% 绘制第2段轨迹线
for i=1:1:length(t)atj2=Rbt.fkine(sqtraj2(i,:));JTA2(i,:)=transl(atj2);jta2=JTA2;plot2(jta2(i,:),'r.')Rbt.plot(sqtraj2(i,:))plot2(JTA2,'b')
end
% 绘制第3段轨迹线
for i=1:1:length(t)atj3=Rbt.fkine(sqtraj3(i,:));JTA3(i,:)=transl(atj3);jta3=JTA3;plot2(jta3(i,:),'r.')Rbt.plot(sqtraj3(i,:))plot2(JTA3,'b')
end
% 绘制第4段轨迹线
for i=1:1:length(t)atj4=Rbt.fkine(sqtraj4(i,:));JTA4(i,:)=transl(atj4);jta4=JTA4;plot2(jta4(i,:),'r.')Rbt.plot(sqtraj4(i,:))plot2(JTA4,'b')
end
% 绘制第5段轨迹线
for i=1:1:length(t)atj5=Rbt.fkine(sqtraj5(i,:));JTA5(i,:)=transl(atj5);jta5=JTA5;plot2(jta5(i,:),'r.')Rbt.plot(sqtraj5(i,:))plot2(JTA5,'b')
end
% 绘制第6段轨迹线
for i=1:1:length(t)atj6=Rbt.fkine(sqtraj6(i,:));JTA6(i,:)=transl(atj6);jta6=JTA6;plot2(jta6(i,:),'r.')Rbt.plot(sqtraj6(i,:))plot2(JTA6,'b')
end
% 绘制第7段轨迹线for i=1:1:length(t)atj7=Rbt.fkine(sqtraj7(i,:));JTA7(i,:)=transl(atj7);jta7=JTA7;plot2(jta7(i,:),'r.')Rbt.plot(sqtraj7(i,:))plot2(JTA7,'b')
end