Matlab机器人仿真（十）：多项式轨迹生成

%% 三次项多项式
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;


%% 两段带有中间点的三项多项式(连接点的速度和加速度相等)
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;


%% 五次多项式
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;



%% 函数jtraj
mdl_puma560;%模型导入    
k_a = pi/180;   
k_b = 180/pi;

tc=[0:0.01:1];%轨迹步长0.01
T1 = p560.fkine([120 0 0 0 0 0]*k_a);%轴1的开始角度为120；轴的结束角度值为60
T2 = p560.fkine([60 0 0 0 0 0]*k_a);%生成关节1变化度数前后的位姿矩阵

q1 = p560.ikine(T1,'mask',[1 1 1 1 1 1]); %如果是[1 1 1 1 1 0]，则最后一个关节角度一直是0
q2 = p560.ikine(T2,'mask',[1 1 1 1 1 1],'q0',q1); %逆解

%jtraj函数的关节空间运动规划
[q,qt,qtt]=jtraj(q1,q2,tc);

% 画图
figure('name','jtraj函数')
subplot(3, 1, 1);
plot(tc,q(:,1)*k_b,'r') %绘制轴1的关节角随时间的变化
grid on;
xlabel('时间(t)');ylabel('关节角度(°)')

subplot(3,1,2);
plot(tc, qt(:,1)*k_b,'b') %绘制关节角速度随时间的变化
grid on;
xlabel('时间(t)');ylabel('角速度(°/s)')

subplot(3, 1, 3);
plot(tc, qtt(:,1)*k_b,'g') %绘制关节角加速度随时间的变化
grid on;disp(qtt);
xlabel('时间(t)');ylabel('角加速度(°/s^2)');

运行结果：
图1 三次多项式生成轨迹图


图3带中间点插值法

图4 jtraj函数生成的轨迹规划