（一）三次多项式轨迹规划
一、什么是多项式？
其实多项式这个概念你在初中就见过（整式），他就是一个表达式： f ( x ) = a n x n + ⋯ + a 1 x + a 0 f(x)=a_nx^n+\cdots+a_1x+a_0 f(x)=anxn+⋯+a1x+a0。在这个式子中， a n , ⋯ , a 1 , a 0 a_n,\cdots,a_1,a_0 an,⋯,a1,a0，这是数域中的数，也叫系数， x x x这里是变量。
在定义多项式次数时，其实大家可以把它和线性空间的维数联系到一起（虽然两者数值上差了一个1）。这是因为 { 1 , x , ⋯ , x n } \{1,x,\cdots,x^n\} {1,x,⋯,xn}可以看作一个基，然后它张成的空间就是一元多项式的全体，当然有一个例外是0，也就是零多项式，它的次数我们定义为 − ∞ -\infty −∞。
多项式中的次数都是非负数，并且多项式是有限项。所以像是 x 1 3 x^{\frac{1}{3}} x31这样的式子就不是多项式。同样的，因为 a i a_i ai这种东西都是数域中的数，如果数域不同，可能也会影响它是否成为一个多项式和。比方说， x + π x+\pi x+π在有理数域中就不是多项式。
二、轨迹规划中的三次多项式
选择满足要求的运动学性质的物理量如，如：位移、速度和加速度其实就完成了一个轨迹的规划。以单轴关节角
x
(
t
)
x(t)
x(t)为例，容易得：
x
(
t
)
=
c
0
+
c
1
t
+
c
2
t
2
+
c
3
t
3
v
(
t
)
=
c
1
+
2
c
2
t
+
3
c
3
t
2
a
(
t
)
=
2
c
2
+
6
c
3
t
\begin{aligned} &x(t)=c_0+c_1t+c_2t^2+c_3t^3\\ &v(t)=c_1+2c_2t+3c_3t^2\\ &a(t)=2c_2+6c_3t\\ \end{aligned}
x(t)=c0+c1t+c2t2+c3t3v(t)=c1+2c2t+3c3t2a(t)=2c2+6c3t
三次多项式对四个运动量进行了规划（约束），显然x(t) v(t) a(t)均是连续函数：
- 起始绝对量
- 起始速度
- 结束绝对量
- 结束速度
每一个约束都对应一个方程，方程如下：
x
(
t
s
)
=
c
0
+
c
1
t
s
+
c
2
t
s
2
+
c
3
t
s
3
=
x
s
x
(
t
e
)
=
c
0
+
c
1
t
e
+
c
2
t
e
2
+
c
3
t
e
3
=
x
e
v
(
t
s
)
=
c
1
+
2
c
2
t
s
+
3
c
3
t
s
2
=
v
s
v
(
t
e
)
=
c
1
+
2
c
2
t
e
+
3
c
3
t
e
2
=
v
e
\begin{aligned} &x(t_s)=c_0+c_1t_s+c_2t_s^2+c_3t_s^3=x_s\\ &x(t_e)=c_0+c_1t_e+c_2t_e^2+c_3t_e^3=x_e\\ &v(t_s)=c_1+2c_2t_s+3c_3t_s^2=v_s\\ &v(t_e)=c_1+2c_2t_e+3c_3t_e^2=v_e \end{aligned}
x(ts)=c0+c1ts+c2ts2+c3ts3=xsx(te)=c0+c1te+c2te2+c3te3=xev(ts)=c1+2c2ts+3c3ts2=vsv(te)=c1+2c2te+3c3te2=ve
写成矩阵形式：
[
1
t
s
t
s
2
t
s
3
1
t
e
t
e
2
t
e
3
0
1
2
t
s
3
t
s
2
0
1
2
t
e
3
t
e
2
]
[
c
0
c
1
c
2
c
3
]
=
[
x
s
x
e
v
s
v
e
]
\begin{bmatrix} 1 & t_s & t_s^2&t_s^3 \\ 1 & t_e & t_e^2&t_e^3 \\ 0 & 1 & 2t_s&3t_s^2\\ 0&1&2t_e&3t_e^2 \end{bmatrix}\begin{bmatrix}c_0\\c_1\\c_2\\c_3\end{bmatrix}=\begin{bmatrix}x_s\\x_e\\v_s\\v_e\end{bmatrix}
⎣⎢⎢⎡1100tste11ts2te22ts2tets3te33ts23te2⎦⎥⎥⎤⎣⎢⎢⎡c0c1c2c3⎦⎥⎥⎤=⎣⎢⎢⎡xsxevsve⎦⎥⎥⎤记为
T
C
=
P
TC=P
TC=P；若矩阵可逆，则有：
C
=
T
−
1
P
C=T^{-1}P
C=T−1P。获得系数
C
C
C之后，轨迹就唯一确定了。即每一时刻都可以对应一个“位移量”。
Matlab实现如下,连续进行了三段曲线连续轨迹规划：0-1，1-2，2-3
% 三次多项式插补
close all
clear;
clc;
% 0-1
t0=0; x0=30; v0=75;
t1=5; x1=60; v1=10;
[x01,v01,a01]=plan(t0,x0,v0,t1,x1,v1);
% 1-2
t2=13; x2=80; v2=10;
[x12,v12,a12]=plan(0,x1,v1,t2-t1,x2,v2);
% 2-3
t3=20; x3=10; v3=0;
[x23,v23,a23]=plan(0,x2,v2,t3-t2,x3,v3);
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]=plan(ts,start_x,start_v,te,end_x,end_v)
para=[start_x,end_x,start_v,end_v]';
Tran=[1,ts,ts^2,ts^3;1,te,te^2,te^3;0,1,2*ts,3*ts^2;0,1,2*te,3*te^2];
C=(inv(Tran))*para;
c0=C(1);
c1=C(2);
c2=C(3);
c3=C(4);
x=[];v=[];a=[];
for i=ts:0.01:te
t=i;
x=[x c0+c1*t+c2*t^2+c3*t^3];
v=[v c1+2*c2*t+3*c3*t^2];
a=[a 2*c2+6*c3*t];
end
end
结果如下：
![[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HuOxbMIa-1587794722228)(https://note.youdao.com/yws/res/45267/BF15337FBB704B28B47A712C4655E2D7)]](https://img-blog.csdnimg.cn/20200425140550816.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zOTI1ODk3OQ==,size_16,color_FFFFFF,t_70)
虽然我们每一段的位移、速度和加速度都是连续的，但是显然对于加速度而言，出现了间断点，在这个例子里，瞬间加速度超过了 20 m m / s 2 20mm/s^2 20mm/s2，是否能够提供这样的加速度取决于硬件（电机）