MATLAB Robotics toolbox工具建立三自由度机械臂 借鉴了各个博客主

clear;
clc;
%建立机器人模型
%定义连杆的D-H参数
%       theta    d        a        alpha     offset
L1=Link([0       0        0.0      pi         0     ]);L1.qlim=[0,pi/6];
L2=Link([0       0        30       pi/2       0     ]);L1.qlim=[0,pi/6];
L3=Link([0       0        70       0          0     ]);L1.qlim=[0,pi/6];
L4=Link([0       0        80       -pi/2      0     ]);L1.qlim=[0,pi/6];
R=SerialLink([L1,L2,L3,L4]); %用定义好的关节建立机器人
qz=[0 0 pi/4 -pi/3];     %6个关节的角度变量值都设为0，可以更改
R.plot(qz,'workspace',[-200,200,-200,200,-200,200]); %显示机器人的图像
%R.display();
T2=transl(140,0,15);%根据给定起始点，得到起始点位姿150 1 140 15 130 30 140 45 150 60 
T5=transl(130,0,30);

%traj_1=ctraj(T2,T5,100);
%JTA=transl(traj_1); 
%plot2(JTA,'b') 
%T7=transl(145,0,30);%根据给定终止点，得到终止点位姿
q1=R.ikine(T2,'mask',[1,1,1,0,0,0]);%根据起始点位姿，得到起始点关节角
%fprintf('q1=%f\n',q1)
q6=R.ikine(T5,'mask',[1,1,1,0,0,0]);%根据终止点位姿，得到终止点关节角
%fprintf('q2=%f\n',q6)
%[q ,qd, qdd]=jtraj(q1,q6,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
%subplot(2,2,1);plot(q);xlabel('Time');ylabel('p');
%subplot(2,2,2);plot(qd);xlabel('Time');ylabel('pd');
%subplot(2,2,3);plot(qdd);xlabel('Time');ylabel('pdd');
q=jtraj(q1,q6,0:0.1:2);%构建轨迹
JTA=transl(R.fkine(q));
plot2(JTA,'b') 
R.plot(q);%动画演示;

笛卡尔空间中进行轨迹规划：
1. traj_1=ctraj(T0,T1,length(t)); % 先调用ctraj函数获得在笛卡尔空间中所规划轨迹的控制点，赋值给变量traj_1，其中T0，T1分别为机械臂末端执行器初始和目标位姿矩阵，length(t)为轨迹控制点个数； 
2. JTA=transl(traj_1); % 利用transl函数求得笛卡尔坐标系中每个轨迹控制点的位置坐标，并将该位置坐标值赋给JTA；
3. plot2(JTA,'b') % 利用蓝色的点绘制所有轨迹。

关节空间中进行轨迹规划：
1. traj_1=jtraj(Q0,Q1,t); % 先调用jtraj函数获得在关节空间中所规划的机器人构型对应的关节角度值，赋值给变量traj_1，其中Q0，Q1分别为机械臂初始和目标构型所对应的关节坐标值，t用来控制为轨迹控制点个数，这里设置为t=0:0.1:2； 
2. JTA=transl(Rbt.fkine(traj_1)); % 利用Rbt.fkine函数求得笛卡尔空间中机器人每个轨迹控制位形所对应的末端执行器位置坐标，并将该位置坐标值赋给JTA；其中，Rbt为利用SerialLink函数建立的机器人模型函数名；fkine为进行正运动学分析的函数，Rbt.fkine函数所输出的是4×4的末端执行器位姿矩阵；transl函数从4×4位姿矩阵中提出位置矩阵。
3. plot2(JTA,'b') % 利用蓝色的点绘制所有轨迹。