/**********************************************************************/
//**********************************************************************
//* 单 位：
//* 室 组：
//* 文 件 名: Fuzzy_PID.c
//* 版 本 号: V1.0
//* 编 译 人:
//* 编译时间：200210724
//* 软件用途:模糊PID算法控制
//*说明：改自blog。csdn。net/shuoyueqishilove/article/details/78236541
//**********************************************************************
/**********************************************************************/
#include "GD32F450ZI_lib.h"
#include "GD32F450ZI_MClib.h"
#include "MC_Globals.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include "Fuzzy_PID.h"
#include "math.h"
//**********************************************************************
//* 函 数 名: Init_Delta_Fuzzy()
//* 软件用途: 初始化模糊PID参数变量
//* 输 入 项:
//* 返 回 值:
//*
//**********************************************************************
void Init_Delta_Fuzzy(void)
{
int 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 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 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}};
float e_mf_paras[]={-3,-3,-2,-3,-2,-1,-2,-1,0,-1,0,1,0,1,2,1,2,3,2,3,3};
float de_mf_paras[]={-3,-3,-2,-3,-2,-1,-2,-1,0,-1,0,1,0,1,2,1,2,3,2,3,3};
float Kp_mf_paras[]={-3,-3,-2,-3,-2,-1,-2,-1,0,-1,0,1,0,1,2,1,2,3,2,3,3};
float Ki_mf_paras[]={-3,-3,-2,-3,-2,-1,-2,-1,0,-1,0,1,0,1,2,1,2,3,2,3,3};
float Kd_mf_paras[]={-3,-3,-2,-3,-2,-1,-2,-1,0,-1,0,1,0,1,2,1,2,3,2,3,3};
Init_PID_Val([1500,650,0.3,0.9,0.6],[0.01,0.04,0.01]);
//FuzzyPID fuzzypid(1500,650,0.3,0.9,0.6,0.01,0.04,0.01);
SetMf("trimf",e_mf_paras,"trimf",de_mf_paras,"trimf",Kp_mf_paras,"trimf",Ki_mf_paras,"trimf",Kd_mf_paras);
SetRuleMatrix(deltaKpMatrix,deltaKiMatrix,deltaKdMatrix);
}
//**********************************************************************
//* 函 数 名: Realize()
//* 软件用途: 实现模糊控制
//* 输 入 项: t:目标值 a:当测量前值
//* 返 回 值: delta_u 控制值
//*
//**********************************************************************
float Realize(float t,float a)
{
float u_e[N],u_de[N],u_u[N];
int u_e_index[3],u_de_index[3];//假设一个输入最多激活3个模糊子集
float delta_Kp,delta_Ki,delta_Kd;
float delta_u;
FuzzyPID.target=t;
FuzzyPID.actual=a;
FuzzyPID.e=FuzzyPID.target-FuzzyPID.actual;
FuzzyPID.de=FuzzyPID.e-FuzzyPID.e_pre_1;
FuzzyPID.e=FuzzyPID.Ke*FuzzyPID.e;
FuzzyPID.de=FuzzyPID.Kde*FuzzyPID.de;
/* 将误差e模糊化*/
int i,j=0;
for(i=0;i<N;i++)
{
if(FuzzyPID.mf_t_e=="trimf")
u_e[i]=Trimf(FuzzyPID.e,FuzzyPID.e_mf_paras[i*3],FuzzyPID.e_mf_paras[i*3+1],FuzzyPID.e_mf_paras[i*3+2]);//e模糊化，计算它的隶属度
else if(mf_t_e=="gaussmf")
u_e[i]=Gaussmf(FuzzyPID.e,FuzzyPID.e_mf_paras[i*2],FuzzyPID.e_mf_paras[i*2+1]);//e模糊化，计算它的隶属度
else if(mf_t_e=="trapmf")
u_e[i]=Trapmf(FuzzyPID.e,FuzzyPID.e_mf_paras[i*4],FuzzyPID.e_mf_paras[i*4+1],FuzzyPID.e_mf_paras[i*4+2],FuzzyPID.e_mf_paras[i*4+3]);//e模糊化，计算它的隶属度
if(u_e[i]!=0)
u_e_index[j++]=i; //存储被激活的模糊子集的下标，可以减小计算量
}
for(j=0;j<3;j++) u_e_index[j]=0; //富余的空间填0
/*将误差变化率de模糊化*/
for(i=0;i<N;i++)
{
if(FuzzyPID.mf_t_de=="trimf")
u_de[i]=Trimf(FuzzyPID.de,FuzzyPID.de_mf_paras[i*3],FuzzyPID.de_mf_paras[i*3+1],FuzzyPID.de_mf_paras[i*3+2]);//de模糊化，计算它的隶属度
else if(FuzzyPID.mf_t_de=="gaussmf")
u_de[i]=Gaussmf(FuzzyPID.de,FuzzyPID.de_mf_paras[i*2],FuzzyPID.de_mf_paras[i*2+1]);//de模糊化，计算它的隶属度
else if(FuzzyPID.mf_t_de=="trapmf")
u_de[i]=Trapmf(FuzzyPID.de,FuzzyPID.de_mf_paras[i*4],FuzzyPID.de_mf_paras[i*4+1],FuzzyPID.de_mf_paras[i*4+2],FuzzyPID.de_mf_paras[i*4+3]);//de模糊化，计算它的隶属度
if(u_de[i]!=0)
u_de_index[j++]=i; //存储被激活的模糊子集的下标，可以减小计算量
}
for(j=0;j<3;j++) u_de_index[j]=0; //富余的空间填0
float den=0,num=0;
int m,n;
/*计算delta_Kp和Kp*/
for(m=0;m<3;m++)
for(n=0;n<3;n++)
{
num+=u_e[u_e_index[m]]*u_de[u_de_index[n]]*FuzzyPID.Kp_rule_matrix[u_e_index[m]][u_de_index[n]];
den+=u_e[u_e_index[m]]*u_de[u_de_index[n]];
}
delta_Kp=num/den;
delta_Kp=FuzzyPID.Ku_p*delta_Kp;
if(delta_Kp>=FuzzyPID.delta_Kp_max) delta_Kp=FuzzyPID.delta_Kp_max;
else if(delta_Kp<=-FuzzyPID.delta_Kp_max) delta_Kp=-FuzzyPID.delta_Kp_max;
FuzzyPID.Kp +=delta_Kp;
if(FuzzyPID.Kp<0) FuzzyPID.Kp=0;
/*计算delta_Ki和Ki*/
den=0;num=0;
for(m=0;m<3;m++)
for(n=0;n<3;n++)
{
num+=u_e[u_e_index[m]]*u_de[u_de_index[n]]*FuzzyPID.Ki_rule_matrix[u_e_index[m]][u_de_index[n]];
den+=u_e[u_e_index[m]]*u_de[u_de_index[n]];
}
delta_Ki=num/den;
delta_Ki=FuzzyPID.Ku_i*delta_Ki;
if(delta_Ki>=FuzzyPID.delta_Ki_max) delta_Ki=FuzzyPID.delta_Ki_max;
else if(delta_Ki<=-FuzzyPID.delta_Ki_max) delta_Ki=-FuzzyPID.delta_Ki_max;
FuzzyPID.Ki+=delta_Ki;
if(FuzzyPID.Ki<0) FuzzyPID.Ki=0;
/*计算delta_Kd和Kd*/
den=0;num=0;
for(m=0;m<3;m++)
for(n=0;n<3;n++)
{
num+=u_e[u_e_index[m]]*u_de[u_de_index[n]]*FuzzyPID.Kd_rule_matrix[u_e_index[m]][u_de_index[n]];
den+=u_e[u_e_index[m]]*u_de[u_de_index[n]];
}
delta_Kd=num/den;
delta_Kd=FuzzyPID.Ku_d*delta_Kd;
if(delta_Kd>=FuzzyPID.delta_Kd_max) delta_Kd=FuzzyPID.delta_Kd_max;
else if(delta_Kd<=-FuzzyPID.delta_Kd_max) delta_Kd=-FuzzyPID.delta_Kd_max;
FuzzyPID.Kd+=delta_Kd;
if(FuzzyPID.Kd<0) FuzzyPID.Kd=0;
FuzzyPID.A=FuzzyPID.Kp+FuzzyPID.Ki+FuzzyPID.Kd;
FuzzyPID.B=-2*FuzzyPID.Kd-FuzzyPID.Kp;
FuzzyPID.C=FuzzyPID.Kd;
delta_u=FuzzyPID.A * FuzzyPID.e + FuzzyPID.B * FuzzyPID.e_pre_1 + FuzzyPID.C * FuzzyPID.e_pre_2;
delta_u=delta_u/FuzzyPID.Ke;
if((delta_u>=0.95*FuzzyPID.target)&&(FuzzyPID.target !=0)) delta_u=0.95*FuzzyPID.target;
else if((delta_u<=-0.95*target)&&(FuzzyPID.target !=0)) delta_u=-0.95*FuzzyPID.target;
FuzzyPID.e_pre_2=FuzzyPID.e_pre_1;
FuzzyPID.e_pre_1=FuzzyPID.e;
return delta_u;
}
//**********************************************************************
//* 函 数 名: SetMf()
//* 软件用途: 设置模糊隶属度函数的类型和参数
//* 输 入 项: mf_type_*：(误差/误差增量/Kp/Ki/Kd)模糊隶属度函数的类型
//* *_mf：(误差/误差增量/Kp/Ki/Kd)模糊隶属度函数的参数
//* 返 回 值:
//**********************************************************************
void SetMf(const string & mf_type_e,float *e_mf,
const string & mf_type_de,float *de_mf,
const string & mf_type_Kp,float *Kp_mf,
const string & mf_type_Ki,float *Ki_mf,
const string & mf_type_Kd,float *Kd_mf)
{
SetMf_Sub(mf_type_e,e_mf,0);
SetMf_Sub(mf_type_de,de_mf,1);
SetMf_Sub(mf_type_Kp,Kp_mf,2);
SetMf_Sub(mf_type_Ki,Ki_mf,3);
SetMf_Sub(mf_type_Kd,Kd_mf,4);
}
//**********************************************************************
//* 函 数 名: SetMf_Sub()
//* 软件用途: 设置模糊隶属度函数的子函数
//* 输 入 项: type:隶属度函数类型；
//* paras:隶属度函数的参数;
//* n:0误差，1误差增量，2:Kp，3:Ki，4:Kd；
//* 返 回 值:
//**********************************************************************
void SetMf_Sub(const string & type,float *paras,int n)
{
int N_mf_e,N_mf_de,N_mf_Kp,N_mf_Ki,N_mf_Kd;
switch(n)
{
case 0:
if(type=="trimf"||type=="gaussmf"||type=="trapmf")
FuzzyPID.mf_t_e=type;
if(FuzzyPID.mf_t_e=="trimf")
N_mf_e=3;
else if(FuzzyPID.mf_t_e=="gaussmf")
N_mf_e=2;
else if(FuzzyPID.mf_t_e=="trapmf")
N_mf_e=4;
//FuzzyPID.e_mf_paras=new float [N*N_mf_e];
for(int i=0;i<N*N_mf_e;i++)
FuzzyPID.e_mf_paras[i]=paras[i];
break;
case 1:
if(type=="trimf"||type=="gaussmf"||type=="trapmf")
FuzzyPID.mf_t_de=type;
if(FuzzyPID.mf_t_de=="trimf")
N_mf_de=3;
else if(FuzzyPID.mf_t_de=="gaussmf")
N_mf_de=2;
else if(FuzzyPID.mf_t_de=="trapmf")
N_mf_de=4;
//FuzzyPID.de_mf_paras=new float [N*N_mf_de];
for(int i=0;i<N*N_mf_de;i++)
FuzzyPID.de_mf_paras[i]=paras[i];
break;
case 2:
if(type=="trimf"||type=="gaussmf"||type=="trapmf")
FuzzyPID.mf_t_Kp=type;
if(FuzzyPID.mf_t_Kp=="trimf")
N_mf_Kp=3;
else if(FuzzyPID.mf_t_Kp=="gaussmf")
N_mf_Kp=2;
else if(FuzzyPID.mf_t_Kp=="trapmf")
N_mf_Kp=4;
//FuzzyPID.Kp_mf_paras=new float [N*N_mf_Kp];
for(int i=0;i<N*N_mf_Kp;i++)
FuzzyPID.Kp_mf_paras[i]=paras[i];
break;
case 3:
if(type=="trimf"||type=="gaussmf"||type=="trapmf")
FuzzyPID.mf_t_Ki=type;
if(FuzzyPID.mf_t_Ki=="trimf")
N_mf_Ki=3;
else if(FuzzyPID.mf_t_Ki=="gaussmf")
N_mf_Ki=2;
else if(FuzzyPID.mf_t_Ki=="trapmf")
N_mf_Ki=4;
//FuzzyPID.Ki_mf_paras=new float [N*N_mf_Ki];
for(int i=0;i<N*N_mf_Ki;i++)
FuzzyPID.Ki_mf_paras[i]=paras[i];
break;
case 4:
if(type=="trimf"||type=="gaussmf"||type=="trapmf")
FuzzyPID.mf_t_Kd=type;
if(FuzzyPID.mf_t_Kd=="trimf")
N_mf_Kd=3;
else if(FuzzyPID.mf_t_Kd=="gaussmf")
N_mf_Kd=2;
else if(FuzzyPID.mf_t_Kd=="trapmf")
N_mf_Kd=4;
//FuzzyPID.Kd_mf_paras=new float [N*N_mf_Kd];
for(int i=0;i<N*N_mf_Kd;i++)
FuzzyPID.Kd_mf_paras[i]=paras[i];
break;
default: break;
}
}
//**********************************************************************
//* 函 数 名: SetRuleMatrix()
//* 软件用途: 设置模糊规则Matrix
//* 输 入 项: kp_m:Kp模糊规则;
//* ki_m:Ki模糊规则;
//* kd_m:Kd模糊规则;
//* 返 回 值:
//**********************************************************************
void SetRuleMatrix(int kp_m[N][N],int ki_m[N][N],int kd_m[N][N])
{
for(int i=0;i<N;i++)
for(int j=0;j<N;j++)
{
FuzzyPID.Kp_rule_matrix[i][j]=kp_m[i][j];
FuzzyPID.Ki_rule_matrix[i][j]=ki_m[i][j];
FuzzyPID.Kd_rule_matrix[i][j]=kd_m[i][j];
}
}
//**********************************************************************
//* 函 数 名: Init_PID_Val()
//* 软件用途: 初始化PID参数
//* 输 入 项: fuzzyLimit:[0]误差最大值，[1]误差增量最大值，[2]Kp最大值，[3]Ki最大值，[4]Kd最大值；
//* pidInitVal:[0]Kp初始值，[1]Ki初始值，[2]Kd初始值;
//* 返 回 值:
//**********************************************************************
void Init_PID_Val(float *fuzzyLimit,float *pidInitVal)
{
FuzzyPID.target=0;
FuzzyPID.actual=0;
FuzzyPID.e=0;
FuzzyPID.e_pre_1=0;
FuzzyPID.e_pre_2=0;
FuzzyPID.de=FuzzyPID.e-FuzzyPID.e_pre_1;
FuzzyPID.emax=fuzzyLimit[0];
FuzzyPID.demax=fuzzyLimit[1];
FuzzyPID.delta_Kp_max=fuzzyLimit[2];
FuzzyPID.delta_Ki_max=fuzzyLimit[3];
FuzzyPID.delta_Kd_max=fuzzyLimit[4];
FuzzyPID.Ke=(N/2)/FuzzyPID.emax;
FuzzyPID.Kde=(N/2)/FuzzyPID.demax;
FuzzyPID.Ku_p=FuzzyPID.delta_Kp_max/(N/2);
FuzzyPID.Ku_i=FuzzyPID.delta_Ki_max/(N/2);
FuzzyPID.Ku_d=FuzzyPID.delta_Kd_max/(N/2);
FuzzyPID.mf_t_e="No type";
FuzzyPID.mf_t_de="No type";
FuzzyPID.mf_t_Kp="No type";
FuzzyPID.mf_t_Ki="No type";
FuzzyPID.mf_t_Kd="No type";
FuzzyPID.e_mf_paras=NULL;
FuzzyPID.de_mf_paras=NULL;
FuzzyPID.Kp_mf_paras=NULL;
FuzzyPID.Ki_mf_paras=NULL;
FuzzyPID.Kd_mf_paras=NULL;
FuzzyPID.Kp=pidInitVal[0];
FuzzyPID.Ki=pidInitVal[1];
FuzzyPID.Kd=pidInitVal[2];
FuzzyPID.A=FuzzyPID.Kp+FuzzyPID.Ki+FuzzyPID.Kd;
FuzzyPID.B=-2*FuzzyPID.Kd-FuzzyPID.Kp;
FuzzyPID.C=FuzzyPID.Kd;
}
//**********************************************************************
//* 函 数 名:
//* 软件用途: 三角隶属度函数
//* 输 入 项:
//* 返 回 值: u
//*
//**********************************************************************
float Trimf(float x,float a,float b,float c)
{
float u;
if(x>=a&&x<=b)
u=(x-a)/(b-a);
else if(x>b&&x<=c)
u=(c-x)/(c-b);
else
u=0.0;
return u;
}
//正态隶属度函数
float Gaussmf(float x,float ave,float sigma)
{
float u;
if(sigma<0)
{
cout<<"In gaussmf, sigma must larger than 0"<<endl;
}
u=exp(-pow(((x-ave)/sigma),2));
return u;
}
//梯形隶属度函数
float Trapmf(float x,float a,float b,float c,float d)
{
float u;
if(x>=a&&x<b)
u=(x-a)/(b-a);
else if(x>=b&&x<c)
u=1;
else if(x>=c&&x<=d)
u=(d-x)/(d-c);
else
u=0;
return u;
}
H文件
#ifndef __FUZZY_PID__H
#define __FUZZY_PID__H
/* Includes ------------------------------------------------------------------*/
#include "gd32f4xx.h"
#include "Struct_Define.h"
/* Exported types类型 ------------------------------------------------------------*/
const static int N=7;
#define NB -3
#define NM -2
#define NS -1
#define ZO 0
#define PS 1
#define PM 2
#define PB 3
/* Exported constants变量 --------------------------------------------------------*/
/* Exported macro宏 ------------------------------------------------------------*/
float trimf(float x,float a,float b,float c); //三角隶属度函数
float gaussmf(float x,float ave,float sigma); //正态隶属度函数
float trapmf(float x,float a,float b,float c,float d); //梯形隶属度函数
void setMf(const string & mf_type_e,float *e_mf,
const string & mf_type_de,float *de_mf,
const string & mf_type_Kp,float *Kp_mf,
const string & mf_type_Ki,float *Ki_mf,
const string & mf_type_Kd,float *Kd_mf); //设置模糊隶属度函数的参数
void SetMf_Sub(const string & type,float *paras,int n);//设置模糊隶属度函数的子函数
void SetRuleMatrix(int kp_m[N][N],int ki_m[N][N],int kd_m[N][N]); //设置模糊规则
void Init_PID_Val(float *fuzzyLimit,float *pidInitVal); //初始化PID参数
/* Exported functions功能 ------------------------------------------------------- */
void Init_Delta_Fuzzy(void);//初始化模糊PID参数变量
float Realize(float t,float a); //实现模糊控制
typedef struct
{
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模糊规则矩阵
string mf_t_e; //e的隶属度函数类型
string mf_t_de; //de的隶属度函数类型
string mf_t_Kp; //kp的隶属度函数类型
string mf_t_Ki; //ki的隶属度函数类型
string 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;
float A;
float B;
float C;
}FUZZYPID,*FuzzyPID; //Kalman Filter parameter
#endif