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;
}
}