五次多項式插值法

五次多項式有6個待定系數(shù)，可同時對起始點和目標點的角度、角速度和角加速度給出約束條件。

數(shù)學推導

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)];%初始狀態(tài)
for i=1:1:length(q_array)-1%每一段規(guī)劃的時間
     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);%計算五次多項式系數(shù) 
     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)            %切回原工作目錄

兩種插值法的效果對比

相對于三次多項式插值， 五次多項式插值法所得到的軌跡加速度也是平滑的曲線，并沒有出現(xiàn)跳變的情況。

在機器人系統(tǒng)中，關(guān)節(jié)角加速度出現(xiàn)跳變現(xiàn)象意味著關(guān)節(jié)的電機會受到?jīng)_擊， 因此為保證電機平穩(wěn)運行，角加速度要求平滑連續(xù)。

雖然三次多項式插值法的計算量和較之更小，但對于離線規(guī)劃而言，該時間成本可以忽略，因此從規(guī)劃的軌跡平穩(wěn)度而言，五次多項式插值法更佳。