分段五次多项式插值(MATLAB实现)

一、问题描述

  给定$n+1$个点序列$(t_i,p_i)$，利用分段五次多项式插值，使得分段多项式经过所有点序列。其中，$t_i$必须单调递增，$i=0,1,...,n$。

二、推导步骤

  起点处一阶导数估计：
$$v_0=(p_1-p_0)/(t_1-t_0)\text{\hspace{1em}(1)}$$
  终点处一阶导数估计：
$$v_n=(p_n-p_{n-1})/(t_n-t_{n-1})\text{\hspace{1em}(2)}$$
  中间点处一阶导数估计：

$$v_k=(d_k+d_{k+1})/2\text{\hspace{1em}(3)}$$
  其中，$d_k=(p_k-p_{k-1})/(t_k-t_{k-1})$。

  起点处二阶导数估计：
$$ac{c}_0=(v_1-v_0)/(t_1-t_0)\text{\hspace{1em}(4)}$$
  终点处二阶导数估计：
$$ac{c}_n=(v_n-v_{n-1})/(t_n-t_{n-1})\text{\hspace{1em}(5)}$$
  中间点处二阶导数估计：
$$ac{c}_k=(e_k+e_{k+1})/2\text{\hspace{1em}(6)}$$
  其中，$e_k=(v_k-v_{k-1})/(t_k-t_{k-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$，一阶导数：$p^{\prime }(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$，二阶导数：$p^{\prime \prime }(t)=2a_2+6a_3(t-t_s)+12a_4(t-t_s{)}^2+20a_5(t-t_s{)}^3$。
  对于每一段五次多项式，利用端点处约束:
$$\{\begin{array}{l}p(t_s)=p_s\\ p^{\prime }(t_s)=v_s\\ p^{\prime \prime }(t_s)=a_s\\ p(t_e)=p_e\\ p^{\prime }(t_e)=v_e\\ p^{\prime \prime }(t_e)=a_e\end{array}\text{\hspace{1em}(7)}$$
  容易求得系数：
$$\{\begin{array}{l}{a}_0=p_s\\ a_1=v_s\\ a_2=a_s/2\\ a_3=[20h-(8v_e+12v_s)T-(3a_s-a_e)T^2]/(2T^3)\\ a_4=[-30h+(14v_e+16v_s)T+(3a_s-2a_e)T^2]/(2T^4)\\ a_5=[12h-6(v_e+v_s)T-(a_s-a_e)T^2]/(2T^5)\end{array}\text{\hspace{1em}(8)}$$
  其中，$h=p_e-p_s,T=t_e-t_s$。

三、MATLAB代码

clc;
clear;
close all;

%{
syms ts te ps pe vs ve as ae T real;
a = [1, 0, 0, 0, 0, 0
    0, 1, 0, 0, 0, 0
    0, 0, 2, 0, 0, 0
    1, T, T^2, T^3, T^4, T^5
    0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4
    0, 0, 2, 6*T, 12*T^2, 20*T^3
    ] \ [ps; vs; as; pe; ve; ae];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4)), simplify(a(5)), simplify(a(6))]'
%}

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);
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

tArray = [];
posArray = [];
velArray = [];
accArray = [];
tArray = [tArray; t(1)];
posArray = [posArray; pos(1)];
velArray = [velArray; v(1)];
accArray = [accArray; acc(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);
    
    h = pe - ps;
    T = t(i + 1) - t(i);
    a0 = ps;
    a1 = vs;
    a2 = 0.5 * as;
    a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
    a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
    a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
    
    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 + a5 .* (tt - ts)))))];
    velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
    accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
end

figure(1)
subplot(3, 1, 1)
plot(t, pos, 'ro');
hold on;
plot(tArray, posArray);
xlabel('t');
ylabel('pos');

subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('vel');

subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('acc');