机器人仿真，轨迹规划

在MATLAB环境下，用软件 Robotics Toolbox for MATLAB

writen by wqj1212

%以下是关节坐标系轨迹规划图

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.name='SV3X';

drivebot(r)

qa=[0 -pi/2 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')