机器人仿真,轨迹规划

在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 ' )

你可能感兴趣的:(规划)