手把手教用matlab做无人驾驶（一）-三次样条插值生成轨迹

matlab做深度学习会继续更新，这里最近看这个，所有写了这个:

关于理论部分可以看这篇文章，https://blog.youkuaiyun.com/adamshan/article/details/80696881，

1.3 算法总结

假定有n+1个数据节点

a. 计算步长 (i = 0, 1, …, n-1)

b. 将数据节点和指定的首位端点条件带入矩阵方程

c. 解矩阵方程，求得二次微分值。该矩阵为三对角矩阵，具体求法参见我的上篇文章：三对角矩阵的求解。

d. 计算样条曲线的系数：

其中i = 0, 1, …, n-1

e. 在每个子区间 中，创建方程

matlab程序，这里function程序是计算a、b、c、d：

function [a,b,c,d]=cubic(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];
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
 

主函数程序：

 

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]=cubic(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')
这里查找用了二分法查找。

仿真结果如下：

从图中可以看出拟合的结果。