|
前言
此处为单关节在关节空间中生成多项式轨迹，利用MATLAB编程建立关节空间轨迹生成器，生成其关节空间的路径。
1、三次多项式
已知起始点和终止点的角速度为0；起始点角度值为120°，终止点角度值为60°；整个运动时间t=1s。根据公式: 进行计算，在MATLAB中编程运行生成三次多项式单关节轨迹，结果如图1所示：  图1 三次多项式轨迹生成图
2、五次多项式
已知起始点和终止点的角速度，角加速度均为0；起始点角度值为120°，终止点角度值为60°；整个运动时间t=1s，则根据公式: 进行计算，在MATLAB中运行生成五次多项式单关节轨迹曲线，结果如图2所示：  图2 五次多项式轨迹生成图
如图3所示，对比a)中三次多项式和五次多项式的结果可知，三次曲线在初始时刻和终止时刻时，加速度不连续、突变的，会存在冲击。而五次多项式的曲线加速度是连续的，可解决掉三次曲线存在的问题。  图3 三次多项式和五次多项式轨迹对比图
3、 两段带有中间点路径的三次
已知起始点为60°，中间点为120°、终止点为30°，起始点、终止点的速度和加速度为0，中间点t1=1，tf=2,中间时刻的速度不为0，因此在两条三次曲线的连接处，用速度和加速度均连续作为新的约束条件，并根据公式: 进行计算。在MATLAB中代码运行计算结果如图4所示，其仍存在三次多项式加速度不连续的冲击问题。  图4 带中间点的三次多项数轨迹生成图
4、Jtraj()函数
利用Corke MATLAB机器人工具箱中的jtraj()函数，如图5所示，对比五次多项式，如图6所示，得出该函数的轨迹规划也是采用五次多项式插值法。
 图5 jtraj函数生成的轨迹图  图6 五次多项式与jtraj函数生成对比图
附录：MATLAB运行代码
%% a)三次项多项式
clear;close all;clc;
figure('name','三次多项式');
q3_s = 120; q3_f = 60;
t3_s=0; t3_f = 1;
v3_s = 0;v3_f = 0;
%计算系数
a0_3 = q3_s;
a1_3 = 0;
a2_3 = (3/t3_f^2)*(q3_f - q3_s);
a3_3 = (-2/t3_f^3)*(q3_f - q3_s);
j = 1;
for tc = 0: 0.01: 1
q_3(j) = a0_3 + a1_3*tc + a2_3*tc^2 + a3_3*tc^3; %角度变化函数
qd_3(j) = a1_3 + 2*a2_3*tc + 3*a3_3*tc^2;%角速度
qdd_3(j) = 2*a2_3 + 6*a3_3*tc;%角加速度
qddd_3(j) = 6*a3_3;%加速度变化率
j = j + 1;
end
subplot(4,1,1),plot([0:0.01:1], q_3,'r'),xlabel('时间(t)'),ylabel('角度(°)');grid on;
subplot(4,1,2),plot([0:0.01:1], qd_3,'b'),xlabel('时间(t)'),ylabel('速度(°/s)');grid on;
subplot(4,1,3),plot([0:0.01:1], qdd_3,'g'),xlabel('时间(t)'),ylabel('加速度(°/s^2)');grid on;
subplot(4,1,4),plot([0:0.01:1], qddd_3,'k'),xlabel('时间(t)'),ylabel('加速度变化率');grid on;
%% b五次多项式
figure('name','五次多项式');
a_array1 = 0;a_array2 = 0;%起止加速度值
t_array1=0;t_array2=1;%起止时间值
q5_s=120;q5_f=60;%起止角度值
v_array1=0;v_array2=0;%起止速度值
T=t_array2-t_array1
a0=q5_s;
a1=v_array1;
a2=a_array1/2;
a3=(20*q5_f-20*q5_s-(8*v_array2+12*v_array1)*T-(3*a_array1-a_array2)*T^2)/(2*T^3);
a4=(30*q5_s-30*q5_f+(14*v_array2+16*v_array1)*T+(3*a_array1-2*a_array2)*T^2)/(2*T^4);
a5=(12*q5_f-12*q5_s-(6*v_array2+6*v_array1)*T-(a_array1-a_array2)*T^2)/(2*T^5);%计算五次多项式系数
tc=t_array1:0.01:t_array2;
q=a0+a1*tc+a2*tc.^2+a3*tc.^3+a4*tc.^4+a5*tc.^5;
v=a1+2*a2*tc+3*a3*tc.^2+4*a4*tc.^3+5*a5*tc.^4;
a=2*a2+6*a3*tc+12*a4*tc.^2+20*a5*tc.^3;%位置，速度，加速度函数的计算
ad = 6*a3 + 24*a4*tc + 60*a5*tc.^2;
subplot(4,1,1),plot(tc,q,'r'),xlabel('时间(t)'),ylabel('位置（°）');hold on;grid on;
%plot(t_array1,q_array1,'*','color','k');plot(t_array2,q_array2,'*','color','b');
subplot(4,1,2),plot(tc,v,'b'),xlabel('时间(t)'),ylabel('速度（°/S）');hold on;grid on;
%plot(t_array1,v_array1,'*','color','k'),plot(t_array2,v_array2,'*','color','b');
subplot(4,1,3),plot(tc,a,'g'),xlabel('时间(t)'),ylabel('加速度(°/S^2)');hold on;grid on;
%plot(t_array1,a_array1,'*','color','k'),plot(t_array2,a_array2,'*','color','b');
subplot(4,1,4),plot(tc,ad,'k'),xlabel('时间(t)'),ylabel('加速度变化率');hold on;grid on;
%% c)两段带有中间点的三项多项式(连接点的速度和加速度相等)
figure('name','带中间点三次多项式');
% theta(t)_1 = a10 + a11*t1 + a12*t1^2 + a13*t1^3
% theta(t)_2 = a20 + a21*t2 + a22*t2^2 + a23*t2^3
theta_s_ = 60; theta_c_ = 120; theta_f_ = 30;%开始、中间、结尾角度值
tc = 1; t3c_f = 2;%中间、结束角度值
theta_s_d_ = 0; theta_s_dd_ = 0;
theta_f_d_ = 0; theta_f_dd_ = 0;
%计算系数值
a10 = theta_s_;
a11 = 0;
a12 = (12*theta_c_ - 3*theta_f_ - 9*theta_s_) / (4*tc^2);
a13 = (-8*theta_c_ + 3*theta_f_ + 5*theta_s_) / (4*tc^3);
a20 = theta_c_;
a21 = (3*theta_f_ - 3*theta_s_) / (4*tc);
a22 = (-12*theta_c_ + 6*theta_f_ + 6*theta_s_) / (4*tc^2);
a23 = (8*theta_c_ - 5*theta_f_ - 3*theta_s_) / (4*tc^3);
s = 1;
for T = 0: 0.02: 1
theta_1(s) = a10 + a11*T + a12*T^2 + a13*T^3;
theta_d_1(s) = a11 + 2*a12*T + 3*a13*T^2;
theta_dd_1(s) = 2*a12 + 6*a13*T;
theta_ddd_1(s) = 6*a13;
s = s + 1;
end
s = 1;
for T = 0: 0.02: 1
theta_2(s) = a20 + a21*T + a22*T^2 + a23*T^3;
theta_d_2(s) = a21 + 2*a22*T + 3*a23*T^2;
theta_dd_2(s) = 2*a22 + 6*a23*T;
theta_ddd_2(s) = 6*a23;
s = s + 1;
end
% 去掉首尾值
theta_ = [theta_1, theta_2(2: 51)];
theta_d_ = [theta_d_1, theta_d_2(2: 51)];
theta_dd_ = [theta_dd_1, theta_dd_2(2: 51)];
theta_ddd_ = [theta_ddd_1, theta_ddd_2(2: 51)];
subplot(4, 1, 1);plot([0:0.02:2], theta_,'r'),xlabel('时间(t)'),ylabel('角度(°)');grid on;
subplot(4, 1, 2);plot([0:0.02:2], theta_d_,'b'),xlabel('时间(t)'),ylabel('速度(°/s)');grid on;
subplot(4, 1, 3);plot([0:0.02:2], theta_dd_,'g'),xlabel('时间(t)'),ylabel('加速度(°/s^2)');grid on;
subplot(4, 1, 4);plot([0:0.02:2], theta_ddd_,'k'),xlabel('时间(t)'),ylabel('加速度变化率');grid on;
%% c)两段带有中间点的三项多项式(连接点的速度和加速度相等)
figure('name','带中间点三次多项式');
% theta(t)_1 = a10 + a11*t1 + a12*t1^2 + a13*t1^3
% theta(t)_2 = a20 + a21*t2 + a22*t2^2 + a23*t2^3
theta_s_ = 60; theta_c_ = 120; theta_f_ = 30;%开始、中间、结尾角度值
tc = 1; t3c_f = 2;%中间、结束角度值
theta_s_d_ = 0; theta_s_dd_ = 0;
theta_f_d_ = 0; theta_f_dd_ = 0;
%计算系数值
a10 = theta_s_;
a11 = 0;
a12 = (12*theta_c_ - 3*theta_f_ - 9*theta_s_) / (4*tc^2);
a13 = (-8*theta_c_ + 3*theta_f_ + 5*theta_s_) / (4*tc^3);
a20 = theta_c_;
a21 = (3*theta_f_ - 3*theta_s_) / (4*tc);
a22 = (-12*theta_c_ + 6*theta_f_ + 6*theta_s_) / (4*tc^2);
a23 = (8*theta_c_ - 5*theta_f_ - 3*theta_s_) / (4*tc^3);
s = 1;
for T = 0: 0.02: 1
theta_1(s) = a10 + a11*T + a12*T^2 + a13*T^3;
theta_d_1(s) = a11 + 2*a12*T + 3*a13*T^2;
theta_dd_1(s) = 2*a12 + 6*a13*T;
theta_ddd_1(s) = 6*a13;
s = s + 1;
end
s = 1;
for T = 0: 0.02: 1
theta_2(s) = a20 + a21*T + a22*T^2 + a23*T^3;
theta_d_2(s) = a21 + 2*a22*T + 3*a23*T^2;
theta_dd_2(s) = 2*a22 + 6*a23*T;
theta_ddd_2(s) = 6*a23;
s = s + 1;
end
% 去掉首尾值
theta_ = [theta_1, theta_2(2: 51)];
theta_d_ = [theta_d_1, theta_d_2(2: 51)];
theta_dd_ = [theta_dd_1, theta_dd_2(2: 51)];
theta_ddd_ = [theta_ddd_1, theta_ddd_2(2: 51)];
subplot(4, 1, 1);plot([0:0.02:2], theta_,'r'),xlabel('时间(t)'),ylabel('角度(°)');grid on;
subplot(4, 1, 2);plot([0:0.02:2], theta_d_,'b'),xlabel('时间(t)'),ylabel('速度(°/s)');grid on;
subplot(4, 1, 3);plot([0:0.02:2], theta_dd_,'g'),xlabel('时间(t)'),ylabel('加速度(°/s^2)');grid on;
subplot(4, 1, 4);plot([0:0.02:2], theta_ddd_,'k'),xlabel('时间(t)'),ylabel('加速度变化率');grid on;
|