【征文FSL 算法经验篇】三次曲线拟合

三寸日光

下面是我的三次曲线拟合的算法，没有使用开方求极大值，经过我一般的试验后，发现在知道上精确度是很高的，误差范围小于1cm.但由于传感器最大值与最小值比标定做的不好，所以诚心将算法拿出来和大家交流，希望能将曲线拟合算法变得跟使用与丰富，也希望大家能将好的传感器最大值与最小值比标定的算法能和我分享510786964@qq.com
//传感器位置-10-5 5 10，三次多项式矩阵的逆矩阵。系数Q[4]=X_Array[4]*L_value[4];
float X_Array[4][4]={ {-0.0007,0.0013,-0.0013,0.0007},
                                      {0.0067,-0.0067,-0.0067,0.0067},
                                      {0.0167,-0.1333,0.1333,-0.0167},
                                      {-0.1667,0.6667,0.6667,-0.1667}  };
char X[4] = {-10,-5,5,10}; //传感器实际位置
unsigned char L_value[4]={0};  //存储传感器归一化后的值
float AD[4];            //存储ad转换的值
float XX;           //存储最终得到的偏移距离


//曲线拟合函数，目的是求出小车偏离磁导线的偏移距离
void Curve_fitting()
{
        float Q[4]={0};  //存储三次多项式系数
        float Xi=0;
        float Y=0,Ymax=0;//二次函数值
        unsigned char ss=0;
        char Xmax=0; //存储X[4]中最大的值
        float Ys,Xs=0;//存储二次函数的最大值对应的x值；
        float Xtemp=0;//存储磁导线偏移的实际位置
        unsigned char i,j;

        
               
        Get_Lvalue();   //归一化函数，得到归一后的值  可是本人对传感器最大值与最小值标定这一块做的不理想

    //找出对应最大值的传感器位置
        ss=L_value[0];
        Xmax=X[0];
        for(i=0;i<3;i++)
        {
                if(ss<L_value[i+1])
                {
                       ss=L_value[i+1];
                       Xmax=X[i+1];
                }
        }
        
        
    //计算三次多项式系数
        for(i=0;i<4;i++)
                for(j=0;j<4;j++)
                {
                       Q += X_Array[j]*L_value[j];
                }

    //计算三次多项式求导后，Xmax对应的二次函数的值Ymax,它主要用于后面的比较与搜素极大值使用
        Ymax=3*Q[0]*Xmax*Xmax+2*Q[1]*Xmax+Q[2];
        
        //计算二次函数的中轴线及其对应的函数值，也是用于后面的比较与搜素极大值使用
        Xs=(-Q[1])/(3*Q[0]);      
        Ys=3*Q[0]*Xs*Xs+2*Q[1]*Xs+Q[2];

//**************************************************************************
            if(Q[0]>0&&Ys<0)        //当二次曲线开口向上，必须保证有解，所以Ys<0
                {
                       if(Xmax>=Xs)         //画图后很直观的能明白这样做的目的
                               for(Xi=Xs;;Xi-=0.1)
                               {
                                      Y=3*Q[0]*Xi*Xi+2*Q[1]*Xi+Q[2];
                                       if(Y>=0)
                                       {
                                              Xtemp = Xi;
                                              break;
                                       }
                               }
                       else
                          {
                               if(Ymax>=0)
                                      for(Xi=Xmax;;Xi+=0.1)
                                       {
                                              Y=3*Q[0]*Xi*Xi+2*Q[1]*Xi+Q[2];
                                              if(Y<=0)
                                              {
                                                      Xtemp = Xi-0.1;
                                                      break;
                                              }
                                       }
                               else
                                       for(Xi=Xmax;;Xi-=0.1)
                                       {
                                              Y=3*Q[0]*Xi*Xi+2*Q[1]*Xi+Q[2];
                                              if(Y>=0)
                                              {
                                                      Xtemp = Xi+0.1;
                                                      break;
                                              }
                                       }
                               }
                }
//**************************************************************************
        
//**************************************************************************      
        if(Q[0]<0&&Ys>0)               //当二次曲线开口向下，必须保证有解，所以Ys<0
        {
                if(Xmax<Xs)
                       for(Xi=Xs;;Xi+=0.1)
                       {
                               Y=3*Q[0]*Xi*Xi+2*Q[1]*Xi+Q[2];
                               if(Y<=0)
                               {
                                       Xtemp = Xi-0.1;
                                       break;
                               }
                       }
                else if(Xmax>=Xs)
                   {
                       if(Ymax>=0)
                               for(Xi=Xmax;;Xi+=0.1)
                               {
                                      Y=3*Q[0]*Xi*Xi+2*Q[1]*Xi+Q[2];
                                       if(Y<=0)
                                       {
                                              Xtemp = Xi-0.1;
                                              break;
                                       }
                               }
                       else
                               for(Xi=Xmax;;Xi-=0.1)
                               {
                                      Y=3*Q[0]*Xi*Xi+2*Q[1]*Xi+Q[2];
                                       if(Y>=0)
                                       {
                                              Xtemp = Xi+0.1;
                                              break;
                                       }
                               }
                       }
        }
//**************************************************************************
//若磁导线在最左的电感之左或在最右电感的之右，将偏移值设最大
       if(Q[0]>0&&Ys>0||Q[0]<0&&Ys<0)
                Xtemp = Xmax;         //这样做就是为了测试用，具体针对中情况的算法还没想好


        
     XX =  Xtemp;      //最后得到偏移值
}

阅读全文

三寸日光

大家觉得咋样呢

三寸日光

大家觉得如何，自顶

FSL_TICS_Robin

非常感谢你的经验分享！