六自由度機械臂三次多項式插值法

對串聯機械臂而言，軌跡規(guī)劃可以分為：關節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃。關節(jié)空間軌跡規(guī)劃是把機器人的關節(jié)變量變換成跟時間的函數，然后對角速度和角加速度進行約束。

笛卡爾空間軌跡規(guī)劃是把機器人末端在笛卡爾空間的位移、速度和加速度變換成跟時間的函數關系。

由于在關節(jié)空間中進行軌跡規(guī)劃是直接用運動時的受控變量規(guī)劃軌跡，有著計算量小，容易實時控制，而且不會發(fā)生機構奇異性等優(yōu)點，所以經常被采用。

現以一維的軌跡為研究對象，利用三次多項式插值法和五次多項式插值法分別對其進行軌跡規(guī)劃，通過對比兩種插值法的效果，選取效果更優(yōu)者對六自由度機械臂進行軌跡規(guī)劃。

** 三次多項式插值法**

三次多項式有4個待定系數，可同時對起始點和目標點的角度和角速度給出約束條件。

數學推導

MATLAB代碼

%三次多項式插值法
clear;
clc;
q_array=[0,50,150,100,0];%指定起止位置
t_array=[0,2,4,8,10];%指定起止時間
v_array=[0,10,20,-15,0];%指定起止速度
t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[0];%初始狀態(tài)
for i=1:1:length(q_array)-1%每一段規(guī)劃的時間
     a0=q_array(i);
     a1=v_array(i);
     a2=(3/(t_array(i+1)-t_array(i))^2)*(q_array(i+1)-q_array(i))-(1/(t_array(i+1)-t_array(i)))*(2*v_array(i)+v_array(i+1));
     a3=(2/(t_array(i+1)-t_array(i))^3)*(q_array(i)-q_array(i+1))+(1/(t_array(i+1)-t_array(i))^2)*(v_array(i)+v_array(i+1));
     ti=t_array(i)+0.001: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;
     vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;
     ai=2*a2+6*a3*(ti-t_array(i));
     t=[t,ti];q=[q,qi];v=[v,vi];a=[a,ai];
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;
% 指定文件夾保存圖片
filepath=pwd;           %保存當前工作目錄
cd('C:UsersAdministratorDesktoppic')                %把當前工作目錄切換到圖片存儲文件夾
print(gcf,'-djpeg','C:UsersAdministratorDesktoppicsan.jpeg'); %將圖片保存為jpg格式，
cd(filepath)            %切回原工作目錄