ABB_IRB120关节运动规划

---

1.基础函数

[q,qd,qdd]=jtraj(q1,q2,N);
[q,qd,qdd]=jtraj(q1,q2,N,qd1,qd2);
[q,qd,qdd]=jtraj(q1,q2,T);
[q,qd,qdd]=jtraj(q1,q2,T,qd1,qd2);
说明:q、qd、qdd分别为输出的角度、角速度和角加速度
     q1、q2分别为初始角度和终点角度
     N为规划的点数，T为给定时间向量
[q,qd,qdd]=mtraj(@tpoly,q1,q2,t);%生成一个五次多项式轨迹
[q,qd,qdd]=mtraj(@lspb,q1,q2,t);%生成梯形轨迹

---

2.机器人工具箱的运动规划

clear;clc;close all;
%          theta     d       a      alpha
L(1)=Link([  0      124      0      -pi/2   ],'standard');
L(2)=Link([  0       0      270      0      ],'standard');L(2).offset=-pi/2;
L(3)=Link([  0       0       70     -pi/2   ],'standard');
L(4)=Link([  0      302      0       pi/2   ],'standard');
L(5)=Link([  0       0       0      -pi/2   ],'standard');
L(6)=Link([  0      72       0        0     ],'standard');
Robot=SerialLink(L,'name','ABB120');
Robot.base=transl(0,0,166);
L(1).qlim=[-165/180*pi,165/180*pi];
L(2).qlim=[-110/180*pi,110/180*pi];
L(3).qlim=[-90/180*pi,70/180*pi];
L(4).qlim=[-160/180*pi,160/180*pi];
L(5).qlim=[-120/180*pi,120/180*pi];
L(6).qlim=[-400/180*pi,400/180*pi];
v=[60 10];%观看视角【方位角，仰角】
w=[-800 800 -800 800 -100 1000];%工作空间大小
light('Position',[-1 -1 -1],'color','w');%光源设置
t=0:1/50:1;
q1=[0 0 0 0 0 0];
q2=[pi/2 pi/2 -pi/2 0 0 0];
q=jtraj(q1,q2,t);
T=Robot.fkine(q);
for i=1:size(q)
    TT(i,:)=T(1,i).t;
    Robot.plot(q(i,:),'tilesize',800);
    hold on
    plot3(TT(:,1),TT(:,2),TT(:,3),'-b','LineWidth',2);
end

3.plot3d函数的运动规划

clear;clc;close all;
%          theta     d       a      alpha
L(1)=Link([  0      124      0      -pi/2   ],'standard');
L(2)=Link([  0       0      270      0      ],'standard');L(2).offset=-pi/2;
L(3)=Link([  0       0       70     -pi/2   ],'standard');
L(4)=Link([  0      302      0       pi/2   ],'standard');
L(5)=Link([  0       0       0      -pi/2   ],'standard');
L(6)=Link([  0      72       0        0     ],'standard');
Robot=SerialLink(L,'name','ABB120');
Robot.base=transl(0,0,166);
L(1).qlim=[-165/180*pi,165/180*pi];
L(2).qlim=[-110/180*pi,110/180*pi];
L(3).qlim=[-90/180*pi,70/180*pi];
L(4).qlim=[-160/180*pi,160/180*pi];
L(5).qlim=[-120/180*pi,120/180*pi];
L(6).qlim=[-400/180*pi,400/180*pi];
v=[60 10];%观看视角【方位角，仰角】
w=[-800 800 -800 800 0 1000];%工作空间大小
light('Position',[-1 0 -1],'color','w','style','local');
%初始位置
q0=[0 0 0 0 0 0];
Robot.plot3d(q0,'tilesize',1,'workspace',w,'path','D:\Desktop\ABB120机器人仿真\RobotLink','nowrist','view',v);
t=0:1/50:1;
q1=[0 0 0 0 0 0];
q2=[pi/2 pi/2 -pi/2 0 0 0];
q=jtraj(q1,q2,t);
T=Robot.fkine(q);
for i=1:size(q)
    clf;
    TT(i,:)=T(1,i).t;
    plot3(TT(:,1),TT(:,2),TT(:,3),'-b','LineWidth',2);
    hold on
    Robot.plot3d(q(i,:),'tilesize',1,'workspace',w,'path','D:\Desktop\ABB120机器人仿真\RobotLink','nowrist','view',v);
end

4.轨迹比较