多项式轨迹--五次多项式轨迹-CSDN博客

转自：https://blog.csdn.net/libing403/article/details/78715418

多项式轨迹–五次多项式轨迹
1.5 Polynomial of degree five
利用三次多项式，根据过q0,q1,…,qnq0,q1,…,qn确定的轨迹的特征是位置和速度连续，但是加速度不连续（参见上一篇博客多项式轨迹–三次多项式轨迹）。尽管三次多项式轨迹确定的轨迹有一定“平滑”，但是对于一些应用的动力学和惯性载荷会产生一些不期待的影响。为了获得一个加速度连续的轨迹，位置和速度需要合适的初始和终止条件，也需要合适的初始和终止加速度值。这样共有六个边界条件，因此需要采用五次多项式：
q(t)=q0+a1(t−t0)+a2(t−t0)2+a3(t−t0)3+a4(t−t0)4+a5(t−t0)5(1-24)
(1-24)q(t)=q0+a1(t−t0)+a2(t−t0)2+a3(t−t0)3+a4(t−t0)4+a5(t−t0)5

根据条件
q(t0)=q0,q˙(t0)=v0,q¨(t0)=a0,q(t1)=q1q˙(t1)=v1q¨(t1)=a1.
q(t0)=q0,q(t1)=q1q˙(t0)=v0,q˙(t1)=v1q¨(t0)=a0,q¨(t1)=a1.

注意，这里aiai表示多项式系数，aiai表示加速度。这里定义T=t1−t0T=t1−t0,求得多项式系数为
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪a0a1a2a3a4a5======a0v012a012T3[20h−(8v1+12v0)T−(3a0−a1)T2]12T4[−30h+(14v1+16v0)T+(3a0−2a1)T2]12T5[12h−6h(v1+v0)T+(a1−a0)T2].(1-25)
(1-25){a0=a0a1=v0a2=12a0a3=12T3[20h−(8v1+12v0)T−(3a0−a1)T2]a4=12T4[−30h+(14v1+16v0)T+(3a0−2a1)T2]a5=12T5[12h−6h(v1+v0)T+(a1−a0)T2].

Example2.9:使用五次多项式确定运动轨迹：
(a)
t0=0,q0=0,v0=0,a0=0,t1=8,q1=10,v1=0,a1=0.
t0=0,t1=8,q0=0,q1=10,v0=0,v1=0,a0=0,a1=0.
(b)
t0=0,q0=0,v0=−5,a0=0,t1=8,q1=10,v1=−10,a1=0.
t0=0,t1=8,q0=0,q1=10,v0=−5,v1=−10,a0=0,a1=0.
可以注意到，使用三次多项式是无法指定加速度的边界条件的。对(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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
Example 2.10:对于example2.7、example2.8 的条件（参见上一篇博客轨迹规划–三次多项式轨迹）使用五次多项式确定轨迹，如下图所示。并与example2.8对比，发现“平滑度”得到了改善。

给定条件
t0=0,q0=10,v0=0,t1=2,q1=20,v1=−10,t2=4,q2=0,v2=10,t3=8,q3=30,v3=3,t4=10,q4=40,v4=0.
t0=0,t1=2,t2=4,t3=8,t4=10,q0=10,q1=20,q2=0,q3=30,q4=40,v0=0,v1=−10,v2=10,v3=3,v4=0.
由此系列点确定的轨迹如下图所示。

图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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
接下来将介绍七次多项式和更高阶次的多项式轨迹。
---------------------
作者：Brian2018
来源：CSDN
原文：https://blog.csdn.net/libing403/article/details/78715418
版权声明：本文为博主原创文章，转载请附上博文链接！