#多项式轨迹–五次多项式
1.5 Polynomial of degree five
利用三次多项式，根据过$q_0,q_1,\ldots,q_n$确定的轨迹的特征是位置和速度连续，但是加速度不连续（参见上一篇博客轨迹规划–三次多项式轨迹）。尽管三次多项式轨迹确定的轨迹有一定“平滑”，但是对于一些应用的动力学和惯性载荷会产生一些不期待的影响。为了获得一个加速度连续的轨迹，位置和速度需要合适的初始和终止条件，也需要合适的初始和终止加速度值。这样共有六个边界条件，因此需要采用五次多项式：

$q(t)=q_0+a_1(t-t_0)+a_2(t-t_0)^2+a_3(t-t_0)^3+a_4(t-t_0)^4+a_5(t-t_0)^5
\tag{1-24}$

根据条件

$\begin{matrix}
q(t_0)=q_0,  &q(t_1)=q_1 \\
\dot{q}(t_0)=v_0, & \dot{q}(t_1)=v_1  \\
\ddot{q}(t_0)=\text a_0, & \ddot{q}(t_1)=\text a_1.
\end{matrix}$

这里定义$T=t_1-t_0$,求得多项式系数为

$\begin{cases}
a_0&=&a_0\\
a_1&=&v_0\\
a_2&=&\frac{1}{2}\text a_0\\
a_3&=&\frac{1}{2T^3}\left[20h-\left(8v_1+12v_0\right)T-(3\text a_0-\text a_1)T^2\right]\\
a_4&=&\frac{1}{2T^4}[-30h+(14v_1+16v_0)T+(3\text a_0-2\text a_1)T^2]\\
a_5&=&\frac{1}{2T^5}[12h-6(v_1+v_0)T+(\text a_1-\text a_0)T^2].
\end{cases}
\tag{1-25}$

Example2.9:使用五次多项式确定运动轨迹：
(a)

$\begin{matrix}
t_0=0,& t_1=8, \\
q_0=0,& q_1=10, \\
 v_0=0, &v_1=0,\\
 \text a_0=0, &\text a_1=0.
\end{matrix}$
(b)

$$

\begin{matrix}
t_0&=&0,& t_1&=&8, \
q_0&=&0,& q_1&=&10, \
v_0&=&-5, &v_1&=&-10,\
\text a_0&=&0, & \text a_1&=&0.
\end{matrix}

$$

可以注意到，使用三次多项式是无法指定加速度的边界条件的。对(a)和(b)条件使用五次多项式确定轨迹如下图所示。可以与上一篇博文的三次多项式（轨迹规划–三次多项式轨迹）比较一下，三次多项式是无法指定加速度的边界条件的。



图7 五次多项式轨迹
matlab仿真代码参见examplesCode文件夹下的example2_9.m文件
%example2.9
clc
clear
%轨迹定义条件
%时间
t0=0;
t1=8;
%位置和速度（a）
q0=0;
q1=10;
v0=0;
v1=0;
acc0=0;
acc1=0;
%利用公式（1-25）求系数
h=q1-q0;
T=t1-t0;
a0=q0;
a1=v0;
a2=1.0/2*acc0;
a3=1.0/(2*T*T*T)*(20*h-(8*v1+12*v0)*T+(acc1-3*acc0)/(T*T));
a4=1.0/(2*T*T*T*T)*(-30*h+(14*v1+16*v0)*T+(3*acc0-2*acc1)/(T*T));
a5=1.0/(2*T*T*T*T*T)*(12*h-6*(v1+v0)*T+(acc1-acc0)/(T*T));
%轨迹生成
t=t0:0.1:t1;
%位置
q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3)+...
    a4*power(t-t0,4)+a5*power(t-t0,5);
%速度
v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2)+4*a4*power(t-t0,3)+...
    5*a5*power(t-t0,4);
%加速度
acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);
%绘图
subplot(3,2,1)
plot(t,q,'r');
axis([0,8,0,11])
ylabel('position')
grid on
subplot(3,2,3)
plot(t,v,'b');
axis([0,8,-1,2.5])
ylabel('velocity')
grid on
subplot(3,2,5)
plot(t,acc,'g');
xlabel('(a)');
ylabel('acceleration')
grid on

%时间
t0=0;
t1=8;
%位置和速度（a）
q0=0;
q1=10;
v0=-5;
v1=-10;
acc0=0;
acc1=0;
%利用公式（1-25）求系数
h=q1-q0;
T=t1-t0;
a0=q0;
a1=v0;
a2=1.0/2*acc0;
a3=1.0/(2*T*T*T)*(20*h-(8*v1+12*v0)*T+(acc1-3*acc0)/(T*T));
a4=1.0/(2*T*T*T*T)*((-30*h+(14*v1+16*v0)*T)+(3*acc0-2*acc1)/(T*T));
a5=1.0/(2*T*T*T*T*T)*(12*h-6*(v1+v0)*T+(acc1-acc0)/(T*T));
%轨迹生成
t=t0:0.1:t1;
%位置
q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3)+...
    a4*power(t-t0,4)+a5*power(t-t0,5);
%速度
v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2)+4*a4*power(t-t0,3)+...
    5*a5*power(t-t0,4);
%加速度
acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);
%绘图
subplot(3,2,2)
plot(t,q,'r');
axis([0,8,-5,30])
ylabel('position')
grid on
subplot(3,2,4)
plot(t,v,'b');
ylabel('velocity')
grid on
subplot(3,2,6)
plot(t,acc,'g');
xlabel('(b)');
ylabel('acceleration')
grid on

Example 2.10:对于example2.7、example2.8 的条件（参见上一篇博客轨迹规划–三次多项式轨迹）使用五次多项式确定轨迹，如下图所示。并与example2.8对比，发现“平滑度”得到了改善。
给定条件

$\begin{matrix}
  {{t}_{0}}=0,{{t}_{1}}=2, &{{t}_{2}}=4,{{t}_{3}}=8,{{t}_{4}}=10, \\
 {{q}_{0}}=10,{{q}_{1}}=20, &{{q}_{2}}=0,{{q}_{3}}=30,{{q}_{4}}=40,\\
 {{v}_{0}}=0,{{v}_{1}}=-10, &{{v}_{2}}=10,{{v}_{3}}=3,{{v}_{4}}=0. \\
 \end{matrix}$

由此系列点确定的轨迹如下图所示。


图8 Example2.7的条件用五次多项式确定的轨迹
matlab仿真代码参见examplesCode文件夹下的example2_10.m文件
%example2.10
clc
clear
close('all')
%轨迹定义条件
%时间、位置和速度（a）
t_array=[0,2,4,8,10];
q_array=[10,20,0,30,40];
v_array=[0,-10,10,3,0];
%起点和终点加速度假设为0，中间点加速度都初始化为0，
acc_array=[0,0,0,0,0];
%计算轨迹
%初始位置
t=t_array(1);
q=q_array(1);
v=v_array(1);
v_array2=v_array;
acc=acc_array(1);

for k=1:length(t_array)-1
    %按照式（1-23）式确定中间点的速度值
    if(k>1)
        dk1=(q_array(k)-q_array(k-1))/(t_array(k)-t_array(k-1));
        dk2=(q_array(k+1)-q_array(k))/(t_array(k+1)-t_array(k));
        if((dk2>=0 && dk1>=0) || (dk2<=0 && dk1<=0))
            v_array2(k)=1.0/2.0*(dk1+dk2);
        else
            v_array2(k)=0;
        end  
    end
end

for k=1:length(t_array)-1
    %利用公式（1-25）求系数

    %计算各段多项式的系数
    h(k)=q_array(k+1)-q_array(k);
    T(k)=t_array(k+1)-t_array(k);
    a0(k)=q_array(k);
    a1(k)=v_array2(k);
    a2(k)=1.0/2*acc_array(k);
    a3(k)=1.0/(2*T(k)*T(k)*T(k))*(20*h(k)-(8*v_array2(k+1)+12*v_array2(k))*T(k)+(acc_array(k+1)-3*acc_array(k))/(T(k)*T(k)));
    a4(k)=1.0/(2*T(k)*T(k)*T(k)*T(k))*(-30*h(k)+(14*v_array2(k+1)+16*v_array2(k))*T(k)+(3*acc_array(k)-2*acc_array(k+1))/(T(k)*T(k)));
    a5(k)=1.0/(2*T(k)*T(k)*T(k)*T(k)*T(k))*(12*h(k)-6*(v_array2(k+1)+v_array2(k))*T(k)+(acc_array(k+1)-acc_array(k))/(T(k)*T(k)));
    
    %生成各段轨迹密化的数据点
    %局部时间坐标
    tau=t_array(k):T(k)/100:t_array(k+1);
    %全局时间坐标，由局部时间坐标组成
    t=[t,tau(2:end)];
    %位置
    qk=a0(k)+a1(k)*power(tau-tau(k),1)+a2(k)*power(tau-tau(k),2)+a3(k)*power(tau-tau(k),3)+...
        a4(k)*power(tau-tau(k),4)+a5(k)*power(tau-tau(k),5);
     %全局位置坐标
    q=[q,qk(2:end)];
    %速度
    vk=a1(k)+2*a2(k)*power(tau-tau(k),1)+3*a3(k)*power(tau-tau(k),2)+4*a4(k)*power(tau-tau(k),3)+...
        5*a5(k)*power(tau-tau(k),4);
     v=[v,vk(2:end)];
    %加速度
    acck=2*a2(k)+6*a3(k)*power(tau-tau(k),1)+12*a4(k)*power(tau-tau(k),2)+20*a5(k)*power(tau-tau(k),3);
    acc=[acc,acck(2:end)];

end
%绘图
subplot(3,1,1)
plot(t,q,'r');
hold on
plot(t_array,q_array,'o');
 axis([0,10,-5,45])
ylabel('position')
grid on
subplot(3,1,2)
plot(t,v,'b');
hold on
plot(t_array,v_array2,'o');
axis([0,10,-20,15])
ylabel('velocity')
grid on
subplot(3,1,3)
plot(t,acc,'g');
axis([0,10,-35,35])
ylabel('acceleration')
grid on

接下来将介绍七次多项式和更高阶次的多项式轨迹。

参考文献
Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.

1.轨迹规划的定义 

轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补，在起始点和终止点之间插入中间点序列，实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划，路径规划是规划位置，在起终点之间经过的路径点，轨迹规划是规划时间，将路径点与时间相对应。 

对于我们的六轴机器人而言轨迹规划可以分为：关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数，然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。 

五次多项式插值能够解决三次多项式插值的角速度变化不平滑且加速度存在跳变的情况。 

2.数学基础 

五次多项式插值，角位移、角速度和角加速度的函数表达式为： 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
\theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5\\
\dot{\theta}(t)=a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4\\
\ddot{\theta}(t)=2a_2+6a_3t+12a_4t^2+20a_5t^3
\end{array}\right.
\end{eqnarray*}$$  

约束条件如下，相对三次多项式插值，增加了对起止点角速度的约束（为简便计算设$t_0$为0） 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
\theta(t_0)=\theta_0=a_0\\
\theta(t_f)=\theta_f=a_0+a_1t_f+a_2t_f^2+a_3t_f^3+a_4t_f^4+a_5t_f^5\\
\dot{\theta}(t_0)=\dot{\theta}_0=a_1\\
\dot{\theta}(t_f)=\dot{\theta}_f=a_1+2a_2t_f+3a_3t_f^2+4a_4t_f^3+5a_5t_f^4\\
\ddot{\theta}(t_0)=\ddot{\theta}_0=2a_2\\
\ddot{\theta}(t_f)=\ddot{\theta}_f=2a_2+6a_3t_f+12a_4t_f^2+20a_5t_f^3
\end{array}\right.
\end{eqnarray*}$$  

求解得 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
a_0=\theta_0\\
a_1=\dot{\theta}_0\\
a_2=\frac{\ddot{\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)t_f-(\ddot\theta_0-\ddot\theta_f)t_f^2}{2t_f^5}\\
\end{array}\right.
\end{eqnarray*}$$  

3.matlab代码实现





  
序号
  
位移
  
速度
  
加速度
  
时间



  
1
  
0
  
0
  
0
  
0


  
2
  
50
  
10
  
20
  
3


  
3
  
150
  
20
  
30
  
6


  
4
  
100
  
-15
  
-20
  
12


  
5
  
0
  
0
  
0
  
14






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)];%初始状态
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.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'),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;

 

PS:相对于三次多项式插值，加速度也是平滑的曲线，并没有出现跳变的情况。然而在机器人系统中，单纯的多项式规划有一个非常严重的问题：没有匀速段，无法根据期望速度提供匀速控制，而在大部分机器人应用中，对加工的速度控制都是有要求的。另一个问题就是，次数越高的多项式，加速过程越慢，整个运动过程中的平均速度越小，影响效率。下次将会为大家介绍能约束速度的三段s曲线加减速直线插值方式。

1.轨迹规划的定义 

轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补，在起始点和终止点之间插入中间点序列，实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划，路径规划是规划位置，在起终点之间经过的路径点，轨迹规划是规划时间，将路径点与时间相对应。 

对于我们的六轴机器人而言轨迹规划可以分为：关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数，然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。 

2.数学基础 

三次多项式插值（适用于起点和终点速度为零的情况，约束关节在起点和终点的角度值，规定轨迹两端点位置角速度为定值）。 

设关节角满足下式 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
\theta(t)=a_0+a_1t+a_2t^2+a_3t^3\\
\dot{\theta}(t)=a_1+2a_2t+3a_3t^2\\
\ddot{\theta}(t)=2a_2+6a_3t
\end{array}\right.
\end{eqnarray*}$$  

将相邻两个点看作一小段轨迹的起点和终点分别用$\theta_0$和$\theta_f$表示，约束起始速度为$v_0$，终止速度为$v_f$ 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
\theta(t_0)=\theta_0\\
\theta(t_f)=\theta_f\\
\dot{\theta}(t_0)=v_0\\
\dot{\theta}(t_f)=v_f\\
\end{array}\right.
\end{eqnarray*}$$  

将约束条件代入函数，可以求得系数（为简便计算，设$t_0=0$） 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
a_0=\theta_0\\
a_1=v_0\\
a_2=\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)\\
a_3=\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)\\
\end{array}\right.
\end{eqnarray*}$$  

三次多项式规划轨迹如下 

 $$\begin{eqnarray*}
  \left\{ \begin{array}{l}
\theta(t)=\theta_0+v_0t+[\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)]t^2+[\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)]t^3\\
\dot{\theta}(t)=v_0+2[\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)]t+3[\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)]t^2\\
\ddot{\theta}(t)=2[\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)]+6[\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)]t
\end{array}\right.
\end{eqnarray*}$$  

当速度为零时适用于一段轨迹的起终点，不为零时适用于一段轨迹的经过点。 

3.matlab代码实现





  
序号
  
位置
  
速度
  
时间



  
1
  
0
  
0
  
0


  
2
  
100
  
0
  
3




clear;
clc;
q0=0;
q1=100; %指定起止位置
t0=0;
t1=3;%指定起止时间
v0=0;
v1=0;%指定起止速度
a0=q0;
a1=v0;
a2=(3/(t1)^2)*(q1-q0)-(1/t1)*(2*v0+v1);
a3=(2/(t1)^3)*(q0-q1)+(1/t1^2)*(v0+v1);%计算三次多项式系数
t=t0:0.01:t1;
q=a0+a1*t+a2*t.^2+a3*t.^3;%三次多项式插值的位置
v=a1+2*a2*t+3*a3*t.^2;%三次多项式插值的速度
a=2*a2+6*a3*t;%三次多项式插值的加速度
subplot(3,1,1),plot(t,q),xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(t,v),xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(t,a),xlabel('t'),ylabel('accelerate');grid on;

下图为插值结果 

 

4.含经过点的插值




  
序号
  
位置
  
速度
  
时间



  
1
  
0
  
0
  
0


  
2
  
50
  
10
  
3


  
3
  
150
  
20
  
6


  
4
  
100
  
-15
  
12


  
5
  
0
  
0
  
14






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'),ylabel('position');grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');grid on;

 

PS:这种三次多项式插值，只要给定离散点位置、速度和时间，就能插补出一段连续平滑的曲线。但是角加速度并不连续，下次为大家介绍高阶多项式插值则可以解决这个问题。

机器人轨迹
这一系列轨迹教程将主要包括以下内容：


点到点轨迹（P2P）

五次多项式插值轨迹
三次多项式插值轨迹
多项式轨迹实战
梯形速度曲线轨迹
双S形速度曲线轨迹
多个自由度轨迹的时间同步



在线轨迹规划

多项式在线轨迹规划
梯形在线轨迹规划
双S形在线轨迹规划
非线性实时轨迹滤波



多点轨迹（Multi-point)

三次样条曲线（cubic spline）
贝赛尔曲线（Bezier Curve）
B样条曲线（BSpline）



时间最优轨迹

三次样条时间最优轨迹
任意路径下的时间最优轨迹
时间最优停止轨迹

不约束路径
有路径约束





这一篇文章讲五次多项式插值轨迹。
文章目录
五次多项式
表达式
代码实现
举例
总结

五次多项式
表达式
位置：
$q(t)=k_0+k_1(t-t_0)+k_2(t-t_0)^2+k_3(t-t_0)t^3+k_4(t-t_0)^4+k_5(t-t_0)^5  \tag{1}$
求一阶导数可以得到速度：

$\dot q(t)=k_1+2k_2(t-t_0)+3k_3(t-t_0)^2+4k_4(t-t_0)^3+5k_5(t-t_0)^4 \tag{2}$
求二阶导数可以得到加速度：
$\ddot q(t)=2k_2+6k_3(t-t_0)+12k_4(t-t_0)^2+20k_5(t-t_0)^3  \tag{3}$
特点：

加速度连续
可以定义起始和结束的位置、速度、加速度

计算

给定起始和结束的位置、速度、加速度以及轨迹时间$T$，可以构造六个方程，解出五次多项式的6个系数。

满足的约束方程为：

$q(t_0) = q_0 , \qquad q(t_1) = q_1 \\\dot q(t_0) = v_0, \qquad\dot q(t_1) = v_1 \\\ddot q(t_0) = a_0, \qquad\ddot q(t_1) = a_1 \tag{4}$
把方程（4）结合（1）、（2）、（3）可以得到6个线性方程组，最终可以得到如下解：

$\begin{aligned}	   &T = t_1 - t_0 \\    &h = q_1 - q_0 \\    &k_0 = q_0 \\    &k_1 = v0 \\    &k_2 = \frac{a_0}{2} \\    &k_3 = \frac {1}{2T^3} [20h − (8v_1 + 12v_0)T − (3a_0 − a_1)T^2] \\    &k_4 = \frac {1}{2T^4} [−30h + (14v_1 + 16v_0)T + (3a_0 − 2a_1)T^2] \\    &k_5 = \frac{1}{2T^5} [12h − 6(v_1 + v_0)T + (a_1 − a_0)T^2] 	\end{aligned} \tag{5}$
代码实现
这里用python来实现五次多项式插值：
#!/usr/bin/python3
"""
Copyright © 2021 boldyoungster. All rights reserved.

@file Polynomial.py
@date: 11:06:12, February 28, 2021
"""

import numpy as np
import matplotlib.pyplot as plt


class PolynomialQuintic:
    def __init__(self, t0, t1, q0, q1, v0=0.0, v1=0.0, a0=0.0, a1=0.0):
        coeffs = self.__ComputeQuinticCoeffs(t0, t1, q0, q1, v0, v1, a0, a1)
        self.poly = np.poly1d(coeffs[::-1])

    @classmethod
    def __ComputeQuinticCoeffs(cls, t0, t1, q0, q1, v0, v1, a0, a1):
        T = t1 - t0
        T2 = T * T
        h = q1 - q0
        k0 = q0
        k1 = v0
        k2 = 0.5 * a0
        k3 = (20. * h - (8. * v1 + 12. * v0) * T -
              (3 * a0 - a1) * T2) / (2. * T * T2)
        k4 = (-30. * h + (14*v1 + 16*v0)*T+(3*a0 - 2*a1)*T2) / (2. * T2 * T2)
        k5 = (12 * h - 6*(v1 + v0) * T + (a1 - a0) * T2) / (2 * T2 * T2 * T)
        return (k0, k1, k2, k3, k4, k5)


if __name__ == "__main__":
    t0, t1 = 0, 3.0
    q0, q1 = 0, 1
    v0, v1 = 0, 0
    a0, a1 = 0, 0
    poly = PolynomialQuintic(t0, t1, q0, q1,
                             v0, v1, a0, a1)
    ts = np.linspace(t0, t1, 200)
    qs = poly.poly(ts)
    vs = poly.poly.deriv(1)(ts)
    dvs = poly.poly.deriv(2)(ts)
    plt.subplots_adjust(hspace=1)
    plt.suptitle("QuinticPolynomial")
    plt.subplot(311)
    plt.plot(ts, qs)
    plt.title("Position")
    plt.subplot(312)
    plt.plot(ts, vs)
    plt.title("Velocity")
    plt.subplot(313)
    plt.plot(ts, dvs)
    plt.title("Acceleration")
    plt.savefig("./QuinticPolynomial.png")
    plt.show()


举例
$t_0 = 0.0, \qquad t_1 = 3.0 \\q_0 = 0.0, \qquad q_1 = 1.0 \\v_0 = 0.0, \qquad v_1 = 0.0 \\a_0 = 0.0, \qquad a_1 = 0.0$
这段轨迹是从速度、加速度为0开始，再到速度、加速度为0停止，称作rest-to-rest，其轨迹形状如下：

总结
五次多项式是一种经常使用的插值方式，它可以保证加速度连续，可以定义轨迹两个端点的速度、加速度，有比较好的平滑性。