六轴机械臂码垛货物堆叠仿真

1、建立模型与仿真

clear,clc,close all
addpath(genpath('.'))
%建立模型参数如下：
L(1) = Link(  'd',     0.122,  'a' , 0 , 'alpha',  pi/2,'offset',0);
L(2) = Link(  'd',    0.019 ,  'a' ,0.408 , 'alpha', 0,'offset',pi/2);
L(3) = Link(  'd',  0 ,  'a' , 0.3759 , 'alpha',0,'offset',0);
L(4) = Link( 'd',  0.1023,  'a' , 0 , 'alpha', -pi/2 ,'offset',pi/2);
L(5) = Link( 'd', -0.1023 ,  'a' , 0 , 'alpha',pi/2,'offset',pi);
L(6) = Link( 'd',  -0.25281 ,  'a' , 0 , 'alpha',0,'offset',0);
robot = SerialLink(L,'name','六轴机械臂'); %建立连杆机器
% robot.display;
% robot.teach; 
%%

%建立工作平台和货物
q0=[0 0 0 0 0 0];%初始位置
T0=robot.fkine(q0);%正运动学得到旋转矩阵
 hold on
 axis([-1.2 1.2 -1.2 1.2 -0.4 1.5] )%定义坐标系范围
 %显示机械臂初始位置
 robot.plot3d(q0,'tilesize',0.1,'view',[120,20],'path',...
   'E:\Project tasks_unfinished2\六月份\6-23 400 机械臂码垛货物堆叠\2023t5')
 %第一个参数是长方体的原点，第二个参数是长宽高，输入命令：
gx=0.5;gy=0.5;gz=0.2;
PlotCuboid([-1,-1,-0.3],[2,2,0.4],0.2)%定义工作台
plot3Cube([gx,gy,gz],0.1,'black');%第一个货物
plot3Cube([gx,-gy,gz],0.1,'black');%第二个货物      
plot3Cube([-gx,gy,gz],0.1,'black'); %第三个货物           
plot3Cube([-gx,-gy,gz],0.1,'black'); %第四个货物         

%%
%搬运第二个货物
T1=transl(gx,-gy,gz)*trotx(0);%转化为旋转矩阵
q1=robot.ikunc(T1);%求逆
[qt1,qt2,qt3]=jtraj(q0,q1,30);%利用五次多项式函数轨迹规划
%显示机械臂动画
 robot.plot3d(qt1,'tilesize',0.1,'view',[120,20])

T2=transl(gx,gy,gz+0.1)*trotx(0);%转化为旋转矩阵
q2=robot.ikunc(T2);%求逆
[qt1,qt2,qt3]=jtraj(q1,q2,30);%利用五次多项式函数轨迹规划
TR1=transl(robot.fkine(qt1));
%循环显示动画
for i=1:length(qt1)
clf
robot.plot3d(qt1(i,:),'tilesize',0.1,'view',[120,20],'delay',0)
hold on
PlotCuboid([-1,-1,-0.3],[2,2,0.4],0.2)%定义工作台
plot3Cube([gx,gy,gz],0.1,'black');%第一个货物
plot3Cube([TR1(i,1),TR1(i,2),TR1(i,3)],0.1,'black');%第二个货物      
plot3Cube([-gx,gy,gz],0.1,'black'); %第三个货物           
plot3Cube([-gx,-gy,gz],0.1,'black'); %第四个货物
pause(0)
end
%%
%搬运第三个货物
T3=transl(-gx,gy,gz)*trotx(0);%转化为旋转矩阵
Tc1=ctraj(T2, T3,30);%直线轨迹规划
q31=robot.ikunc(Tc1);%求逆得关节角度
 robot.plot3d(q31,'tilesize',0.1,'view',[120,20])

T4=transl(gx,gy,gz+0.2)*trotx(0);%转化为旋转矩阵
q4=robot.ikunc(T4);%求逆得到关节角度
Tc1=ctraj(T3, T4,30);%直线轨迹规划
q41=robot.ikunc(Tc1);%求逆得关节角度
TR2=transl(Tc1);%提取空间坐标点
%循环显示动画
for i=1:length(q41)
clf
robot.plot3d(q41(i,:),'tilesize',0.1,'view',[120,20],'delay',0)
hold on
PlotCuboid([-1,-1,-0.3],[2,2,0.4],0.2)%定义工作台
plot3Cube([gx,gy,gz],0.1,'black');%第一个货物
plot3Cube([TR1(end,1),TR1(end,2),TR1(end,3)],0.1,'black');%第二个货物      
plot3Cube([TR2(i,1),TR2(i,2),TR2(i,3)],0.1,'black'); %第三个货物           
plot3Cube([-gx,-gy,gz],0.1,'black'); %第四个货物
pause(0)
end

%%
%搬运第四个货物
T5=transl(-gx,-gy,gz)*trotx(0);%转化为旋转矩阵
q5=robot.ikunc(T5);%求逆
[qt1,qt2,qt3]=jtraj(q4,q5,30);%利用五次多项式函数轨迹规划
 robot.plot3d(qt1,'tilesize',0.1,'view',[120,20])

T5=transl(-gx,-gy,gz)*trotx(0);%转化为旋转矩阵
q5=robot.ikunc(T5);%求逆
q51=q5;
q51(1)=q51(1)+pi/2;
T=robot.fkine(q51);
T51=[T.n(1),T.o(1),T.a(1),T.t(1);%%得到旋转矩阵
    T.n(2),T.o(2),T.a(2),T.t(2);
    T.n(3),T.o(3),T.a(3),T.t(3);
    0 0 0 1];
[qt1,qt2,qt3]=jtraj(q5,q51,30);%利用五次多项式函数轨迹规划
TR31=transl(robot.fkine(qt1));%得到空间坐标点

T6=transl(gx,gy,gz+0.3)*trotx(0);%转化为旋转矩阵
% q6=robot.ikunc(T6);%求逆
Tc1=ctraj(T51, T6,30);%直线轨迹规划
q61=robot.ikunc(Tc1);%求逆得到关节角度
qtd=[qt1;q61];%合并关节角度
TR32=transl(Tc1);%提取空间坐标点
TR3=[TR31;TR32];%得到空间坐标点
%循环显示动画
for i=1:length(qtd)
clf
robot.plot3d(qtd(i,:),'tilesize',0.1,'view',[120,20],'delay',0)
hold on
PlotCuboid([-1,-1,-0.3],[2,2,0.4],0.2)%定义工作台
plot3Cube([gx,gy,gz],0.1,'black');%第一个货物
plot3Cube([TR1(end,1),TR1(end,2),TR1(end,3)],0.1,'black');%第二个货物      
plot3Cube([TR2(end,1),TR2(end,2),TR2(end,3)],0.1,'black'); %第三个货物           
plot3Cube([TR3(i,1),TR3(i,2),TR3(i,3)],0.1,'black'); %第四个货物
pause(0)
end

仿真视频如下：