机器人仿真，轨迹规划

在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 = robot( {L1 L2 L3 L4 L5 L6} );
 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 ' )