六自由度機(jī)械臂三次多項(xiàng)式插值法

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

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

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

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

** 三次多項(xiàng)式插值法**

三次多項(xiàng)式有4個(gè)待定系數(shù)，可同時(shí)對(duì)起始點(diǎn)和目標(biāo)點(diǎn)的角度和角速度給出約束條件。

數(shù)學(xué)推導(dǎo)

MATLAB代碼

%三次多項(xiàng)式插值法
clear;
clc;
q_array=[0,50,150,100,0];%指定起止位置
t_array=[0,2,4,8,10];%指定起止時(shí)間
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ī)劃的時(shí)間
     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;           %保存當(dāng)前工作目錄
cd('C:UsersAdministratorDesktoppic')                %把當(dāng)前工作目錄切換到圖片存儲(chǔ)文件夾
print(gcf,'-djpeg','C:UsersAdministratorDesktoppicsan.jpeg'); %將圖片保存為jpg格式，
cd(filepath)            %切回原工作目錄