三次样条插值算法实现曲线拟合

三次样条插值

#部分内容及布局参考了CSDN博主 宁悦 的内容。
原文链接：https://blog.csdn.net/deramer1/article/details/79034201

背景

当前正在做的项目有一个需求是可以通过手动调节曲线来改变规划好的航迹。初步想法是将曲线用有限的几个点来固定，然后通过样条插值算法来实现曲线的拟合。
目前项目正处于前期，后续实施之后会在来更新。

样条插值法定义

一种以 可变样条 来作出一条经过一系列点的光滑曲线的数学方法。插值样条是由一些多项式组成的，每一个多项式都是由相邻的两个数据点决定的，这样，任意的两个相邻的多项式以及它们的导数在连接点处都是连续的。

实景举例

假设我们已知四个离散点的数据，如下表所示。

x	y
-2	2
-1	0
1	2
3	1

在我目前的浅薄理解中，根据已知的这四个点来实现这个区间(-2,3)内曲线的拟合，大概就是我想要做的事情，把问题简化一下，如何得到当x=2时的y值呢？

直线插值（自己起的名字，实际上就是一次函数）

可以利用我们初中学过的知识，将这四个点连线，根据（1,2）和（3,1）这两个点确定该连线的方程，然后求出x=2是的y值，但是很明显这种做法会使得求得的曲线不够光滑，而且会导致函数的一阶导数不连续。

二次样条

既然一次函数的直线不能够满足我们的需求，我们使用简单的曲线，二次函数来实现。如果我们用二次函数：aX^2+bx+c来描述曲线，最后的结果可能会好一点，一共有4个点，可以分成3个区间。每一个区间都需要一个二次函数来描述，一共需要9个未知数。然后根据以下条件找出方程组，

1. 曲线方程在节点处的值必须相等，即函数在x1,x2两个点处的值必须符合两个方程，这里一共是4个方程。
2. 第一个端点和最后一个端点必须过第一个和最后一个方程：这里一共是2个方程。
3. 节点处的一阶导数的值必须相等。这里为两个方程。
4. 在这里假设第一个方程的二阶导数为0：这里为一个方程。
   将上述9个方程联立求解，最后就可以实现预测x=2处节点的值。

三次样条

三次样条的原理：
三次样条的原理和二次样条的原理相同，我们用函数aX^3 + bX2+cX+d这个函数来进行操作，这里一共是4个点，分为3个区间，每个区间一个三次样条函数的话，一共是12个方程，只要我们找出这12个方程，这个问题就算解决了。

1>内部节点处的函数值应该相等，这里一共是4个方程。

2>函数的第一个端点和最后一个端点，应该分别在第一个方程和最后一个方程中。这里是2个方程。

3>两个函数在节点处的一阶导数应该相等。这里是两个方程。

4>两个函数在节点处的二阶导数应该相等，这里是两个方程。

5>端点处的二阶导数为零，这里是两个方程。

三次样条的C++实现（追赶法）

#include <iostream>
#include <malloc.h>
#include <stdlib.h>
#include <math.h>
using namespace std;

double *zuigan(double *a,double *b,double *c,double *f,int n) //追赶法求线性方程组
{
    double *x=NULL;
    double *p=NULL;
    double *q=NULL;

    x=(double*)malloc(sizeof(double)*n);
    p=(double*)malloc(sizeof(double)*(n-1));
    q=(double*)malloc(sizeof(double)*n);
    double t=0;
    
    p[0]=f[0]/b[0];
    q[0]=c[0]/b[0];
    
    for (int i=1;i<n;i++)
    {
        t=b[i]-a[i-1]*q[i-1];
        p[i]=(f[i]-a[i-1]*p[i-1])/t;
        q[i]=c[i]/t;
    }

    for (int i=0;i<n;i++)
    {
        x[i]=p[i];
    }

    for (int i=n-2;i>=0;i--)
    {
        x[i]=p[i]-q[i]*x[i+1];
    }

    return x;
}


int main()
{
    double x[]={-3,-2,1,3};
    double y[]={2,0,3,1};
    double S1=-1,S2=1;   //上下边界导数值
    double resX=1.5;
    double resY=0.0;

    double *h=NULL;
    double *u=NULL;
    double *v=NULL;    
    double *b=NULL;
    double *d=NULL;
    int n=0;
    
    double *m=NULL;

    n=sizeof(x)/sizeof(double);
    h=(double*)malloc(sizeof(double)*(n-1));
    for (int i=0;i<n-1;i++)
    {
        h[i]=x[i+1]-x[i];
    }

    u=(double*)malloc(sizeof(double)*(n-2));
    v=(double*)malloc(sizeof(double)*(n-2));
    for (int i=0;i<n-2;i++)
    {
        u[i]=h[i]/(h[i]+h[i+1]);
        v[i]=1-u[i];
    }
    u[n-2]=1;
    
    d=(double*)malloc(sizeof(double)*n);
    for (int i=1;i<n-1;i++)
    {
        d[i]=(6/(h[i-1]+h[i]))*((y[i+1]-y[i])/h[i]-(y[i]-y[i-1])/h[i-1]);
    }

    d[0]=6/h[0]*((y[1]-y[0])/h[0]-S1);
    d[n-1]=6/h[n-2]*(S2-(y[n-1]-y[n-2])/h[n-2]);

    b=(double*)malloc(sizeof(double)*n);
    for (int i=0;i<n;i++)
    {
        b[i]=2;
    }
    

    double tmp=0;
    for (int i=n-2;i>0;i--)
    {
        v[i]=v[i-1];
        
    }
    v[0]=1;

    m=(double*)malloc(sizeof(double)*n);
    m=zuigan(u,b,v,d,n);

    int j=0;
    
    for (double k=x[0];k<=x[n-1];k+=0.1)   //求拟合值，步进为0.1
    {
        resX=k;
        for (int i=0;i<n;i++)
        {
            if (resX>=x[i]&&resX<=x[i+1])
            {
                j=i;
                break;
            }
        }    
        double r1=x[j+1]-resX;
        double r2=resX-x[j];

        resY=(pow(r1,3.0)/(6.0*h[j]))*m[j]+
               (pow(r2,3.0)/(6.0*h[j]))*m[j+1]+
             (y[j]-m[j]*h[j]*h[j]/6.0)*(r1/h[j])+
             (y[j+1]-m[j+1]*h[j]*h[j]/6.0)*(r2/h[j]);

        cout<<resY<<endl;
    }
    
    system("pause");
    return 0;
}