机械臂五次多项式插值法介绍

五次多项式插值法

五次多项式有6个待定系数，可同时对起始点和目标点的角度、角速度和角加速度给出约束条件。

数学推导

MATLAB代码

%五次多项式插值法
clear;
clc;
q_array=[0,50,150,100,40];%指定起止位置
t_array=[0,3,6,12,14];%指定起止时间
v_array=[0,10,20,-15,0];%指定起止速度
a_array=[0,20,30,-20,0];%指定起止加速度
t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[a_array(1)];%初始状态
for i=1:1:length(q_array)-1%每一段规划的时间
     T=t_array(i+1)-t_array(i);
     a0=q_array(i);
     a1=v_array(i);
     a2=a_array(i)/2;
     a3=(20*q_array(i+1)-20*q_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);
     a4=(30*q_array(i)-30*q_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);
     a5=(12*q_array(i+1)-12*q_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%计算五次多项式系数 
     ti=t_array(i):0.001:t_array(i+1);
     qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3+a4*(ti-t_array(i)).^4+a5*(ti-t_array(i)).^5;
     vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2+4*a4*(ti-t_array(i)).^3+5*a5*(ti-t_array(i)).^4;
     ai=2*a2+6*a3*(ti-t_array(i))+12*a4*(ti-t_array(i)).^2+20*a5*(ti-t_array(i)).^3;
     t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];
end
subplot(3,1,1),plot(t,q,'r'),xlabel('t/s'),ylabel('p/m');hold on; plot(t_array,q_array,'o','color','r'),grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t/s'),ylabel('v/(m/s)');hold on;plot(t_array,v_array,'*','color','r'),grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t/s'),ylabel('a/(m/s^2)');hold on;plot(t_array,a_array,'^','color','r'),grid on;
% 指定文件夹保存图片
filepath=pwd;           %保存当前工作目录
cd('C:UsersAdministratorDesktoppic')                %把当前工作目录切换到图片存储文件夹
print(gcf,'-djpeg','C:UsersAdministratorDesktoppicwu.jpeg'); %将图片保存为jpg格式，
cd(filepath)            %切回原工作目录

两种插值法的效果对比

相对于三次多项式插值， 五次多项式插值法所得到的轨迹加速度也是平滑的曲线，并没有出现跳变的情况。

在机器人系统中，关节角加速度出现跳变现象意味着关节的电机会受到冲击， 因此为保证电机平稳运行，角加速度要求平滑连续。

虽然三次多项式插值法的计算量和较之更小，但对于离线规划而言，该时间成本可以忽略，因此从规划的轨迹平稳度而言，五次多项式插值法更佳。