一、通用的4-3-4分段多项式插值，支持三段或大于三段插值，起始速度和加速度、结束速度和加速度可设(MATLAB代码)

%{
    
    
Function: four_three_four_piecewise_polynomial
Description: 计算4-3-4分段多项式的系数
Input: 向量t(递增序列), 向量p, 起始速度vs, 结束速度ve, 起始加速度as, 结束加速度ae, 向量维数n
Output: 4-3-4分段多项式的系数a
Author: Marc Pony(marc_pony@163.com)
%}
function a = four_three_four_piecewise_polynomial(t, p, vs, ve, as, ae, n)

if n < 4
    error('向量维数小于4!');
end

segmentCount = n - 1;
parameterCount = 5 + 4 * (segmentCount - 2) + 5;
a = zeros(segmentCount, 5);
A = zeros(parameterCount, parameterCount);
B = zeros(parameterCount, 1);

%% （1）起点约束
%位置
A(1, 1) = 1.0;
A(1, 2) = t(1);
A(1, 3) = t(1)^2;
A(1, 4) = t(1)^3;
A(1, 5) = t(1)^4;
B(1) = p(1);

%速度
A(2, 1) = 0.0;
A(2, 2) = 1.0;
A(2, 3) = 2.0 * t(1);
A(2, 4) = 3.0 * t(1)^2;
A(2, 5) = 4.0 * t(1)^3;
B(2) = vs;

%加速度
A(3, 1) = 0.0;
A(3, 2) = 0.0;
A(3, 3) = 2.0;
A(3, 4) = 6.0 * t(1);
A(3, 5) = 12.0 * t(1)^2;
B(3) = as;

%% （2）第一二段连接处约束
%位置
A(4, 1) = 1.0;
A(4, 2) = t(2);
A(4, 3) = t(2)^2;
A(4, 4) = t(2)^3;
A(4, 5) = t(2)^4;
B(4) = p(2);

A(5, 6) = 1.0;
A(5, 7) = t(2);
A(5, 8) = t(2)^2;
A(5, 9) = t(2)^3;
B(5) = p(2);

%速度
A(6, 1) = 0.0;
A(6, 2) = 1.0;
A(6, 3) = 2.0 * t(2);
A(6, 4) = 3.0 * t(2)^2;
A(6, 5) = 4.0 * t(2)^3;
A(6, 6) = 0.0;
A(6, 7) = -1.0;
A(6, 8) = -2.0 * t(2);
A(6, 9) = -3.0 * t(2)^2;
B(6) = 0.0;

%加速度
A(7, 1) = 0.0;
A(7, 2) = 0.0;
A(7, 3) = 2.0;
A(7, 4) = 6.0 * t(2);
A(7, 5) = 12.0 * t(2)^2;
A(7, 6) = 0.0;
A(7, 7) = 0.0;
A(7, 8) = -2.0;
A(7, 9) = -6.0 * t(2);
B(7) = 0.0;

%% （3）中间三次多项式连接处约束
i = 8;
j = 6;
k = 3;
for m = 1 : segmentCount - 3
    %位置
    A(i, j) = 1.0;
    A(i, j + 1) = t(k);
    A(i, j + 2) = t(k)^2;
    A(i, j + 3) = t(k)^3;
    B(i) = p(k);
    
    A(i + 1, j + 4) = 1.0;
    A(i + 1, j + 5) = t(k);
    A(i + 1, j + 6) = t(k)^2;
    A(i + 1, j + 7) = t(k)^3;
    B(i + 1) = p(k);
    
    %速度
    A(i + 2, j) = 0.0;
    A(i + 2, j + 1) = 1.0;
    A(i + 2, j + 2) = 2.0 * t(k);
    A(i + 2, j + 3) = 3.0 * t(k)^2;
    A(i + 2, j + 4) = 0.0;
    A(i + 2, j + 5) = -1.0;
    A(i + 2, j + 6) = -2.0 * t(k);
    A(i + 2, j + 7) = -3.0 * t(k)^2;
    B(i + 2) = 0.0;
    
    %加速度
    A(i + 3, j) = 0.0;
    A(i + 3, j + 1) = 0.0;
    A(i + 3, j + 2) = 2.0;
    A(i + 3, j + 3) = 6.0 * t(k);
    A(i + 3, j + 4) = 0.0;
    A(i + 3, j + 5) = 0.0;
    A(i + 3, j + 6) = -2.0;
    A(i + 3, j + 7) = -6.0 * t(k);
    B(i + 3) = 0.0;
    
    k = k + 1;
    i = i + 4;
    j = j + 4;
end

%% （4）最后两段连接处约束
%位置
i = 8 + 4 * (segmentCount - 3);
j = 6 + 4 * (segmentCount - 3);
A(i, j) = 1.0;
A(i, j + 1) = t(n - 1);
A(i, j + 2) = t(n - 1)^2;
A(i, j + 3) = t(n - 1)^3;
B(i) = p(n - 1);

A(i + 1, j + 4) = 1.0;
A(i + 1, j + 5) = t(n - 1);
A(i + 1, j + 6) = t(n - 1)^2;
A(i + 1, j + 7) = t(n - 1)^3;
A(i + 1, j + 8) = t(n - 1)^4;
B(i + 1) = p(n - 1);

%速度
A(i + 2, j) = 0.0;
A(i + 2, j + 1) = 1.0;
A(i + 2, j + 2) = 2.0 * t(n - 1);
A(i + 2, j + 3) = 3.0 * t(n - 1)^2;
A(i + 2, j + 4) = 0.0;
A(i + 2, j + 5) = -1.0;
A(i + 2, j + 6) = -2.0 * t(n - 1);
A(i + 2, j + 7) = -3.0 * t(n - 1)^2;
A(i + 2, j + 8) = -4.0 * t(n - 1)^3;
B(i + 2) = 0.0;

%加速度
A(i + 3, j) = 0.0;
A(i + 3, j + 1) = 0.0;
A(i + 3, j + 2) = 2.0;
A(i + 3, j + 3) = 6.0 * t(n - 1);
A(i + 3, j + 4) = 0.0;
A(i + 3, j + 5) = 0.0;
A(i + 3, j + 6) = -2.0;
A(i + 3, j + 7) = -6.0 * t(n - 1);
A(i + 3, j + 8) = -12.0 * t(n - 1)^2;
B(i + 3) = 0.0;


%% （5）终点约束
%位置
A(parameterCount - 2, parameterCount - 4) = 1.0;
A(parameterCount - 2, parameterCount - 3) = t(n);
A(parameterCount - 2, parameterCount - 2) = t(n)^2;
A(parameterCount - 2, parameterCount - 1) = t(n)^3;
A(parameterCount - 2, parameterCount) = t(n)^4;
B(parameterCount - 2) = p(n);

%速度
A(parameterCount - 1, parameterCount - 4) = 0.0;
A(parameterCount - 1, parameterCount - 3) = 1.0;
A(parameterCount - 1, parameterCount - 2) = 2.0 * t(n);
A(parameterCount - 1, parameterCount - 1) = 3.0 * t(n)^2;
A(parameterCount - 1, parameterCount) = 4.0 * t(n)^3;
B(parameterCount - 1) = ve;

%加速度
A(parameterCount, parameterCount - 4) = 0.0;
A(parameterCount, parameterCount - 3) = 0.0;
A(parameterCount, parameterCount - 2) = 2.0;
A(parameterCount, parameterCount - 1) = 6.0 * t(n);
A(parameterCount, parameterCount) = 12.0 * t(n)^2;
B(parameterCount) = ae;

%% 计算4-3-4分段多项式的系数
x = A \ B;
a(1, :) = x(1 : 5);
k = 6;
for i = 2 : segmentCount - 1
    a(i, 1 : 4) = x(k : k + 3);
    k = k + 4;
end
a(segmentCount, :) = x(parameterCount - 4 : parameterCount);

end

clc;
clear;
close all;

n = 10; %插值点数(n >= 4)
t = linspace(0, 1, n); %递增时间序列
p = 10.0 * t.^3 - 15.0 * t.^4 + 6.0 * t.^5;
vs = 0; %起始速度
ve = 0; %结束速度
as = 0; %起始加速度
ae = 0; %结束加速度
deltaT = 0.001; %插补周期

a = four_three_four_piecewise_polynomial(t, p, vs, ve, as,ae, n);

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; p(1)];
vel = [vel; vs];
acc = [acc; as];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    s = a(i, 1) + tt .* (a(i, 2) + tt .* (a(i, 3) + tt .* (a(i, 4) + a(i, 5) .* tt)));
    ds = a(i, 2) + tt .* (2.0 * a(i, 3) + tt .* (3.0 * a(i, 4) + 4.0 * a(i, 5) .* tt));
    dds = 2.0 * a(i, 3) + tt .* (6.0 * a(i, 4) + 12.0 * a(i, 5) .* tt);
    time = [time; tt];
    pos = [pos; s];
    vel = [vel; ds];
    acc = [acc; dds];
end
if abs(time(end) - t(n)) > 1.0e-8
    time = [time; t(n)];
    pos = [pos; p(n)];
    vel = [vel; ve];
    acc = [acc; ae];
end

figure(1)
plot(time, pos);
hold on
plot(t, p, 'ro');
xlabel('t')
ylabel('pos')

figure(2)
plot(time, vel);
hold on
plot([t(1), t(n)], [vs, ve], 'ro');
xlabel('t')
ylabel('vel')

figure(3)
plot(time, acc);
hold on
plot([t(1), t(n)], [as, ae], 'ro');
xlabel('t')
ylabel('acc')