机械臂轨迹规划篇（二）MATLAB测试五次多项式样条插值

前言

前一阵子写了三次多项式样条插值反响还不错，所以一直在抽时间将五次多项式样条插值补上，其实理解了三次多项式的基础上，写五次的也不难，表达式和约束条件多了一点而已。

五次插值原理

五次多项式共有六个待定系数，要想六个系数得到确定，至少需要六个条件。五次多项式可以看作是关节角度的时间函数，因此其一阶可导和二阶可导分别可以看作是关节角速度和关节角加速度的时间函数。
五次多项式及一阶、二阶导数公式如下：
$$\{\begin{array}{l}\theta (t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5\\ \dot{\theta (t)}=a_1+2a_{2}t+3a_3{t}^2+4a_4{t}^3+5a_5{t}^4\\ \ddot{\theta (t)}=2a_2+6a_{3}t+12a_4{t}^2+20a_5{t}^3\end{array}$$
为了求得待定系数a0,a1,a2，a3，a4，a5, 对起始点和目标同时给出关于角度和角加速度的约束条件：
$$\{\begin{array}{l}\theta (t_0)=a_0+a_1{t}_0+a_2{t}_0^2+a_3{t}_0^3+a_4{t}_0^4+a_5{t}_0^5\\ \theta (t_f)=a_0+a_1{t}_f+a_2{t}_f^2+a_3{t}_f^3+a_4{t}_f^4+a_5{t}_f^5\\ \dot{\theta (t_0)}=a_1+2a_2{t}_0+3a_3{t}_0^2+4a_4{t}_0^3+5a_5{t}_0^4\\ \dot{\theta (t_f)}=a_1+2a_2{t}_f+3a_3{t}_f^2+4a_4{t}_f^3+5a_5{t}_f^4\\ \ddot{\theta (t_0)}=2a_2+6a_3{t}_0+12a_4{t}_0^2+20a_5{t}_0^3\\ \ddot{\theta (t_f)}=2a_2+6a_3{t}_f+12a_4{t}_f^2+20a_5{t}_f^3\end{array}$$
式中θ_t0 、θ_tf 分别表示起始点和目标点的关节角，$$\dot{\theta (t_0)}$$、$$\dot{\theta (t_f)}$$分别表示起始点和目标点的关节角
速度，$$\ddot{\theta (t_0)}$$ 、$$\ddot{\theta (t_f)}$$ 分别表示起始点和目标点的关节角加速度。
将起始时间设为0，解得表达式为
$$\{\begin{array}{l}{a}_0={\theta }_0\\ a_1=\dot{{\theta }_0}\\ a_2=\ddot{\frac{{\theta }_0}{2}}\\ a_3=\frac{20{\theta }_f-20{\theta }_0-(8\dot{{\theta }_f}+12\dot{{\theta }_0})t_f-(3{\ddot{\theta }}_0-\ddot{{\theta }_f)}{t}_f^2}{2t_f^3}\\ a_4=\frac{30{\theta }_0-30{\theta }_f+(14\dot{{\theta }_f}+16\dot{{\theta }_0})t_f+(3{\ddot{\theta }}_0-2\ddot{{\theta }_f)}{t}_f^2}{2t_f^4}\\ a_5=\frac{12{\theta }_f-12{\theta }_0-(6\dot{{\theta }_f}+6\dot{{\theta }_0})-({\ddot{\theta }}_0-\ddot{{\theta }_f)}{t}_f^2}{2t_f^5}\end{array}$$
为了对比上回所讲的三次多项式关节插值算法与五次多项式插值算法的效果，同样要求机械臂从起始点开始运动，经过5s到达终点，仿真时起始点和目标点的关节角速度为20。中间点的关节角加速度还可以对相邻两段轨迹角加速度进行平均值求解，使该值为中间点的瞬时加速度。利用MATLAB对五次多项式插值进行仿真，将结果与三次多项式插值进行对比，增加角加速度约束：
$$\begin{gathered} {\theta }_A=25,{\theta }_B=70,{\theta }_C=50； \\ \dot{{\theta }_A}=30,\dot{{\theta }_B}=20,\dot{{\theta }_C}=30; \\ \ddot{{\theta }_A}=2,\ddot{{\theta }_B}=4,\ddot{{\theta }_C}=3 \end{gathered}$$

Matlab仿真测试

clear;
clc;
q_array=[25,70,50];%指定起止位置
t_array=[0,2,5];%指定起止时间
v_array=[30,20,30];%指定起止速度
a_array=[2,4,3];%指定起止加速度
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.02: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'),ylabel('position');
hold on;
plot(t_array,q_array,'o','color','g'),grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');
hold on;
plot(t_array,v_array,'*','color','y'),grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');
hold on;
plot(t_array,a_array,'^','color','r'),grid on;

相比于三次多项式插值，关节角度和角速度曲线显示的都相对平滑，而角加速度曲线在中间点2s处变化稍大。结果分析得出，五次多项式插值法虽然计算量有所增加，但是其关节空间轨迹平滑、运动稳定，且阶数越高满足的约束项越多。由加速度曲线对比，采用五次多项式插值，保证速度平滑，加速度不突变。