三次样条函数matalb程序

使用的是假设速度为未知量的方法求解，插补时间段不能太小，插补时间段太小会产生矢量不对齐。
大部分参考以下两个链接来源
https://www.guyuehome.com/752
https://www.guyuehome.com/18462

% 三次样条函数，边界条件为初始速度和终止速度，并且中间各个插补点角度位移已知
% 输入参数： t：时间序列  q：角度序列  n：插补点位数量
%  v0：初始速度  vn：终止速度  tt：插补的时间间隔
% 输出：
% yy dyy ddyy：样条曲线函数值、速度、加速度值
function [yy,dyy,ddyy] = cubicspline(q,t,n,v0,vn,tt)
if length(q) ~= length(t)
    error('输入的数据应成对输入')
end

h = zeros(1,n-1); % 代表时间间隔
A = zeros(n-1,4); % 代表三次样条函数的系数矩阵
a = zeros(1,n-2); % 代表λ
c = zeros(1,n-2); % 代表υ
g = zeros(1,n-2); % 代表g

%% 计算所有点位的运动速度
for i = 1:n-1
    h(i) = t(i+1) - t(i);
end
for i = 2:n-1
    a(i-1) = h(i)/(h(i)+h(i-1));
    b(i-1) = 2;
    c(i-1) = h(i-1)/(h(i)+h(i-1));
    g(i-1) = 3*(c(i-1)*(q(i+1)-q(i))/h(i)+a(i-1)*(q(i)-q(i-1))/h(i-1));
end
g(1) = g(1)-a(1)*v0;
g(n-2) = g(n-2) - c(n-2)*vn;
[vk] = crout(a,b,c,g);
v = [v0,vk,vn];

%% 计算出各段函数的系数
for i=1:n-1
    A(i,1) = q(i);
    A(i,2) = v(i);
    A(i,3) = 3*(q(i+1)-q(i))/(h(i)*h(i))-(v(i+1)+2*v(i))/h(i);
    A(i,4) = (v(i+1)+v(i))/(h(i)*h(i))-2*(q(i+1)-q(i))/(h(i)*h(i)*h(i));
end

%% 计算所有插补点的位置、速度、加速度
s = 0-tt;
j = 0;
while s < t(n)
    s = s + tt;
    j = j + 1;
end

yy = zeros(1,j);
dyy = zeros(1,j);
ddyy = zeros(1,j);

s = 0-tt;
for i = 1:j-1
     s = s + tt;
     klo = 1;
     khi = n;
    while (khi-klo)>1
        k =fix((khi+klo)/2);
        if t(k)>s
             khi = k;
        else
             klo = k;
        end   
    end   
    yy(i) = A(klo,1) + A(klo,2)*(s-t(klo)) + A(klo,3)*power(s-t(klo),2)+A(klo,4)*power(s-t(klo),3);
    dyy(i) = A(klo,2) + 2*A(klo,3)*(s-t(klo)) + 3*A(klo,4)*power(s-t(klo),2);
    ddyy(i) = 2*A(klo,3) + 6*A(klo,4)*(s-t(klo));
end
yy(j) = A(n-1,1) + A(n-1,2)*h(n-1) + A(n-1,3)*power(h(n-1),2)+A(n-1,4)*power(h(n-1),3);
dyy(j) =  A(n-1,2) + 2*A(n-1,3)*h(n-1) +3*A(n-1,4)*power(h(n-1),2);
ddyy(j) = 2*A(n-1,3) + 6*A(n-1,4)*h(n-1);
    
end

%追赶法求解三对角线性方程组，Ax=b，A用一维数组a，c，d存储。
function [x]=crout(a,b,c,f)%数组a存储三角矩阵A的主对角线元素，c、d存储主对角线上边下边带宽为1的元素
    n = length(b);
    n1 = length(a);
    n2 = length(c);
     %错误检查
    if n1~=n2%存储矩阵的数组维数错误
        error('MATLAB:Crout:不是三对角矩阵，参数数组中元素个数错误.');
    end
    p = zeros(1,n-1);%代表贝尔塔
    x = zeros(1,n);
    y = zeros(1,n);
    %追赶法程序主体
    y(1) = f(1)/b(1);
    d = b(1);
    for i=2:n
        p(i-1) = c(i-1)/d;
        d = b(i) - a(i)*p(i-1);
        y(i) = (f(i) - a(i)*y(i-1))/d;
    end
    x(n) = y(n);
    for i=n-1:-1:1
        x(i) = y(i) - p(i)*x(i+1);
    end
end

 clc
 clear all

 q = [0, 0.14881055128822188, 0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358, 0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498, 1.4880380234595283, 1.6368410759230068];
 t = [0, 0.422955130, 0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131, 1.274332912, 1.409208218, 1.585026197, 2];

n = length(t);
v0 = 2; vn = -3; tt = 0.01;
[yy,dyy,ddyy] = cubicspline(q, t,n, v0, vn, tt);
 subplot(3, 1, 1)
 plot(t, q, 'o');
 ylabel('位置')
 grid on
 hold on
 plot(t(1):tt:t(n), yy);
 subplot(3, 1, 2)
 plot([t(1), t(n)], [v0, vn], 'o');
 grid on
 hold on
 plot(t(1):tt:t(n), dyy);
 ylabel('速度')
 subplot(3, 1, 3)
 grid on
 hold on
 plot(t(1):tt:t(n), ddyy);
 ylabel('加速度')