轨迹规划
机器人轨迹规划分为关节空间轨迹规划和笛卡尔空间轨迹规划
关节空间轨迹规划
关节空间轨迹规划是以关节角的函数来描述轨迹(在时间和空间)的轨迹生成方法。
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;