目录

    • 系列文章
    • DYY Warning
    • 算法参考链接:
    • 代码
      • .c文件
      • .h文件
      • 调用方法
    • 希望有大佬能帮我指正错误之处。以后假如有机会的话再搞这个模糊吧,头疼!!!

系列文章

智能车复工日记【1】——菜单索引回顾
智能车复工日记【2】——普通PID、变结构PID、微分先行PID、模糊PID、专家PID
智能车复工日记【3】:图像处理——基本扫线和基本特征提取和十字补线
智能车复工日记【4】:关于图像的上下位机的调整问题总结

智能车复工日记【5】:起跑线的识别与车库入库

DYY Warning

今天调入车库的舵机PD时发现了一个重大bug,就是很有可能我写的模糊pid是有问题的。我之前一直以为输出限制为0就和普通PID一样,结果发现并不是,这里贴出关于模糊的代码,当时也是跟着网上的来写的,也不会验证代码的正确性,权当看个乐呵。(赶紧变回了原来的pid)

算法参考链接:

https://blog.csdn.net/shuoyueqishilove/article/details/78236541

https://blog.csdn.net/coder_leaf/article/details/81712821

https://blog.csdn.net/qingfengxd1/article/details/88023414

https://blog.csdn.net/foxclever/article/details/83932107

https://download.csdn.net/download/wm8utr38/10019636

https://download.csdn.net/download/johnsonxjq/10621123

代码

.c文件

#include "fuzzy_control.h"
/*************模糊规则表*******************************/
int STEER_deltaKpMatrix[7][7]={{PB,PB,PM,PM,PS,ZO,ZO},
                             {PB,PB,PM,PS,PS,ZO,NS},
                             {PM,PM,PM,PS,ZO,NS,NS},
                             {PM,PM,PS,ZO,NS,NM,NM},
                             {PS,PS,ZO,NS,NS,NM,NM},
                             {PS,ZO,NS,NM,NM,NM,NB},
                             {ZO,ZO,NM,NM,NM,NB,NB}};
int STEER_deltaKiMatrix[7][7]={{NB,NB,NM,NM,NS,ZO,ZO},
                             {NB,NB,NM,NS,NS,ZO,ZO},
                             {NB,NM,NS,NS,ZO,PS,PS},
                             {NM,NM,NS,ZO,PS,PM,PM},
                             {NM,NS,ZO,PS,PS,PM,PB},
                             {ZO,ZO,PS,PS,PM,PB,PB},
                             {ZO,ZO,PS,PM,PM,PB,PB}};
int STEER_deltaKdMatrix[7][7]={{PS,NS,NB,NB,NB,NM,PS},
                                    {PS,NS,NB,NM,NM,NS,ZO},
                             {ZO,NS,NM,NM,NS,NS,ZO},
                             {ZO,NS,NS,NS,NS,NS,ZO},
                             {ZO,ZO,ZO,ZO,ZO,ZO,ZO},
                             {PB,NS,PS,PS,PS,PS,PB},
                             {PB,PM,PM,PM,PS,PS,PB}}; 

int MOTOR_deltaKpMatrix[7][7]={{PB,PB,PM,PM,PS,ZO,ZO},
                             {PB,PB,PM,PS,PS,ZO,NS},
                             {PM,PM,PM,PS,ZO,NS,NS},
                             {PM,PM,PS,ZO,NS,NM,NM},
                             {PS,PS,ZO,NS,NS,NM,NM},
                             {PS,ZO,NS,NM,NM,NM,NB},
                             {ZO,ZO,NM,NM,NM,NB,NB}};
int MOTOR_deltaKiMatrix[7][7]={{NB,NB,NM,NM,NS,ZO,ZO},
                             {NB,NB,NM,NS,NS,ZO,ZO},
                             {NB,NM,NS,NS,ZO,PS,PS},
                             {NM,NM,NS,ZO,PS,PM,PM},
                             {NM,NS,ZO,PS,PS,PM,PB},
                             {ZO,ZO,PS,PS,PM,PB,PB},
                             {ZO,ZO,PS,PM,PM,PB,PB}};
int MOTOR_deltaKdMatrix[7][7]={{PS,NS,NB,NB,NB,NM,PS},
                             {PS,NS,NB,NM,NM,NS,ZO},
                             {ZO,NS,NM,NM,NS,NS,ZO},
                             {ZO,NS,NS,NS,NS,NS,ZO},
                             {ZO,ZO,ZO,ZO,ZO,ZO,ZO},
                             {PB,NS,PS,PS,PS,PS,PB},
                             {PB,PM,PM,PM,PS,PS,PB}};

//舵机fuzzyPID初始化
void steer_fuzzy_PIDInit(float p_base,float d_base,float deltap_max,float deltap_min,float deltad_max,float deltad_min)
{
    steerPID.target=93; 
    steerPID.actual=0;  
    steerPID.e=0;
    steerPID.de=0;
    steerPID.error_fuzzy=0;
    steerPID.derror_fuzzy=0;
    steerPID.error_fuzzy_leftline=0;
    steerPID.error_fuzzy_rightline=0;
    steerPID.membership_error_fuzzy_leftline=0;
    steerPID.membership_error_fuzzy_rightline=0;
    steerPID.error_Matrix_coordinate[0]=0;
    steerPID.error_Matrix_coordinate[1]=0; 
    steerPID.derror_fuzzy_leftline=0;
    steerPID.derror_fuzzy_rightline=0;
    steerPID.membership_derror_fuzzy_leftline=0;
    steerPID.membership_derror_fuzzy_rightline=0;
    steerPID.derror_Matrix_coordinate[0]=0;    
    steerPID.derror_Matrix_coordinate[1]=0;
    /**需要预设值的参数**/ 
    steerPID.kp_base=p_base;
    steerPID.ki_base=0;
    steerPID.kd_base=d_base;     
    steerPID.input_max=185;
    steerPID.input_min=-185;
    steerPID.kp_max=deltap_max;
    steerPID.kp_min=deltap_min;    
    steerPID.ki_max=0;
    steerPID.ki_min=0;
    steerPID.kd_max=deltad_max;
    steerPID.kd_min=deltad_min;       
    steerPID.error_gauss_sigma=0;    
    steerPID.derror_gauss_sigma=0; 
    /***************************/    
    byte i,j;
    //初始化KP,KI,KD模糊规则
    for(i=0;i<7;i++)
    {
        for(j=0;j<7;j++)
        {
            steerPID.deltaKpMatrix[i][j]=STEER_deltaKpMatrix[i][j];
            steerPID.deltaKiMatrix[i][j]=STEER_deltaKiMatrix[i][j];
            steerPID.deltaKiMatrix[i][j]=STEER_deltaKiMatrix[i][j];
        }
    }
    for(i=0;i<4;i++)
    {
        steerPID.Kp_fuzzy[i]=0;       
        steerPID.Ki_fuzzy[i]=0;       
        steerPID.Kd_fuzzy[i]=0;       
        steerPID.membership_Kp_fuzzy[i]=0;       
        steerPID.membership_Ki_fuzzy[i]=0;       
        steerPID.membership_Kd_fuzzy[i]=0;       
    }
    steerPID.output_Kp=0;
    steerPID.output_Ki=0;
    steerPID.output_Kd=0;
}
void chauncan(float p_base,float d_base,float deltap_max,float deltap_min,float deltad_max,float deltad_min )
{
    steerPID.kp_base=p_base;
    steerPID.kd_base=d_base;
    steerPID.kp_max=deltap_max;
    steerPID.kp_min=deltap_min;
    steerPID.kd_max=deltad_max;
    steerPID.kd_min=deltad_min; 
}
//三角隶属度函数
float FuzzyPID_trimf_left(float x,float leftline,float rightline)
{
   float u;
   u=rightline-x/(rightline-leftline);
   return u;

}
//正态隶属度函数
float FuzzyPID_gaussmf_left(float x,float leftline,float sigma) 
{
    float u;
    if(sigma<0)
    {
       //cout<<"In gaussmf, sigma must larger than 0"<<endl;
        sigma=1;
    }
    u=exp(-pow(((x-leftline)/sigma),2));
    return u;
}

//通过量化函数,得到error和derror对应的模糊值
//输入:error fuzzyPID类型(舵机 or 电机)
float quantization_of_error(int error,fuzzy_PID *sptr)         
{    
    return 6*error*1.0f/(2*(sptr->input_max-sptr->input_min));
}
//输入:error fuzzyPID类型(舵机 or 电机)
float quantization_of_derror(int derror,fuzzy_PID *sptr)
{
    return 6*derror*1.0f/(4*(sptr->input_max-sptr->input_min));
}
//通过量化后的模糊值知道对应的两个边际模糊值
//输入: 模糊值   模糊值的类型(e or ec) fuzzyPID类型(舵机 or 电机)
void find_ploar(float x,byte type,fuzzy_PID *sptr)        
{
    if(type==0) //error
    {
        if(x<NB)       //极小时,左右极限都是NB      
        {
           sptr->error_fuzzy_leftline=NB; 
           sptr->error_fuzzy_rightline=NB;
        }
        else if(x>=NB && x<= NM)
        {
           sptr->error_fuzzy_leftline=NB; 
           sptr->error_fuzzy_rightline=NM;
        }
        else if(x>NM && x<= NS)
        {
           sptr->error_fuzzy_leftline=NM; 
           sptr->error_fuzzy_rightline=NS;
        }
        else if(x>NS && x<= ZO)
        {
           sptr->error_fuzzy_leftline=NS; 
           sptr->error_fuzzy_rightline=ZO;
        }
        else if(x>ZO && x<= PS)
        {
           sptr->error_fuzzy_leftline=ZO; 
           sptr->error_fuzzy_rightline=PS;
        }
        else if(x>PS && x<= PM)
        {
           sptr->error_fuzzy_leftline=PS; 
           sptr->error_fuzzy_rightline=PM;
        }
        else if(x>PM && x<= PB)
        {
           sptr->error_fuzzy_leftline=PM; 
           sptr->error_fuzzy_rightline=PB;
        }
        else //极大时,左右极限都是PB  
        {
           sptr->error_fuzzy_leftline=PB; 
           sptr->error_fuzzy_rightline=PB;
        }            
    }
    else    //derror
    {
     if(x<NB)       //极小时,左右极限都是NB      
        {
           sptr->derror_fuzzy_leftline=NB; 
           sptr->derror_fuzzy_rightline=NB;
        }
        else if(x>=NB && x<= NM)
        {
           sptr->derror_fuzzy_leftline=NB; 
           sptr->derror_fuzzy_rightline=NM;
        }
        else if(x>NM && x<= NS)
        {
           sptr->derror_fuzzy_leftline=NM; 
           sptr->derror_fuzzy_rightline=NS;
        }
        else if(x>NS && x<= ZO)
        {
           sptr->derror_fuzzy_leftline=NS; 
           sptr->derror_fuzzy_rightline=ZO;
        }
        else if(x>ZO && x<= PS)
        {
           sptr->derror_fuzzy_leftline=ZO; 
           sptr->derror_fuzzy_rightline=PS;
        }
        else if(x>PS && x<= PM)
        {
           sptr->derror_fuzzy_leftline=PS; 
           sptr->derror_fuzzy_rightline=PM;
        }
        else if(x>PM && x<= PB)
        {
           sptr->derror_fuzzy_leftline=PM; 
           sptr->derror_fuzzy_rightline=PB;
        }
        else //极大时,左右极限都是PB  
        {
           sptr->derror_fuzzy_leftline=PB; 
           sptr->derror_fuzzy_rightline=PB;
        }
    }
}
//模糊推理的第一步,根据量化函数得到的模糊值求出对应的隶属度
// NB -3
// NM -2
// NS -1
// ZO 0
// PS 1
// PM 2
// PB 3
//输入  模糊值的类型(e or ec) 隶属度函数的类型  fuzzyPID类型(舵机 or 电机)
void find_membership_degree(byte type,byte function_type,fuzzy_PID *sptr)           
{
    if(type==0)      //error
    {
        //当超出极限时,直接拉满
        if((sptr->error_fuzzy_leftline==PB && sptr->error_fuzzy_rightline==PB) ||(sptr->error_fuzzy_leftline==NB && sptr->error_fuzzy_rightline==NB))
        {
            sptr->membership_error_fuzzy_leftline=1;
            sptr->membership_error_fuzzy_rightline=1;
        }
        else 
        {
            switch (function_type)
            {
                case 0: //三角
                {
                    sptr->membership_error_fuzzy_leftline=FuzzyPID_trimf_left(sptr->error_fuzzy,sptr->error_fuzzy_leftline,sptr->error_fuzzy_rightline);
                    sptr->membership_error_fuzzy_rightline=1-sptr->membership_error_fuzzy_leftline;
                }
                break;
                case 1://高斯
                {
                    sptr->membership_error_fuzzy_leftline=FuzzyPID_gaussmf_left(sptr->error_fuzzy,sptr->error_fuzzy_leftline,sptr->error_gauss_sigma);
                    sptr->membership_error_fuzzy_rightline=1-sptr->membership_error_fuzzy_leftline;
                }
                break;         
                default :
                {
                    sptr->membership_error_fuzzy_leftline=FuzzyPID_trimf_left(sptr->error_fuzzy,sptr->error_fuzzy_leftline,sptr->error_fuzzy_rightline);
                    sptr->membership_error_fuzzy_rightline=1-sptr->membership_error_fuzzy_leftline;
                }
                break;               
            }            
        }            
    }
    else                //derror
    {
        //当超出极限时,直接拉满
        if((sptr->derror_fuzzy_leftline==PB && sptr->derror_fuzzy_rightline==PB) ||(sptr->derror_fuzzy_leftline==NB && sptr->derror_fuzzy_rightline==NB))
        {
            sptr->membership_derror_fuzzy_leftline=1;
            sptr->membership_derror_fuzzy_rightline=1;
        }
        else 
        {
            switch (function_type)
            {
                case 0: //三角
                {
                    sptr->membership_derror_fuzzy_leftline=FuzzyPID_trimf_left(sptr->derror_fuzzy,sptr->derror_fuzzy_leftline,sptr->derror_fuzzy_rightline);
                    sptr->membership_derror_fuzzy_rightline=1-sptr->membership_derror_fuzzy_leftline;
                }
                break;
                case 1://高斯
                {
                    sptr->membership_derror_fuzzy_leftline=FuzzyPID_gaussmf_left(sptr->derror_fuzzy,sptr->derror_fuzzy_leftline,sptr->derror_gauss_sigma);
                    sptr->membership_derror_fuzzy_rightline=1-sptr->membership_derror_fuzzy_leftline;
                }
                break;         
                default :
                {
                    sptr->membership_derror_fuzzy_leftline=FuzzyPID_trimf_left(sptr->derror_fuzzy,sptr->derror_fuzzy_leftline,sptr->derror_fuzzy_rightline);
                    sptr->membership_derror_fuzzy_rightline=1-sptr->membership_derror_fuzzy_leftline;
                }
                break;               
            }            
        }            
    }
}
//进行到这一步现在已经求得了对应的隶属度了
//接下来的是输入:error derror输出:KP KI KD对应的三个规则表!!!可能需要修改!(仅展示舵机)
//横坐标是derror 纵坐标是error
//fuzzy_PID steerPID;
//fuzzy_PID motorPID;
void chage_fuzzy_to_coordinate_fourzhi(fuzzy_PID *sptr)    //  fuzzyPID类型(舵机 or 电机)  转化坐标值
{  
    sptr->derror_Matrix_coordinate[0]=sptr->derror_fuzzy_leftline+3;   
    sptr->derror_Matrix_coordinate[1]=sptr->derror_fuzzy_rightline+3;  
    sptr->error_Matrix_coordinate[0]=sptr->error_fuzzy_leftline+3;   
    sptr->error_Matrix_coordinate[1]=sptr->error_fuzzy_rightline+3;  
}
void membership_fuzzy(byte KPID,fuzzy_PID *sptr)// 求Kp还是Ki还是Kd    fuzzyPID类型(舵机 or 电机)
{
    if(KPID==0) //KP
    {
        chage_fuzzy_to_coordinate_fourzhi(&steerPID);        //将模糊值转化为坐标
        //求出Kp对应的四个模糊值
        sptr->Kp_fuzzy[0]=sptr->deltaKpMatrix[sptr->derror_Matrix_coordinate[0]][sptr->error_Matrix_coordinate[0]]; //左上
        sptr->Kp_fuzzy[1]=sptr->deltaKpMatrix[sptr->derror_Matrix_coordinate[1]][sptr->error_Matrix_coordinate[0]]; //右上
        sptr->Kp_fuzzy[2]=sptr->deltaKpMatrix[sptr->derror_Matrix_coordinate[0]][sptr->error_Matrix_coordinate[1]]; //左下
        sptr->Kp_fuzzy[3]=sptr->deltaKpMatrix[sptr->derror_Matrix_coordinate[1]][sptr->error_Matrix_coordinate[1]]; //右下  
        //求出Kp四个模糊值各自的隶属度
        sptr->membership_Kp_fuzzy[0]=sptr->membership_derror_fuzzy_leftline * sptr->membership_error_fuzzy_leftline;//左上
        sptr->membership_Kp_fuzzy[1]=sptr->membership_derror_fuzzy_rightline * sptr->membership_error_fuzzy_leftline;//右上
        sptr->membership_Kp_fuzzy[2]=sptr->membership_derror_fuzzy_leftline * sptr->membership_error_fuzzy_rightline;//左下
        sptr->membership_Kp_fuzzy[3]=sptr->membership_derror_fuzzy_rightline * sptr->membership_error_fuzzy_rightline;//右下                 
    }
    else if(KPID==1) //KI
    {
        chage_fuzzy_to_coordinate_fourzhi(&steerPID);        //将模糊值转化为坐标
        //求出Ki对应的四个模糊值
        sptr->Ki_fuzzy[0]=sptr->deltaKiMatrix[sptr->derror_Matrix_coordinate[0]][sptr->error_Matrix_coordinate[0]]; //左上
        sptr->Ki_fuzzy[1]=sptr->deltaKiMatrix[sptr->derror_Matrix_coordinate[1]][sptr->error_Matrix_coordinate[0]]; //右上
        sptr->Ki_fuzzy[2]=sptr->deltaKiMatrix[sptr->derror_Matrix_coordinate[0]][sptr->error_Matrix_coordinate[1]]; //左下
        sptr->Ki_fuzzy[3]=sptr->deltaKiMatrix[sptr->derror_Matrix_coordinate[1]][sptr->error_Matrix_coordinate[1]]; //右下  
        //求出Kp四个模糊值各自的隶属度
        sptr->membership_Ki_fuzzy[0]=sptr->membership_derror_fuzzy_leftline * sptr->membership_error_fuzzy_leftline;//左上
        sptr->membership_Ki_fuzzy[1]=sptr->membership_derror_fuzzy_rightline * sptr->membership_error_fuzzy_leftline;//右上
        sptr->membership_Ki_fuzzy[2]=sptr->membership_derror_fuzzy_leftline * sptr->membership_error_fuzzy_rightline;//左下
        sptr->membership_Ki_fuzzy[3]=sptr->membership_derror_fuzzy_rightline * sptr->membership_error_fuzzy_rightline;//右下
    } 
    else
    {
        chage_fuzzy_to_coordinate_fourzhi(&steerPID);        //将模糊值转化为坐标
        //求出Ki对应的四个模糊值
        sptr->Kd_fuzzy[0]=sptr->deltaKdMatrix[sptr->derror_Matrix_coordinate[0]][sptr->error_Matrix_coordinate[0]]; //左上
        sptr->Kd_fuzzy[1]=sptr->deltaKdMatrix[sptr->derror_Matrix_coordinate[1]][sptr->error_Matrix_coordinate[0]]; //右上
        sptr->Kd_fuzzy[2]=sptr->deltaKdMatrix[sptr->derror_Matrix_coordinate[0]][sptr->error_Matrix_coordinate[1]]; //左下
        sptr->Kd_fuzzy[3]=sptr->deltaKdMatrix[sptr->derror_Matrix_coordinate[1]][sptr->error_Matrix_coordinate[1]]; //右下  
        //求出Kp四个模糊值各自的隶属度
        sptr->membership_Kd_fuzzy[0]=sptr->membership_derror_fuzzy_leftline * sptr->membership_error_fuzzy_leftline;//左上
        sptr->membership_Kd_fuzzy[1]=sptr->membership_derror_fuzzy_rightline * sptr->membership_error_fuzzy_leftline;//右上
        sptr->membership_Kd_fuzzy[2]=sptr->membership_derror_fuzzy_leftline * sptr->membership_error_fuzzy_rightline;//左下
        sptr->membership_Kd_fuzzy[3]=sptr->membership_derror_fuzzy_rightline * sptr->membership_error_fuzzy_rightline;//右下
    }            
}
//解模糊
/*
第一步:利用规则表,求出对应的模糊值和隶属度乘积,得到的是kpid在论域上的模糊值
第二部:利用论域与kpid输出的线性关系,将论域上的模糊值转化为kpid
*/
//舵机还是电机 求Kp还是Ki还是Kd kpid的论域值 kpid的论域最大值 kpid的论域最小值
float fan_quantization(fuzzy_PID *sptr,byte KPID,float fuzzy_kpid)   
{
    float result=0;
    if(KPID==0) //KP
    {
        result= ((sptr->kp_max-sptr->kp_min)/6)*fuzzy_kpid;    //fuzzy_kpid范围:【-3:3】   result范围:【kpid_min:kpid_max】
    }
    else if(KPID==1)    //KI
    {
        result= ((sptr->ki_max-sptr->ki_min)/6)*fuzzy_kpid;    //fuzzy_kpid范围:【-3:3】   result范围:【kpid_min:kpid_max】
    }
    else            //KD
    {
        result= ((sptr->kd_max-sptr->kd_min)/6)*fuzzy_kpid;    //fuzzy_kpid范围:【-3:3】   result范围:【kpid_min:kpid_max】
    }

}
//加权平均法解模糊
void ave_deblurring(fuzzy_PID *sptr,byte KPID) //舵机还是电机 求Kp还是Ki还是Kd
{
    if(KPID==0) //KP
    {
        float fenzi;
        float fenmu;
        float result;
        byte i=0;
        for(i=0;i<=3;i++)
        {
            fenzi+= sptr->Kp_fuzzy[i]*sptr->membership_Kp_fuzzy[i]; //Kp对应的四个模糊值乘上对应的隶属度
            fenmu+=sptr->membership_Kp_fuzzy[i];                    //不出意外应该是1
        }
        result=fenzi/fenmu;                         //得出来的值仍然是一种模糊值
        sptr->output_Kp=fan_quantization(sptr,KPID,result)+sptr->kp_base;     //对应到论域上 加上基础值
    }
    else if(KPID==1)    //KI
    {
        float fenzi;
        float fenmu;
        float result;
        byte i=0;
        for(i=0;i<=3;i++)
        {
            fenzi+= sptr->Ki_fuzzy[i]*sptr->membership_Ki_fuzzy[i]; //Kp对应的四个模糊值乘上对应的隶属度
            fenmu+=sptr->membership_Ki_fuzzy[i];                    //不出意外应该是1
        }
        result=fenzi/fenmu;
        sptr->output_Ki=fan_quantization(sptr,KPID,result)+sptr->ki_base;     //对应到论域上
    }
    else            //KD
    {
        float fenzi;
        float fenmu;
        float result;
        byte i=0;
        for(i=0;i<=3;i++)
        {
            fenzi+= sptr->Kd_fuzzy[i]*sptr->membership_Kd_fuzzy[i]; //Kp对应的四个模糊值乘上对应的隶属度
            fenmu+=sptr->membership_Kd_fuzzy[i];                    //不出意外应该是1
        }
        result=fenzi/fenmu;
        sptr->output_Kd=fan_quantization(sptr,KPID,result)+sptr->kd_base;     //对应到论域上
    }
}
/*********舵机模糊PID调用函数**************/
/*
输入: 实际中线值 目标中线值
输出: 无(最终结果都存储在结构体里面) 
*/
void steer_fuzzypid_cal(byte point_zhi,byte real_zhi)
{
    steerPID.target=point_zhi; 
    steerPID.actual=real_zhi;  
    steerPID.e=steerPID.target-steerPID.actual;
    steerPID.de=steerPID.e-steerPID.e_pre_1;
    //求出error和derror对应的模糊值
    steerPID.error_fuzzy=quantization_of_error(steerPID.e,&steerPID);
    steerPID.derror_fuzzy=quantization_of_derror(steerPID.de,&steerPID);
    //通过量化后的模糊值知道对应的两个边际模糊值,得出来的值自动存入结构体中
    find_ploar(steerPID.error_fuzzy,0,&steerPID);       //error
    find_ploar(steerPID.error_fuzzy,1,&steerPID);       //derror
    //至此,模糊化结束!模糊推理开始
    //模糊推理的第一步,根据量化函数得到的模糊值求出对应的隶属度
    find_membership_degree(0,0,&steerPID);      //error  隶属度函数类型: 三角
    find_membership_degree(1,0,&steerPID);      //derror 隶属度函数类型:三角
    //将输入坐标转换
    chage_fuzzy_to_coordinate_fourzhi(&steerPID);
    membership_fuzzy(0,&steerPID);              //求Kp对应的四个模糊值和对应的隶属度
    membership_fuzzy(1,&steerPID);              //求Ki对应的四个模糊值和对应的隶属度
    membership_fuzzy(2,&steerPID);              //求Kd对应的四个模糊值和对应的隶属度
    //至此模糊推理结束,开始解模糊
    //加权平均法解模糊
    ave_deblurring(&steerPID,0);                //输出deltaKp
    ave_deblurring(&steerPID,1);                //输出deltaKi
    ave_deblurring(&steerPID,2);                //输出deltaKd    
    /*最后处理*/
    steerPID.e_pre_1=steerPID.e; 
}

.h文件

#ifndef _fuzzy_control_h
#define _fuzzy_control_h
#define  N_MAtrix  7
#define NB -3
#define NM -2
#define NS -1
#define ZO 0
#define PS 1
#define PM 2
#define PB 3
#define STEER_PID 0
#define MOTOR_PID 1
#include "headfile.h"
typedef struct fuzzy_PID
{
    float target;  //系统的控制目标
    float actual;  //采样获得的实际值
    float e;       //误差
    float e_pre_1; //上一次的误差
    //float e_pre_2; //上上次的误差
    float de;      //误差的变化率
    //byte PIDtype;
    float error_fuzzy;
    int error_fuzzy_leftline;
    int error_fuzzy_rightline;
    float membership_error_fuzzy_leftline;
    float membership_error_fuzzy_rightline;
    int error_Matrix_coordinate[2];     //将模糊值转化成坐标
    float derror_fuzzy;
    int derror_fuzzy_leftline;
    int derror_fuzzy_rightline;
    float membership_derror_fuzzy_leftline;
    float membership_derror_fuzzy_rightline;
    int derror_Matrix_coordinate[2];    //将模糊值转化成坐标
    float error_gauss_sigma;    //可调参数!!!
    float derror_gauss_sigma;    //可调参数!!!
    int deltaKpMatrix[N_MAtrix][N_MAtrix];
    int deltaKiMatrix[N_MAtrix][N_MAtrix];
    int deltaKdMatrix[N_MAtrix][N_MAtrix];    
    int Kp_fuzzy[4];       //存放由error和derror求解出来的Kp对应的四个模糊值
    int Ki_fuzzy[4];       //存放由error和derror求解出来的Ki对应的四个模糊值
    int Kd_fuzzy[4];       //存放由error和derror求解出来的Kd对应的四个模糊值
    float membership_Kp_fuzzy[4];       //存放对应四个模糊值的隶属度
    float membership_Ki_fuzzy[4];       //存放对应四个模糊值的隶属度
    float membership_Kd_fuzzy[4];       //存放对应四个模糊值的隶属度
    float kp_base;
    float ki_base;
    float kd_base; 
    float output_Kp;
    float output_Ki;
    float output_Kd;
    int input_max;
    int input_min;
    float kp_max;       
    float ki_max;
    float kd_max;
    float kp_min;
    float ki_min;
    float kd_min;   
}fuzzy_PID;

extern fuzzy_PID steerPID;
extern fuzzy_PID motorPID;

void steer_fuzzypid_cal(byte point_zhi,byte real_zhi);
void chauncan(float p_base,float d_base,float deltap_max,float deltap_min,float deltad_max,float deltad_min );
void steer_fuzzy_PIDInit(float p_base,float d_base,float deltap_max,float deltap_min,float deltad_max,float deltad_min);
//typedef struct fuzzy_PID
//{
//    float target;  //系统的控制目标
//    float actual;  //采样获得的实际值
//    float e;       //误差
//    float e_pre_1; //上一次的误差
//    float e_pre_2; //上上次的误差
//    float de;      //误差的变化率
//    float emax;    //误差基本论域上限
//    float demax;   //误差辩化率基本论域的上限
//    float delta_Kp_max;   //delta_kp输出的上限
//    float delta_Ki_max;   //delta_ki输出上限
//    float delta_Kd_max;   //delta_kd输出上限
//    float Ke;      //Ke=n/emax,量化论域为[-3,-2,-1,0,1,2,3]
//    float Kde;     //Kde=n/demax,量化论域为[-3,-2,-1,0,1,2,3]
//    float Ku_p;    //Ku_p=Kpmax/n,量化论域为[-3,-2,-1,0,1,2,3]
//    float Ku_i;    //Ku_i=Kimax/n,量化论域为[-3,-2,-1,0,1,2,3]
//    float Ku_d;    //Ku_d=Kdmax/n,量化论域为[-3,-2,-1,0,1,2,3]
//    int Kp_rule_matrix[N][N];//Kp模糊规则矩阵
//    int Ki_rule_matrix[N][N];//Ki模糊规则矩阵
//    int Kd_rule_matrix[N][N];//Kd模糊规则矩阵
//    byte mf_t_e;       //e的隶属度函数类型
//    byte mf_t_de;      //de的隶属度函数类型
//    byte mf_t_Kp;      //kp的隶属度函数类型
//    byte mf_t_Ki;      //ki的隶属度函数类型
//    byte mf_t_Kd;      //kd的隶属度函数类型
//    float *e_mf_paras; //误差的隶属度函数的参数
//    float *de_mf_paras;//误差的偏差隶属度函数的参数
//    float *Kp_mf_paras; //kp的隶属度函数的参数
//    float *Ki_mf_paras; //ki的隶属度函数的参数
//    float *Kd_mf_paras; //kd的隶属度函数的参数
//    float Kp;
//    float Ki;
//    float Kd;
//}fuzzy_PID;
#endif

调用方法

float deltap_max_different_road[8]={0,0.16,0.06,0.1,0.1,0.02,0.1,0.1};
float deltap_min_different_road[8]={-0,-0.16,-0.06,-0.1,-0.1,-0.02,-0.1,-0.1};
float deltad_max_different_road[8]={0,0.3,0.1,0.3,0.3,0.1,0.2,0.2};
float deltad_min_different_road[8]={-0,-0.3,-0.1,-0.3,-0.3,-0.1,-0.2,-0.2};
int LocPIDcalc_new(int type,int nextpoint)
{   
   int result=0;
   int matrix_x=0;
    /**********直到***********/
    if(type==1)
    {     
        matrix_x=0;
        steer_P=Proportion_str;
        steer_D=Derivative_str;
        chauncan(steer_P,steer_D,deltap_max_different_road[matrix_x],deltap_min_different_road[matrix_x],deltad_max_different_road[matrix_x],deltad_min_different_road[matrix_x]);        
        steer_fuzzypid_cal(steer_setpoint,nextpoint);    //输入目标值和实际值,得到KP和KD
        result=steerPID.output_Kp*steerPID.e+steerPID.output_Kd*steerPID.de; 
        return   result;          
    } 
}

希望有大佬能帮我指正错误之处。以后假如有机会的话再搞这个模糊吧,头疼!!!