1.基础函数
T=ctraj(T1,T2,t);
T=ctraj(T1,T2,N);
代码:
%设置初始位置
q0=[theta1 theta2 theta3 theta4 theta5 theta6];
T=Robot.fkine(T);
RPY=tr2rpy(T);%将运动姿态设置成与初始姿态相同
T1=transl(x1,y1,z1)*rpy2tr(RPY);
T2=transl(x2,y2,z2)*rpy2tr(RPY);
t=0:1/fs:1;
T=ctraj(T1,T2,t);
q=Robot.ikine(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=[90 0];%观看视角【方位角,仰角】
w=[-800 800 -800 800 -100 1000];%工作空间大小
%初始位置
q0=[0 0 0 0 pi/2 0];
T=Robot.fkine(q0);
RPY=tr2rpy(T);
t=0:1/50:1;
T1=transl(302,-300,300)*rpy2tr(RPY);
T2=transl(302,300,300)*rpy2tr(RPY);
T=ctraj(T1,T2,t);
q=Robot.ikine(T);
for i=1:size(q)
TT(i,:)=T(1:3,4,i);
Robot.plot(q(i,:),'tilesize',800,'workspace',w,'view',v);
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=[90 20];%观看视角【方位角,仰角】
w=[-800 800 -800 800 -100 1000];%工作空间大小
%初始位置
q0=[0 0 0 0 pi/2 0];
Robot.plot3d(q0,'tilesize',1,'workspace',w,'path','D:\Desktop\ABB120机器人仿真\RobotLink','nowrist','view',v);
T=Robot.fkine(q0);
RPY=tr2rpy(T);
t=0:1/50:1;
T1=transl(302,-300,300)*rpy2tr(RPY);
T2=transl(302,300,300)*rpy2tr(RPY);
T=ctraj(T1,T2,t);
q=Robot.ikine(T);
for i=1:size(q)
clf
TT(i,:)=T(1:3,4,i);
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.轨迹比较