一、问题描述

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

二、推导步骤

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

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

起点处二阶导数估计：
$acc_0=(v_1-v_0)/(t_1-t_0)\tag 4$
终点处二阶导数估计：
$acc_n=(v_n-v_{n-1})/(t_n-t_{n-1})\tag 5$
中间点处二阶导数估计：
$acc_k=(e_k+e_{k+1})/2 \tag 6$
其中， $e_k=(v_k-v_{k-1})/(t_k-t_{k-1})$ 。

起点处三阶导数估计：
$jerk_0=(acc_1-acc_0)/(t_1-t_0)\tag 7$
终点处三阶导数估计：
$jerk_n=(acc_n-acc_{n-1})/(t_n-t_{n-1})\tag 8$
中间点处三阶导数估计：
$jerk_k=(f_k+f_{k+1})/2 \tag 9$
其中， $f_k=(acc_k-acc_{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+a_6(t -t_s)^6+a_7(t -t_s)^7$ 。
一阶导数： $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) = 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) = 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$ 。
对于每一段七次多项式，利用端点处约束:
$\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}$
容易求得系数：
$\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}$
其中， $h=p_e-p_s,T=t_e-t_s$ 。

三、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');

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

文章目录

一、问题描述

二、推导步骤

三、MATLAB代码

猜你喜欢