三、五、七次多项式

在机器人运动控制领域，轨迹规划是很关键的一步，决定了机器人运动效率，常用的轨迹规划方法是多项式插值。

% 三次多项式插值两点间
clear;
clc;
q0=0;q1=10;
v0=0;
v1=0;
t0=0;t1=8;
T=t1-t0;
Q=q1-q0;
a0=q0;
a1=v0;
a2=（3*Q-（2*v0+v1）*T）/power（T2）;
a3=（-2*Q+（v0+v1）*T）/power（T3）;
t=t0:0.01:t1;
q=a0+a1*power（（t-t0）1）+a2*power（（t-t0）2）+a3*power（（t-t0）3）;
v=a1+2*a2*power（（t-t0）1）+3*a3*power（（t-t0）2）;
a=2*a2+6*a3*power（（t-t0）1）;
subplot（321）;plot（tq‘r‘）;ylabel（‘position‘）;
grid on;
subplot（323）;plot（tv‘b‘）;ylabel（‘velocity‘）;
grid on;
subplot（325）;plot（ta‘y‘）;ylabel（‘acceleration‘）;
grid on;

% %三次多项式插值多个点
% clear;
% clc;
% t_array=0:2:100;
% q_array=0:10:500;
% v_array（1）=0;
% v_array（51）=0;
% t=t_array（1）;
% q=q_array（1）;
% v=v_array（1）;
% a=[0];
% for i=1:length（t_array）-1
%    %确定中间点的速度值
%     if（i>1）
%         di1=（q_array（i）-q_array（i-1））/（t_array（i）-t_array（i-1））;
%         di2=（q_array（i+1）-q_array（i））/（t_array（i+1）-t_array（i））;
%         if（（di2>=0 && di1>=0） || （di2<=0 && di1<=0））
%             v_array（i）=1.0/2.0*（di1+di2）;
%         else
%             v_array（i）=0;
%         end  
%     end
% end
% for i=1:length（t_array）-1
%     Qi=q_array（i+1）-q_array（i）;
%     Ti=t_array（i+1）-t_array（i）;
%     a0（i）=q_array（i）;
%     a1（i）=v_array（i）;
%     a2（i）=（3*Qi-（2*v_array（i）+v_array（i+1））*Ti）/power（Ti2）;
%     a3（i）=（-2*Qi+（v_array（i）+v_array（i+1））*Ti）/power（Ti3）;
%     tau=t_array（i）:Ti/100:t_array（i+1）;
%     qi=a0（i）+a1（i）*power（（tau-tau（i））1）+a2（i）*power（（tau-tau（i））2）+a3（i）*power（（tau-tau（i））3）;
%     vi=a1（i）+2*a2（i）*power（（tau-tau（i））1）+3*a3（i）*power（（tau-tau（i））2）;
%     ai=2*a2（i）+6*a3（i）*power（（tau-tau（i））1）;
%     t=[ttau（2:end）];q=[qqi（2:end）];v=[vvi（2:end）];a=[aai（2:end）];
% end
% subplot（311）;plot（tq‘r‘）;ylabel（‘position‘）;
% hold on;
% %plot（t_arrayq_array‘o‘）;
% grid on;
% subplot（312）;plot（tv‘b‘）;ylabel（‘velocity‘）;
% hold on;
% %plot（t_arrayv_array‘o‘）;
% grid on;
% subplot（313）;plot（ta‘y‘）;ylabel（‘acceleration‘）;
% grid on;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1997  2018-11-09 10:49  cubic.m
     文件         770  2018-11-09 10:47  quintic.m
     文件        2265  2018-11-09 10:50  seventh.m