1、设置曲线路径运行

clear;
clc;
%建立机器人模型
%alpha:连杆扭角；?
%a:连杆长度；?
%theta:关节转角；?
%d:关节距离；?
%offset:偏移

%       theta    d        a        alpha     offset
L1=Link([0       0.4      0.025    pi/2      0     ]);  %定义连杆的D-H参数
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','BiPan');   %连接连杆，机器人取名BiPan
T1=transl(0.5,0,0);     %根据给定起始点，得到起始点位姿
T2=transl(0,0.5,1);     %根据给定终止点，得到终止点位姿
%T3=transl(0,0.5,1);    %根据给定终止点，得到终止点位姿
q1=robot.ikine(T1);     %根据起始点位姿，得到起始点关节角
q2=robot.ikine(T2);     %根据终止点位姿，得到终止点关节角
[q ,qd, qdd]=jtraj(q1,q2,50);    %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
grid on
T=robot.fkine(q);       %根据插值，得到末端执行器位姿
plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:)));%输出末端轨迹
hold on
robot.plot(q);      %动画演示

teach(robot);   %调整各个关节角度

运行结果，蓝色的曲线为运动路径轨迹。

2、 设置直线路径运行

clc;
clear;
%建立机器人模型
%       theta    d        a        alpha     offset
L1=Link([0       0.4      0.025    pi/2      0     ]); %定义连杆的D-H参数
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','BigPan'); %连接连杆，机器人取名BigPan
T1=transl(0.5,0,-0.2);          %根据给定起始点，得到起始点位姿
T2=transl(0,0.5,1.2);           %根据给定终止点，得到终止点位姿
T=ctraj(T1,T2,50);               %点T1到T2路径规划，总插值50个点
Tj=transl(T);
plot3(Tj(:,1),Tj(:,2),Tj(:,3)); %输出末端轨迹
grid on;
q=robot.ikine(T)                %根据点位姿态，得到当前关节角度
hold on
robot.plot(q);                  %动画演示

运行结果，计算得到关节数据

 程序1和2下载链接：https://download.csdn.net/download/panjinliang066333/52724825