MATLAB机器人工具箱【2】—— 轨迹规划

本文在参考B站up主刘海涛大佬的视频分享基础上，结合自己学习的机器人学知识，利用MATLAB机器人工具箱加深理解和运用，机器人工具箱安装教程见Robotic toolbox 工具箱的安装和初步了解，安装了工具箱后就可以用来愉快的学习啦。

1. 关节空间轨迹规划

关节空间轨迹规划是对机械臂的每个关节的运行进行插值，以保证机械臂的各个关节运动的连续性和稳定性。

%%改进D-H模型
%       关节角   连杆偏距    连杆长度  连杆转角   
%       theta(i) d(i)        a(i-1)   alpha(i-1)  offset
SL1=Link([0      0           0        0           0     ],'modified');
SL2=Link([0      0.14909     0        -pi/2       0     ],'modified');
SL3=Link([0      0           0.4318   0           0     ],'modified');
SL4=Link([0      0.43307     0.02032  -pi/2       0     ],'modified');
SL5=Link([0      0           0        pi/2        0     ],'modified');
SL6=Link([0      0           0        -pi/2       0     ],'modified');
p560=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','puma560');%SerialLink 类函数

%定义轨迹规划初始关节角度（First_Theta）和终止关节角度（Final_Theta）,步数100
First_Theta = [0     0     0     0     0     0];
Final_Theta = [-pi/2    -pi/3    -pi/4    pi/3    pi/5   pi/6];
step = 100;
[q,qd,qdd] = jtraj(First_Theta,Final_Theta,step);
p560.plot(q)

figure(2);
subplot(2,2,1);
i = 1:6;
plot(q(:,i));
grid on;
title('关节位置');

subplot(2,2,2);
i = 1:6;
plot(qd(:,i));
grid on;
title('关节速度');

subplot(2,2,3);
i = 1:6;
plot(qdd(:,i));
grid on;
title('关节加速度');

2. 笛卡尔空间轨迹规划

笛卡尔空间轨迹规划常用于焊接，切割等场合，即对规定好的轨迹进行绘制，即保证机械臂末端的以确定的姿态在规定轨迹上运动。

利用匀加速、匀减速运动来规划轨迹

%%改进D-H模型
%       关节角   连杆偏距    连杆长度  连杆转角   
%       theta(i) d(i)        a(i-1)   alpha(i-1)  offset
SL1=Link([0      0           0        0           0     ],'modified');
SL2=Link([0      0.14909     0        -pi/2       0     ],'modified');
SL3=Link([0      0           0.4318   0           0     ],'modified');
SL4=Link([0      0.43307     0.02032  -pi/2       0     ],'modified');
SL5=Link([0      0           0        pi/2        0     ],'modified');
SL6=Link([0      0           0        -pi/2       0     ],'modified');
p560=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','puma560');%SerialLink 类函数

%根据 First_Theta 和 Final_Theta 正运动学得到初始和终止位姿矩阵
T0 = puma560_fk(0,0,0,0,0,0);
Tf = puma560_fk(-pi/2,-pi/3,-pi/4,pi/3,pi/5,pi/6);
%利用 ctraj 在笛卡尔空间规划轨迹
step = 100;
Tc = ctraj(T0,Tf,step);
Tjtraj = transl(Tc);
plot2(Tjtraj,'r');
grid on;
title('T0到Tf笛卡尔空间轨迹');

for A = 1:100
    qq(:,A) = puma560_ik(Tc(:,:,A));
end
p560.plot(qq');