Robotics（学习笔记）——第七章轨迹生成(Trajectory Generation)MATLAB编程练习

HerryTaoo

机器人学导论（John J.Crag）第164页的MATLAB编程练习作业

a)三次多项式（起始点和终止点速度为零）

% 3次多项式，起始点终止点速度为零。
clear;clc;
figure('name','三次多项式');
theta_s=120;theta_f=60;
i=1;
% 定义动态数组
theta_t=[];dtheta_t=[];
for t = 0:0.01:1
    theta_t(i)=120-180*t^2 + 120*t^3; % 角度
    dtheta_t(i)=-360*t+360*t^2; % 角速度
    ddtheta_t(i)=-360+720*t; % 角加速度
    dddtheta_t(i)=720; % 角加速度率 
    i=i+1;
%% 绘图
subplot(4,1,1),plot(0:0.01:1,theta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角度(°)');
subplot(4,1,2),plot(0:0.01:1,dtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角速度(°/s)');
subplot(4,1,3),plot(0:0.01:1,ddtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角加速度(°/s^2)');
subplot(4,1,4),plot(0:0.01:1,dddtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角加速度率(°/s^3)');

运行结果：

b)五次多项式（起始点、终止点的速度和加速度为零）

%% 5次多项式，起始点终止点速度和加速度为零,。
clear;clc;close all;
figure('name','五次多项式');
% 题目要求
theta_s=120;theta_f=60;T=1;
dtheta_s=0;ddtheta_s=0;
dtheta_f=0;ddtheta_f=0;
% 定义动态数组
theta_t=[];dtheta_t=[];
%计算多项式的6个系数
a0=theta_s;
a1=dtheta_s;
a2=ddtheta_s/2;
a3=(20*theta_f-20*theta_s-(8*dtheta_f+12*dtheta_s)*T-(3*ddtheta_s-ddtheta_f)*T^2)/(2*T^3);
a4=(30*theta_s-30*theta_f+(14*dtheta_f+16*dtheta_s)*T+(3*ddtheta_s-2*ddtheta_f)




    
*T^2)/(2*T^4);
a5=(12*theta_f-12*theta_s-(6*dtheta_f+6*dtheta_s)*T-(ddtheta_s-ddtheta_f)*T^2)/(2*T^5);
% 计算各个时刻的角度角加速度等
i=1;
for t = 0:0.01:1
    theta_t(i)=a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5; % 角度
    dtheta_t(i)=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4; % 角速度
    ddtheta_t(i)=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3; % 角加速度
    dddtheta_t(i)=6*a3+24*a4*t+60*a5*t^2; % 角加速度率
    i=i+1;
%% 绘图
subplot(4,1,1),plot(0:0.01:1,theta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角度(°)');
subplot(4,1,2),plot(0:0.01:1,dtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角速度(°/s)');
subplot(4,1,3),plot(0:0.01:1,ddtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角加速度(°/s^2)');
subplot(4,1,4),plot(0:0.01:1,dddtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角加速度率(°/s^3)');

运行结果：

c)带有中间点的三次多项式（起始点终止点速度为零,经过中间点速度加速度连续）

%% 带有中间点的三次多项式，起始点终止点速度为零,经过中间点速度加速度连续。
clear;clc;close all;
figure('name','带有中间点的三次多项式');
% 题目要求
theta_s=60;theta_v=120;theta_f=30;t1=1;t2=1;
tf=t1;
%% 计算两段一共8个系数
a10=theta_s;
a11=0;
a12=(12*theta_v-3*theta_f-9*theta_s)/(4*tf^2);
a13 = (-8*theta_v + 3*theta_f + 5*theta_s) / (4*tf^3);
a20 = theta_v; 
a21 = (3*theta_f - 3*theta_s) / (4*tf);
a22 = (-12*theta_v + 6*theta_f + 6*theta_s) / (4*tf^2);
a23 = (8*theta_v - 5*theta_f - 3*theta_s) / (4*tf^3);
%% 计算各个时刻的角度角加速度等
i=1;
for t = 0:0.01:1
    theta_t1(i)=a10+a11*t+a12*t^2+a13*t^3; % 角度
    dtheta_t1(i)=a11+2*a12*t+3*a13*t^2; % 角速度
    ddtheta_t1(i)=2*a12+6*a13*t; % 角加速度
    dddtheta_t1(i)=6*a13; % 角加速度率 
    i=i+1;
i=1;
for t = 0:0.01:1
    theta_t2(i)=a20+a21*t+a22*t^2+a23*t^3; % 角度
    dtheta_t2(i)=a21+2*a22*t+3*a23*t^2; % 角速度
    ddtheta_t2(i)=2*a22+6*a23*t; % 角加速度
    dddtheta_t2(i)=6*a23; % 角加速度率 
    i=i+1;
% 把两段接上
theta_t=[theta_t1 theta_t2(2:101)]; %theta_t2需要去掉第一项
dtheta_t=[dtheta_t1 dtheta_t2(2:101)];
ddtheta_t=[ddtheta_t1 ddtheta_t2(2:101)];
dddtheta_t=[dddtheta_t1 dddtheta_t2(2:101)];
%% 绘图
subplot(4,1,1),plot(0:0.01:2,theta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角度(°)');
subplot(4,1,2),plot(0:0.01:2,dtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角速度(°/s)');
subplot(4,1,3),plot(0:0.01:2,ddtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角加速度(°/s^2)');
subplot(4,1,4),plot(0:0.01:2,dddtheta_t,'r','LineWidth',1);
xlabel('时间(t)'),ylabel('角加速度率(°/s^3)');

运行结果：

根据运行结果中角速度和角加速度图像可知，在中间点位置处，角加速度和角速度均连续，符合题干要求。

d)使用机器人工具箱，jtraj()函数验证

机器人工具箱提供了函数tpoly来产生一个五次多项式轨迹，jtraj()默认也是产生五次多项式轨迹。

%% 使用jtraj()函数进行轨迹生成
clear;clc;close all;
%figure('name','带有中间点的三次多项式');
% 题目要求
t1=1;t2=1;
mdl_puma560;
theta_s = zeros(1,6);theta_f = zeros(1,6);
theta_s(1)=120*pi/180; % 起始点
theta_f(1)=60*pi/180; % 终止点
% 轨迹生成，计算角速度角加速度等
[Q,QD,QDD]=jtraj(theta_s,theta_f,51);
%% 动画显示
subplot(3,2,[1 3 5]);
%figure('name','动画');
p560.plot(Q); % 运行动画显示
%% 显示角度，角速度，角加速度变化曲线
subplot(3,2,2);
plot(0:0.02:1,Q(:,1));
title('关节1角度');
xlabel('时间(t)'),ylabel('角度(°)');
grid on;
subplot(3,2,4);
plot(0:0.02:1,QD(:,1));
title('关节1角速度');
xlabel('时间(t)'),ylabel('角速度(°/s)');
grid on;