guihua.m

机械臂规划算法,朋友写的,可以用来学习交流

Qa=[0 -90 0 0 0 0];   %Qa是机器人原点位姿时，六个关节的角度
Qb=[-18.4954,-37.6183,-59.7864,19.86,110.9672,7.3648];  %Qb是机器人目标位置，各关节的角度
t=[0:0.015:1];
Q=jtraj(Qa,Qb,t);
q1=Q(:,1);
q2=Q(:,2);
q3=Q(:,3);
q4=Q(:,4);
q5=Q(:,5);
q6=Q(:,6);
% 画出关节1关节角的轨迹
   
plot(t,q1)
grid
title('关节1角度与时间的关系')
xlabel('t/s')
ylabel('角度/度')
% 画出关节2关节角的轨迹
figure
plot(t,q2)
grid
title('关节2角度与时间的关系')
xlabel('t/s')
ylabel('角度/度')

% 画出关节3关节角的轨迹
figure
plot(t,q3)
grid
title('关节3角度与时间的关系')
xlabel('t/s')
ylabel('角度/度')

% 画出关节4关节角的轨迹
figure
plot(t,q4)
grid
title('关节4角度与时间的关系')
xlabel('t/s')
ylabel('角度/度')

% 画出关节5关节角的轨迹
figure
plot(t,q5)
grid
title('关节5角度与时间的关系')
xlabel('t/s')
ylabel('角度/度')

% 画出关节6关节角的轨迹
figure
plot(t,q6)
grid
title('关节3角度与时间的关系')
xlabel('t/s')
ylabel('角度/度')

%以下是直角坐标轨迹规划图
L1=link([-pi/2 150 0 0]);
L2=link([pi 260 0 0 0]);
L3=link([-pi/2 60 0 0 0]);
L4=link([pi/2 0 0 -260 0]);
L5=link([-pi/2 0 0 0 0]);
L6=link([pi 0 0 -90 0]);
r=robot({L1 L2 L3 L4 L5 L6});
 r.name='SV3X';
 drivebot(r)
qa=[0 0 0 0 0 0];
qb=[-18.4954*pi/180,-37.6183*pi/180,-59.7864*pi/180,19.86*pi/180,110.9672*pi/180,7.3648*pi/180];
t=[0:0.015:1];
tt= jtraj(0, 1, t);
Ta=fkine(r,qa);
Tb=fkine(r,qb);
TC = ctraj(Ta, Tb, tt);
xyz=transl(TC);
x=xyz(:,1);
y=xyz(:,2);
z=xyz(:,3);
plot(t,x)
grid
title('末端在机器人坐标系下X轴位置与时间的关系')
xlabel('t/s')
ylabel('x/m')

figure
plot(t,y)
grid
title('末端在机器人坐标系下Y轴位置与时间的关系')
xlabel('t/s')
ylabel('y/m')
figure
plot(t,z)
grid
title('末端在机器人坐标系下Z轴位置与时间的关系')
xlabel('t/s')
ylabel('z/m')