（二）五次多项式轨迹规划

一、三次多项式轨迹规划的缺陷

上一篇文章说道，三次多项式轨迹规划只能够保证速度和位移连续，并不能保证加速度连续。加速度不连续将会对使电机抖动、甚至冲击。

二、轨迹规划中的五次多项式

我们对加速度数值进行指定，将会增加两个加速度方程，总计6个方程组。他们分别是：

• 起末位移方程组
• 起末速度方程组
• 起末加速度方程组

$x(t)=c_0+c_{1}t+c_2{t}^2+c_3{t}^3+c_4{t}^4+c_5{t}^5$

$v(t)=c_{1}t+2c_{2}t+3c_3{t}^2+4c_4{t}^3+5c_5{t}^4$

$a(t)=2c_2+6c_{3}t+12c_4{t}^2+20c_5{t}^3$

带入初始条件有：

$x(t_s)=c_0+c_1{t}_s+c_2{t}_s^2+c_3{t}_s^3+c_4{t}_s^4+c_5{t}_s^5$

$v(t_s)=c_1{t}_s+2c_2{t}_s+3c_3{t}_s^2+4c_4{t}_s^3+5c_5{t}_s^4$

$a(t_s)=2c_2+6c_3{t}_s+12c_4{t}_s^2+20c_5{t}_s^3$

$x(t_e)=c_0+c_{1}0t_e+c_2{t}_e^2+c_3{t}_e^3+c_4{t}_e^4+c_5{t}_e^5$

$v(t_e)=c_1{t}_e+2c_2{t}_e+3c_3{t}_e^2+4c_4{t}_e^3+5c_5{t}_e^4$

$a(t_e)=2c_2+6c_3{t}_e+12c_4{t}_e^2+20c_5{t}_e^3$

写成矩阵形式：
$$\left(\begin{array}{cccccc}1& ts& t{s}^2& t{s}^3& t{s}^4& t{s}^5\\ 1& te& t{s}^2& t{s}^3& t{s}^4& t{s}^5\\ 0& 1& 2ts& 3t{s}^2& 4t{s}^3& 5t{s}^4\\ 0& 1& 2te& 3t{e}^2& 4t{e}^3& 5t{e}^4\\ 0& 0& 2& 6ts& 12t{s}^2& 20t{s}^3\\ 0& 0& 2& 6te& 12t{e}^2& 20t{e}^3\end{array}\right)\left(\begin{array}{c}{c}_0\\ c_1\\ c_2\\ c_3\\ c_4\\ c_5\end{array}\right)=\left(\begin{array}{c}{x}_s\\ x_e\\ v_s\\ v_e\\ a_s\\ a_e\end{array}\right)$$
记为$T\ast C=P$，则$C=T^{-1}P$

三、Matlab仿真与实现

% 五次多项式插补
close all
clear;
clc;

% 0-1
t0=0; x0=30; v0=30;a0=10;
t1=5; x1=60; v1=40;a1=13;
[x01,v01,a01]=fivetimes_plan(t0,x0,v0,a0,t1,x1,v1,a1);

% 1-2
t2=13; x2=80; v2=10;a2=13;
[x12,v12,a12]=fivetimes_plan(0,x1,v1,a1,t2-t1,x2,v2,a2);

% 2-3
t3=20; x3=10; v3=0;a3=23;
[x23,v23,a23]=fivetimes_plan(0,x2,v2,a2,t3-t2,x3,v3,a3);


x=[x01 x12 x23];
v=[v01 v12 v23];
a=[a01 a12 a23];

figure(1);
subplot(3,1,1);
plot(x,'r','LineWidth',1.2);
axis([ t0*100 t3*100 -inf inf]);
ylabel('position')

subplot(3,1,2);
plot(v,'g','LineWidth',1.2)
axis([ t0*100 t3*100 -inf inf]);
ylabel('velocity')

subplot(3,1,3);
plot(a,'b','LineWidth',1.2);
axis([ t0*100 t3*100 -inf inf]);
ylabel('acceleration')
xlabel('time')

function [x,v,a]=fivetimes_plan(ts,start_x,start_v,start_a,te,end_x,end_v,end_a)
    para=[start_x,end_x,start_v,end_v,start_a,end_a]';
    Tran=[1,ts,ts^2,ts^3,ts^4,ts^5;
          1,te,te^2,te^3,te^4,te^5
          0,1,2*ts,3*ts^2,4*ts^3,5*ts^4;
          0,1,2*te,3*te^2,4*te^3,5*te^4;
          0,0,2,6*ts,12*ts^2,20*ts^3
          0,0,2,6*te,12*te^2,20*te^3];
    C=(inv(Tran))*para;
    c0=C(1);
    c1=C(2);
    c2=C(3);
    c3=C(4);
    c4=C(5);
    c5=C(6);
    x=[];v=[];a=[];
    for i=ts:0.01:te
        t=i;
        x=[x c0+c1*t+c2*t^2+c3*t^3+c4*t^4+c5*t^5];
        v=[v c1+2*c2*t+3*c3*t^2+4*c4*t^3+5*c5*t^4];
        a=[a 2*c2+6*c3*t+12*c4*t^2+20*c5*t^3];
    end
end

可以看出，五次多项式确实没有加速度突变，符合开始的设想。无论是三次还是五次多项式插值速度曲线并没有匀速段，这种规划方式不适用于笛卡尔空间的规划，因为大多数工业应用如涂胶、焊接都要求有匀速段，为了拥有匀速段，就有了后边的梯形和S型速度规划。