一、问题描述
给定 n + 1 n+1 n+1个点序列 ( t i , p i ) (t_i,p_i) (ti,pi),利用分段七次多项式插值,使得分段多项式经过所有点序列。其中, t i t_i ti必须单调递增, i = 0 , 1 , . . . , n i=0,1,...,n i=0,1,...,n。
二、推导步骤
起点处一阶导数估计:
v
0
=
(
p
1
−
p
0
)
/
(
t
1
−
t
0
)
(1)
v_0=(p_1-p_0)/(t_1-t_0)\tag 1
v0=(p1−p0)/(t1−t0)(1)
终点处一阶导数估计:
v
n
=
(
p
n
−
p
n
−
1
)
/
(
t
n
−
t
n
−
1
)
(2)
v_n=(p_n-p_{n-1})/(t_n-t_{n-1})\tag 2
vn=(pn−pn−1)/(tn−tn−1)(2)
中间点处一阶导数估计:
v
k
=
(
d
k
+
d
k
+
1
)
/
2
(3)
v_k=(d_k+d_{k+1})/2 \tag 3
vk=(dk+dk+1)/2(3)
其中,
d
k
=
(
p
k
−
p
k
−
1
)
/
(
t
k
−
t
k
−
1
)
d_k=(p_k-p_{k-1})/(t_k-t_{k-1})
dk=(pk−pk−1)/(tk−tk−1)。
起点处二阶导数估计:
a
c
c
0
=
(
v
1
−
v
0
)
/
(
t
1
−
t
0
)
(4)
acc_0=(v_1-v_0)/(t_1-t_0)\tag 4
acc0=(v1−v0)/(t1−t0)(4)
终点处二阶导数估计:
a
c
c
n
=
(
v
n
−
v
n
−
1
)
/
(
t
n
−
t
n
−
1
)
(5)
acc_n=(v_n-v_{n-1})/(t_n-t_{n-1})\tag 5
accn=(vn−vn−1)/(tn−tn−1)(5)
中间点处二阶导数估计:
a
c
c
k
=
(
e
k
+
e
k
+
1
)
/
2
(6)
acc_k=(e_k+e_{k+1})/2 \tag 6
acck=(ek+ek+1)/2(6)
其中,
e
k
=
(
v
k
−
v
k
−
1
)
/
(
t
k
−
t
k
−
1
)
e_k=(v_k-v_{k-1})/(t_k-t_{k-1})
ek=(vk−vk−1)/(tk−tk−1)。
起点处三阶导数估计:
j
e
r
k
0
=
(
a
c
c
1
−
a
c
c
0
)
/
(
t
1
−
t
0
)
(7)
jerk_0=(acc_1-acc_0)/(t_1-t_0)\tag 7
jerk0=(acc1−acc0)/(t1−t0)(7)
终点处三阶导数估计:
j
e
r
k
n
=
(
a
c
c
n
−
a
c
c
n
−
1
)
/
(
t
n
−
t
n
−
1
)
(8)
jerk_n=(acc_n-acc_{n-1})/(t_n-t_{n-1})\tag 8
jerkn=(accn−accn−1)/(tn−tn−1)(8)
中间点处三阶导数估计:
j
e
r
k
k
=
(
f
k
+
f
k
+
1
)
/
2
(9)
jerk_k=(f_k+f_{k+1})/2 \tag 9
jerkk=(fk+fk+1)/2(9)
其中,
f
k
=
(
a
c
c
k
−
a
c
c
k
−
1
)
/
(
t
k
−
t
k
−
1
)
f_k=(acc_k-acc_{k-1})/(t_k-t_{k-1})
fk=(acck−acck−1)/(tk−tk−1)。
设七次多项式为:
p
(
t
)
=
a
0
+
a
1
(
t
−
t
s
)
+
a
2
(
t
−
t
s
)
2
+
a
3
(
t
−
t
s
)
3
+
a
4
(
t
−
t
s
)
4
+
a
5
(
t
−
t
s
)
5
+
a
6
(
t
−
t
s
)
6
+
a
7
(
t
−
t
s
)
7
p(t) = a_0 + a_1(t -t_s)+ a_2(t -t_s)^2+a_3(t -t_s)^3+a_4(t -t_s)^4+a_5(t -t_s)^5+a_6(t -t_s)^6+a_7(t -t_s)^7
p(t)=a0+a1(t−ts)+a2(t−ts)2+a3(t−ts)3+a4(t−ts)4+a5(t−ts)5+a6(t−ts)6+a7(t−ts)7。
一阶导数:
p
′
(
t
)
=
a
1
+
2
a
2
(
t
−
t
s
)
+
3
a
3
(
t
−
t
s
)
2
+
4
a
4
(
t
−
t
s
)
3
+
5
a
5
(
t
−
t
s
)
4
+
6
a
6
(
t
−
t
s
)
5
+
7
a
7
(
t
−
t
s
)
6
p'(t) = a_1+ 2a_2(t -t_s)+3a_3(t -t_s)^2+4a_4(t -t_s)^3+5a_5(t -t_s)^4+6a_6(t -t_s)^5+7a_7(t -t_s)^6
p′(t)=a1+2a2(t−ts)+3a3(t−ts)2+4a4(t−ts)3+5a5(t−ts)4+6a6(t−ts)5+7a7(t−ts)6。
二阶导数:
p
′
′
(
t
)
=
2
a
2
+
6
a
3
(
t
−
t
s
)
+
12
a
4
(
t
−
t
s
)
2
+
20
a
5
(
t
−
t
s
)
3
+
30
a
6
(
t
−
t
s
)
4
+
42
a
7
(
t
−
t
s
)
5
p''(t) = 2a_2+6a_3(t -t_s)+12a_4(t -t_s)^2+20a_5(t -t_s)^3+30a_6(t -t_s)^4+42a_7(t -t_s)^5
p′′(t)=2a2+6a3(t−ts)+12a4(t−ts)2+20a5(t−ts)3+30a6(t−ts)4+42a7(t−ts)5。
三阶导数:
p
′
′
′
(
t
)
=
6
a
3
+
24
a
4
(
t
−
t
s
)
+
60
a
5
(
t
−
t
s
)
2
+
120
a
6
(
t
−
t
s
)
3
+
210
a
7
(
t
−
t
s
)
4
p'''(t) = 6a_3+24a_4(t -t_s)+60a_5(t -t_s)^2+120a_6(t -t_s)^3+210a_7(t -t_s)^4
p′′′(t)=6a3+24a4(t−ts)+60a5(t−ts)2+120a6(t−ts)3+210a7(t−ts)4。
对于每一段七次多项式,利用端点处约束:
{
p
(
t
s
)
=
p
s
p
′
(
t
s
)
=
v
s
p
′
′
(
t
s
)
=
a
s
p
′
′
′
(
t
s
)
=
j
s
p
(
t
e
)
=
p
e
p
′
(
t
e
)
=
v
e
p
′
′
(
t
e
)
=
a
e
p
′
′
′
(
t
e
)
=
j
e
(10)
\begin{cases} p(t_s)=p_s \\ p'(t_s)=v_s \\ p''(t_s)=a_s \\ p'''(t_s)=j_s \\ p(t_e)=p_e \\ p'(t_e)=v_e \\ p''(t_e)=a_e \\ p'''(t_e)=j_e \\ \tag {10} \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧p(ts)=psp′(ts)=vsp′′(ts)=asp′′′(ts)=jsp(te)=pep′(te)=vep′′(te)=aep′′′(te)=je(10)
容易求得系数:
{
a
0
=
p
s
a
1
=
v
s
a
2
=
a
s
/
2
a
3
=
j
s
/
6
a
4
=
{
210
h
−
[
(
30
a
s
−
15
a
e
)
T
+
(
4
j
s
+
j
e
)
T
2
+
120
v
s
+
90
v
e
]
T
}
/
(
6
T
4
)
a
5
=
{
−
168
h
+
[
(
20
a
s
−
14
a
e
)
T
+
(
2
j
s
+
j
e
)
T
2
+
90
v
s
+
78
v
e
]
T
}
/
(
2
T
5
)
a
6
=
{
420
h
−
[
(
45
a
s
−
39
a
e
)
T
+
(
4
j
s
+
3
j
e
)
T
2
+
216
v
s
+
204
v
e
]
T
}
/
(
6
T
6
)
a
7
=
{
−
120
h
+
[
(
12
a
s
−
12
a
e
)
T
+
(
j
s
+
j
e
)
T
2
+
60
v
s
+
60
v
e
]
T
}
/
(
6
T
7
)
(11)
\begin{cases} a_0=p_s \\ a_1=v_s \\ a_2=a_s/2 \\ a_3=j_s/6 \\ a_4 = \{210 h - [(30 a_s - 15 a_e) T + (4 j_s + j_e) T^2 + 120 v_s + 90v_e] T\}/ (6T^4)\\ a_5 =\{-168 h + [(20 a_s - 14a_e) T + (2 j_s + j_e) T^2 + 90 v_s + 78v_e] T\} / (2 T^5)\\ a_6 = \{420 h - [(45 a_s - 39 a_e) T + (4 j_s + 3 j_e) T^2 + 216 v_s + 204 v_e] T\} / (6 T^6)\\ a_7 = \{-120h + [(12 a_s - 12 a_e) T + (j_s + j_e) T^2 + 60 v_s + 60 v_e] T\} / (6 T^7)\\ \tag {11} \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧a0=psa1=vsa2=as/2a3=js/6a4={210h−[(30as−15ae)T+(4js+je)T2+120vs+90ve]T}/(6T4)a5={−168h+[(20as−14ae)T+(2js+je)T2+90vs+78ve]T}/(2T5)a6={420h−[(45as−39ae)T+(4js+3je)T2+216vs+204ve]T}/(6T6)a7={−120h+[(12as−12ae)T+(js+je)T2+60vs+60ve]T}/(6T7)(11)
其中,
h
=
p
e
−
p
s
,
T
=
t
e
−
t
s
h=p_e-p_s,T=t_e-t_s
h=pe−ps,T=te−ts。
三、MATLAB代码
clc;
clear;
close all;
%{
syms ts te ps pe vs ve as ae js je T real;
a = [1, 0, 0, 0, 0, 0, 0, 0
0, 1, 0, 0, 0, 0, 0, 0
0, 0, 2, 0, 0, 0, 0, 0
0, 0, 0, 6, 0, 0, 0, 0
1, T, T^2, T^3, T^4, T^5, T^6, T^7
0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4, 6*T^5, 7*T^6
0, 0, 2, 6*T, 12*T^2, 20*T^3, 30*T^4, 42*T^5
0, 0, 0, 6, 24*T, 60*T^2, 120*T^3, 210*T^4
] \ [ps; vs; as; js; pe; ve; ae; je];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4)), simplify(a(5)), simplify(a(6)), simplify(a(7)), simplify(a(8))]'
%}
t = [0, 2, 4, 8, 10]';
pos = [10, 20, 0, 30, 40]';
dt = 0.001;
n = length(t);
v = zeros(n, 1);
acc = zeros(n, 1);
jerk = zeros(n, 1);
v(1) = (pos(2) - pos(1)) / (t(2) - t(1));
v(n) = (pos(n) - pos(n - 1)) / (t(n) - t(n - 1));
for k = 2 : n - 1
v(k) = 0.5 * ((pos(k) - pos(k - 1)) / (t(k) - t(k - 1)) + (pos(k + 1) - pos(k)) / (t(k + 1) - t(k)));
end
acc(1) = (v(2) - v(1)) / (t(2) - t(1));
acc(n) = (v(n) - v(n - 1)) / (t(n) - t(n - 1));
for k = 2 : n - 1
acc(k) = 0.5 * ((v(k) - v(k - 1)) / (t(k) - t(k - 1)) + (v(k + 1) - v(k)) / (t(k + 1) - t(k)));
end
jerk(1) = (acc(2) - acc(1)) / (t(2) - t(1));
jerk(n) = (acc(n) - acc(n - 1)) / (t(n) - t(n - 1));
for k = 2 : n - 1
jerk(k) = 0.5 * ((acc(k) - acc(k - 1)) / (t(k) - t(k - 1)) + (acc(k + 1) - acc(k)) / (t(k + 1) - t(k)));
end
tArray = [];
posArray = [];
velArray = [];
accArray = [];
jerkArray = [];
tArray = [tArray; t(1)];
posArray = [posArray; pos(1)];
velArray = [velArray; v(1)];
accArray = [accArray; acc(1)];
jerkArray = [jerkArray; jerk(1)];
for i = 1 : n - 1
ts = t(i);
te = t(i + 1);
ps = pos(i);
pe = pos(i + 1);
vs = v(i);
ve = v(i + 1);
as = acc(i);
ae = acc(i + 1);
js = jerk(i);
je = jerk(i + 1);
h = pe - ps;
T = t(i + 1) - t(i);
a0 = ps;
a1 = vs;
a2 = 0.5 * as;
a3 = js / 6.0;
a4 = (210.0 * h - ((30.0 * as - 15.0 * ae) * T + (4.0 * js + je) * T^2 + 120.0 * vs + 90.0 * ve) * T) / (6.0 * T^4);
a5 = (-168.0 * h + ((20.0 * as - 14.0 * ae) * T + (2.0 * js + je) * T^2 + 90.0 * vs + 78.0 * ve) * T) / (2.0 * T^5);
a6 = (420.0 * h - ((45.0 * as - 39.0 * ae) * T + (4.0 * js + 3.0 * je) * T^2 + 216.0 * vs + 204.0 * ve) * T) / (6.0 * T^6);
a7 = (-120.0 * h + ((12.0 * as - 12.0 * ae) * T + (js + je) * T^2 + 60.0 * vs + 60.0 * ve) * T) / (6.0 * T^7);
tt = (t(i) + dt : dt : t(i + 1))';
if abs(tt(end) - t(i + 1)) > 1.0e-8
tt = [tt; t(i + 1)];
end
tArray = [tArray; tt];
posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + (tt - ts) .* (a5 + (tt - ts) .* (a6 + a7 .* (tt - ts)))))))];
velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + (tt - ts) .* (5.0 * a5 + (tt - ts) .* (6.0 * a6 + 7.0 * a7 .* (tt - ts))))))];
accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + (tt - ts) .* (20.0 * a5 + (tt - ts) .* (30.0 * a6 + 42.0 * a7 .* (tt - ts)))))];
jerkArray = [jerkArray; 6.0 * a3 + (tt - ts) .* (24.0 * a4 + (tt - ts) .* (60.0 * a5 + (tt - ts) .* (120 * a6 + 210 * a7 .* (tt - ts))))];
end
figure(1)
subplot(4, 1, 1)
plot(t, pos, 'ro');
hold on;
plot(tArray, posArray);
xlabel('t');
ylabel('pos');
subplot(4, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('vel');
subplot(4, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('acc');
subplot(4, 1, 4)
plot(tArray, jerkArray);
xlabel('t');
ylabel('jerk');