MATLAB三次样条插值：轨迹生成与跟踪应用

理论部分在此不再赘述：https://blog.csdn.net/caokaifa/article/details/81605914

目的是分享MATLAB源码。

有需要的朋友点击链接获取：

链接：https://pan.baidu.com/s/1CQcWnYZZ3EyoLvYs1eXiIA 

提取码：zyy7

matlab程序分为两部分，Coefficient函数用于计算a、b、c、d：

function [a,b,c,d]=Coefficient(x,y)

%{

这段程序主要是求三次样条插值的系数 y=ax^3+bx^2+cx+d

作者：Robert.Cao

 %}

nx=length(x);  %计算x长度

h=diff(x);  %计算x之间的差

a=y;   %系数a

A=zeros(7,7);

%A=[1,1];   

A(1,1)=1;    

for i=2:nx

    if i~=nx

         A(i,i)= 2.0 * (h(i-1) + h(i));

    end

    A(i, i-1)= h(i-1);

    A(i-1, i)= h(i-1);

    A(1,2) = 0.0;

    A(nx,nx - 1)= 0.0;

    A(nx, nx)= 1.0;

end

B=zeros(7,1);

for i=2:nx-1

    B(i,1)=6.0 * (a(i + 1) - a(i)) / h(i) - 6.0 * (a(i) -a(i-1)) / h(i-1);

end

m=inv(A)*B;   %解方程组求m

c=m/2.0;  %系数c

for i=1:nx-1

    d(i)=(c(i + 1) - c(i)) / (3.0 * h(i));  %计算系数d

    b(i) = (a(i + 1) - a(i)) / h(i) - h(i) * (c(i + 1) + 2.0 * c(i)) / 3.0;%系数b

end

主程序Trajectory_generation用于轨迹生成基保存轨迹：

x = [-4., -2, 0.0, 2, 4, 6, 10];

y = [1.2, 0.6, 0.0, 1.5, 3.8, 5.0, 3.0];

[a,b,c,d]=Coefficient(x,y);

mx=-4:0.01:10;

L=length(mx);

for i=1:L

    rx=mx(i);

    if rx==10

        rx=rx-0.01;

    else

        rx=rx;

    end

    rx=floor(rx);

    if mod(rx,2)==0

        rx=rx;

    else

        rx=rx-1;

    end

    if rx==8

        rx=rx-2;

    else

        rx=rx;

    end

    low = 0; 

    high = length(x); 

    while(low <= high)

        mid = (low + high)/2;

        mid=ceil(mid);

        midval = x(mid);

        if midval < rx

            low = mid + 1; 

        elseif midval > rx

            high = mid - 1; 

        else

           nx(i)=mid;

           break;

        end

    end

end

for i=1:L

 dx(i)=mx(i)-x(nx(i));

 ry(i) = a(nx(i))+ b(nx(i)) * dx(i)+ c(nx(i))* dx(i) ^ 2.0 +d(nx(i)) * dx(i)^3.0;

end

figure

plot(x,y)

hold on

plot(mx,ry,'-r')

%保存生成的轨迹

path = [];

path(:,1)= mx;

path(:,2)=ry;

save path.mat path

三次样条插值绘图

生成路径并保持