MATLAB机器人工具箱3-及轨迹规划

轨迹规划

机器人轨迹规划分为关节空间轨迹规划和笛卡尔空间轨迹规划

关节空间轨迹规划

关节空间轨迹规划是以关节角的函数来描述轨迹（在时间和空间）的轨迹生成方法。

clc;
clear;

%%轨迹规划的第一步是建立机器人模型，这里建立一个6R机器人
%定义机器人的D-H参数
%  a--连杆长度；alpha--连杆扭角；d--连杆偏距；theta--关节转角
a2=0.4318; a3=0.02032;
d2=0.14909; d4=0.43307;
%            thetai     di      ai-1     alphai-1
L1 = Link([pi/2        0        0           0], 'modified');
L2 = Link([0           d2       0      -pi/2], 'modified');
L3 = Link([-pi/2       0       a2          0], 'modified');
L4 = Link([0           d4      a3     -pi/2], 'modified');
L5 = Link([0            0        0       pi/2], 'modified');
L6 = Link([0            0        0      -pi/2], 'modified');
%连接连杆，为机器人命名
robot=SerialLink([L1 L2 L3 L4 L5 L6], 'name', 'Tuesday');     %另一种命名方式robot.name = 'Tuesday'
%robot.display();
%robot.plot([-pi/2 -pi/2 0 0 0 0]);
%robot.teach();

%%关节空间轨迹规划
%[Q,QD,QDD] = jtraj(Q0, QF,M), 其中Q为关节空间轨迹，QD为关节速度，QDD为加速度；
% Q0为初始关节角度，QF为目标关节角度，时间t假设在M步内从0到1，采用五次多项式
initial_theta = [0 0 0 0 0 0];
target_theta = [0 pi/2 0 pi/3 pi/4 pi/5];
step=50;
[q,qd,qdd] = jtraj(initial_theta, target_theta, step);
robot.plot(q);

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

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

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

 

 

笛卡尔空间轨迹规划

%%笛卡尔空间轨迹规划
%TC = ctraj(T0, T1, N)，其实T0为初始关节位姿，T1为目标关节位姿。有N个点沿着路径遵循梯形速度剖面
T0 = robot.fkine(initial_theta);
T1 = robot.fkine(target_theta);
TC = ctraj(T0, T1, step);
Tjtraj = transl(TC);
subplot(2,2,4);
plot2(Tjtraj, 'g');
title('T0到T1笛卡尔空间直线轨迹');
grid on;