三次样条插值-轨迹规划

已知路径点生成无人车运动轨迹的方法–三次样条插值
三次样条的基本理论和求解方式不做具体赘述,可参考博客
需要注意的几个问题:

端点条件

由i的取值范围可知,共有n-1个公式, 但却有n+1个未知量m 。要想求解该方程组,还需另外两个式子。所以需要对两端点x0和xn的微分加些限制。 选择不是唯一的,3种比较常用的限制如下。

1 自由边界

2 固定边界

3 非节点边界

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
#include <iostream> #include <cmath> #include <fstream> using namespace std; class scyt { float *x,*y,*d,*h,*u,*q,*a,*b,*c,*l,*r,*o,*M; int m; float y0,y3; public: scyt(); void qiudao(); void zgf(); void qiujie(); ~scyt(); }; void main() { scyt hello; hello.qiudao(); hello.zgf(); hello.qiujie(); } scyt::scyt() { ifstream fin("三次样条插值.txt"); for(float j;fin>>j;) { m=int(j); break; } x=new float[m]; y=new float[m]; d=new float[m]; h=new float[m-1]; u=new float[m-2]; q=new float[m-2]; a=new float[m-1]; b=new float[m]; c=new float[m-1]; l=new float[m]; r=new float[m-1];//此处的r为追赶法中的u; o=new float[m];//此处o为追赶法中的y M=new float[m];//此处M为追赶法中的x; int jishu=0; for(j;fin>>j;) { if(jishu<=m-1) x[jishu]=j; if(jishu>m-1&&jishu;<2*m) { y[jishu-m]=j; } if(jishu==2*m) { y0=j; } if(jishu==2*m+1) { y3=j; } jishu++; } fin.close(); } void scyt::qiudao() { for(int i=0;i<m-1;i++) { h[i]=x[i+1]-x[i]; } for(i=0;i<m-2;i++) { u[i]=h[i] / (h[i] + h[i+1]); } for(i=0;i<m-2;i++) { q[i]=1-u[i]; } d[0]=6/h[0]*((y[1]-y[0])/h[0]-y0); for(i=1;i<m-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[m-1]=6/h[m-2]*(y3-(y[m-1]-y[m-2])/h[m-2]); } void scyt::zgf() { u[m-2]=1; for(int i=0;i<m;i++) { b[i]=2; } c[0]=1; for(i=1;i<m-1;i++) { c[i]=q[i-1]; } //........................................ l[0]=b[0]; for(i=0;i<m-1;i++) { r[i]=c[i] / l[i]; l[i+1]=b[i+1] - (u[i] * r[i]); } o[0]=d[0] / l[0]; for(i=1;i<m;i++) { o[i]=(d[i]-u[i-1]*o[i-1]) / l[i]; } M[m-1]=o[m-1]; for(i=m-2;i>=0;i--) { M[i]=o[i]-r[i] * M[i+1]; } cout<<m<<"个点的导数值分别是:"<<endl; for(i=0;i<m;i++) { cout<<"M"<<i+1<<"="; cout<<M[i]<<endl; } //M的值求出。。。。。。追赶法调用完毕 } void scyt::qiujie() { float S; for(;;) { float f; cout<<"请输入待求x的值(输入1000)时退出:"; cin>>f; if(f==1000) break; for(int i=0;i<m;i++) { if(f>x[i]&&f<x[i+1]) { S=pow((x[i+1]-f),3)*M[i]/(6*h[i]) + pow(f-x[i],3)*M[i+1]/(6*h[i]) + (x[i+1]-f)*(y[i]-h[i]*h[i]*M[i]/6)/h[i] + (f-x[i])*(y[i+1]-h[i]*h[i]*M[i+1]/6)/h[i]; cout<<"S["<<f<<"]="<<S<<endl; } } } } scyt::~scyt() { delete []x,y,d,h,u,q,a,b,c,l,r,o,M; }

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值