三次样条插值曲线的C语言实现

标签:
财经 |
分类: 数学物理,概率统计,机器学习 |
最近一个师弟问我关于机器人路径生成的问题,我也考虑这个问题很长时间了。去年做机器人比赛时就把机器人路径生成规划和存储跟随等这些功能实现了,但是当时因为没接触到三次样条曲线,所以路径函数的生成是用了比较笨的方法。最近接触到了三次样条曲线,刚好实现机器人路径生成的要求。正好师弟他们也要用,写出来也许有用。
程序如下:
//=====================三次样条插值的函数S(x)实现=============================
//
-
-
#include
-
////////////////////////////////////////////////////////////////////////////////
-
#define
MAXNUM 50 //定义样条数据区间个数最多为50个 -
typedef
struct SPLINE //定义样条结构体,用于存储一条样条的所有信息 -
{
//初始化数据输入 -
float x[MAXNUM+1]; //存储样条上的点的x坐标,最多51个点 -
float y[MAXNUM+1]; //存储样条上的点的y坐标,最多51个点 -
unsigned int point_num; //存储样条上的实际的点 的个数 -
float begin_k1; //开始点的一阶导数信息 -
float end_k1; //终止点的一阶导数信息 -
//float begin_k2; //开始点的二阶导数信息 -
//float end_k2; //终止点的二阶导数信息 -
//计算所得的样条函数S(x) -
float k1[MAXNUM+1]; //所有点的一阶导数信息 -
float k2[MAXNUM+1]; //所有点的二阶导数信息 -
//51个点之间有50个段,func[]存储每段的函数系数 -
float a3[MAXNUM],a1[MAXNUM]; -
float b3[MAXNUM],b1[MAXNUM]; -
//分段函数的形式为 Si(x) = a3[i] * {x(i+1) - x}^3 + a1[i] * {x(i+1) - x} + -
// b3[i] * {x - x(i)}^3 + b1[i] * {x - x(i)} -
//xi为x[i]的值,xi_1为x[i+1]的值 -
}SPLINE,*pSPLINE;
-
typedef
int RESULT; //返回函数执行的结果状态,下面为具体的返回选项 -
#ifndef
TRUE -
#define
TRUE 1 -
#endif
-
#ifndef
FALSE -
#define
FALSE -1 -
#endif
-
#ifndef
NULL -
#define
NULL 0 -
#endif
-
#ifndef
ERR -
#define
ERR -2 -
#endif
-
//////////////////////////////////////////////////////////////////////////////////
-
-
RESULT
Spline3(pSPLINE pLine) -
{
-
float H[MAXNUM] //小区间的步长= {0}; -
float Fi[MAXNUM] //中间量= {0}; -
float U[MAXNUM+1] //中间量= {0}; -
float A[MAXNUM+1] //中间量= {0}; -
float D[MAXNUM+1] //中间量= {0}; -
float M[MAXNUM+1] //M矩阵= {0}; -
float B[MAXNUM+1] //追赶法中间量= {0}; -
float Y[MAXNUM+1] //追赶法中间变量= {0}; -
int i = 0; -
////////////////////////////////////////计算中间参数 -
if((pLine->point_num < 3) || (pLine->point_num > MAXNUM + 1)) -
{ -
return ERR; //输入数据点个数太少或太多 -
} -
for(i = 0;i <= pLine->point_num - 2;i++) -
{ //求H[i] -
H[i] = pLine->x[i+1] - pLine->x[i]; -
Fi[i] = (pLine->y[i+1] - pLine->y[i]) / H[i]; //求F[x(i),x(i+1)] -
} -
for(i = 1;i <= pLine->point_num - 2;i++) -
{ //求U[i]和A[i]和D[i] -
U[i] = H[i-1] / (H[i-1] + H[i]); -
A[i] = H[i] / (H[i-1] + H[i]); -
D[i] = 6 * (Fi[i] - Fi[i-1]) / (H[i-1] + H[i]); -
} -
//若边界条件为1号条件,则 -
U[i] = 1; -
A[0] = 1; -
D[0] = 6 * (Fi[0] - pLine->begin_k1) / H[0]; -
D[i] = 6 * (pLine->end_k1 - Fi[i-1]) / H[i-1]; -
//若边界条件为2号条件,则 -
//U[i] = 0; -
//A[0] = 0; -
//D[0] = 2 * begin_k2; -
//D[i] = 2 * end_k2; -
/////////////////////////////////////////追赶法求解M矩阵 -
B[0] = A[0] / 2; -
for(i = 1;i <= pLine->point_num - 2;i++) -
{ -
B[i] = A[i] / (2 - U[i] * B[i-1]); -
} -
Y[0] = D[0] / 2; -
for(i = 1;i <= pLine->point_num - 1;i++) -
{ -
Y[i] = (D[i] - U[i] * Y[i-1]) / (2 - U[i] * B[i-1]); -
} -
M[pLine->point_num - 1] = Y[pLine->point_num - 1]; -
for(i = pLine->point_num - 1;i > 0;i--) -
{ -
M[i-1] = Y[i-1] - B[i-1] * M[i]; -
} -
//////////////////////////////////////////计算方程组最终结果 -
for(i = 0;i <= pLine->point_num - 2;i++) -
{ -
pLine->a3[i] = M[i] / (6 * H[i]); -
pLine->a1[i] = (pLine->y[i] - M[i] * H[i] * H[i] / 6) / H[i]; -
pLine->b3[i] = M[i+1] / (6 * H[i]); -
pLine->b1[i] = (pLine->y[i+1] - M[i+1] * H[i] * H[i] / 6) /H[i]; -
} -
return TRUE; -
}
-
//////////////////////////////////////////////////////////////////////////////////
-
SPLINE
line1; -
pSPLINE
pLine1 = &line1; -
//////////////////////////////////////////////////////////////////////////////////
-
main()
-
{
-
line1.x[0] = 27.7; -
line1.x[1] = 28; -
line1.x[2] = 29; -
line1.x[3] = 30; -
line1.y[0] = 4.1; -
line1.y[1] = 4.3; -
line1.y[2] = 4.1; -
line1.y[3] = 3.0; -
line1.point_num = 4; -
line1.begin_k1 = 3.0; -
line1.end_k1 = -4.0; -
Spline3(pLine1); -
return 0; -
}
-
//////////////////////////////////////////////////////////////////////////////////
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 void main() { double x[N]={0,70,130,210,337,578,776,1012,1142,1462,1841},
int
i,k;
h[0] =
x[1]-x[0]; 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++)
for(i=1;i<N-1;i++)
{
} for(i=1;i<N-1;i++) { //求Ai,Bi
} 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--)
for(k=1;k<=36;k++)
{
} printf("所求的外形曲线在x[k]=50*k(k=1,2,...,36)处的函数值分别为:\n");
for(k=1;k<=36;k++)
printf("\n"); printf("m[i]的值分别为:\n"); for(i=0;i<N;i++)
printf("\n"); |