多项式轨迹–七次多项式
1.6 Polynomial of degree seven
在某些特定场合，可能需要定义更高阶次的多项式来获得平滑的轨迹。具有七次的多项式如下
q
(
t
)
=
a
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
+
a
6
(
t
−
t
0
)
6
+
a
7
(
t
−
t
0
)
7
(1-26)
q(t)=a_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+a_6(t-t_0)^6+a_7(t-t_0)^7 \\ \tag{1-26}
q(t)=a0+a1(t−t0)+a2(t−t0)2+a3(t−t0)3+a4(t−t0)4+a5(t−t0)5+a6(t−t0)6+a7(t−t0)7(1-26)
七次多项式可以指定八个边界条件
q
(
t
0
)
=
q
0
,
q
˙
(
t
0
)
=
v
0
,
q
¨
(
t
0
)
=
a
c
c
0
,
q
(
3
)
(
t
0
)
=
j
0
,
q
(
t
1
)
=
q
1
,
q
˙
(
t
1
)
=
v
1
,
q
¨
(
t
1
)
=
a
c
c
1
,
q
(
3
)
(
t
1
)
=
j
1
,
\begin{matrix} q(t_0)=q_0, & \dot{q}(t_0)=v_0, & \ddot{q}(t_0)=acc_0, &q^{(3)}(t_0)=j_0, \\ q(t_1)=q_1, & \dot{q}(t_1)=v_1, & \ddot{q}(t_1)=acc_1, &q^{(3)}(t_1)=j_1, \\ \end{matrix}
q(t0)=q0,q(t1)=q1,q˙(t0)=v0,q˙(t1)=v1,q¨(t0)=acc0,q¨(t1)=acc1,q(3)(t0)=j0,q(3)(t1)=j1,
定义
T
=
t
1
−
t
0
T=t_1-t_0
T=t1−t0和
h
=
q
1
−
q
0
h=q_1-q_0
h=q1−q0，系数
a
i
,
i
=
0
,
…
,
7
a_i,i=0,\ldots,7
ai,i=0,…,7可求得
{
a
0
=
q
0
a
1
=
v
0
a
2
=
a
c
c
0
2
a
3
=
j
0
6
a
4
=
210
h
−
T
[
(
30
a
c
c
0
−
15
a
c
c
1
)
T
+
(
4
j
0
+
j
1
)
T
2
+
120
v
0
+
90
v
1
]
6
T
4
a
5
=
−
168
h
+
T
[
(
20
a
c
c
0
−
14
a
c
c
1
)
T
+
(
2
j
0
+
j
1
)
T
2
+
90
v
0
+
78
v
1
]
2
T
5
a
6
=
420
h
−
T
[
(
20
a
c
c
0
−
39
a
c
c
1
)
T
+
(
4
j
0
+
3
j
1
)
T
2
+
216
v
0
+
204
v
1
]
6
T
6
a
7
=
−
120
h
+
T
[
(
12
a
c
c
0
−
12
∗
a
c
c
1
)
T
+
(
j
0
+
j
1
)
T
2
+
60
v
0
+
60
v
1
]
6
T
7
\begin{cases} a_0=q_0&\\ a_1=v_0 &\\ a_2=\frac{acc_0}{2}&\\ a_3=\frac{j_0}{6}\\ a_4=\frac{210h-T[(30acc_0-15acc_1)T+(4j_0+j_1)T^2+120v_0+90v_1]}{6T^4}&\\ a_5=\frac{-168h+T[(20acc_0-14acc_1)T+(2j_0+j_1)T^2+90v_0+78v_1]}{2T^5} &\\ a_6=\frac{420h-T[(20acc_0-39acc_1)T+(4j_0+3j_1)T^2+216v_0+204v_1]}{6T^6}\\ a_7=\frac{-120h+T[(12acc_0-12*acc_1)T+(j_0+j_1)T^2+60v_0+60v_1]}{6T^7} \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧a0=q0a1=v0a2=2acc0a3=6j0a4=6T4210h−T[(30acc0−15acc1)T+(4j0+j1)T2+120v0+90v1]a5=2T5−168h+T[(20acc0−14acc1)T+(2j0+j1)T2+90v0+78v1]a6=6T6420h−T[(20acc0−39acc1)T+(4j0+3j1)T2+216v0+204v1]a7=6T7−120h+T[(12acc0−12∗acc1)T+(j0+j1)T2+60v0+60v1]
Eample 2.11:根据下面的条件利用七次多项式确定运动轨迹
t
0
=
0
,
t
1
=
8
,
q
0
=
0
,
q
1
=
10
,
v
0
=
0
,
v
1
=
0
,
a
c
c
0
=
0
,
a
c
c
1
=
0
,
j
0
=
0
,
j
1
=
0.
\begin{matrix} t_0=0,&t_1=8,\\ q_0=0,&q_1=10,\\ v_0=0,&v_1=0,\\ acc_0=0,&acc_1=0,\\ j_0=0,&j_1=0. \end{matrix}
t0=0,q0=0,v0=0,acc0=0,j0=0,t1=8,q1=10,v1=0,acc1=0,j1=0.
结果如下图所示
图 9 七次多项式轨迹
matlab仿真代码参见examplesCode文件夹下的example2_11.m文件
%example2.11,七次多项式轨迹
clc
clear
close('all')
%轨迹定义条件
t0=0;t1=8;
q0=0;q1=10;
v0=0;v1=0;
acc0=0;acc1=0;
j0=0;j1=0;
%利用（1-26）式求系数
h=q1-q0;
T=t1-t0;
a0=q0;
a1=v0;
a2=acc0/2;
a3=j0/6;
a4=1.0/(6*power(T,4))*(210*h-T*((30*acc0-15*acc1)*T+(4*j0+j1)*T*T+120*v0+90*v0));
a5=1.0/(2*power(T,5))*(-168*h+T*((20*acc0-14*acc1)*T+(2*j0+j1)*T*T+90*v0+78*v1));
a6=1.0/(6*power(T,6))*(420*h-T*((45*acc0-39*acc1)*T+(4*j0+3*j1)*T*T+216*v0+204*v1));
a7=1.0/(6*power(T,7))*(-120*h+T*((12*acc0-12*acc1)*T+(j0+j1)*T*T+60*v0+60*v1));
%计算轨迹
t=linspace(t0,t1,100);
%位置
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)+a6*power(t-t0,6)+a7*power(t-t0,7);
%速度
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)+...
+6*a6*power(t-t0,5)+7*a7*power(t-t0,6);
%加速度
acc=2*a2+2*3*a3*power(t-t0,1)+3*4*a4*power(t-t0,2)+4*5*a5*power(t-t0,3)+...
+5*6*a6*power(t-t0,4)+6*7*a7*power(t-t0,5);
%加加速度
j=2*3*a3+2*3*4*a4*power(t-t0,1)+3*4*5*a5*power(t-t0,2)+...
+4*5*6*a6*power(t-t0,3)+5*6*7*a7*power(t-t0,4);
%绘图
subplot(4,1,1)
plot(t,q,'r');
hold on
axis([0,8,-1,12])
ylabel('position')
grid on
subplot(4,1,2)
plot(t,v,'b');
hold on
axis([0,8,-1,3])
ylabel('velocity')
grid on
subplot(4,1,3)
plot(t,acc,'g');
axis([0,8,-1.5,1.5])
ylabel('acceleration')
grid on
subplot(4,1,4)
plot(t,j,'k');
axis([0,8,-1.1,1])
ylabel('jerk')
grid on
参考文献：
[1]Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.
4908
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
