三次样条插值 python opencv 链接点

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

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

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

程序如下：

//=====================三次样条插值的函数S(x)实现=============================
// 说 明： 根据研究生教材《数值分析》(李庆杨)第51页~第56页编写
#include
#define MAXNUM 50 //定义样条数据区间个数最多为50个
typedefstructSPLINE//定义样条结构体，用于存储一条样条的所有信息
{ //初始化数据输入
floatx[MAXNUM+1];//存储样条上的点的x坐标，最多51个点
floaty[MAXNUM+1];//存储样条上的点的y坐标，最多51个点
unsigned intpoint_num;//存储样条上的实际的 点 的个数
floatbegin_k1;//开始点的一阶导数信息
floatend_k1;//终止点的一阶导数信息
//float begin_k2; //开始点的二阶导数信息
//float end_k2; //终止点的二阶导数信息
//计算所得的样条函数S(x)
floatk1[MAXNUM+1];//所有点的一阶导数信息
floatk2[MAXNUM+1];//所有点的二阶导数信息
//51个点之间有50个段，func[]存储每段的函数系数
floata3[MAXNUM],a1[MAXNUM];
floatb3[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;
typedefintRESULT;//返回函数执行的结果状态，下面为具体的返回选项
#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)
{
floatH[MAXNUM] = {0};//小区间的步长
floatFi[MAXNUM] = {0};//中间量
floatU[MAXNUM+1] = {0};//中间量
floatA[MAXNUM+1] = {0};//中间量
floatD[MAXNUM+1] = {0};//中间量
floatM[MAXNUM+1] = {0};//M矩阵
floatB[MAXNUM+1] = {0};//追赶法中间量
floatY[MAXNUM+1] = {0};//追赶法中间变量
inti = 0;

计算中间参数

if((pLine->point_num point_num > MAXNUM + 1))
{
returnERR;//输入数据点个数太少或太多
}
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];
}
returnTRUE;
}
//
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);
return0;
}
//

头文件：

#ifndef SPLINE3INTERP_H
#define SPLINE3INTERP_H
#include 
#include 
namespace splab
{
template 
class Spline3Interp : public Interpolation
{
public:
using Interpolation::xi;
using Interpolation::yi;
Spline3Interp( const Vector &xn, const Vector &yn,
Type d2l=Type(0), Type d2r=Type(0) );
~Spline3Interp();
void calcCoefs();
Type evaluate( Type x );
Matrix getCoefs() const;
private:
void derivative2( Vector &dx, Vector &d1,
Vector &d2 );
Type M0,
Mn;
Matrix coefs;
};
// class Spline3Interp
#include 
}
// namespace splab
#endif
// SPLINE3INTERP_H

实现文件：

template 
Spline3Interp::Spline3Interp( const Vector &xn,
const Vector &yn,
Type d2l, Type d2r )
: Interpolation( xn, yn ), M0(d2l), Mn(d2r)
{
}
template 
Spline3Interp::~Spline3Interp()
{
}
template 
void Spline3Interp::derivative2( Vector &dx, Vector &d1,
Vector &d2 )
{
int N = xi.size(),
M = N-1;
Vector b(M),
v(M),
y(M),
alpha(M),
beta(M-1);
for( int i=1; 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
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
alpha[i] = b[i] - dx[i]*dx[i-1]/alpha[i-1];
for( int i=1; i
beta[i] = dx[i]/alpha[i];
y[1] = v[1]/alpha[1];
for( int i=2; 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 
void Spline3Interp::calcCoefs()
{
int N = xi.size(),
M = N-1;
Vector m(N),
h(M),
d(M);
m[0] = M0;
m[M] = Mn;
for( int i=0; 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
{
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 
Type Spline3Interp::evaluate( Type x )
{
int k = -1,
N = xi.size(),
M = N-1;
Type dx,
y;
for( int i=0; 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 
inline Matrix Spline3Interp::getCoefs() const
{
return coefs;
}

测试代码：

#define BOUNDS_CHECK
#include 
#include 
#include 
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 x( 11, xi ), y( 11, yi );
Spline3Interp 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
＃i nclude
#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
h[i]=x[i+1]-x[i];
for(i=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
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
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
printf("m(-) =
�n",i,m[i]);
printf("n");