六自由度机械臂三次多项式插值法

对串联机械臂而言，轨迹规划可以分为：关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数，然后对角速度和角加速度进行约束。

笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。

由于在关节空间中进行轨迹规划是直接用运动时的受控变量规划轨迹，有着计算量小，容易实时控制，而且不会发生机构奇异性等优点，所以经常被采用。

现以一维的轨迹为研究对象，利用三次多项式插值法和五次多项式插值法分别对其进行轨迹规划，通过对比两种插值法的效果，选取效果更优者对六自由度机械臂进行轨迹规划。

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

三次多项式有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];%初始状态
for i=1:1:length(q_array)-1%每一段规划的时间
     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)            %切回原工作目录