上一篇文章说道，三次多项式轨迹规划只能够保证速度和位移连续，并不能保证加速度连续。加速度不连续将会对使电机抖动、甚至冲击。
我们对加速度数值进行指定，将会增加两个加速度方程，总计6个方程组。他们分别是：
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
+
2
c
2
t
s
+
3
c
3
t
s
2
+
4
c
4
t
s
3
+
5
c
5
t
s
4
a
(
t
s
)
=
2
c
2
+
6
c
3
t
s
+
12
c
4
t
s
2
+
20
c
5
t
s
3
x
(
t
e
)
=
c
0
+
c
1
t
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
+
2
c
2
t
e
+
3
c
3
t
e
2
+
4
c
4
t
e
3
+
5
c
5
t
e
4
a
(
t
e
)
=
2
c
2
+
6
c
3
t
e
+
12
c
4
t
e
2
+
20
c
5
t
e
3
(2)
\begin{aligned} &x(t_s)=c_0+c_1t_s+c_2t_s^2+c_3t_s^3+c_4t_s^4+c_5t_s^5\\ &v(t_s)=c_1+2c_2t_s+3c_3t_s^2+4c_4t_s^3+5c_5t_s^4\\ &a(t_s)=2c_2+6c_3t_s+12c_4t_s^2+20c_5t_s^3\\ &x(t_e)=c_0+c_1t_e+c_2t_e^2+c_3t_e^3+c_4t_e^4+c_5t_e^5\\ &v(t_e)=c_1+2c_2t_e+3c_3t_e^2+4c_4t_e^3+5c_5t_e^4\\ &a(t_e)=2c_2+6c_3t_e+12c_4t_e^2+20c_5t_e^3 \end{aligned}\tag{2}
x(ts)=c0+c1ts+c2ts2+c3ts3+c4ts4+c5ts5v(ts)=c1+2c2ts+3c3ts2+4c4ts3+5c5ts4a(ts)=2c2+6c3ts+12c4ts2+20c5ts3x(te)=c0+c1te+c2te2+c3te3+c4te4+c5te5v(te)=c1+2c2te+3c3te2+4c4te3+5c5te4a(te)=2c2+6c3te+12c4te2+20c5te3(2)
写成矩阵形式：
[
1
t
s
t
s
2
t
s
3
t
s
4
t
s
5
1
t
e
t
s
2
t
s
3
t
s
4
t
s
5
0
1
2
t
s
3
t
s
2
4
t
s
3
5
t
s
4
0
1
2
t
e
3
t
e
2
4
t
e
3
5
t
e
4
0
0
2
6
t
s
12
t
s
2
20
t
s
3
0
0
2
6
t
e
12
t
e
2
20
t
e
3
]
[
c
0
c
1
c
2
c
3
c
4
c
5
]
=
[
x
s
x
e
v
s
v
e
a
s
a
e
]
(3)
\begin{bmatrix} 1 & t_s & t_s^2&t_s^3&t_s^4&t_s^5 \\ 1 & t_e & 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&2t_e&3t_e^2&4t_e^3&5t_e^4\\ 0&0&2&6t_s&12t_s^2&20t_s^3\\ 0&0&2&6t_e&12t_e^2&20t_e^3\\ \end{bmatrix} \begin{bmatrix}\tag{3} c_0 \\ c_1 \\ c_2 \\ c_3\\ c_4\\ c_5 \end{bmatrix}= \begin{bmatrix} x_s \\ x_e \\ v_s \\ v_e\\ a_s\\ a_e \end{bmatrix}
⎣⎢⎢⎢⎢⎢⎢⎡110000tste1100ts2ts22ts2te22ts3ts33ts23te26ts6tets4ts44ts34te312ts212te2ts5ts55ts45te420ts320te3⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡c0c1c2c3c4c5⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡xsxevsveasae⎦⎥⎥⎥⎥⎥⎥⎤(3)
记为
T
C
=
P
TC=P
TC=P，则
C
=
T
−
1
P
C=T^{-1}P
C=T−1P
% 五次多项式插补
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型速度规划。