KUKA_kr16机器人建模正逆解及轨迹规划——matlab机器人工具箱_机器人工具箱轨迹规划动画保存

1.KUKA_kr16机器人本体结构

2.建立DH坐标系

 

3.matlab机器人建模

3.1设置初始姿态

对Link函数里面的offset参数进行设定，对各关节进行关节偏移量的设定，来使模型的初始姿态与我们设想的一样。

偏移值为DH参数表中的theta项

matlab代码如下：

clear
clc
%theta      d         a        alpha
 
%标准DH建模
L(1)=Link('d',0,'a',0.260,'alpha',-pi/2);
L(2)=Link('d',0,'a',0.680,'alpha',0);
L(3)=Link('d',0,'a',0.035,'alpha',pi/2);
L(4)=Link('d',0.670,'a',0,'alpha',pi/2);
L(5)=Link('d',0,'a',0,'alpha',pi/2);
L(6)=Link('d',0.158,'a',0,'alpha',0);
robot=SerialLink(L,'name','kr16');
 
%定义关节两偏移量 
robot.base=transl(0,0,0.675);
L(2).offset=-pi/2;
L(3).offset=pi;
L(4).offset=pi;
L(5).offset=pi;
L(6).offset=pi/2;
 
%定义关节变量范围
L(1).qlim=[deg2rad(-185) deg2rad(185)];
L(2).qlim=[deg2rad(-155) deg2rad(35)];
L(3).qlim=[deg2rad(-130) deg2rad(154)];
L(4).qlim=[deg2rad(-350) deg2rad(350)];
L(5).qlim=[deg2rad(-130) deg2rad(130)];
L(6).qlim=[deg2rad(-350) deg2rad(350)];
 
%调整坐标轴即视野
set(gca,'XLim',[-1,1.5]);  %将X轴范围设定为[-1.000,1.500]
set(gca,'YLim',[-1,1.5]);  %将X轴范围设定为[-1.000,1.500]
set(gca,'ZLim',[-1,1.5]);  %将Z轴范围设定为[-1.000,1.500]
% set(gca,'XDir','reverse');    %将x轴方向设置为反向
% set(gca,'YDir','reverse');    %将Y轴方向设置为反向
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
robot.display;
robot.teach;

4.运动学正解

matlab代码如下：

 

% % 正运动学求解
theta_0=[0,0,0,0,0,0];
theta_1=[pi/3,0,pi/6,0,pi/4,0];
T0=robot.fkine(theta_0);
T1=robot.fkine(theta_1);
figure (2)
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
robot.plot(theta_0);
hold on
figure (3)
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
robot.plot(theta_1);

5.运动学逆解

matlab代码如下：

%逆运动学求解
itheta_0=robot.ikine(T0);
itheta_1=robot.ikine(T1);

6.轨迹规划

%首先我们先规划之前得到的两个位姿之间的轨迹
t=[0:0.05:4];%设定步数
p=mtraj(@tpoly, itheta_0, itheta_1, 100);
p1=mtraj(@lspb, itheta_0, itheta_1, 100);
[q,dq,ddq]=mtraj(@tpoly, itheta_0, itheta_1, 100); 
[q2,dq2,ddq2]=mtraj(@lspb, itheta_0, itheta_1, 100);
 
subplot(6,3,[1 4 7  2 5 8 10 13 16  11 14 17])
robot.plot([0 0 0 0 0 0])
 
robot.plot(p)
robot.plot(p1)
 
%绘制角度，速度，加速度曲线
subplot(6,3,3)
plot(q)
subplot(6,3,6)
plot(dq)
subplot(6,3,9)
plot(ddq)
subplot(6,3,12)
plot(q2)
subplot(6,3,15)
plot(dq2)
subplot(6,3,18)
plot(ddq2)
 
7.保存动画
 
figure(4)
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
%将轨迹创建生成视频
out=VideoWriter('robot.avi');
out.FrameRate=10;    %设定每秒10帧
open(out);
robot.plot([0 0 0 0 0 0])
for K=1:100
    robot.plot(p(K,:))
    F=getframe(gcf);
    writeVideo(out,F);
end
close(out);

完整源码如下：

clear
clc
%theta      d         a        alpha
 
%标准DH建模
L(1)=Link('d',0,'a',0.260,'alpha',-pi/2);
L(2)=Link('d',0,'a',0.680,'alpha',0);
L(3)=Link('d',0,'a',0.035,'alpha',pi/2);
L(4)=Link('d',0.670,'a',0,'alpha',pi/2);
L(5)=Link('d',0,'a',0,'alpha',pi/2);
L(6)=Link('d',0.158,'a',0,'alpha',0);
robot=SerialLink(L,'name','kr16');
 
%定义关节两偏移量 
robot.base=transl(0,0,0.675);
L(2).offset=-pi/2;
L(3).offset=pi;
L(4).offset=pi;
L(5).offset=pi;
L(6).offset=pi/2;
 
%定义关节变量范围
L(1).qlim=[deg2rad(-185) deg2rad(185)];
L(2).qlim=[deg2rad(-155) deg2rad(35)];
L(3).qlim=[deg2rad(-130) deg2rad(154)];
L(4).qlim=[deg2rad(-350) deg2rad(350)];
L(5).qlim=[deg2rad(-130) deg2rad(130)];
L(6).qlim=[deg2rad(-350) deg2rad(350)];
 
%调整坐标轴即视野
set(gca,'XLim',[-1,1.5]);  %将X轴范围设定为[-1.000,1.500]
set(gca,'YLim',[-1,1.5]);  %将X轴范围设定为[-1.000,1.500]
set(gca,'ZLim',[-1,1.5]);  %将Z轴范围设定为[-1.000,1.500]
% set(gca,'XDir','reverse');    %将x轴方向设置为反向
% set(gca,'YDir','reverse');    %将Y轴方向设置为反向
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
robot.display;
robot.teach;
 
% % 正运动学求解
theta_0=[0,0,0,0,0,0];
theta_1=[pi/3,0,pi/6,0,pi/4,0];
T0=robot.fkine(theta_0);
T1=robot.fkine(theta_1);
figure (2)
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
robot.plot(theta_0);
hold on
figure (3)
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
robot.plot(theta_1);
 
%逆运动学求解
itheta_0=robot.ikine(T0);
itheta_1=robot.ikine(T1);
 
% 轨迹规划
%首先我们先规划之前得到的两个位姿之间的轨迹
t=[0:0.05:4];%设定步数
p=mtraj(@tpoly, itheta_0, itheta_1, 100);
p1=mtraj(@lspb, itheta_0, itheta_1, 100);
[q,dq,ddq]=mtraj(@tpoly, itheta_0, itheta_1, 100); 
[q2,dq2,ddq2]=mtraj(@lspb, itheta_0, itheta_1, 100);
 
subplot(6,3,[1 4 7  2 5 8 10 13 16  11 14 17])
robot.plot([0 0 0 0 0 0])
 
robot.plot(p)
robot.plot(p1)
 
%绘制角度，速度，加速度曲线
subplot(6,3,3)
plot(q)
subplot(6,3,6)
plot(dq)
subplot(6,3,9)
plot(ddq)
subplot(6,3,12)
plot(q2)
subplot(6,3,15)
plot(dq2)
subplot(6,3,18)
plot(ddq2)
 
figure(4)
set(gca,'View',[50,20]);     %设定视野方向角和俯仰角
%将轨迹创建生成视频
out=VideoWriter('robot.avi');
out.FrameRate=10;    %设定每秒10帧
open(out);
robot.plot([0 0 0 0 0 0])
for K=1:100
    robot.plot(p(K,:))
    F=getframe(gcf);
    writeVideo(out,F);
end
close(out);