[转载]三次样条插值曲线的C语言实现

原文地址：三次样条插值曲线的C语言实现作者：QuantWay

最近一个师弟问我关于机器人路径生成的问题，我也考虑这个问题很长时间了。去年做机器人比赛时就把机器人路径生成规划和存储跟随等这些功能实现了，但是当时因为没接触到三次样条曲线，所以路径函数的生成是用了比较笨的方法。最近接触到了三次样条曲线，刚好实现机器人路径生成的要求。正好师弟他们也要用，写出来也许有用。

    我是根据李庆阳的《数值分析》这本教材中的讲解编写的程序，使用的是第一边界条件，用追赶法求解了M矩阵。

    为了调用方便，我将整个函数所有的信息构造成一个结构体，输入插值点的坐标和数量，定义边界条件后，将这个结构体的指针作为参数传给Spline3（）函数，就完成了函数计算，计算结果也存储在该结构体中。

 

程序如下：

 

//=====================三次样条插值的函数S(x)实现=============================

// 说    明： 根据研究生教材《数值分析》（李庆杨）第51页~第56页编写

 

 

[cpp] view plaincopy

1.   
2. #include   
3.   
4. #define  MAXNUM  50   //定义样条数据区间个数最多为50个  
5. typedef struct SPLINE    //定义样条结构体，用于存储一条样条的所有信息  
6. { //初始化数据输入  
7.  float x[MAXNUM+1];    //存储样条上的点的x坐标，最多51个点  
8.  float y[MAXNUM+1];    //存储样条上的点的y坐标，最多51个点  
9.  unsigned int point_num;   //存储样条上的实际的 点 的个数  
10.  float begin_k1;     //开始点的一阶导数信息  
11.  float end_k1;     //终止点的一阶导数信息  
12.  //float begin_k2;    //开始点的二阶导数信息  
13.  //float end_k2;     //终止点的二阶导数信息  
14.  //计算所得的样条函数S(x)  
15.  float k1[MAXNUM+1];    //所有点的一阶导数信息  
16.  float k2[MAXNUM+1];    //所有点的二阶导数信息  
17.  //51个点之间有50个段，func[]存储每段的函数系数  
18.  float a3[MAXNUM],a1[MAXNUM];      
19.  float b3[MAXNUM],b1[MAXNUM];  
20.  //分段函数的形式为 Si(x) = a3[i] * {x(i+1) - x}^3  + a1[i] * {x(i+1) - x} +  
21.  //        b3[i] * {x - x(i)}^3 + b1[i] * {x - x(i)}  
22.  //xi为x[i]的值，xi_1为x[i+1]的值        
23. }SPLINE,*pSPLINE;  
24. typedef int RESULT;      //返回函数执行的结果状态，下面为具体的返回选项  
25. #ifndef TRUE  
26. #define TRUE 1  
27. #endif  
28. #ifndef FALSE  
29. #define FALSE -1  
30. #endif  
31. #ifndef NULL  
32. #define NULL 0  
33. #endif  
34. #ifndef ERR  
35. #define ERR  -2  
36. #endif  
37. //  
38.   
39. RESULT Spline3(pSPLINE pLine)  
40. {  
41.  float H[MAXNUM] = {0};     //小区间的步长  
42.  float Fi[MAXNUM] = {0};     //中间量  
43.  float U[MAXNUM+1] = {0};    //中间量  
44.  float A[MAXNUM+1] = {0};    //中间量  
45.  float D[MAXNUM+1] = {0};    //中间量  
46.  float M[MAXNUM+1] = {0};    //M矩阵  
47.  float B[MAXNUM+1] = {0};    //追赶法中间量  
48.  float Y[MAXNUM+1] = {0};    //追赶法中间变量  
49.  int i = 0;  
50.  计算中间参数  
51.  if((pLine->point_num < 3) || (pLine->point_num > MAXNUM + 1))  
52.  {  
53.   return ERR;       //输入数据点个数太少或太多  
54.  }  
55.  for(i = 0;i <= pLine->point_num - 2;i++)  
56.  {          //求H[i]  
57.   H[i] = pLine->x[i+1] - pLine->x[i];  
58.   Fi[i] = (pLine->y[i+1] - pLine->y[i]) / H[i]; //求F[x(i),x(i+1)]  
59.  }  
60.  for(i = 1;i <= pLine->point_num - 2;i++)  
61.  {          //求U[i]和A[i]和D[i]  
62.   U[i] = H[i-1] / (H[i-1] + H[i]);  
63.   A[i] = H[i] / (H[i-1] + H[i]);  
64.   D[i] = 6 * (Fi[i] - Fi[i-1]) / (H[i-1] + H[i]);  
65.  }  
66.  //若边界条件为1号条件，则  
67.  U[i] = 1;  
68.  A[0] = 1;  
69.  D[0] = 6 * (Fi[0] - pLine->begin_k1) / H[0];  
70.  D[i] = 6 * (pLine->end_k1 - Fi[i-1]) / H[i-1];  
71.  //若边界条件为2号条件，则  
72.  //U[i] = 0;  
73.  //A[0] = 0;  
74.  //D[0] = 2 * begin_k2;  
75.  //D[i] = 2 * end_k2;  
76.  /追赶法求解M矩阵  
77.  B[0] = A[0] / 2;  
78.  for(i = 1;i <= pLine->point_num - 2;i++)  
79.  {  
80.   B[i] = A[i] / (2 - U[i] * B[i-1]);  
81.  }  
82.  Y[0] = D[0] / 2;  
83.  for(i = 1;i <= pLine->point_num - 1;i++)  
84.  {  
85.   Y[i] = (D[i] - U[i] * Y[i-1]) / (2 - U[i] * B[i-1]);  
86.  }  
87.  M[pLine->point_num - 1] = Y[pLine->point_num - 1];  
88.  for(i = pLine->point_num - 1;i > 0;i--)  
89.  {  
90.   M[i-1] = Y[i-1] - B[i-1] * M[i];  
91.  }  
92.  //计算方程组最终结果  
93.  for(i = 0;i <= pLine->point_num - 2;i++)  
94.  {  
95.   pLine->a3[i] = M[i] / (6 * H[i]);  
96.   pLine->a1[i] = (pLine->y[i] - M[i] * H[i] * H[i] / 6) / H[i];  
97.   pLine->b3[i] = M[i+1] / (6 * H[i]);  
98.   pLine->b1[i] = (pLine->y[i+1] - M[i+1] * H[i] * H[i] / 6) /H[i];  
99.  }  
100.  return TRUE;  
101. }  
102. //  
103. SPLINE line1;  
104. pSPLINE pLine1 = &line1;  
105. //  
106. main()  
107. {  
108.  line1.x[0] = 27.7;  
109.  line1.x[1] = 28;  
110.  line1.x[2] = 29;  
111.  line1.x[3] = 30;  
112.  line1.y[0] = 4.1;  
113.  line1.y[1] = 4.3;  
114.  line1.y[2] = 4.1;  
115.  line1.y[3] = 3.0;  
116.  line1.point_num = 4;  
117.  line1.begin_k1 = 3.0;  
118.  line1.end_k1 = -4.0;  
119.  Spline3(pLine1);  
120.  return 0;  
121. }  
122. // 

 

http://blog.csdn.net/keyearth/article/details/4036755

 

头文件：

 

#ifndef SPLINE3INTERP_H
#define SPLINE3INTERP_H


#include <matrix.h>
#include <interpolation.h>


namespace splab
{

    template <typename Type>
    class Spline3Interp : public Interpolation<Type>
    {

     public:

        using Interpolation<Type>::xi;
        using Interpolation<Type>::yi;

        Spline3Interp( const Vector<Type> &xn, const Vector<Type> &yn,
                       Type d2l=Type(0), Type d2r=Type(0) );
        ~Spline3Interp();

        void calcCoefs();
        Type evaluate( Type x );
        Matrix<Type> getCoefs() const;

    private:

        void derivative2( Vector<Type> &dx, Vector<Type> &d1,
                          Vector<Type> &d2 );

        Type M0,
             Mn;
        Matrix<Type> coefs;

    };
    // class Spline3Interp


    #include <spline3interp-impl.h>

}
// namespace splab


#endif
// SPLINE3INTERP_H

实现文件：

 

template <typename Type>
Spline3Interp<Type>::Spline3Interp( const Vector<Type> &xn,
                                    const Vector<Type> &yn,
                                    Type d2l, Type d2r )
                     : Interpolation<Type>( xn, yn ), M0(d2l), Mn(d2r)
{
}

template <typename Type>
Spline3Interp<Type>::~Spline3Interp()
{
}



template <typename Type>
void Spline3Interp<Type>::derivative2( Vector<Type> &dx, Vector<Type> &d1,
                                  Vector<Type> &d2 )
{
    int N = xi.size(),
        M = N-1;
    Vector<Type> b(M),
                 v(M),
                 y(M),
                 alpha(M),
                 beta(M-1);

        for( int i=1; i<M; ++i )
                b[i] = 2 * (dx[i]+dx[i-1]);

        v[1] = 6*(d1[1]-d1[0]) - dx[0]*d2[0];
        for( int i=1; i<M-1; ++i )
                v[i] = 6 * (d1[i]-d1[i-1]);
        v[M-1] = 6*(d1[M-1]-d1[M-2]) - dx[M-1]*d2[M];

        alpha[1] = b[1];
        for( int i=2; i<M; ++i )
                alpha[i] = b[i] - dx[i]*dx[i-1]/alpha[i-1];

        for( int i=1; i<M-1; ++i )
                beta[i] = dx[i]/alpha[i];

        y[1] = v[1]/alpha[1];
        for( int i=2; i<M; ++i )
                y[i] = (v[i]-dx[i]*y[i-1]) / alpha[i];

        d2[M-1] = y[M-1];
        for( int i=M-2; i>0; --i )
                d2[i] = y[i] - beta[i]*d2[i+1];
}



template <typename Type>
void Spline3Interp<Type>::calcCoefs()
{
    int N = xi.size(),
        M = N-1;

    Vector<Type> m(N),
                 h(M),
                 d(M);

    m[0] = M0;
    m[M] = Mn;
    for( int i=0; i<M; ++i )
    {
        h[i] = xi[i+1]-xi[i];
        d[i] = (yi[i+1]-yi[i]) / h[i];
    }

    derivative2( h, d, m );

        coefs.resize( M, 4 );
        for( int i=0; i<M; ++i )
        {
                coefs[i][0] = yi[i];
                coefs[i][1] = d[i] - h[i]*(2*m[i]+m[i+1])/6;
                coefs[i][2] = m[i] / 2;
                coefs[i][3] = (m[i+1]-m[i]) / (6*h[i]);
        }
}



template <typename Type>
Type Spline3Interp<Type>::evaluate( Type x )
{
    int k = -1,
        N = xi.size(),
        M = N-1;

        Type dx,
         y;

        for( int i=0; i<M; ++i )
        {
                if( (xi[i]<=x) && (xi[i+1]>=x) )
                {
                        k = i;
                        dx = x-xi[i];
                        break;
                }
        }
        if(k!=-1)
        {
                y = ( ( coefs[k][3]*dx + coefs[k][2] ) * dx + coefs[k][1] ) * dx
                  + coefs[k][0];
                return y;
        }
        else
        {
                cerr << "The value is out of range!" << endl;
                return Type(0);
        }
}



template <typename Type>
inline Matrix<Type> Spline3Interp<Type>::getCoefs() const
{
        return coefs;
}

测试代码：

 

#define BOUNDS_CHECK

#include <iostream>
#include <iomanip>
#include <spline3interp.h>


using namespace std;
using namespace splab;


typedef double   Type;


int main()
{
    // f(x) = 1 / (1+25*x^2)   -1 <= x <= 1
    Type xi[11] = { -1.0,   -0.8,   -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8,   1.0 };
    Type yi[11] = { 0.0385, 0.0588, 0.1,  0.2,  0.5,  1.0, 0.5, 0.2, 0.1, 0.0588, 0.0385 };
    Type Ml = 0.2105, Mr = 0.2105;
    Vector<Type> x( 11, xi ), y( 11, yi );

    Spline3Interp<Type> poly( x, y, Ml, Mr );
    poly.calcCoefs();

    cout << setiosflags(ios::fixed) << setprecision(4);
    cout << "Coefficients of cubic spline interpolated polynomial:   "
         << poly.getCoefs() << endl;

    cout << "The true and interpolated values:" << endl;
    cout << "0.0755" << "   " << poly.evaluate(-0.7) << endl
         << "0.3077" << "   " << poly.evaluate(0.3) << endl
         << "0.0471" << "   " << poly.evaluate(0.9) << endl << endl;

    return 0;
}

运行结果：

 

Coefficients of cubic spline interpolated polynomial:   size: 10 by 4
0.0385  0.0737  0.1052  0.1694
0.0588  0.1291  0.2069  0.8886
0.1000  0.3185  0.7400  0.8384
0.2000  0.7151  1.2431  13.4078
0.5000  2.8212  9.2877  -54.4695
1.0000  -0.0000 -23.3940        54.4704
0.5000  -2.8212 9.2882  -13.4120
0.2000  -0.7153 1.2410  -0.8225
0.1000  -0.3176 0.7476  -0.9482
0.0588  -0.1323 0.1787  -0.1224

The true and interpolated values:
0.0755   0.0747
0.3077   0.2974
0.0471   0.0472


Process returned 0 (0x0)   execution time : 0.078 s
Press any key to continue.

 

三次样条插值2

2008-04-06 17:44

 

＃i nclude<stdio.h> ＃i nclude<math.h> #define N 11            //N表示节点的个数 void main() { double x[N]={0,70,130,210,337,578,776,1012,1142,1462,1841},      y[N]={0,57,78,103,135,182,214,244,256,272,275},         h[N],a[N],b[N],A[N],B[N],m[N],s[37],xx[37]; //s[N]表示曲线,其中数组xx[N]表示自变量x[k] int i,k;          h[0] = x[1]-x[0];       //初始化h0,a0,b0,A0,B0 a[0] = 1; b[0] = 3*(y[1]-y[0])/h[0]; A[0] = -a[0]/2; B[0] = b[0]/2; for(i=0;i<N;i++)        //求hi    h[i]=x[i+1]-x[i]; for(i=1;i<N-1;i++) {    //求ai,bi    a[i]=h[i-1]/(h[i-1]+h[i]);    b[i] = 3 * ((1-a[i])*(y[i]-y[i-1])/h[i-1] + a[i]*(y[i+1]-y[i])/h[i]); } for(i=1;i<N-1;i++) { //求Ai,Bi    A[i] = -a[i]/(2+(1-a[i])*A[i-1]);    B[i] = (b[i]-(1-a[i])*B[i-1])/(2+(1-a[i])*A[i-1]); } m[N-1]=(b[N-1]-(1-a[N-1])*B[N-2])/(2+(1-a[N-1])*A[N-2]);//求Mn的值 for(i=N-2;i>=0;i--)        //求m0,m1,-----mn-1的值    m[i] = A[i] * m[i+1] + B[i]; for(k=1;k<=36;k++) {       //求曲线在x[k]处的函数值    xx[k] = 50 * k;    for(i=0;i<N;i++)      //找出x[k]所在的区间     if(xx[k] >= x[i] && xx[k] <= x[i+1])     //s[k]即为x[k]所在区间的三次样条插值函数,以下即为求在x[k]处的函数值     s[k] = (1+2*(xx[k]-x[i])/(x[i+1]-x[i]))*pow((xx[k]-x[i+1])/(x[i]-x[i+1]),2)*y[i] +        (1+2*(xx[k]-x[i+1])/(x[i]-x[i+1]))*pow((xx[k]-x[i])/(x[i+1]-x[i]),2)*y[i+1] +        (xx[k]-x[i])*pow((xx[k]-x[i+1])/(x[i]-x[i+1]),2)*m[i] +        (xx[k]-x[i+1])*pow((xx[k]-x[i])/(x[i+1]-x[i]),2)*m[i+1] ; } printf("所求的外形曲线在x[k]=50*k(k=1,2,...,36)处的函数值分别为:n"); for(k=1;k<=36;k++)        //输出在x[k]处的函数值    printf("s(M) = .8fn",50*k,s[k]); printf("n"); printf("m[i]的值分别为:n"); for(i=0;i<N;i++)    printf("m(-) = �n",i,m[i]); printf("n");